More reimplementations

CMakeLists.txt

@@ -31,6 +31,7 @@ add_library(Math
     include/CMatrix3f.hpp
     include/CProjection.hpp
     include/CAxisAngle.hpp
+    include/CRelAngle.hpp
     include/CPlane.hpp
     include/CTransform.hpp
     include/CColor.hpp

include/CAABox.hpp

@@ -118,6 +118,13 @@ public:
                 (other.z >= -0.f) ? m_min.z : m_max.z};
     }
 
+    inline CVector3f furthestPointAlongVector(const CVector3f& other)
+    {
+        return {(other.x >= -0.f) ? m_max.x : m_min.x,
+                (other.y >= -0.f) ? m_max.y : m_min.y,
+                (other.z >= -0.f) ? m_max.z : m_min.z};
+    }
+
     inline CVector3f getPoint(const int point)
     {
         int zOff = point & 4;

include/CPlane.hpp

@@ -11,6 +11,14 @@ public:
 
     inline CPlane() {}
     CPlane(float a, float b, float c, float d) : a(a), b(b), c(c), d(d) {}
+    CPlane(const CVector3f& a, const CVector3f& b, const CVector3f& c)
+    {
+        CVector3f ab = b - a;
+        CVector3f ac = c - a;
+        vec = ab.cross(ac).normalized();
+        d = -a.dot(vec);
+    }
+
     CPlane(const CVector3f& point, float displacement)
     {
 #if __SSE__

include/CRelAngle.hpp

@@ -0,0 +1,30 @@
+#ifndef CRELANGLE_HPP
+#define CRELANGLE_HPP
+
+#include "CVector3f.hpp"
+#include "Math.hpp"
+
+/**
+ * @brief The CRelAngle class represents relative angles in radians
+ */
+class ZE_ALIGN(16) CRelAngle : public CVector3f
+{
+public:
+    /**
+     * @brief CRelAngle
+     * @param angles In degrees
+     */
+    CRelAngle(const CVector3f& angles)
+    {
+        x = Math::degToRad(angles.x);
+        y = Math::degToRad(angles.y);
+        z = Math::degToRad(angles.z);
+    }
+
+    CRelAngle(float x, float y, float z)
+        : CRelAngle(CVector3f{x, y, z})
+    {
+    }
+};
+
+#endif // CRELANGLE_HPP

include/CTransform.hpp

@@ -5,6 +5,7 @@
 #include "CMatrix3f.hpp"
 #include "CMatrix4f.hpp"
 #include "CVector3f.hpp"
+#include "CQuaternion.hpp"
 
 class ZE_ALIGN(16) CTransform
 {
@@ -29,7 +30,54 @@ public:
         m_basis = CMatrix3f::skIdentityMatrix3f;
         m_origin = position;
     }
-    
+
+    inline void rotate(const CVector3f& euler)
+    {
+        *this = *this * CMatrix3f(CQuaternion(euler));
+    }
+
+    inline void scaleBy(float factor)
+    {
+        CTransform xfrm(CMatrix3f(CVector3f(factor, factor, factor)));
+        *this = *this * xfrm;
+    }
+
+    inline void scale(const CVector3f& scale)
+    {
+        m_basis = CMatrix3f(true);
+        m_basis[0][0] = scale.x;
+        m_basis[1][1] = scale.y;
+        m_basis[2][2] = scale.z;
+        m_origin.zeroOut();
+    }
+
+    inline void scale(float x, float y, float z)
+    {
+        scale({x, y, z});
+    }
+
+    inline void multiplyIgnoreTranslation(const CTransform& xfrm)
+    {
+        m_basis = m_basis*xfrm.m_basis;
+    }
+
+    inline CTransform getRotation()
+    {
+        CTransform ret = *this;
+        ret.m_origin.zeroOut();
+        return ret;
+    }
+
+    /**
+     * @brief buildMatrix3f Returns the stored matrix
+     * buildMatrix3f is here for compliance with Retro's Math API
+     * @return The Matrix (Neo, you are the one)
+     */
+    inline CMatrix3f buildMatrix3f()
+    {
+        return m_basis;
+    }
+
     inline CVector3f operator*(const CVector3f& other) const
     {return m_origin + m_basis * other;}
     
@@ -48,6 +96,24 @@ public:
 #endif
     }
 
+    static inline CTransform fromColumns(const CVector3f& m0, const CVector3f& m1, const CVector3f& m2, const CVector3f& m3)
+    {
+        CTransform ret;
+        ret.m_basis[0][0] = m0[0];
+        ret.m_basis[0][1] = m1[0];
+        ret.m_basis[0][2] = m2[0];
+        ret.m_origin[0]   = m3[0];
+        ret.m_basis[1][0] = m0[1];
+        ret.m_basis[1][1] = m1[1];
+        ret.m_basis[1][2] = m2[1];
+        ret.m_origin[1]   = m3[1];
+        ret.m_basis[2][0] = m0[2];
+        ret.m_basis[2][1] = m1[2];
+        ret.m_basis[2][2] = m2[2];
+        ret.m_origin[2]   = m3[2];
+        return ret;
+    }
+
     CMatrix3f m_basis;
     CVector3f m_origin;
 };

