Add CEulerAngles representation

This commit is contained in:
Jack Andersen 2017-03-19 19:06:53 -10:00
parent 8603353cf7
commit c50a319a39
5 changed files with 94 additions and 5 deletions

View File

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

View File

@ -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

View File

@ -235,7 +235,24 @@ public:
inline CTransform toTransform() const { return CTransform(CMatrix3f(*this)); } inline CTransform toTransform() const { return CTransform(CMatrix3f(*this)); }
inline CTransform toTransform(const zeus::CVector3f& origin) const { return CTransform(CMatrix3f(*this), origin); } 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 lerp(const CQuaternion& a, const CQuaternion& b, double t);
static CQuaternion slerp(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)); } 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 float& operator[](size_t idx) { return (&w)[idx]; }
inline const float& operator[](size_t idx) const { return (&w)[idx]; } inline const float& operator[](size_t idx) const { return (&w)[idx]; }

43
src/CEulerAngles.cpp Normal file
View File

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

View File

@ -170,8 +170,6 @@ CQuaternion CQuaternion::exp() const
return ret; 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::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(); } 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); 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);
}
} }