WXYZ quaternion layout

include/zeus/CQuaternion.hpp

@@ -20,22 +20,24 @@ class alignas(16) CQuaternion
 public:
     ZE_DECLARE_ALIGNED_ALLOCATOR();
 
-    CQuaternion() : r(1.0f) {}
+    CQuaternion() : w(1.0f), x(0.0f), y(0.0f), z(0.0f) {}
-    CQuaternion(float r, float x, float y, float z) : v(x, y, z){ this->r = r; }
+    CQuaternion(float wi, float xi, float yi, float zi) : w(wi), x(xi), y(yi), z(zi) {}
-    CQuaternion(float x, float y, float z) { fromVector3f(CVector3f(x, y, z)); }
+    CQuaternion(float xi, float yi, float zi) { fromVector3f(CVector3f(xi, yi, zi)); }
-    CQuaternion(float r, const CVector3f& vec) : v(vec){ this->r = r;}
+    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();
+        w = input.readFloatBig();
-        v.readBig(input);
+        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;
@@ -133,23 +147,26 @@ public:
 
     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; };
+        __m128 mVec128;
-        CVector3f v;
+        struct { float w, x, y, z; };
     };
 };
 

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

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 x2 = nq.x * nq.x;
-    float y2 = nq.v[1] * nq.v[1];
+    float y2 = nq.y * nq.y;
-    float z2 = nq.v[2] * nq.v[2];
+    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[1][0] = 2.0 * nq.x * nq.y - 2.0 * nq.z * nq.w;
-    m[2][0] = 2.0 * nq.v[0] * nq.v[2] + 2.0 * nq.v[1] * nq.r;
+    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[0][2] = 2.0 * nq.x * nq.z - 2.0 * nq.y * nq.w;
-    m[1][2] = 2.0 * nq.v[1] * nq.v[2] + 2.0 * nq.v[0] * nq.r;
+    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;

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;
+    w = cosZ * cosY * cosX + sinZ * sinY * sinX;
-    v.x = cosZ * cosY * sinX - sinZ * sinY * cosX;
+    x = cosZ * cosY * sinX - sinZ * sinY * cosX;
-    v.y = cosZ * sinY * cosX + sinZ * cosY * sinX;
+    y = cosZ * sinY * cosX + sinZ * cosY * sinX;
-    v.z = sinZ * cosY * cosX - cosZ * sinY * sinX;
+    z = sinZ * cosY * cosX - cosZ * sinY * sinX;
 }
 
 CQuaternion& CQuaternion::operator=(const CQuaternion& q)
 {
-    r = q.r;
+#if __SSE__
-    v = q.v;
+    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),
+    return CQuaternion(w*q.w - CVector3f(x, y, z).dot({q.x, q.y, q.z}),
-                       v.y * q.v.z - v.z * q.v.y + r * q.v.x + v.x*q.r,
+                       y * q.z - z * q.y + w * q.x + x*q.w,
-                       v.z * q.v.x - v.x * q.v.z + r * q.v.y + v.y*q.r,
+                       z * q.x - x * q.z + w * q.y + y*q.w,
-                       v.x * q.v.y - v.y * q.v.x + r * q.v.z + v.z*q.r);
+                       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;
+    w += q.w;
-    v += q.v;
+    x += q.x;
+    y += q.y;
+    z += q.z;
     return *this;
 }
 
 const CQuaternion& CQuaternion::operator-=(const CQuaternion& q)
 {
-    r -= q.r;
+    w -= q.w;
-    v -= q.v;
+    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);
+    w = w*q.w - CVector3f(x, y, z).dot({q.x, q.y, q.z});
-   v.x = v.y * q.v.z - v.z * q.v.y + r * q.v.x + v.x*q.r;
+    x = y * q.z - z * q.y + w * q.x + x*q.w;
-   v.y = v.z * q.v.x - v.x * q.v.z + r * q.v.y + v.y*q.r;
+    y = z * q.x - x * q.z + w * q.y + y*q.w;
-   v.z = v.x * q.v.y - v.y * q.v.x + r * q.v.z + v.z*q.r;
+    z = x * q.y - y * q.x + w * q.z + z*q.w;
 
     return *this;
 }
 
 const CQuaternion& CQuaternion::operator *=(float scale)
 {
-    r *= scale;
+    w *= scale;
-    v *= scale;
+    x *= scale;
+    y *= scale;
+    z *= scale;
     return *this;
 }
 
 const CQuaternion& CQuaternion::operator/=(float scale)
 {
-    r /= scale;
+    w /= scale;
-    v /= 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.x = a * x / sina;
-        ret.v.y = a * v.y / sina;
+        ret.y = a * y / sina;
-        ret.v.z = a * v.z / 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.x = sina * x / a;
-        ret.v.y = sina * v.y / a;
+        ret.y = sina * y / a;
-        ret.v.z = sina * v.z / 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.x = (float)(a.x * s0 + b.x * s1) * d;
-        ret.v.y = (float)(a.v.y * s0 + b.v.y * s1) * d;
+        ret.y = (float)(a.y * s0 + b.y * s1) * d;
-        ret.v.z = (float)(a.v.z * s0 + b.v.z * s1) * d;
+        ret.z = (float)(a.z * s0 + b.z * s1) * d;
-        ret.r   = (float)(a.r   * s0 + b.r   * 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);
 }
 }