Add CEulerAngles representation

CMakeLists.txt

@@ -20,7 +20,8 @@ set(SOURCES
     src/CRectangle.cpp
     src/CVector4f.cpp
     src/CMatrix4f.cpp
-    src/CAABox.cpp)
+    src/CAABox.cpp
+    src/CEulerAngles.cpp)
 
 # SSELegacy.cpp compiled separately to escape the effects of link-time optimization
 if(NOT MSVC)
@@ -76,7 +77,8 @@ add_library(zeus
     include/zeus/CLineSeg.hpp
     include/zeus/CSphere.hpp
     include/zeus/CUnitVector.hpp
-    include/zeus/CMRay.hpp)
+    include/zeus/CMRay.hpp
+    include/zeus/CEulerAngles.hpp)
 
 add_subdirectory(test)
 

include/zeus/CEulerAngles.hpp

@@ -0,0 +1,18 @@
+#ifndef CEULERANGES_HPP
+#define CEULERANGES_HPP
+
+#include "zeus/CVector3f.hpp"
+
+namespace zeus
+{
+class CQuaternion;
+
+class CEulerAngles : public CVector3f
+{
+public:
+    CEulerAngles(const CQuaternion& quat);
+};
+
+}
+
+#endif // CEULERANGES_HPP

include/zeus/CQuaternion.hpp

@@ -235,7 +235,24 @@ public:
 
     inline CTransform toTransform() const { return CTransform(CMatrix3f(*this)); }
     inline CTransform toTransform(const zeus::CVector3f& origin) const { return CTransform(CMatrix3f(*this), origin); }
-    float dot(const CQuaternion& quat) const;
+    inline float dot(const CQuaternion& rhs) const
+    {
+#if __SSE__
+        TVectorUnion result;
+#if __SSE4_1__ || __SSE4_2__
+        if (cpuFeatures().SSE41 || cpuFeatures().SSE42)
+        {
+            result.mVec128 = _mm_dp_ps(mVec128, rhs.mVec128, 0xF1);
+            return result.v[0];
+        }
+#endif
+
+        result.mVec128 = _mm_mul_ps(mVec128, rhs.mVec128);
+        return result.v[0] + result.v[1] + result.v[2] + result.v[3];
+#else
+        return (x * rhs.x) + (y * rhs.y) + (z * rhs.z) + (w * rhs.w);
+#endif
+    }
 
     static CQuaternion lerp(const CQuaternion& a, const CQuaternion& b, double t);
     static CQuaternion slerp(const CQuaternion& a, const CQuaternion& b, double t);
@@ -247,6 +264,8 @@ public:
 
     inline float yaw() const { return std::asin(-2.f * (x * z - w * y)); }
 
+    CQuaternion buildEquivalent() const;
+
     inline float& operator[](size_t idx) { return (&w)[idx]; }
     inline const float& operator[](size_t idx) const { return (&w)[idx]; }
 

src/CEulerAngles.cpp

@@ -0,0 +1,43 @@
+#include "zeus/CEulerAngles.hpp"
+#include "zeus/CQuaternion.hpp"
+
+namespace zeus
+{
+
+CEulerAngles::CEulerAngles(const CQuaternion& quat)
+{
+    float quatDot = quat.dot(quat);
+    float t0 = 0.f;
+    if (quatDot > 0.f)
+        t0 = 2.f / quatDot;
+    double t1 = 1.0 - (t0 * quat.x * quat.x + t0 * quat.z * quat.z);
+    double t2 = t0 * quat.y * quat.x - t0 * quat.z * quat.w;
+    double t3 = t1 * t1 + t2 * t2;
+
+    double t4 = 0.0;
+    if (t3 > 0.0)
+    {
+        double sqrtT3 = std::sqrt(t3);
+        double t5 = 0.5 * sqrtT3 * -(t3 * sqrtT3 * sqrtT3 - 3.0);
+        double t6 = 0.5 * t5 * -(t5 * t5 - 3.0);
+        double t7 = 0.5 * t6 * -(t3 * t6 * t6 - 3.0);
+        t4 = t3 * 0.5 * t7 * -(t3 * t7 * t7 - 3.0);
+    }
+
+    if (std::abs(t4) > 0.00001)
+    {
+        x = -std::atan2(t0 * quat.z * quat.y + t0 * quat.x * quat.w, t4);
+        y = -std::atan2(t0 * quat.z * quat.x - t0 * quat.y * quat.w,
+                        1.0 - (t0 * quat.x * quat.x + t0 * quat.y * quat.y));
+        z = -std::atan2(t2, t1);
+    }
+    else
+    {
+        x = -std::atan2(t0 * quat.z * quat.y + t0 * quat.x * quat.w, t4);
+        y = -std::atan2(t0 * quat.z * quat.x + t0 * quat.y * quat.w,
+                        1.0 - (t0 * quat.y * quat.y + t0 * quat.z * quat.z));
+        z = 0.f;
+    }
+}
+
+}

src/CQuaternion.cpp

@@ -170,8 +170,6 @@ CQuaternion CQuaternion::exp() const
     return ret;
 }
 
-float CQuaternion::dot(const CQuaternion& b) const { return x * b.x + y * b.y + z * b.z + w * b.w; }
-
 CQuaternion CQuaternion::lerp(const CQuaternion& a, const CQuaternion& b, double t) { return (a + t * (b - a)); }
 
 CQuaternion CQuaternion::nlerp(const CQuaternion& a, const CQuaternion& b, double t) { return lerp(a, b, t).normalized(); }
@@ -222,4 +220,13 @@ CQuaternion operator*(float lhs, const CQuaternion& rhs)
 {
     return CQuaternion(lhs * rhs.w, lhs * rhs.x, lhs * rhs.y, lhs * rhs.z);
 }
+
+CQuaternion CQuaternion::buildEquivalent() const
+{
+    float tmp = std::acos(clamp(-1.f, w, 1.f)) * 2.0;
+    if (std::fabs(tmp) < 1.0e-7)
+        return {-1.f, 0.f, 0.f, 0.f};
+    else
+        return CQuaternion::fromAxisAngle(CUnitVector3f(x, y, z), tmp + 2.0 * M_PI);
+}
 }