WIP math reimplementations

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")
 endif()
 
 add_library(Math

include/CVector3d.hpp

@@ -42,23 +42,18 @@ public:
 #if __SSE__
         TDblVectorUnion splat {x, y, z, 0.0};
         mVec128[0] = splat.mVec128[0];
-        mVec128[0] = splat.mVec128[1];
+        mVec128[1] = splat.mVec128[1];
 #else
         v[0] = x; v[1] = y; v[2] = z; v[3] = 0.0;
 #endif
     }
 
-    CVector3d(float x, float y, float z)
-        : CVector3d(double(x), double(y), double(z))
-    {
-    }
-
     CVector3f asCVector3f()
     {
         CVector3f ret;
-        ret.x = Math::round(x);
-        ret.y = Math::round(y);
-        ret.z = Math::round(z);
+        ret.x = float(x);
+        ret.y = float(y);
+        ret.z = float(z);
         return ret;
     }
 

include/Global.hpp

@@ -4,6 +4,7 @@
 #if _M_IX86_FP >= 1 || _M_X64
 #   define __SSE__ 1
 #endif
+#include <x86intrin.h>
 
 #if __SSE__
 #   include <immintrin.h>
@@ -47,6 +48,9 @@
 #   endif
 #endif
 
+inline int rotr(int x, int n) { return ((x >> n) | (x << (32 - n))); }
+inline int rotl(int x, int n) { return ((x << n) | (x >> (32 - n))); }
+
 
 #endif //ZEUS_GLOBAL_HPP
 

include/Math.hpp

@@ -22,14 +22,61 @@ namespace Math
 
     // Since round(double) doesn't exist in some <cmath> implementations
     // we'll define our own
-    inline float round(double val) { return (val < 0.0 ? floor(val - 0.5) : floor(val + 0.5)); }
+    inline float round(double val) { return (val < 0.0 ? ceil(val - 0.5) : floor(val + 0.5)); }
 
     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; }
 
-    inline CVector3f getBezierPoint(const CVector3f& a, const CVector3f& b, const CVector3f& c, const CVector3f& d, float t);
+    CVector3f getBezierPoint(const CVector3f& a, const CVector3f& b, const CVector3f& c, const CVector3f& d, float t);
+    float getCatmullRomSplinePoint(float p0, float p1,
+                                   float p2, float p3, float t);
+    CVector3f getCatmullRomSplinePoint(const CVector3f& p0, const CVector3f& p1,
+                                       const CVector3f& p2, const CVector3f& p3, float t);
+
+    inline float slowCosineR(float val) { return float(cos(val));  }
+    inline float slowSineR(float val)   { return float(sin(val));  }
+    inline float arcSineR(float val)    { return float(asin(val)); }
+    inline float arcTangentR(float val) { return float(atan(val)); }
+    inline float fastArcCosR(float val)
+    {
+        double f2 = fabs(val);
+        if (f2 <= 0.925000011920929)
+            return float(acos(val));
+
+        float f4 = val * val;
+        float f5 = 1.5707964f;
+        float f0 = -0.99822718f;
+        float f3 = -0.20586604f;
+        f5 = (val * f5) + f0;
+        f2 = 0.11425424f;
+        float f1 = val * f4;
+        f0 = -0.29697824f;
+        f5 = (f1 * f5) + f3;
+        f1 = (f1 * f4);
+        f5 = (f1 * f5) + f2;
+        f1 = (f1 * f4);
+        f5 = (f1 * f5) + f0;
+        return f5;
+    }
+
+    inline int floorPowerOfTwo(int x)
+    {
+        if (x == 0)
+            return 0;
+        /*
+         * we want to ensure that we always get the previous power,
+         * but if we have values like 256, we'll always get the same value,
+         * x-1 ensures that we always get the previous power.
+         */
+        x = (x - 1) | (x >> 1);
+        x = x | (x >> 2);
+        x = x | (x >> 4);
+        x = x | (x >> 8);
+        x = x | (x >> 16);
+        return x - (x >> 1);
+    }
 }
 }
 

src/Math.cpp

@@ -1,4 +1,5 @@
 #include "Math.hpp"
