Implement Mat4x4::Swaps

include/J3ML/LinearAlgebra/Matrix3x3.h

@@ -74,10 +74,7 @@ namespace LinearAlgebra {
 
         static Matrix3x3 LookAt(const Vector3& forward, const Vector3& target, const Vector3& localUp, const Vector3& worldUp);
 
-        static Matrix3x3 FromQuat(const Quaternion& orientation)
-        {
-            return Matrix3x3(orientation);
-        }
+        static Matrix3x3 FromQuat(const Quaternion& orientation);
 
         Quaternion ToQuat() const;
 
@@ -87,14 +84,8 @@ namespace LinearAlgebra {
         // Transforming a vector v using this matrix computes the vector
         // v' == M * v == R*S*v == (R * (S * v)) which means the scale operation
         // is applied to the vector first, followed by rotation, and finally translation
-        static Matrix3x3 FromRS(const Quaternion& rotate, const Vector3& scale)
-        {
-            return Matrix3x3(rotate) * Matrix3x3::Scale(scale);
-        }
-        static Matrix3x3 FromRS(const Matrix3x3 &rotate, const Vector3& scale)
-        {
-            return rotate * Matrix3x3::Scale(scale);
-        }
+        static Matrix3x3 FromRS(const Quaternion& rotate, const Vector3& scale);
+        static Matrix3x3 FromRS(const Matrix3x3 &rotate, const Vector3& scale);
 
 
         /// Creates a new transformation matrix that scales by the given factors.

include/J3ML/LinearAlgebra/Matrix4x4.h

@@ -116,15 +116,77 @@ namespace LinearAlgebra {
         void SetRow(int row, const Vector4& rowVector);
         void SetRow(int row, float m_r0, float m_r1, float m_r2, float m_r3);
 
-
         Vector4 GetRow(int index) const;
         Vector4 GetColumn(int index) const;
         Vector3 GetRow3(int index) const;
-
         Vector3 GetColumn3(int index) const;
+
         float &At(int row, int col);
         float At(int x, int y) const;
 
+        template <typename T>
+        void Swap(T &a, T &b)
+        {
+            T temp = std::move(a);
+            a = std::move(b);
+            b = std::move(temp);
+        }
+
+
+        void SwapColumns(int col1, int col2)
+        {
+            Swap(At(0, col1), At(0, col2));
+            Swap(At(1, col1), At(1, col2));
+            Swap(At(2, col1), At(2, col2));
+            Swap(At(3, col1), At(3, col2));
+        }
+
+        /// Swaps two rows.
+        void SwapRows(int row1, int row2)
+        {
+            Swap(At(row1, 0), At(row2, 0));
+            Swap(At(row1, 1), At(row2, 1));
+            Swap(At(row1, 2), At(row2, 2));
+            Swap(At(row1, 3), At(row2, 3));
+        }
+        /// Swapsthe xyz-parts of two rows element-by-element
+        void SwapRows3(int row1, int row2)
+        {
+            Swap(At(row1, 0), At(row2, 0));
+            Swap(At(row1, 1), At(row2, 1));
+            Swap(At(row1, 2), At(row2, 2));
+        }
+
+        /// Algorithm from Eric Lengyel's Mathematics for 3D Game Programming & Computer Graphics, 2nd Ed.
+        void Pivot()
+        {
+            int rowIndex = 0;
+
+            for(int col = 0; col < Cols; ++col)
+            {
+                int greatest = rowIndex;
+
+                // find the rowIndex k with k >= 1 for which Mkj has the largest absolute value.
+                for(int i = rowIndex; i < Rows; ++i)
+                    if (std::abs(At(i, col)) > std::abs(At(greatest, col)))
+                        greatest = i;
+
+                if (std::abs(At(greatest, col)) != 0)
+                {
+                    if (rowIndex != greatest)
+                        SwapRows(rowIndex, greatest); // the greatest now in rowIndex
+
+                    ScaleRow(rowIndex, 1.f/At(rowIndex, col));
+
+                    for(int r = 0; r < Rows; ++r)
+                        if (r != rowIndex)
+                            SetRow(r, GetRow(r) - GetRow(rowIndex) * At(r, col));
+
+                    ++rowIndex;
+                }
+            }
+        }
+
         /// Tests if this matrix does not contain any NaNs or infs.
         /** @return Returns true if the entries of this float4x4 are all finite, and do not contain NaN or infs. */
         bool IsFinite() const;
@@ -190,9 +252,6 @@ namespace LinearAlgebra {
         static Matrix4x4 OpenGLPerspProjLH(float nearPlane, float farPlane, float hViewportSize, float vViewportSize);
         static Matrix4x4 OpenGLPerspProjRH(float nearPlane, float farPlane, float hViewportSize, float vViewportSize);
 
-        Vector4 GetRow() const;
-        Vector4 GetColumn() const;
-
         Vector4 operator[](int row);
 
         Matrix4x4 operator-() const;

src/J3ML/LinearAlgebra/Matrix3x3.cpp

@@ -300,5 +300,29 @@ namespace LinearAlgebra {
         return elems[row][col];
     }
 
+    Vector3 Matrix3x3::WorldZ() const {
+        return GetColumn(2);
+    }
+
+    Vector3 Matrix3x3::WorldY() const {
+        return GetColumn(1);
+    }
+
+    Vector3 Matrix3x3::WorldX() const {
+        return GetColumn(0);
+    }
+
+    Matrix3x3 Matrix3x3::FromRS(const Quaternion &rotate, const Vector3 &scale) {
+        return Matrix3x3(rotate) * Matrix3x3::Scale(scale);
+    }
+
+    Matrix3x3 Matrix3x3::FromRS(const Matrix3x3 &rotate, const Vector3 &scale) {
+        return rotate * Matrix3x3::Scale(scale);
+    }
+
+    Matrix3x3 Matrix3x3::FromQuat(const Quaternion &orientation) {
+        return Matrix3x3(orientation);
+    }
+
 }
 

src/J3ML/LinearAlgebra/Matrix4x4.cpp

@@ -428,4 +428,16 @@ namespace LinearAlgebra {
         m.SetRow(3, 0,0,0,1);
         return m;
     }
+
+    Vector4 Matrix4x4::GetRow(int index) const {
+        return { At(index, 0), At(index, 1), At(index, 2), At(index, 3)};
+    }
+
+    Vector4 Matrix4x4::GetColumn(int index) const {
+        return { At(0, index), At(1, index), At(2, index), At(3, index)};
+    }
+
+    Vector3 Matrix4x4::GetRow3(int index) const {
+        return Vector3{ At(index, 0), At(index, 1), At(index, 2)};
+    }
 }