diff --git a/include/J3ML/LinearAlgebra/Matrix4x4.h b/include/J3ML/LinearAlgebra/Matrix4x4.h
index 5cf6132..b10693e 100644
--- a/include/J3ML/LinearAlgebra/Matrix4x4.h
+++ b/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;
diff --git a/src/J3ML/LinearAlgebra/Matrix4x4.cpp b/src/J3ML/LinearAlgebra/Matrix4x4.cpp
index 68a39f4..016f7bd 100644
--- a/src/J3ML/LinearAlgebra/Matrix4x4.cpp
+++ b/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) };
+    }
 }
\ No newline at end of file