324 lines
8.6 KiB
C++
324 lines
8.6 KiB
C++
#pragma once
|
|
|
|
#include <Orange/Math/Vector.h>
|
|
#include <Orange/Math/Angle.h>
|
|
#include <Orange/Math/Basic.h>
|
|
|
|
namespace orange
|
|
{
|
|
template <typename T>
|
|
struct Quaternion : public Vec<T, 4>
|
|
{
|
|
constexpr Quaternion()
|
|
: Vec<T, 4>{ T{ 0 }, T{ 0 }, T{ 0 }, T{ 1 } } { }
|
|
|
|
constexpr Quaternion(T x, T y, T z, T w)
|
|
: Vec<T, 4>{ x, y, z, w } { }
|
|
|
|
constexpr Quaternion(const Vec<T, 3> v, T s)
|
|
: Vec<T, 4>{ v[0], v[1], v[2], s } { }
|
|
|
|
constexpr Quaternion(const Quaternion& other) = default;
|
|
|
|
|
|
constexpr Quaternion<T> operator*(const Quaternion<T>& b) const
|
|
{
|
|
const Quaternion<T>& a = *this;
|
|
return Quaternion<T>
|
|
{
|
|
a.w * b.x + a.x * b.w + a.y * b.z - a.z * b.y,
|
|
a.w * b.y - a.x * b.z + a.y * b.w + a.z * b.x,
|
|
a.w * b.z + a.x * b.y - a.y * b.x + a.z * b.w,
|
|
a.w * b.w - a.x * b.x - a.y * b.y - a.z * b.z
|
|
};
|
|
}
|
|
|
|
constexpr Quaternion<T> operator*(float s) const
|
|
{
|
|
Quaternion<T> c = *this;
|
|
|
|
c.x *= s;
|
|
c.y *= s;
|
|
c.z *= s;
|
|
c.w *= s;
|
|
|
|
return c;
|
|
}
|
|
|
|
constexpr Quaternion<T> operator/(const Quaternion<T>& other) = delete;
|
|
|
|
constexpr Quaternion<T>& operator*=(const Quaternion<T>& other)
|
|
{
|
|
*this = *this * other;
|
|
return *this;
|
|
}
|
|
|
|
constexpr Quaternion<T>& operator*=(float s)
|
|
{
|
|
Quaternion<T>& c = *this;
|
|
|
|
c.x *= s;
|
|
c.y *= s;
|
|
c.z *= s;
|
|
c.w *= s;
|
|
|
|
return c;
|
|
}
|
|
|
|
constexpr Quaternion<T>& operator/=(const Quaternion<T>& other) = delete;
|
|
|
|
// TODO: This sucks! Make this cleaner
|
|
// We should get a proper subclass thing
|
|
constexpr const Vec<T, 3>& vector() const
|
|
{
|
|
return *reinterpret_cast<const Vec<T, 3>*>(this);
|
|
}
|
|
|
|
constexpr Vec<T, 3>& vector()
|
|
{
|
|
return *reinterpret_cast<Vec<T, 3>*>(this);
|
|
}
|
|
|
|
constexpr const T& scalar() const
|
|
{
|
|
return (*this)[3];
|
|
}
|
|
|
|
constexpr T& scalar()
|
|
{
|
|
return (*this)[3];
|
|
}
|
|
};
|
|
|
|
template <typename T>
|
|
constexpr Quaternion<T> operator*(float s, const Quaternion<T>& q)
|
|
{
|
|
return q * s;
|
|
}
|
|
|
|
template <typename T>
|
|
constexpr Vec<T, 3> vector(const Quaternion<T>& q)
|
|
{
|
|
return q.vector();
|
|
}
|
|
|
|
template <typename T>
|
|
constexpr T scalar(const Quaternion<T>& q)
|
|
{
|
|
return q.scalar();
|
|
}
|
|
|
|
template <typename T>
|
|
float lengthSqr(const Quaternion<T>& q) { return dot(q, q); }
|
|
|
|
template <typename T>
|
|
float length(const Quaternion<T>& q) { return sqrtf(lengthSqr(q)); }
|
|
|
|
template <typename T>
|
|
float dot(const Quaternion<T>& a, const Quaternion<T>& b)
|
|
{
|
|
return dot(a.vector(), b.vector()) + a.w * b.w;
|
|
}
|
|
|
|
template <typename T>
|
|
constexpr Quaternion<T> cross(const Quaternion<T>& a, const Quaternion<T>& b)
|
|
{
|
|
return Quaternion<T>
|
|
{
|
|
a[3] * b[0] + a[0] * b[3] + a[1] * b[2] - a[2] * b[1],
|
|
a[3] * b[1] + a[1] * b[3] + a[2] * b[0] - a[0] * b[2],
|
|
a[3] * b[2] + a[2] * b[3] + a[0] * b[1] - a[1] * b[0],
|
|
a[3] * b[3] - a[0] * b[0] - a[1] * b[1] - a[2] * b[2]
|
|
};
|
|
}
|
|
|
|
template <typename T>
|
|
constexpr Quaternion<T> normalize(const Quaternion<T>& q) { return q * (1.0f / length(q)); }
|
|
|
|
template <typename T>
|
|
constexpr Quaternion<T> conjugate(const Quaternion<T>& q)
|
|
{
|
|
return Quaternion<T> { -vector(q), scalar(q) };
|
|
}
|
|
|
|
template <typename T>
|
|
constexpr Quaternion<T> inverse(const Quaternion<T>& q)
|
|
{
|
|
return Quaternion<T> { conjugate(q) / length(q) };
|
|
}
|
|
|
|
|
|
template <typename T>
|
|
constexpr Vec<T, 3> operator*(const Quaternion<T>& q, const Vec<T, 3> v)
|
|
{
|
|
const Vec<T, 3> t = 2.0f * cross(vector(q), v);
|
|
return Vec<T, 3>{ v + scalar(q) * t + cross(vector(q), t) };
|
|
}
|
|
|
|
using quat = Quaternion<float>;
|
|
|
|
inline Radian angle(const quat& q) { return 2.0f * Math::acos(q.w); }
|
|
|
|
inline vec3 axis(const quat& q)
|
|
{
|
|
// 1 - cos(theta/2)*cos(theta/2) = sin(theta/2)*sin(theta/2)
|
|
float s2 = 1.0f - q.w * q.w;
|
|
|
|
if (s2 <= 0.0f)
|
|
return vec3(0.0f, 0.0f, 1.0f);
|
|
|
|
float invs2 = 1.0f / sqrtf(s2);
|
|
|
|
return q.vector() * invs2;
|
|
}
|
|
|
|
inline quat angleAxis(const Radian& angle, const vec3& axis)
|
|
{
|
|
quat q;
|
|
|
|
const vec3 a = normalize(axis);
|
|
|
|
const float s = Math::sin(0.5f * angle);
|
|
|
|
q.vector() = a * s;
|
|
q.w = Math::cos(0.5f * angle);
|
|
|
|
return q;
|
|
}
|
|
|
|
inline mat4 quaternionToMatrix4(const quat& q)
|
|
{
|
|
mat4 mat;
|
|
quat a = normalize(q);
|
|
|
|
const float xx = a.x * a.x;
|
|
const float yy = a.y * a.y;
|
|
const float zz = a.z * a.z;
|
|
const float xy = a.x * a.y;
|
|
const float xz = a.x * a.z;
|
|
const float yz = a.y * a.z;
|
|
const float wx = a.w * a.x;
|
|
const float wy = a.w * a.y;
|
|
const float wz = a.w * a.z;
|
|
|
|
mat[0][0] = 1.0f - 2.0f * (yy + zz);
|
|
mat[0][1] = 2.0f * (xy + wz);
|
|
mat[0][2] = 2.0f * (xz - wy);
|
|
|
|
mat[1][0] = 2.0f * (xy - wz);
|
|
mat[1][1] = 1.0f - 2.0f * (xx + zz);
|
|
mat[1][2] = 2.0f * (yz + wx);
|
|
|
|
mat[2][0] = 2.0f * (xz + wy);
|
|
mat[2][1] = 2.0f * (yz - wx);
|
|
mat[2][2] = 1.0f - 2.0f * (xx + yy);
|
|
|
|
return mat;
|
|
}
|
|
|
|
inline quat matrix4ToQuaternion(const mat4& m)
|
|
{
|
|
float fourXSquaredMinus1 = m[0][0] - m[1][1] - m[2][2];
|
|
float fourYSquaredMinus1 = m[1][1] - m[0][0] - m[2][2];
|
|
float fourZSquaredMinus1 = m[2][2] - m[0][0] - m[1][1];
|
|
float fourWSquaredMinus1 = m[0][0] + m[1][1] + m[2][2];
|
|
|
|
int biggestIndex = 0;
|
|
float fourBiggestSquaredMinus1 = fourWSquaredMinus1;
|
|
if (fourXSquaredMinus1 > fourBiggestSquaredMinus1)
|
|
{
|
|
fourBiggestSquaredMinus1 = fourXSquaredMinus1;
|
|
biggestIndex = 1;
|
|
}
|
|
if (fourYSquaredMinus1 > fourBiggestSquaredMinus1)
|
|
{
|
|
fourBiggestSquaredMinus1 = fourYSquaredMinus1;
|
|
biggestIndex = 2;
|
|
}
|
|
if (fourZSquaredMinus1 > fourBiggestSquaredMinus1)
|
|
{
|
|
fourBiggestSquaredMinus1 = fourZSquaredMinus1;
|
|
biggestIndex = 3;
|
|
}
|
|
|
|
float biggestVal = sqrtf(fourBiggestSquaredMinus1 + 1.0f) * 0.5f;
|
|
float mult = 0.25f / biggestVal;
|
|
|
|
quat q;
|
|
|
|
switch (biggestIndex)
|
|
{
|
|
case 0:
|
|
{
|
|
q.w = biggestVal;
|
|
q.x = (m[1][2] - m[2][1]) * mult;
|
|
q.y = (m[2][0] - m[0][2]) * mult;
|
|
q.z = (m[0][1] - m[1][0]) * mult;
|
|
}
|
|
break;
|
|
case 1:
|
|
{
|
|
q.w = (m[1][2] - m[2][1]) * mult;
|
|
q.x = biggestVal;
|
|
q.y = (m[0][1] + m[1][0]) * mult;
|
|
q.z = (m[2][0] + m[0][2]) * mult;
|
|
}
|
|
break;
|
|
case 2:
|
|
{
|
|
q.w = (m[2][0] - m[0][2]) * mult;
|
|
q.x = (m[0][1] + m[1][0]) * mult;
|
|
q.y = biggestVal;
|
|
q.z = (m[1][2] + m[2][1]) * mult;
|
|
}
|
|
break;
|
|
case 3:
|
|
{
|
|
q.w = (m[0][1] - m[1][0]) * mult;
|
|
q.x = (m[2][0] + m[0][2]) * mult;
|
|
q.y = (m[1][2] + m[2][1]) * mult;
|
|
q.z = biggestVal;
|
|
}
|
|
break;
|
|
}
|
|
|
|
return q;
|
|
}
|
|
|
|
inline Radian roll(const quat& q)
|
|
{
|
|
return Math::atan2(2.0f * q[0] * q[1] + q[2] * q[3],
|
|
q[0] * q[0] + q[3] * q[3] - q[1] * q[1] - q[2] * q[2]);
|
|
}
|
|
|
|
inline Radian pitch(const quat& q)
|
|
{
|
|
return Math::atan2(2.0f * q[1] * q[2] + q[3] * q[0],
|
|
q[3] * q[3] - q[0] * q[0] - q[1] * q[1] + q[2] * q[2]);
|
|
}
|
|
|
|
inline Radian yaw(const quat& q)
|
|
{
|
|
return Math::asin(-2.0f * (q[0] * q[2] - q[3] * q[1]));
|
|
}
|
|
|
|
#if 0
|
|
EulerAngles quaternionToEulerAngles(const quat& q)
|
|
{
|
|
return {pitch(q), yaw(q), roll(q)};
|
|
}
|
|
|
|
Quaternion eulerAnglesToQuaternion(const EulerAngles& e,
|
|
const vec3& xAxis,
|
|
const vec3& yAxis,
|
|
const vec3& zAxis)
|
|
{
|
|
quat p{angleAxis(e.pitch, xAxis)};
|
|
quat y{angleAxis(e.pitch, yAxis)};
|
|
quat r{angleAxis(e.pitch, zAxis)};
|
|
|
|
return y * p * r;
|
|
}
|
|
#endif
|
|
}
|