diff --git a/include/J3ML/LinearAlgebra/Matrix3x3.h b/include/J3ML/LinearAlgebra/Matrix3x3.h
index 3d1a16b..1f8dc14 100644
--- a/include/J3ML/LinearAlgebra/Matrix3x3.h
+++ b/include/J3ML/LinearAlgebra/Matrix3x3.h
@@ -43,6 +43,8 @@ namespace LinearAlgebra {
 
         Vector3 GetRow(int index) const;
         Vector3 GetColumn(int index) const;
+        Vector3 Diagonal() const;
+
         float At(int x, int y) const;
 
         void SetRotatePart(const Vector3& a, float angle);
diff --git a/src/J3ML/LinearAlgebra/Matrix3x3.cpp b/src/J3ML/LinearAlgebra/Matrix3x3.cpp
index 97107f0..ecabc4a 100644
--- a/src/J3ML/LinearAlgebra/Matrix3x3.cpp
+++ b/src/J3ML/LinearAlgebra/Matrix3x3.cpp
@@ -278,5 +278,12 @@ namespace LinearAlgebra {
         return m2;
     }
 
+    Vector3 Matrix3x3::Diagonal() const {
+        return {elems[0][0],
+                elems[1][1],
+                elems[2][2]
+        };
+    }
+
 }