Giga Geometry Implementation

src/J3ML/LinearAlgebra/Matrix4x4.cpp

@@ -646,4 +646,64 @@ namespace J3ML::LinearAlgebra {
                && GetRow(1).IsPerpendicular(GetRow(2), epsilon);
     }
 
+    Vector4 Matrix4x4::Col(int i) const { return GetColumn(i);}
+
+    Vector4 Matrix4x4::Row(int i) const { return GetRow(i);}
+
+    Vector4 Matrix4x4::Col3(int i) const { return GetColumn3(i);}
+
+    Vector4 Matrix4x4::Row3(int i) const { return GetRow3(i);}
+
+    Vector3 Matrix4x4::TransformDir(float tx, float ty, float tz) const
+    {
+        assert(!this->ContainsProjection()); // This function does not divide by w or output it, so cannot have projection.
+        return Vector3(At(0, 0) * tx + At(0, 1) * ty + At(0, 2) * tz,
+                      At(1, 0) * tx + At(1, 1) * ty + At(1, 2) * tz,
+                      At(2, 0) * tx + At(2, 1) * ty + At(2, 2) * tz);
+
+    }
+
+    void Matrix4x4::InverseOrthonormal()
+    {
+        assert(!ContainsProjection());
+
+        // a) Transpose the top-left 3x3 part in-place to produce R^t.
+        Swap(elems[0][1], elems[1][0]);
+        Swap(elems[0][2], elems[2][0]);
+        Swap(elems[1][2], elems[2][1]);
+
+        // b) Replace the top-right 3x1 part by computing R^t(-T).
+        SetTranslatePart(TransformDir(-At(0, 3), -At(1, 3), -At(2, 3)));
+
+    }
+
+    void Matrix4x4::SetCol(int col, const float *data) {
+        assert(data != nullptr);
+        SetCol(col, data[0], data[1], data[2], data[3]);
+    }
+
+    void Matrix4x4::SetCol(int column, float m_0c, float m_1c, float m_2c, float m_3c)
+    {
+        assert(column >= 0);
+        assert(column < Cols);
+        assert(std::isfinite(m_0c));
+        assert(std::isfinite(m_1c));
+        assert(std::isfinite(m_2c));
+        assert(std::isfinite(m_3c));
+        At(0, column) = m_0c;
+        At(1, column) = m_1c;
+        At(2, column) = m_2c;
+        At(3, column) = m_3c;
+    }
+
+    void Matrix4x4::SetCol(int column, const Vector3 &columnVector, float m_3c)
+    {
+        SetCol(column, columnVector.x, columnVector.y, columnVector.z, m_3c);
+    }
+
+    void Matrix4x4::SetCol(int column, const Vector4 &columnVector)
+    {
+        SetCol(column, columnVector.x, columnVector.y, columnVector.z, columnVector.w);
+    }
+
 }