Add CMatrix constructor for CQuaternion, close_enough helper, and CTransform constness fix

include/zeus/CQuaternion.hpp

@@ -217,6 +217,12 @@ class alignas(16) CNUQuaternion : public CQuaternion
 {
 public:
     CNUQuaternion() = default;
+    CNUQuaternion(const CMatrix3f& mtx)
+        : CQuaternion(mtx)
+    {
+        normalize();
+    }
+
     CNUQuaternion(const CQuaternion& other)
     {
         *this = other;

include/zeus/CTransform.hpp

@@ -187,7 +187,7 @@ public:
      * buildMatrix3f is here for compliance with Retro's Math API
      * @return The Matrix (Neo, you are the one)
      */
-    inline CMatrix3f buildMatrix3f() { return basis; }
+    inline CMatrix3f buildMatrix3f() const { return basis; }
 
     inline CVector3f operator*(const CVector3f& other) const { return origin + basis * other; }
 

include/zeus/Math.hpp

@@ -47,6 +47,7 @@ struct CPUInfo
 void detectCPU();
 const CPUInfo& cpuFeatures();
 class CVector3f;
+class CVector2f;
 class CTransform;
 
 template <typename T>
@@ -120,6 +121,15 @@ typename std::enable_if<std::is_enum<E>::value, int>::type PopCount(E e)
 {
     return PopCount(static_cast<typename std::underlying_type<E>::type>(e));
 }
+
+
+bool close_enough(const CVector3f &a, const CVector3f &b, float epsilon = 0.000099999997f);
+bool close_enough(const CVector2f& a, const CVector2f& b, float epsilon = 0.000099999997f);
+
+inline bool close_enough(float a, float b, double epsilon = 0.000009999999747378752)
+{
+    return std::fabs(a - b) < epsilon;
+}
 }
 
 #endif // MATH_HPP

src/Math.cpp

@@ -1,6 +1,7 @@
 #include "zeus/Math.hpp"
 #include "zeus/CTransform.hpp"
 #include "zeus/CVector3f.hpp"
+#include "zeus/CVector2f.hpp"
 #if _WIN32
 #include <intrin.h>
 #else
@@ -312,4 +313,18 @@ CVector3f baryToWorld(const CVector3f& p0, const CVector3f& p1, const CVector3f&
 {
     return bary.x * p0 + bary.y * p1 + bary.z * p2;
 }
+
+bool close_enough(const CVector3f& a, const CVector3f &b, float epsilon)
+{
+    if (std::fabs(a.x - b.x) < epsilon && std::fabs(a.y - b.y) < epsilon && std::fabs(a.z - b.z) < epsilon)
+        return true;
+    return false;
+}
+
+bool close_enough(const CVector2f& a, const CVector2f& b, float epsilon)
+{
+    if (std::fabs(a.x - b.x) < epsilon && std::fabs(a.y - b.y) < epsilon)
+        return true;
+    return false;
+}
 }