mirror of https://github.com/AxioDL/zeus.git
WXYZ quaternion layout
This commit is contained in:
Jack Andersen
parent
65a49aca92
commit
baaca3136b
|
|
@ -20,22 +20,24 @@ class alignas(16) CQuaternion
|
|||
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;
|
||||
|
|
@ -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; };
|
||||
CVector3f v;
|
||||
__m128 mVec128;
|
||||
struct { float w, x, y, z; };
|
||||
};
|
||||
};
|
||||
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
|
|
|
|||
|
|
@ -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);
|
||||
}
|
||||
}
|
||||
|
|
|
|||
Loading…
Reference in New Issue