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

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