From baaca3136ba58d76650dfcc90d8962ae3fd6d2a0 Mon Sep 17 00:00:00 2001 From: Jack Andersen Date: Wed, 6 Apr 2016 17:39:24 -1000 Subject: [PATCH] WXYZ quaternion layout --- include/zeus/CQuaternion.hpp | 67 ++++++++++++------- include/zeus/CVector2f.hpp | 2 +- src/CMatrix3f.cpp | 18 ++--- src/CQuaternion.cpp | 126 +++++++++++++++++++++-------------- 4 files changed, 127 insertions(+), 86 deletions(-) diff --git a/include/zeus/CQuaternion.hpp b/include/zeus/CQuaternion.hpp index d342d06..ee6dff1 100644 --- a/include/zeus/CQuaternion.hpp +++ b/include/zeus/CQuaternion.hpp @@ -19,23 +19,25 @@ class alignas(16) CQuaternion #endif public: ZE_DECLARE_ALIGNED_ALLOCATOR(); - - CQuaternion() : r(1.0f) {} - CQuaternion(float r, float x, float y, float z) : v(x, y, z){ this->r = r; } - CQuaternion(float x, float y, float z) { fromVector3f(CVector3f(x, y, z)); } - CQuaternion(float r, const CVector3f& vec) : v(vec){ this->r = r;} + + CQuaternion() : w(1.0f), x(0.0f), y(0.0f), z(0.0f) {} + CQuaternion(float wi, float xi, float yi, float zi) : w(wi), x(xi), y(yi), z(zi) {} + CQuaternion(float xi, float yi, float zi) { fromVector3f(CVector3f(xi, yi, zi)); } + CQuaternion(float wi, const CVector3f& vec) : w(wi), x(vec.x), y(vec.y), z(vec.z) {} #if ZE_ATHENA_TYPES inline void readBig(athena::io::IStreamReader& input) { - r = input.readFloatBig(); - v.readBig(input); + w = input.readFloatBig(); + x = input.readFloatBig(); + y = input.readFloatBig(); + z = input.readFloatBig(); } CQuaternion(const atVec4f& vec) { #if __SSE__ - v.mVec128 = vec.mVec128; + mVec128 = vec.mVec128; #else - x = vec.vec[0]; y = vec.vec[1]; z = vec.vec[2]; r = vec.vec[3]; + x = vec.vec[1]; y = vec.vec[2]; z = vec.vec[3]; w = vec.vec[0]; #endif } @@ -43,9 +45,12 @@ public: { atVec4f ret; #if __SSE__ - ret.mVec128 = v.mVec128; + ret.mVec128 = mVec128; #else - ret.vec = v; + ret.vec[0] = w; + ret.vec[1] = x; + ret.vec[2] = y; + ret.vec[3] = z; #endif return ret; } @@ -53,9 +58,12 @@ public: { atVec4f ret; #if __SSE__ - ret.mVec128 = v.mVec128; + ret.mVec128 = mVec128; #else - ret.vec = v; + ret.vec[0] = w; + ret.vec[1] = x; + ret.vec[2] = y; + ret.vec[3] = z; #endif return ret; } @@ -65,13 +73,19 @@ public: CQuaternion(const CVector4f& vec) { #if __SSE__ - v.mVec128 = vec.mVec128; + mVec128 = vec.mVec128; #else - v.x = vec.x; v.y = vec.y; v.z = vec.z; r = vec.w; + x = vec[1]; y = vec[2]; z = vec[3]; w = vec[0]; #endif } - virtual ~CQuaternion() {} + CQuaternion(const CVector3f& vecA, const CVector3f& vecB) + { + CVector3f vecAN = vecA.normalized(); + CVector3f vecBN = vecB.normalized(); + CVector3f w = vecAN.cross(vecBN); + *this = CQuaternion(1.f + vecAN.dot(vecBN), w.x, w.y, w.z).normalized(); + } void fromVector3f(const CVector3f& vec); @@ -118,7 +132,7 @@ public: CQuaternion q = rotation * v; q *= rotation.inverse(); - return q.v; + return {q.x, q.y, q.z}; } CQuaternion log() const; @@ -130,26 +144,29 @@ public: static CQuaternion lerp(CQuaternion& a, CQuaternion& b, double t); static CQuaternion slerp(CQuaternion& a, CQuaternion& b, double t); static CQuaternion nlerp(CQuaternion& a, CQuaternion& b, double t); - + inline float roll() const { - return std::atan2(2.f * (v.x * v.y + r * v.z), r * r + v.x * v.x - v.y * v.y - v.z * v.z); + return std::atan2(2.f * (x * y + w * z), w * w + x * x - y * y - z * z); } - + inline float pitch() const { - return std::atan2(2.f * (v.y * v.z + r * v.x), r * r - v.x * v.x - v.y * v.y + v.z * v.z); + return std::atan2(2.f * (y * z + w * x), w * w - x * x - y * y + z * z); } - + inline float yaw() const { - return std::asin(-2.f * (v.x * v.z - r * v.y)); + return std::asin(-2.f * (x * z - w * y)); } + + inline float& operator[](size_t idx) {return (&x)[idx];} + inline const float& operator[](size_t idx) const {return (&x)[idx];} union { - struct { float x, y, z, r; }; - CVector3f v; + __m128 mVec128; + struct { float w, x, y, z; }; }; }; diff --git a/include/zeus/CVector2f.hpp b/include/zeus/CVector2f.hpp index 85bfbc2..3d1d56f 100644 --- a/include/zeus/CVector2f.hpp +++ b/include/zeus/CVector2f.hpp @@ -20,7 +20,7 @@ class alignas(16) CVector2f float clangVec __attribute__((__vector_size__(8))); #endif public: - ZE_DECLARE_ALIGNED_ALLOCATOR(); + //ZE_DECLARE_ALIGNED_ALLOCATOR(); union { struct diff --git a/src/CMatrix3f.cpp b/src/CMatrix3f.cpp index b69c336..869baf6 100644 --- a/src/CMatrix3f.cpp +++ b/src/CMatrix3f.cpp @@ -9,20 +9,20 @@ const CMatrix3f CMatrix3f::skIdentityMatrix3f = CMatrix3f(); CMatrix3f::CMatrix3f(const CQuaternion& quat) { CQuaternion nq = quat.normalized(); - float x2 = nq.v[0] * nq.v[0]; - float y2 = nq.v[1] * nq.v[1]; - float z2 = nq.v[2] * nq.v[2]; + float x2 = nq.x * nq.x; + float y2 = nq.y * nq.y; + float z2 = nq.z * nq.z; m[0][0] = 1.0 - 2.0 * y2 - 2.0 * z2; - m[1][0] = 2.0 * nq.v[0] * nq.v[1] - 2.0 * nq.v[2] * nq.r; - m[2][0] = 2.0 * nq.v[0] * nq.v[2] + 2.0 * nq.v[1] * nq.r; + m[1][0] = 2.0 * nq.x * nq.y - 2.0 * nq.z * nq.w; + m[2][0] = 2.0 * nq.x * nq.z + 2.0 * nq.y * nq.w; - m[0][1] = 2.0 * nq.v[0] * nq.v[1] + 2.0 * nq.v[2] * nq.r; + m[0][1] = 2.0 * nq.x * nq.y + 2.0 * nq.z * nq.w; m[1][1] = 1.0 - 2.0 * x2 - 2.0 * z2; - m[2][1] = 2.0 * nq.v[1] * nq.v[2] - 2.0 * nq.v[0] * nq.r; + m[2][1] = 2.0 * nq.y * nq.z - 2.0 * nq.x * nq.w; - m[0][2] = 2.0 * nq.v[0] * nq.v[2] - 2.0 * nq.v[1] * nq.r; - m[1][2] = 2.0 * nq.v[1] * nq.v[2] + 2.0 * nq.v[0] * nq.r; + m[0][2] = 2.0 * nq.x * nq.z - 2.0 * nq.y * nq.w; + m[1][2] = 2.0 * nq.y * nq.z + 2.0 * nq.x * nq.w; m[2][2] = 1.0 - 2.0 * x2 - 2.0 * y2; m[0][3] = 0.0f; diff --git a/src/CQuaternion.cpp b/src/CQuaternion.cpp index a594c95..3b1e96d 100644 --- a/src/CQuaternion.cpp +++ b/src/CQuaternion.cpp @@ -13,35 +13,41 @@ void CQuaternion::fromVector3f(const CVector3f& vec) float sinY = std::sin(0.5f * vec.y); float sinZ = std::sin(0.5f * vec.z); - r = cosZ * cosY * cosX + sinZ * sinY * sinX; - v.x = cosZ * cosY * sinX - sinZ * sinY * cosX; - v.y = cosZ * sinY * cosX + sinZ * cosY * sinX; - v.z = sinZ * cosY * cosX - cosZ * sinY * sinX; + w = cosZ * cosY * cosX + sinZ * sinY * sinX; + x = cosZ * cosY * sinX - sinZ * sinY * cosX; + y = cosZ * sinY * cosX + sinZ * cosY * sinX; + z = sinZ * cosY * cosX - cosZ * sinY * sinX; } CQuaternion& CQuaternion::operator=(const CQuaternion& q) { - r = q.r; - v = q.v; +#if __SSE__ + mVec128 = q.mVec128; +#else + w = q.w; + x = q.x; + y = q.y; + z = q.z; +#endif return *this; } CQuaternion CQuaternion::operator+(const CQuaternion& q) const { - return CQuaternion(r + q.r, v+q.v); + return CQuaternion(w + q.w, x + q.x, y + q.y, z + q.z); } CQuaternion CQuaternion::operator-(const CQuaternion& q) const { - return CQuaternion(r - q.r, v-q.v); + return CQuaternion(w - q.w, x - q.x, y - q.y, z - q.z); } CQuaternion CQuaternion::operator*(const CQuaternion& q) const { - return CQuaternion(r*q.r - v.dot(q.v), - v.y * q.v.z - v.z * q.v.y + r * q.v.x + v.x*q.r, - v.z * q.v.x - v.x * q.v.z + r * q.v.y + v.y*q.r, - v.x * q.v.y - v.y * q.v.x + r * q.v.z + v.z*q.r); + return CQuaternion(w*q.w - CVector3f(x, y, z).dot({q.x, q.y, q.z}), + y * q.z - z * q.y + w * q.x + x*q.w, + z * q.x - x * q.z + w * q.y + y*q.w, + x * q.y - y * q.x + w * q.z + z*q.w); } CQuaternion CQuaternion::operator/(const CQuaternion& q) const @@ -53,54 +59,62 @@ CQuaternion CQuaternion::operator/(const CQuaternion& q) const CQuaternion CQuaternion::operator*(float scale) const { - return CQuaternion(r*scale, v*scale); + return CQuaternion(w*scale, x*scale, y*scale, z*scale); } CQuaternion CQuaternion::operator/(float scale) const { - return CQuaternion(r/scale, v/scale); + return CQuaternion(w/scale, x/scale, y/scale, z/scale); } CQuaternion CQuaternion::operator-() const { - return CQuaternion(-r, -v); + return CQuaternion(-w, -x, -y, -z); } const CQuaternion& CQuaternion::operator+=(const CQuaternion& q) { - r += q.r; - v += q.v; + w += q.w; + x += q.x; + y += q.y; + z += q.z; return *this; } const CQuaternion& CQuaternion::operator-=(const CQuaternion& q) { - r -= q.r; - v -= q.v; + w -= q.w; + x -= q.x; + y -= q.y; + z -= q.z; return *this; } const CQuaternion& CQuaternion::operator *=(const CQuaternion& q) { - r = r*q.r - v.dot(q.v); - v.x = v.y * q.v.z - v.z * q.v.y + r * q.v.x + v.x*q.r; - v.y = v.z * q.v.x - v.x * q.v.z + r * q.v.y + v.y*q.r; - v.z = v.x * q.v.y - v.y * q.v.x + r * q.v.z + v.z*q.r; + w = w*q.w - CVector3f(x, y, z).dot({q.x, q.y, q.z}); + x = y * q.z - z * q.y + w * q.x + x*q.w; + y = z * q.x - x * q.z + w * q.y + y*q.w; + z = x * q.y - y * q.x + w * q.z + z*q.w; - return *this; + return *this; } const CQuaternion& CQuaternion::operator *=(float scale) { - r *= scale; - v *= scale; + w *= scale; + x *= scale; + y *= scale; + z *= scale; return *this; } const CQuaternion& CQuaternion::operator/=(float scale) { - r /= scale; - v /= scale; + w /= scale; + x /= scale; + y /= scale; + z /= scale; return *this; } @@ -111,7 +125,7 @@ float CQuaternion::magnitude() const float CQuaternion::magSquared() const { - return (r*r + (v.dot(v))); + return w*w + x*x + y*y + z*z; } void CQuaternion::normalize() @@ -126,12 +140,14 @@ CQuaternion CQuaternion::normalized() const void CQuaternion::invert() { - v = -v; + x = -x; + y = -y; + z = -z; } CQuaternion CQuaternion::inverse() const { - return CQuaternion(r, -v); + return CQuaternion(w, -x, -y, -z); } CAxisAngle CQuaternion::toAxisAngle() @@ -153,47 +169,55 @@ CAxisAngle CQuaternion::toAxisAngle() CQuaternion CQuaternion::log() const { - float a = std::acos(r); + float a = std::acos(w); float sina = std::sin(a); CQuaternion ret; - ret.r = 0.f; + ret.w = 0.f; if (sina > 0.f) { - ret.v.x = a * v.x / sina; - ret.v.y = a * v.y / sina; - ret.v.z = a * v.z / sina; + ret.x = a * x / sina; + ret.y = a * y / sina; + ret.z = a * z / sina; } else - ret.v = CVector3f(0.f); + { + ret.x = 0.f; + ret.y = 0.f; + ret.z = 0.f; + } return ret; } CQuaternion CQuaternion::exp() const { - float a = (v.magnitude()); + float a = (CVector3f(x, y, z).magnitude()); float sina = std::sin(a); float cosa = std::cos(a); CQuaternion ret; - ret.r = cosa; + ret.w = cosa; if (a > 0.f) { - ret.v.x = sina * v.x / a; - ret.v.y = sina * v.y / a; - ret.v.z = sina * v.z / a; + ret.x = sina * x / a; + ret.y = sina * y / a; + ret.z = sina * z / a; } else - ret.v = CVector3f(0.f); + { + ret.x = 0.f; + ret.y = 0.f; + ret.z = 0.f; + } return ret; } float CQuaternion::dot(const CQuaternion& b) { - return v.x * b.v.x + v.y * b.v.y + v.z * b.v.z + r * b.r; + return x * b.x + y * b.y + z * b.z + w * b.w; } CQuaternion CQuaternion::lerp(CQuaternion& a, CQuaternion& b, double t) @@ -228,10 +252,10 @@ CQuaternion CQuaternion::slerp(CQuaternion& a, CQuaternion& b, double t) const double d = 1.0 / std::sin(theta); const double s0 = std::sin((1.0 - t) * theta); - ret.v.x = (float)(a.v.x * s0 + b.v.x * s1) * d; - ret.v.y = (float)(a.v.y * s0 + b.v.y * s1) * d; - ret.v.z = (float)(a.v.z * s0 + b.v.z * s1) * d; - ret.r = (float)(a.r * s0 + b.r * s1) * d; + ret.x = (float)(a.x * s0 + b.x * s1) * d; + ret.y = (float)(a.y * s0 + b.y * s1) * d; + ret.z = (float)(a.z * s0 + b.z * s1) * d; + ret.w = (float)(a.w * s0 + b.w * s1) * d; return ret; } @@ -240,16 +264,16 @@ CQuaternion CQuaternion::slerp(CQuaternion& a, CQuaternion& b, double t) CQuaternion operator+(float lhs, const CQuaternion& rhs) { - return CQuaternion(lhs + rhs.r, lhs * rhs.v); + return CQuaternion(lhs + rhs.w, lhs * rhs.x, lhs * rhs.y, lhs * rhs.z); } CQuaternion operator-(float lhs, const CQuaternion& rhs) { - return CQuaternion(lhs - rhs.r, lhs * rhs.v); + return CQuaternion(lhs - rhs.w, lhs * rhs.x, lhs * rhs.y, lhs * rhs.z); } CQuaternion operator*(float lhs, const CQuaternion& rhs) { - return CQuaternion(lhs * rhs.r, lhs * rhs.v); + return CQuaternion(lhs * rhs.w, lhs * rhs.x, lhs * rhs.y, lhs * rhs.z); } }