diff --git a/CMakeLists.txt b/CMakeLists.txt
index 0e8a1d3..a216f8b 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -7,7 +7,7 @@ endif()
 include_directories(include ${ATHENA_INCLUDE_DIR})
 
 if(NOT WIN32)
-#set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -msse4.1")
+set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -msse4.1 -std=c++14")
 endif()
 
 add_library(Math
diff --git a/include/CAABox.hpp b/include/CAABox.hpp
index 5fa3ffe..de5ba35 100644
--- a/include/CAABox.hpp
+++ b/include/CAABox.hpp
@@ -2,20 +2,34 @@
 #define CAABOX_HPP
 
 #include "CVector3f.hpp"
+#include "CUnitVector.hpp"
 #include "CTransform.hpp"
 #include "CPlane.hpp"
+#include "CLine.hpp"
 #include "Math.hpp"
 #include <Athena/IStreamReader.hpp>
 
 namespace Zeus
 {
-class ZE_ALIGN(16) CAABox
+class alignas(16) CAABox
 {
 public:
     ZE_DECLARE_ALIGNED_ALLOCATOR();
 
-    enum EBoxEdgeID
+    enum EBoxEdgeId
     {
+        UnknownEdge0,
+        UnknownEdge1,
+        UnknownEdge2,
+        UnknownEdge3,
+        UnknownEdge4,
+        UnknownEdge5,
+        UnknownEdge6,
+        UnknownEdge7,
+        UnknownEdge8,
+        UnknownEdge9,
+        UnknownEdge10,
+        UnknownEdge11
     };
 
     enum EBoxFaceID
@@ -113,6 +127,52 @@ public:
 
     CVector3f volume() const {return (m_max - m_min) * 0.5f;}
 
+    inline CLine getEdge(EBoxEdgeId id)
+    {
+        switch (id)
+        {
+            case UnknownEdge0:
+                return CLine({m_min.x, m_min.y, m_min.z},
+                             CUnitVector3f({m_min.x, m_min.y, m_max.z}));
+            case UnknownEdge1:
+                return CLine({m_max.x, m_min.y, m_min.z},
+                             CUnitVector3f({m_min.x, m_min.y, m_min.z}));
+            case UnknownEdge2:
+                return CLine({m_max.x, m_min.y, m_max.z},
+                             CUnitVector3f({m_max.x, m_min.y, m_max.z}));
+            case UnknownEdge3:
+                return CLine({m_min.x, m_min.y, m_max.z},
+                             CUnitVector3f({m_max.x, m_min.y, m_max.z}));
+            case UnknownEdge4:
+                return CLine({m_max.x, m_max.y, m_min.z},
+                             CUnitVector3f({m_max.x, m_max.y, m_max.z}));
+            case UnknownEdge5:
+                return CLine({m_min.x, m_max.y, m_min.z},
+                             CUnitVector3f({m_max.x, m_max.y, m_min.z}));
+            case UnknownEdge6:
+                return CLine({m_min.x, m_max.y, m_max.z},
+                             CUnitVector3f({m_min.x, m_max.y, m_min.z}));
+            case UnknownEdge7:
+                return CLine({m_max.x, m_max.y, m_max.z},
+                             CUnitVector3f({m_min.x, m_max.y, m_max.z}));
+            case UnknownEdge8:
+                return CLine({m_min.x, m_max.y, m_max.z},
+                             CUnitVector3f({m_min.x, m_min.y, m_max.z}));
+            case UnknownEdge9:
+                return CLine({m_min.x, m_max.y, m_min.z},
+                             CUnitVector3f({m_min.x, m_min.y, m_min.z}));
+            case UnknownEdge10:
+                return CLine({m_max.x, m_max.y, m_min.z},
+                             CUnitVector3f({m_max.x, m_min.y, m_min.z}));
+            case UnknownEdge11:
+                return CLine({m_max.x, m_max.y, m_max.z},
+                             CUnitVector3f({m_max.x, m_min.y, m_max.z}));
+            default:
+                return CLine({m_min.x, m_min.y, m_min.z},
+                             CUnitVector3f({m_min.x, m_min.y, m_max.z}));
+        }
+    }
+
     inline CAABox getTransformedAABox(const CTransform& xfrm)
     {
         CAABox box;
diff --git a/include/CAxisAngle.hpp b/include/CAxisAngle.hpp
index c9fe56f..61f251f 100644
--- a/include/CAxisAngle.hpp
+++ b/include/CAxisAngle.hpp
@@ -3,24 +3,25 @@
 
 #include "Global.hpp"
 #include "CVector3f.hpp"
+#include "CUnitVector.hpp"
 
 namespace Zeus
 {
-struct ZE_ALIGN(16) CAxisAngle
+struct alignas(16) CAxisAngle : CVector3f
 {
     ZE_DECLARE_ALIGNED_ALLOCATOR();
     
-    CAxisAngle()
-        : axis(CVector3f::skOne),
-          angle(0)
-    {}
-    CAxisAngle(const CVector3f& axis, float angle)
-        : axis(axis),
-          angle(angle)
+    CAxisAngle() = default;
+    CAxisAngle(const CUnitVector3f& axis, float angle)
+        : CVector3f(axis * angle)
     {}
 
-    CVector3f axis;
-    float     angle;
+    CAxisAngle(const CVector3f& axisAngle)
+        : CVector3f(axisAngle)
+    {}
+
+    float angle() { return magnitude(); }
+    const CVector3f& getVector() { return *this; }
 };
 }
 
diff --git a/include/CColor.hpp b/include/CColor.hpp
index 52de5f4..25b168d 100644
--- a/include/CColor.hpp
+++ b/include/CColor.hpp
@@ -22,7 +22,7 @@ typedef union
     unsigned int rgba;
 } RGBA32;
 
-class ZE_ALIGN(16) CColor
+class alignas(16) CColor
 {
 public:
     ZE_DECLARE_ALIGNED_ALLOCATOR();
diff --git a/include/CLine.hpp b/include/CLine.hpp
index e69de29..dddc4dd 100644
--- a/include/CLine.hpp
+++ b/include/CLine.hpp
@@ -0,0 +1,32 @@
+#ifndef CLINE_HPP
+#define CLINE_HPP
+
+#include "Global.hpp"
+#include "CVector3f.hpp"
+#include "CUnitVector.hpp"
+
+namespace Zeus
+{
+class alignas(16) CLine
+{
+public:
+    CLine(const CVector3f& a, const CUnitVector3f& b)
+    {
+        CVector3f ab = (b - a).normalized();
+
+        start = a;
+        if (ab.x != 0.0f || ab.y != 0.0f || ab.z != 0.0f)
+            normal = ab;
+        else
+            normal = CVector3f::skZero;
+        end = b;
+    }
+
+
+    CVector3f start;
+    CVector3f normal;
+    CUnitVector3f end;
+};
+}
+
+#endif
diff --git a/include/CMatrix3f.hpp b/include/CMatrix3f.hpp
index ec55138..2f9e04d 100644
--- a/include/CMatrix3f.hpp
+++ b/include/CMatrix3f.hpp
@@ -9,7 +9,7 @@
 namespace Zeus
 {
 class CQuaternion;
-class ZE_ALIGN(16) CMatrix3f
+class alignas(16) CMatrix3f
 {
 public:
     ZE_DECLARE_ALIGNED_ALLOCATOR();
@@ -46,6 +46,18 @@ public:
 #if __SSE__
     CMatrix3f(const __m128& r0, const __m128& r1, const __m128& r2)
     {vec[0].mVec128 = r0; vec[1].mVec128 = r1; vec[2].mVec128 = r2;}
+#endif
+#if ZE_ATHENA_TYPES
+    CMatrix3f(const atVec4f& r0, const atVec4f& r1, const atVec4f& r2)
+    {
+#if __SSE__
+        vec[0].mVec128 = r0.mVec128; vec[1].mVec128 = r1.mVec128; vec[2].mVec128 = r2.mVec128;
+#else
+        vec[0].x = r0.vec[0]; vec[0].y = r0.vec[1]; vec[0].z = r0.vec[2];
+        vec[1].x = r1.vec[0]; vec[1].y = r1.vec[1]; vec[1].z = r1.vec[2];
+        vec[2].x = r2.vec[0]; vec[2].y = r2.vec[1]; vec[2].z = r2.vec[2];
+#endif
+    }
 #endif
     CMatrix3f(const CVector3f& axis, float angle);
     CMatrix3f(const CQuaternion& quat);
@@ -87,6 +99,15 @@ public:
         return vec[i];
     }
 
+    inline const CMatrix3f orthonormalized()
+    {
+        CMatrix3f ret;
+        ret.vec[0] = vec[0].normalized();
+        ret.vec[1] = vec[2].normalized();
+        ret.vec[2] = vec[1].normalized();
+        return ret;
+    }
+
     static const CMatrix3f skIdentityMatrix3f;
     
     void transpose();
diff --git a/include/CMatrix4f.hpp b/include/CMatrix4f.hpp
index 43751a3..14e96f0 100644
--- a/include/CMatrix4f.hpp
+++ b/include/CMatrix4f.hpp
@@ -6,7 +6,7 @@
 
 namespace Zeus
 {
-class ZE_ALIGN(16) CMatrix4f
+class alignas(16) CMatrix4f
 {
 public:
     static const CMatrix4f skIdentityMatrix4f;
diff --git a/include/COBBox.hpp b/include/COBBox.hpp
index c903074..03ac901 100644
--- a/include/COBBox.hpp
+++ b/include/COBBox.hpp
@@ -7,7 +7,7 @@
 
 namespace Zeus
 {
-class ZE_ALIGN(16) COBBox
+class alignas(16) COBBox
 {
 public:
     ZE_DECLARE_ALIGNED_ALLOCATOR();
diff --git a/include/CPlane.hpp b/include/CPlane.hpp
index 46854b4..91de374 100644
--- a/include/CPlane.hpp
+++ b/include/CPlane.hpp
@@ -7,7 +7,7 @@
 
 namespace Zeus
 {
-class ZE_ALIGN(16) CPlane
+class alignas(16) CPlane
 {
 public:
     ZE_DECLARE_ALIGNED_ALLOCATOR();
@@ -43,7 +43,6 @@ public:
     {
         float nd = d;
         float mag = vec.magnitude();
-        assert(mag != 0.0f);
         mag = 1.0 / mag;
         vec *= mag;
         d = nd * mag;
diff --git a/include/CProjection.hpp b/include/CProjection.hpp
index ffa9c7f..3ef969a 100644
--- a/include/CProjection.hpp
+++ b/include/CProjection.hpp
@@ -30,7 +30,7 @@ struct SProjPersp
 };
 extern const SProjOrtho kOrthoIdentity;
 
-class ZE_ALIGN(16) CProjection
+class alignas(16) CProjection
 {
     void _updateCachedMatrix();
 public:
diff --git a/include/CQuaternion.hpp b/include/CQuaternion.hpp
index 6273dc8..397bc00 100644
--- a/include/CQuaternion.hpp
+++ b/include/CQuaternion.hpp
@@ -10,7 +10,7 @@
 
 namespace Zeus
 {
-class ZE_ALIGN(16) CQuaternion
+class alignas(16) CQuaternion
 {
 public:
     ZE_DECLARE_ALIGNED_ALLOCATOR();
diff --git a/include/CRelAngle.hpp b/include/CRelAngle.hpp
index 58325f6..7e95519 100644
--- a/include/CRelAngle.hpp
+++ b/include/CRelAngle.hpp
@@ -9,7 +9,7 @@ namespace Zeus
 /**
  * @brief The CRelAngle class represents relative angles in radians
  */
-class ZE_ALIGN(16) CRelAngle : public CVector3f
+class alignas(16) CRelAngle : public CVector3f
 {
 public:
     /**
diff --git a/include/CSphere.hpp b/include/CSphere.hpp
index cb68271..bfb8370 100644
--- a/include/CSphere.hpp
+++ b/include/CSphere.hpp
@@ -5,7 +5,7 @@
 
 namespace Zeus
 {
-class ZE_ALIGN(16) CSphere
+class alignas(16) CSphere
 {
 public:
     ZE_DECLARE_ALIGNED_ALLOCATOR();
diff --git a/include/CTransform.hpp b/include/CTransform.hpp
index e2ab4d5..f377ece 100644
--- a/include/CTransform.hpp
+++ b/include/CTransform.hpp
@@ -9,7 +9,7 @@
 
 namespace Zeus
 {
-class ZE_ALIGN(16) CTransform
+class alignas(16) CTransform
 {
 public:
     ZE_DECLARE_ALIGNED_ALLOCATOR();
diff --git a/include/CUnitVector.hpp b/include/CUnitVector.hpp
index 98084ec..37f32d7 100644
--- a/include/CUnitVector.hpp
+++ b/include/CUnitVector.hpp
@@ -5,15 +5,20 @@
 
 namespace Zeus
 {
-class ZE_ALIGN(16) CUnitVector3f : public CVector3f
+class alignas(16) CUnitVector3f : public CVector3f
 {
 public:
     ZE_DECLARE_ALIGNED_ALLOCATOR();
 
-    CUnitVector3f(const CVector3f& vec)
+    CUnitVector3f()
+        : CVector3f(0, 1, 0)
+    {
+    }
+
+    CUnitVector3f(const CVector3f& vec, bool doNormalize = false)
         : CVector3f(vec)
     {
-        if (canBeNormalized())
+        if (doNormalize && canBeNormalized())
             normalize();
     }
 };
diff --git a/include/CVector2f.hpp b/include/CVector2f.hpp
index ddebabb..975b56c 100644
--- a/include/CVector2f.hpp
+++ b/include/CVector2f.hpp
@@ -2,6 +2,7 @@
 #define CVECTOR2f_HPP
 
 #include "Global.hpp"
+#include "Math.hpp"
 #include "TVectorUnion.hpp"
 
 #include <Athena/IStreamReader.hpp>
@@ -10,7 +11,7 @@
 
 namespace Zeus
 {
-class ZE_ALIGN(16) CVector2f
+class alignas(16) CVector2f
 {
     public:
     ZE_DECLARE_ALIGNED_ALLOCATOR();
@@ -199,24 +200,15 @@ class ZE_ALIGN(16) CVector2f
     inline void normalize()
     {
         float mag = magnitude();
-        if (mag > 1e-6f)
-        {
-            mag = 1.0 / mag;
-            *this *= mag;
-        }
-        else
-            zeroOut();
+        mag = 1.0 / mag;
+        *this *= mag;
     }
 
     inline CVector2f normalized() const
     {
         float mag = magnitude();
-        if (mag > 1e-6f)
-        {
-            mag = 1.0 / mag;
-            return *this * mag;
-        }
-        return {0, 0};
+        mag = 1.0 / mag;
+        return *this * mag;
     }
 
     inline float cross(const CVector2f& rhs) const
@@ -252,9 +244,7 @@ class ZE_ALIGN(16) CVector2f
 #endif
     }
     inline float magnitude() const
-    {
-        return sqrtf(magSquared());
-    }
+    { return Math::sqrtF(magSquared()); }
 
     inline void zeroOut()
     {
@@ -287,15 +277,19 @@ class ZE_ALIGN(16) CVector2f
     }
     static CVector2f slerp(const CVector2f& a, const CVector2f& b, float t);
 
-    inline bool isNormalized(float thresh = 1e-5f) const
+    inline bool canBeNormalized() const
     {
-        return (fabs(1.0f - magSquared()) <= thresh);
+        const float epsilon = 1.1920929e-7f;
+        if (fabs(x) >= epsilon || fabs(y) >= epsilon)
+            return true;
+        return false;
     }
 
-    inline bool canBeNormalized()
-    {
-        return !isNormalized();
-    }
+    inline bool isNormalized() const
+    { return !canBeNormalized(); }
+
+    inline bool isZero() const
+    { return magSquared() <= 1.1920929e-7f; }
 
     inline float& operator[](size_t idx) {return (&x)[idx];}
     inline const float& operator[](size_t idx) const {return (&x)[idx];}
diff --git a/include/CVector3d.hpp b/include/CVector3d.hpp
index 1531537..328ed29 100644
--- a/include/CVector3d.hpp
+++ b/include/CVector3d.hpp
@@ -8,7 +8,7 @@
 
 namespace Zeus
 {
-class ZE_ALIGN(16) CVector3d
+class alignas(16) CVector3d
 {
 public:
     ZE_DECLARE_ALIGNED_ALLOCATOR();
@@ -23,13 +23,14 @@ public:
 #endif
 #if ZE_ATHENA_TYPES
     CVector3d(const atVec3d& vec)
-#if __SSE__
-        : CVector3d(vec.mVec128){}
-#else
     {
+#if __SSE__
+        mVec128[0] = vec.mVec128[0];
+        mVec128[1] = vec.mVec128[1];
+#else
         x = v[0], y = v[1], z = v[2], v[3] = 0.0f;
-    }
 #endif
+    }
 #endif
 
     CVector3d(double xyz) { splat(xyz); }
diff --git a/include/CVector3f.hpp b/include/CVector3f.hpp
index a8d59f0..ecf7e7b 100644
--- a/include/CVector3f.hpp
+++ b/include/CVector3f.hpp
@@ -2,6 +2,7 @@
 #define CVECTOR3F_HPP
 
 #include "Global.hpp"
+#include "Math.hpp"
 #include "CVector2f.hpp"
 #include "TVectorUnion.hpp"
 #include <Athena/IStreamReader.hpp>
@@ -10,27 +11,19 @@
 
 namespace Zeus
 {
-class ZE_ALIGN(16) CVector3f
+class alignas(16) CVector3f
 {
 public:
     ZE_DECLARE_ALIGNED_ALLOCATOR();
     
     inline CVector3f() {zeroOut();}
-#if __SSE__
+#if __SSE__ || __GEKKO_PS__
     CVector3f(const __m128& mVec128) : mVec128(mVec128) {v[3] = 0.0f;}
 #endif
 #if ZE_ATHENA_TYPES
     CVector3f(const atVec3f& vec)
-#if __SSE__
-    : mVec128(vec.mVec128){}
-#else
-    {
-        x = vec.vec[0], y = vec.vec[1], z = vec.vec[2], v[3] = 0.0f;
-    }
-#endif
-    CVector3f(const atVec4f& vec)
-#if __SSE__
-    : mVec128(vec.mVec128){}
+#if __SSE__ || __GEKKO_PS__
+        : mVec128(vec.mVec128){}
 #else
     {
         x = vec.vec[0], y = vec.vec[1], z = vec.vec[2], v[3] = 0.0f;
@@ -62,6 +55,8 @@ public:
     {
 #if __SSE__
         return CVector3f(_mm_add_ps(mVec128, rhs.mVec128));
+#elif __GEKKO_PS__
+        return CVector3f(__mm_gekko_add_ps(mVec128, rhs.mVec128));
 #else
         return CVector3f(x + rhs.x, y + rhs.y, z + rhs.z);
 #endif
@@ -78,6 +73,8 @@ public:
     {
 #if __SSE__
         return CVector3f(_mm_sub_ps(_mm_xor_ps(mVec128, mVec128), mVec128));
+#elif __GEKKO_PS__
+        return CVector3f(_mm_gekko_neg_ps(mVec128));
 #else
         return CVector3f(-x, -y, -z);
 #endif
@@ -109,27 +106,39 @@ public:
     }
     inline CVector3f operator-(float val) const
     {
-#if __SSE__
+#if __SSE__ || __GEKKO_PS__
         TVectorUnion splat = {{val, val, val, 0.0}};
+#endif
+#if __SSE__
         return CVector3f(_mm_sub_ps(mVec128, splat.mVec128));
+#elif __GEKKO_PS__
+        return CVector3f(_mm_gekko_sub_ps(mVec128, splat.mVec128));
 #else
         return CVector3f(x - val, y - val, z - val);
 #endif
     }
     inline CVector3f operator*(float val) const
     {
-#if __SSE__
+#if __SSE__ || __GEKKO_PS__
         TVectorUnion splat = {{val, val, val, 0.0}};
+#endif
+#if __SSE__
         return CVector3f(_mm_mul_ps(mVec128, splat.mVec128));
+#elif __GEKKO_PS__
+        return CVector3f(_mm_gekko_mul_ps(mVec128, splat.mVec128));
 #else
         return CVector3f(x * val, y * val, z * val);
 #endif
     }
     inline CVector3f operator/(float val) const
     {
-#if __SSE__
+#if __SSE__ || __GEKKO_PS__
         TVectorUnion splat = {{val, val, val, 0.0}};
+#endif
+#if __SSE__
         return CVector3f(_mm_div_ps(mVec128, splat.mVec128));
+#elif __GEKKO_PS__
+        return CVector3f(_mm_gekko_div_ps(mVec128, splat.mVec128));
 #else
         return CVector3f(x / val, y / val, z / val);
 #endif
@@ -138,6 +147,8 @@ public:
     {
 #if __SSE__
         mVec128 = _mm_add_ps(mVec128, rhs.mVec128);
+#elif __GEKKO_PS__
+        mVec128 = _mm_gekko_add_ps(mVec128, rhs.mVec128);
 #else
         x += rhs.x; y += rhs.y; z += rhs.z;
 #endif
@@ -173,23 +184,14 @@ public:
     inline void normalize()
     {
         float mag = magnitude();
-        if (mag > 1e-6f)
-        {
-            mag = 1.0 / mag;
-            *this *= mag;
-        }
-        else
-            zeroOut();
+        mag = 1.0 / mag;
+        *this *= mag;
     }
     inline CVector3f normalized() const
     {
         float mag = magnitude();
-        if (mag > 1e-6f)
-        {
-            mag = 1.0 / mag;
-            return *this * mag;
-        }
-        return {0, 0, 0};
+        mag = 1.0 / mag;
+        return *this * mag;
     }
     inline CVector3f cross(const CVector3f& rhs) const
     { return CVector3f(y * rhs.z - z * rhs.y, z * rhs.x - x * rhs.z, x * rhs.y - y * rhs.x); }
@@ -223,7 +225,7 @@ public:
 #endif
     }
     inline float magnitude() const
-    { return sqrtf(magSquared()); }
+    { return Math::sqrtF(magSquared()); }
     
     inline void zeroOut()
     {
@@ -253,19 +255,24 @@ public:
     static CVector3f slerp(const CVector3f& a, const CVector3f& b, float t);
     //static CVector3f slerp(const CVector3f& a, const CVector3f& b, const CRelAngle& angle);
 
-    inline bool isNormalized(float thresh = 1e-5f) const
-    { return (fabs(1.0f - magSquared()) <= thresh); }
+    inline bool canBeNormalized() const
+    {
+        const float epsilon = 1.1920929e-7f;
+        if (fabs(x) >= epsilon || fabs(y) >= epsilon || fabs(z) >= epsilon)
+            return true;
+        return false;
+    }
 
-    inline bool canBeNormalized()
-    { return !isNormalized(); }
+    inline bool isNormalized() const
+    { return !canBeNormalized(); }
 
     inline bool isZero() const
-    { return magSquared() <= 1e-7; }
+    { return magSquared() <= 1.1920929e-7f; }
 
     inline void scaleToLength(float newLength)
     {
         float length = magSquared();
-        if (length < 1e-6f)
+        if (length < 1.1920929e-7f)
         {
             x = newLength, y = 0.f, z = 0.f;
             return;
@@ -283,7 +290,7 @@ public:
         return v;
     }
 
-    inline bool isEqu(const CVector3f& other, float epsilon=1e-7f)
+    inline bool isEqu(const CVector3f& other, float epsilon=1.1920929e-7f)
     {
         CVector3f diffVec = other - *this;
         return (diffVec.x <= epsilon && diffVec.y <= epsilon && diffVec.z <= epsilon);
@@ -299,6 +306,8 @@ public:
         float v[4];
 #if __SSE__
         __m128 mVec128;
+#elif __GEKKO_PS__
+        ps128_t mVec128;
 #endif
     };
 
diff --git a/include/CVector4f.hpp b/include/CVector4f.hpp
index 1310eeb..21adfea 100644
--- a/include/CVector4f.hpp
+++ b/include/CVector4f.hpp
@@ -11,7 +11,7 @@
 
 namespace Zeus
 {
-class ZE_ALIGN(16) CVector4f
+class alignas(16) CVector4f
 {
     public:
     ZE_DECLARE_ALIGNED_ALLOCATOR();
@@ -224,23 +224,14 @@ class ZE_ALIGN(16) CVector4f
     inline void normalize()
     {
         float mag = magnitude();
-        if (mag > 1e-6f)
-        {
-            mag = 1.0 / mag;
-            *this *= mag;
-        }
-        else
-            zeroOut();
+        mag = 1.0 / mag;
+        *this *= mag;
     }
     inline CVector4f normalized() const
     {
         float mag = magnitude();
-        if (mag > 1e-6f)
-        {
-            mag = 1.0 / mag;
-            return *this * mag;
-        }
-        return {0, 0, 0, 0};
+        mag = 1.0 / mag;
+        return *this * mag;
     }
 
     inline float dot(const CVector4f& rhs) const
@@ -304,15 +295,16 @@ class ZE_ALIGN(16) CVector4f
         return lerp(a, b, t).normalized();
     }
 
-    inline bool isNormalized(float thresh = 1e-5f) const
+    inline bool canBeNormalized() const
     {
-        return (fabs(1.0f - magSquared()) <= thresh);
+        const float epsilon = 1.1920929e-7f;
+        if (fabs(x) >= epsilon || fabs(y) >= epsilon || fabs(z) >= epsilon || fabs(w) >= epsilon)
+            return true;
+        return false;
     }
 
-    inline bool canBeNormalized()
-    {
-        return !isNormalized();
-    }
+    inline bool isNormalized() const
+    { return !canBeNormalized(); }
 
     inline float& operator[](size_t idx) {return (&x)[idx];}
     inline const float& operator[](size_t idx) const {return (&x)[idx];}
diff --git a/include/Global.hpp b/include/Global.hpp
index e596be9..8658e8e 100644
--- a/include/Global.hpp
+++ b/include/Global.hpp
@@ -7,20 +7,18 @@
 
 #if __SSE__
 #   include <immintrin.h>
-#   ifdef _MSC_VER
-#       define ZE_ALIGN(x) __declspec(align(x))
-#   else
+#   ifndef _MSC_VER
 #       include <mm_malloc.h>
-#       define ZE_ALIGN(x) alignas(16)
 #   endif
-#   define ZE_SHUFFLE(x,y,z,w) ((w)<<6 | (z)<<4 | (y)<<2 | (x))
 #   define zeAlloc(sz, align) _mm_malloc(sz, align)
 #   define zeFree(ptr) _mm_free(ptr)
-#else
-#   define ZE_ALIGN(x)
+#elif GEKKO
+#   include <ps_intrins.h>
+#   define zeAlloc(sz, align) _ps_malloc(sz, align)
+#   define zeFree(ptr) _ps_free(ptr)
 #endif
 
-#if __SSE__
+#if __SSE__ || __GEKKO_PS__
 #   define ZE_DECLARE_ALIGNED_ALLOCATOR() \
         inline void* operator new(size_t sizeInBytes)    { return zeAlloc(sizeInBytes,16); } \
         inline void  operator delete(void* ptr)          { zeFree(ptr); } \
@@ -45,6 +43,8 @@
 #   else
 #       define zeCastiTo128f(a)  ((__m128) (a))
 #   endif
+#elif __GEKKO_PS__
+
 #endif
 
 inline int rotr(int x, int n) { return ((x >> n) | (x << (32 - n))); }
diff --git a/include/Math.hpp b/include/Math.hpp
index c2de7fa..8aa86e2 100644
--- a/include/Math.hpp
+++ b/include/Math.hpp
@@ -10,11 +10,10 @@
 #include <math.h>
 #include <algorithm>
 
-#include "CVector3f.hpp"
-#include "CTransform.hpp"
-
 namespace Zeus
 {
+class CVector3f;
+class CTransform;
 namespace Math
 {
     template<typename T>
@@ -29,13 +28,12 @@ namespace Math
 
     extern const CVector3f kRadToDegVec;
     extern const CVector3f kDegToRadVec;
-    inline CVector3f radToDeg(CVector3f rad) {return rad * kRadToDegVec;}
-    inline CVector3f degToRad(CVector3f deg) {return deg * kDegToRadVec;}
+    CVector3f radToDeg(const CVector3f& rad);
+    CVector3f degToRad(const CVector3f& deg);
 
     extern const CVector3f kUpVec;
     CTransform lookAt(const CVector3f& pos, const CVector3f& lookPos, const CVector3f& up=kUpVec);
-    inline CVector3f baryToWorld(const CVector3f& p0, const CVector3f& p1, const CVector3f& p2, const CVector3f& bary)
-    { return bary.x * p0 + bary.y * p1 + bary.z * p2; }
+    CVector3f baryToWorld(const CVector3f& p0, const CVector3f& p1, const CVector3f& p2, const CVector3f& bary);
 
     CVector3f getBezierPoint(const CVector3f& a, const CVector3f& b,
                              const CVector3f& c, const CVector3f& d, float t);
@@ -51,7 +49,7 @@ namespace Math
     inline float slowTangentR(float val) { return float(tan(val)); }
     inline float arcSineR(float val)     { return float(asin(val)); }
     inline float arcTangentR(float val)  { return float(atan(val)); }
-    inline float arcCosineR(float val)      { return float(acos(val)); }
+    inline float arcCosineR(float val)   { return float(acos(val)); }
     inline float powF(float a, float b)  { return float(exp(b * log(a))); }
     inline float floorF(float val)       { return float(floor(val)); }
     inline float ceilingF(float val)
@@ -63,7 +61,7 @@ namespace Math
     // Since round(double) doesn't exist in some <cmath> implementations
     // we'll define our own
     inline double round(double val) { return (val < 0.0 ? ceilingF(val - 0.5) : floorF(val + 0.5)); }
-    inline double powD(float a, float b) { return exp(a * log(b)); }
+    inline double powD(float a, float b) { return exp(b * log(a)); }
 
     double sqrtD(double val);
     inline double invSqrtD(double val)  { return 1.0 / sqrtD(val); }
diff --git a/src/CQuaternion.cpp b/src/CQuaternion.cpp
index c628af1..73f8d3b 100644
--- a/src/CQuaternion.cpp
+++ b/src/CQuaternion.cpp
@@ -136,18 +136,19 @@ CQuaternion CQuaternion::inverse() const
 
 CAxisAngle CQuaternion::toAxisAngle()
 {
-    CAxisAngle ret;
-    ret.angle = acosf(r);
+//    CAxisAngle ret;
+//    ret.angle = acosf(r);
 
-    float thetaInv = 1.0f/sinf(ret.angle);
+//    float thetaInv = 1.0f/sinf(ret.angle);
 
-    ret.axis.x = v.x * thetaInv;
-    ret.axis.y = v.y * thetaInv;
-    ret.axis.z = v.z * thetaInv;
+//    ret.axis.x = v.x * thetaInv;
+//    ret.axis.y = v.y * thetaInv;
+//    ret.axis.z = v.z * thetaInv;
 
-    ret.angle *= 2;
+//    ret.angle *= 2;
 
-    return ret;
+//    return ret;
+    return CAxisAngle();
 }
 
 CQuaternion CQuaternion::log() const
diff --git a/src/CVector2f.cpp b/src/CVector2f.cpp
index 932117e..f964010 100644
--- a/src/CVector2f.cpp
+++ b/src/CVector2f.cpp
@@ -18,7 +18,8 @@ float CVector2f::getAngleDiff(const CVector2f& a, const CVector2f& b)
     if (!mag1 || !mag2)
         return 0;
 
-    float theta = acosf(a.dot(b) / (mag1 * mag2));
+    float dot = a.dot(b);
+    float theta = Math::arcCosineR(dot / (mag1 * mag2));
     return theta;
 }
 
diff --git a/src/CVector3f.cpp b/src/CVector3f.cpp
index 9dfb9d5..de7a84b 100644
--- a/src/CVector3f.cpp
+++ b/src/CVector3f.cpp
@@ -18,7 +18,8 @@ float CVector3f::getAngleDiff(const CVector3f& a, const CVector3f& b)
     if (!mag1 || !mag2)
         return 0;
 
-    float theta = acosf(a.dot(b) / (mag1 * mag2));
+    float dot = a.dot(b);
+    float theta = Math::arcCosineR(dot / (mag1 * mag2));
     return theta;
 }
 
diff --git a/src/Math.cpp b/src/Math.cpp
index 40161c5..a93a523 100644
--- a/src/Math.cpp
+++ b/src/Math.cpp
@@ -1,4 +1,6 @@
 #include "Math.hpp"
+#include "CTransform.hpp"
+#include "CVector3f.hpp"
 
 namespace Zeus
 {
@@ -239,5 +241,12 @@ CVector3f getRoundCatmullRomSplinePoint(const CVector3f& a, const CVector3f& b,
     return getCatmullRomSplinePoint(b, c, bVelocity * cbDistance, cVelocity * cbDistance, t);
 }
 
+CVector3f baryToWorld(const CVector3f& p0, const CVector3f& p1, const CVector3f& p2, const CVector3f& bary)
+{ return bary.x * p0 + bary.y * p1 + bary.z * p2; }
+
+CVector3f radToDeg(const CVector3f& rad) {return rad * kRadToDegVec;}
+
+CVector3f degToRad(const CVector3f& deg) {return deg * kDegToRadVec;}
+
 }
 }
diff --git a/test/main.cpp b/test/main.cpp
index 5139a25..c5a88ff 100644
--- a/test/main.cpp
+++ b/test/main.cpp
@@ -40,6 +40,7 @@ int main()
     std::cout << Math::powF(6.66663489, 2) << std::endl;
     std::cout << Math::invSqrtF(1) << std::endl;
     std::cout << Math::floorPowerOfTwo(256) << std::endl;
+    CLine line({-89.120926, 59.328712, 3.265882}, CUnitVector3f({-90.120926, 59.328712, 3.265882}));
 
     return 0;
 }