Implement Matrix4x4::Set() Adjugate() SetIdentity() InverseColOrthogonal()

include/J3ML/LinearAlgebra/Matrix4x4.h

@@ -565,7 +565,7 @@ namespace J3ML::LinearAlgebra {
         void Set(float _00, float _01, float _02, float _03,
                  float _10, float _11, float _12, float _13,
                  float _20, float _21, float _22, float _23,
-                 float _30, float _31, float _32, float _34);
+                 float _30, float _31, float _32, float _33);
 
         /// Sets this to be a copy of the matrix rhs.
         void Set(const Matrix4x4 &rhs);
@@ -573,7 +573,7 @@ namespace J3ML::LinearAlgebra {
         /// Sets all values of this matrix.
         /** @param values The values in this array will be copied over to this matrix. The source must contain 16 floats in row-major order
             (the same order as the Set() ufnction above has its input parameters in. */
-        void Set(const float *values);
+        void Set(const float *p);
 
         /// Sets this matrix to equal the identity.
         void SetIdentity();

src/J3ML/LinearAlgebra/Matrix4x4.cpp

@@ -959,4 +959,65 @@ namespace J3ML::LinearAlgebra {
         return true;
     }
 
+    void
+    Matrix4x4::Set(float _00, float _01, float _02, float _03, float _10, float _11, float _12, float _13, float _20,
+                   float _21, float _22, float _23, float _30, float _31, float _32, float _33) {
+        At(0, 0) = _00; At(0, 1) = _01; At(0, 2) = _02; At(0, 3) = _03;
+        At(1, 0) = _10; At(1, 1) = _11; At(1, 2) = _12; At(1, 3) = _13;
+        At(2, 0) = _20; At(2, 1) = _21; At(2, 2) = _22; At(2, 3) = _23;
+        At(3, 0) = _30; At(3, 1) = _31; At(3, 2) = _32; At(3, 3) = _33;
+    }
+
+    void Matrix4x4::Set(const Matrix4x4 &rhs) {
+        Set(rhs.ptr());
+    }
+
+    void Matrix4x4::Set(const float *p) {
+        assert(p);
+        Set(p[0], p[1], p[2], p[3], p[4], p[5], p[6], p[7], p[8], p[9], p[10], p[11], p[12], p[13], p[14], p[15]);
+    }
+
+    Matrix4x4 Matrix4x4::Adjugate() const {
+        Matrix4x4 a;
+        for(int iy = 0; iy < Rows; ++iy)
+            for(int ix = 0; ix < Cols; ++ix)
+                a[iy][ix] = (((ix+iy) & 1) != 0) ? -Minor(iy, ix) : Minor(iy, ix);
+
+        return a;
+    }
+
+    void Matrix4x4::SetIdentity() {
+        Set(1,0,0,0,
+            0,1,0,0,
+            0,0,1,0,
+            0,0,0,1);
+    }
+
+    bool Matrix4x4::InverseColOrthogonal() {
+        assert(IsColOrthogonal3());
+        float s1 = Vector3(At(0,0), At(1,0), At(2,0)).LengthSquared();
+        float s2 = Vector3(At(0,1), At(1,1), At(2,1)).LengthSquared();
+        float s3 = Vector3(At(0,2), At(1,2), At(2,2)).LengthSquared();
+
+        if (s1 < 1e-8f || s2 < 1e-8f || s3 < 1e-8f)
+            return false;
+
+        s1 = 1.f / s1;
+        s2 = 1.f / s2;
+        s3 = 1.f / s3;
+
+        Swap(At(0,1), At(1,0));
+        Swap(At(0,2), At(2,0));
+        Swap(At(1,2), At(2,1));
+
+        At(0,0) *= s1; At(0,1) *= s1; At(0,2) *= s1;
+        At(1,0) *= s2; At(1,1) *= s2; At(1,2) *= s2;
+        At(2,0) *= s3; At(2,1) *= s3; At(2,2) *= s3;
+
+        SetTranslatePart(TransformDir(-At(0, 3), -At(1,3), -At(2,3)));
+        assert(IsRowOrthogonal3());
+        return true;
+    }
+
+
 }