diff --git a/CMakeLists.txt b/CMakeLists.txt index 6005d12..523c73f 100644 --- a/CMakeLists.txt +++ b/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) diff --git a/include/zeus/CEulerAngles.hpp b/include/zeus/CEulerAngles.hpp new file mode 100644 index 0000000..dca900d --- /dev/null +++ b/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 diff --git a/include/zeus/CQuaternion.hpp b/include/zeus/CQuaternion.hpp index 4ce3e7e..6466656 100644 --- a/include/zeus/CQuaternion.hpp +++ b/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]; } diff --git a/src/CEulerAngles.cpp b/src/CEulerAngles.cpp new file mode 100644 index 0000000..97b8112 --- /dev/null +++ b/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; + } +} + +} diff --git a/src/CQuaternion.cpp b/src/CQuaternion.cpp index 37624ce..7634cef 100644 --- a/src/CQuaternion.cpp +++ b/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); +} }