Add Athena constructor to CMatrix3f

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

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;

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()
+    CAxisAngle() = default;
-        : axis(CVector3f::skOne),
+    CAxisAngle(const CUnitVector3f& axis, float angle)
-          angle(0)
+        : CVector3f(axis * angle)
-    {}
-    CAxisAngle(const CVector3f& axis, float angle)
-        : axis(axis),
-          angle(angle)
     {}
 
-    CVector3f axis;
+    CAxisAngle(const CVector3f& axisAngle)
-    float     angle;
+        : CVector3f(axisAngle)
+    {}
+
+    float angle() { return magnitude(); }
+    const CVector3f& getVector() { return *this; }
 };
 }
 

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();

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

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();

include/CMatrix4f.hpp

@@ -6,7 +6,7 @@
 
 namespace Zeus
 {
-class ZE_ALIGN(16) CMatrix4f
+class alignas(16) CMatrix4f
 {
 public:
     static const CMatrix4f skIdentityMatrix4f;

include/COBBox.hpp

@@ -7,7 +7,7 @@
 
 namespace Zeus
 {
-class ZE_ALIGN(16) COBBox
+class alignas(16) COBBox
 {
 public:
     ZE_DECLARE_ALIGNED_ALLOCATOR();

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;

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:

include/CQuaternion.hpp

@@ -10,7 +10,7 @@
 
 namespace Zeus
 {
-class ZE_ALIGN(16) CQuaternion
+class alignas(16) CQuaternion
 {
 public:
     ZE_DECLARE_ALIGNED_ALLOCATOR();

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:
     /**

include/CSphere.hpp

@@ -5,7 +5,7 @@
 
 namespace Zeus
 {
-class ZE_ALIGN(16) CSphere
+class alignas(16) CSphere
 {
 public:
     ZE_DECLARE_ALIGNED_ALLOCATOR();

include/CTransform.hpp

@@ -9,7 +9,7 @@
 
 namespace Zeus
 {
-class ZE_ALIGN(16) CTransform
+class alignas(16) CTransform
 {
 public:
     ZE_DECLARE_ALIGNED_ALLOCATOR();

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

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,25 +200,16 @@ class ZE_ALIGN(16) CVector2f
     inline void normalize()
     {
         float mag = magnitude();
-        if (mag > 1e-6f)
-        {
         mag = 1.0 / mag;
         *this *= mag;
     }
-        else
-            zeroOut();
-    }
 
     inline CVector2f normalized() const
     {
         float mag = magnitude();
-        if (mag > 1e-6f)
-        {
         mag = 1.0 / mag;
         return *this * mag;
     }
-        return {0, 0};
-    }
 
     inline float cross(const CVector2f& rhs) const
     {
@@ -252,9 +244,7 @@ class ZE_ALIGN(16) CVector2f
 #endif
     }
     inline float magnitude() const
-    {
+    { return Math::sqrtF(magSquared()); }
-        return 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()
+    inline bool isNormalized() const
-    {
+    { return !canBeNormalized(); }
-        return !isNormalized();
+
-    }
+    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];}

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

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,26 +11,18 @@
 
 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__
+#if __SSE__ || __GEKKO_PS__
-    : 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){}
 #else
     {
@@ -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,24 +184,15 @@ public:
     inline void normalize()
     {
         float mag = magnitude();
-        if (mag > 1e-6f)
-        {
         mag = 1.0 / mag;
         *this *= mag;
     }
-        else
-            zeroOut();
-    }
     inline CVector3f normalized() const
     {
         float mag = magnitude();
-        if (mag > 1e-6f)
-        {
         mag = 1.0 / mag;
         return *this * mag;
     }
-        return {0, 0, 0};
-    }
     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
+    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)
+            return true;
+        return false;
+    }
 
-    inline bool canBeNormalized()
+    inline bool isNormalized() const
-    { return !isNormalized(); }
+    { 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
     };
 

include/CVector4f.hpp

@@ -11,7 +11,7 @@
 
 namespace Zeus
 {
-class ZE_ALIGN(16) CVector4f
+class alignas(16) CVector4f
 {
     public:
     ZE_DECLARE_ALIGNED_ALLOCATOR();
@@ -224,24 +224,15 @@ class ZE_ALIGN(16) CVector4f
     inline void normalize()
     {
         float mag = magnitude();
-        if (mag > 1e-6f)
-        {
         mag = 1.0 / mag;
         *this *= mag;
     }
-        else
-            zeroOut();
-    }
     inline CVector4f normalized() const
     {
         float mag = magnitude();
-        if (mag > 1e-6f)
-        {
         mag = 1.0 / mag;
         return *this * mag;
     }
-        return {0, 0, 0, 0};
-    }
 
     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()
+    inline bool isNormalized() const
-    {
+    { return !canBeNormalized(); }
-        return !isNormalized();
-    }
 
     inline float& operator[](size_t idx) {return (&x)[idx];}
     inline const float& operator[](size_t idx) const {return (&x)[idx];}

include/Global.hpp

@@ -7,20 +7,18 @@
 
 #if __SSE__
 #   include <immintrin.h>
-#   ifdef _MSC_VER
+#   ifndef _MSC_VER
-#       define ZE_ALIGN(x) __declspec(align(x))
-#   else
 #       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
+#elif GEKKO
-#   define ZE_ALIGN(x)
+#   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))); }

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;}
+    CVector3f radToDeg(const CVector3f& rad);
-    inline CVector3f degToRad(CVector3f deg) {return deg * kDegToRadVec;}
+    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)
+    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 getBezierPoint(const CVector3f& a, const CVector3f& b,
                              const CVector3f& c, const CVector3f& d, float t);
@@ -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); }

src/CQuaternion.cpp

@@ -136,18 +136,19 @@ CQuaternion CQuaternion::inverse() const
 
 CAxisAngle CQuaternion::toAxisAngle()
 {
-    CAxisAngle ret;
+//    CAxisAngle ret;
-    ret.angle = acosf(r);
+//    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.x = v.x * thetaInv;
-    ret.axis.y = v.y * thetaInv;
+//    ret.axis.y = v.y * thetaInv;
-    ret.axis.z = v.z * thetaInv;
+//    ret.axis.z = v.z * thetaInv;
 
-    ret.angle *= 2;
+//    ret.angle *= 2;
 
-    return ret;
+//    return ret;
+    return CAxisAngle();
 }
 
 CQuaternion CQuaternion::log() const

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

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

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

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