Implement Mat4x4::Diagonal

include/J3ML/LinearAlgebra/Matrix4x4.h

@@ -92,6 +92,7 @@ namespace LinearAlgebra {
         bool IsInvertible(float epsilon = 1e-3f) const;
 
         Vector4 Diagonal() const;
+        Vector3 Diagonal3() const;
         Vector4 WorldX() const;
         Vector4 WorldY() const;
         Vector4 WorldZ() const;

src/J3ML/LinearAlgebra/Matrix4x4.cpp

@@ -366,4 +366,12 @@ namespace LinearAlgebra {
         copy.elems[3][0] = elems[0][3]; copy.elems[3][1] = elems[1][3]; copy.elems[3][2] = elems[2][3]; copy.elems[3][3] = elems[3][3];
         return copy;
     }
+
+    Vector4 Matrix4x4::Diagonal() const {
+        return Vector4{At(0, 0), At(1,1), At(2,2), At(3,3)};
+    }
+
+    Vector3 Matrix4x4::Diagonal3() const {
+        return Vector3 { At(0, 0), At(1,1), At(2,2) };
+    }
 }