Implement Matrix3x3 FromScale and ScaleBy

src/J3ML/LinearAlgebra/Matrix3x3.cpp

@@ -313,16 +313,54 @@ namespace LinearAlgebra {
     }
 
     Matrix3x3 Matrix3x3::FromRS(const Quaternion &rotate, const Vector3 &scale) {
-        return Matrix3x3(rotate) * Matrix3x3::Scale(scale);
+        return Matrix3x3(rotate) * Matrix3x3::FromScale(scale);
     }
 
     Matrix3x3 Matrix3x3::FromRS(const Matrix3x3 &rotate, const Vector3 &scale) {
-        return rotate * Matrix3x3::Scale(scale);
+        return rotate * Matrix3x3::FromScale(scale);
     }
 
     Matrix3x3 Matrix3x3::FromQuat(const Quaternion &orientation) {
         return Matrix3x3(orientation);
     }
 
+    Matrix3x3 Matrix3x3::ScaleBy(const Vector3 &rhs) {
+        return *this * FromScale(rhs);
+    }
+
+    Vector3 Matrix3x3::GetScale() const {
+        return Vector3(GetColumn(0).Length(), GetColumn(1).Length(), GetColumn(2).Length());
+    }
+
+    Vector3 Matrix3x3::operator[](int row) const {
+        return Vector3{elems[row][0], elems[row][1], elems[row][2]};
+    }
+
+    Matrix3x3 Matrix3x3::FromScale(const Vector3 &scale) {
+        Matrix3x3 m;
+        m.At(0,0) = scale.x;
+        m.At(1,1) = scale.y;
+        m.At(2,2) = scale.z;
+        return m;
+    }
+
+    Matrix3x3 Matrix3x3::FromScale(float sx, float sy, float sz) {
+        Matrix3x3 m;
+        m.At(0,0) = sx;
+        m.At(1,1) = sy;
+        m.At(2,2) = sz;
+        return m;
+    }
+
+    void Matrix3x3::Orthonormalize(int c0, int c1, int c2) {
+        Vector3 v0 = GetColumn(c0);
+        Vector3 v1 = GetColumn(c1);
+        Vector3 v2 = GetColumn(c2);
+        Vector3::Orthonormalize(v0, v1, v2);
+        SetColumn(c0, v0);
+        SetColumn(c1, v1);
+        SetColumn(c2, v2);
+    }
+
 }