diff --git a/include/zeus/CMatrix3f.hpp b/include/zeus/CMatrix3f.hpp
index d82a881..39bd7fc 100644
--- a/include/zeus/CMatrix3f.hpp
+++ b/include/zeus/CMatrix3f.hpp
@@ -110,12 +110,13 @@ public:
         return vec[i];
     }
 
-    inline const CMatrix3f orthonormalized()
+    inline const CMatrix3f orthonormalized() const
     {
         CMatrix3f ret;
-        ret.vec[0] = vec[0].normalized();
-        ret.vec[1] = vec[2].normalized();
-        ret.vec[2] = vec[1].normalized();
+        ret[0] = vec[0].normalized();
+        ret[2] = ret[0].cross(vec[1]);
+        ret[2].normalize();
+        ret[1] = ret[2].cross(ret[0]);
         return ret;
     }
 
diff --git a/include/zeus/CTransform.hpp b/include/zeus/CTransform.hpp
index 0035e9e..0075f7d 100644
--- a/include/zeus/CTransform.hpp
+++ b/include/zeus/CTransform.hpp
@@ -214,6 +214,14 @@ public:
         return ret;
     }
 
+    inline void orthonormalize()
+    {
+        m_basis[0].normalize();
+        m_basis[2] = m_basis[0].cross(m_basis[1]);
+        m_basis[2].normalize();
+        m_basis[1] = m_basis[2].cross(m_basis[0]);
+    }
+
     CMatrix3f m_basis;
     CVector3f m_origin;
 };