include/CVector2f.hpp

@@ -277,9 +277,14 @@ class ZE_ALIGN(16) CVector2f
     }
     static CVector2f slerp(const CVector2f& a, const CVector2f& b, float t);
 
-    inline bool isNormalized(float thresh = 0.0001f) const
+    inline bool isNormalized(float thresh = 1e-5f) const
     {
-        return (length() > thresh);
+        return (fabs(1.0f - lengthSquared()) <= thresh);
+    }
+
+    inline bool canBeNormalized()
+    {
+        return !isNormalized();
     }
 
     inline float& operator[](size_t idx) {return (&x)[idx];}

include/CVector3f.hpp

@@ -163,22 +163,29 @@ public:
     inline void normalize()
     {
         float mag = length();
-        assert(mag != 0.0);
-        mag = 1.0 / mag;
-        *this *= mag;
+        if (mag > 1e-6f)
+        {
+            mag = 1.0 / mag;
+            *this *= mag;
+        }
+        else
+            x = 1.0, y = 0.0, z = 0.0;
     }
     inline CVector3f normalized() const
     {
         float mag = length();
-        assert(mag != 0.0);
-        mag = 1.0 / mag;
-        return *this * mag;
+        if (mag > 1e-6f)
+        {
+            mag = 1.0 / mag;
+            return *this * mag;
+        }
+        return {1, 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);
     }
-    inline float     dot(const CVector3f& rhs) const
+    inline float dot(const CVector3f& rhs) const
     {
 #if __SSE4_1__
         TVectorUnion result;
@@ -192,7 +199,7 @@ public:
         return (x * rhs.x) + (y * rhs.y) + (z * rhs.z);
 #endif
     }
-    inline float     lengthSquared() const
+    inline float lengthSquared() const
     {
 #if __SSE4_1__
         TVectorUnion result;
@@ -206,7 +213,7 @@ public:
         return x*x + y*y + z*z;
 #endif
     }
-    inline float     length() const
+    inline float length() const
     {
         return sqrtf(lengthSquared());
     }
@@ -241,10 +248,37 @@ public:
         return lerp(a, b, t).normalized();
     }
     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 = 0.0001f) const
+    inline bool isNormalized(float thresh = 1e-5f) const
     {
-        return (length() > thresh);
+        return (fabs(1.0f - lengthSquared()) <= thresh);
+    }
+
+    inline bool canBeNormalized()
+    {
+        return !isNormalized();
+    }
+
+    inline void scaleToLength(float newLength)
+    {
+        float length = lengthSquared();
+        if (length < 1e-6f)
+        {
+            x = newLength, y = 0.f, z = 0.f;
+            return;
+        }
+
+        length = sqrt(length);
+        float scalar = newLength / length;
+        *this *= scalar;
+    }
+
+    inline CVector3f scaledToLength(float newLength) const
+    {
+        CVector3f v = *this;
+        v.scaleToLength(newLength);
+        return v;
     }
 
     inline float& operator[](size_t idx) {return (&x)[idx];}

include/CVector4f.hpp

@@ -295,9 +295,14 @@ class ZE_ALIGN(16) CVector4f
         return lerp(a, b, t).normalized();
     }
 
-    inline bool isNormalized(float thresh = 0.0001f) const
+    inline bool isNormalized(float thresh = 1e-5f) const
     {
-        return (length() > thresh);
+        return (fabs(1.0f - lengthSquared()) <= thresh);
+    }
+
+    inline bool canBeNormalized()
+    {
+        return !isNormalized();
     }
 
     inline float& operator[](size_t idx) {return (&x)[idx];}

include/MathLib.hpp

@@ -2,6 +2,7 @@
 #define __MATHLIB_HPP
 
 #include "CAxisAngle.hpp"
+#include "CRelAngle.hpp"
 #include "CMatrix3f.hpp"
 #include "CMatrix4f.hpp"
 #include "CProjection.hpp"

src/CTransform.cpp

@@ -57,3 +57,4 @@ CTransform CTransformFromAxisAngle(const CVector3f& axis, float angle)
     
     return result;
 }
+