From b721e09ef5707d554cd7e70b87262bbd258ded42 Mon Sep 17 00:00:00 2001
From: Jack Andersen <jackoalan@gmail.com>
Date: Wed, 9 Mar 2016 17:47:01 -1000
Subject: [PATCH] CTransform::orthonormalize added

---
 include/zeus/CMatrix3f.hpp  | 9 +++++----
 include/zeus/CTransform.hpp | 8 ++++++++
 2 files changed, 13 insertions(+), 4 deletions(-)

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;
 };