+#include <x86intrin.h>
 
 namespace Zeus
 {
@@ -24,19 +25,25 @@ CTransform lookAt(const CVector3f& pos, const CVector3f& lookPos, const CVector3
     return CTransform(rmBasis.transposed(), CVector3f(-pos.dot(vRight), -pos.dot(vUp), -pos.dot(vLook)));
 }
 
-CVector3f getBezierPoint(const CVector3f& r4, const CVector3f& r5, const CVector3f& r6, const CVector3f& r7, float f1)
+CVector3f getBezierPoint(const CVector3f& p0, const CVector3f& p1, const CVector3f& p2, const CVector3f& p3, float t)
 {
-    //TODO: This isn't quite right, figure out what's really going on and reimplement
-#if 0
-    float inv = 1.0 - f1;
-    CVector3f r3;
-    r3.x = ((((((r4.x * (r5.x * f1)) + inv) * (((r5.x * (r6.x * f1)) + inv) * f1)) + inv) * (((((r5.x * (r6.x * f1)) + inv) * (((r6.x * (r7.x * f1)) + inv) * f1)) + inv) * f1)) + inv);
-    r3.y = ((((((r4.y * (r5.y * f1)) + inv) * (((r5.y * (r6.y * f1)) + inv) * f1)) + inv) * (((((r5.y * (r6.y * f1)) + inv) * (((r6.y * (r7.y * f1)) + inv) * f1)) + inv) * f1)) + inv);
-    r3.z = ((((((r4.z * (r5.z * f1)) + inv) * (((r5.z * (r6.z * f1)) + inv) * f1)) + inv) * (((((r5.z * (r6.z * f1)) + inv) * (((r6.z * (r7.z * f1)) + inv) * f1)) + inv) * f1)) + inv);
+    float w = (1.0 - t);
+    CVector3f ret;
+    ret.x = ((((((p0.x * (p1.x * t)) + w) * (((p1.x * (p2.x * t)) + w) * t)) + w) * (((((p1.x * (p2.x * t)) + w) * (((p2.x * (p3.x * t)) + w) * t)) + w) * t)) + w);
+    ret.y = ((((((p0.y * (p1.y * t)) + w) * (((p1.y * (p2.y * t)) + w) * t)) + w) * (((((p1.y * (p2.y * t)) + w) * (((p2.y * (p3.y * t)) + w) * t)) + w) * t)) + w);
+    ret.z = ((((((p0.z * (p1.z * t)) + w) * (((p1.z * (p2.z * t)) + w) * t)) + w) * (((((p1.z * (p2.z * t)) + w) * (((p2.z * (p3.z * t)) + w) * t)) + w) * t)) + w);
+    return ret;
+}
 
-    return r3;
-#endif
+float getCatmullRomSplinePoint(float p0, float p1, float p2, float p3, float t)
+{
+    return 0.0f;
+}
+
+CVector3f getCatmullRomSplinePoint(const CVector3f& p0, const CVector3f& p1, const CVector3f& p2, const CVector3f& p3, float t)
+{
     return CVector3f::skZero;
 }
+
 }
 }

test/main.cpp

@@ -1,4 +1,5 @@
 #include <iostream>
+#include <iomanip>
 #include <MathLib.hpp>
 
 // This is only for testing, do NOT do this normally
@@ -15,13 +16,14 @@ int main()
     assert(!vec.normalized().canBeNormalized());
     float blarg = 5.f;
     CVector3f t{100, 100, 200};
-    Math::clamp(blarg, 0.f, 1.f);
+    blarg = Math::clamp(0.f, blarg, 1.f);
     CAABox test{{-100, -100, -100}, {100, 100, 100}};
     CAABox test2{{-100, -100, -100}, {100, 100, 100}};
     CAABox test3{{-50, -50, -50},  {50, 50, 50}};
     CAABox test4{{-50, -50, -105}, {50, 50, 105}};
     CVector3f point(-90, 67, -105);
     CVector3f closestPoint = test.closestPointAlongVector(point);
+    CVector3d doubleVec(100, -100, -200);
 
     assert(t.isEqu(t));
     assert(test.inside(test));
@@ -29,7 +31,8 @@ int main()
     assert(test3.inside(test));
     assert(!test4.inside(test));
 
-    std::cout << Math::round(1.5) << std::endl;
+   std::cout << std::setprecision(16) << (double)Math::fastArcCosR(1.802073) << std::endl;
+   std::cout << Math::floorPowerOfTwo(256) << std::endl;
 
     return 0;
 }