243 lines
7.5 KiB
C++
243 lines
7.5 KiB
C++
#include "Core/Math/Quaternion.h"
|
|
#include "Core/Math/Matrix4.h"
|
|
#include "Core/Math/Vector3.h"
|
|
|
|
namespace XCEngine {
|
|
namespace Math {
|
|
|
|
Quaternion Quaternion::FromAxisAngle(const Vector3& axis, float radians) {
|
|
float halfAngle = radians * 0.5f;
|
|
float s = std::sin(halfAngle);
|
|
return Quaternion(
|
|
axis.x * s,
|
|
axis.y * s,
|
|
axis.z * s,
|
|
std::cos(halfAngle)
|
|
);
|
|
}
|
|
|
|
Quaternion Quaternion::FromEulerAngles(float pitch, float yaw, float roll) {
|
|
float halfPitch = pitch * 0.5f;
|
|
float halfYaw = yaw * 0.5f;
|
|
float halfRoll = roll * 0.5f;
|
|
|
|
float sinPitch = std::sin(halfPitch);
|
|
float cosPitch = std::cos(halfPitch);
|
|
float sinYaw = std::sin(halfYaw);
|
|
float cosYaw = std::cos(halfYaw);
|
|
float sinRoll = std::sin(halfRoll);
|
|
float cosRoll = std::cos(halfRoll);
|
|
|
|
return Quaternion(
|
|
cosYaw * sinPitch * cosRoll + sinYaw * cosPitch * sinRoll,
|
|
sinYaw * cosPitch * cosRoll - cosYaw * sinPitch * sinRoll,
|
|
cosYaw * cosPitch * sinRoll - sinYaw * sinPitch * cosRoll,
|
|
cosYaw * cosPitch * cosRoll + sinYaw * sinPitch * sinRoll
|
|
);
|
|
}
|
|
|
|
Quaternion Quaternion::FromEulerAngles(const Vector3& euler) {
|
|
return FromEulerAngles(euler.x, euler.y, euler.z);
|
|
}
|
|
|
|
Quaternion Quaternion::FromRotationMatrix(const Matrix4& matrix) {
|
|
float trace = matrix.m[0][0] + matrix.m[1][1] + matrix.m[2][2];
|
|
|
|
Quaternion q;
|
|
|
|
if (trace > 0.0f) {
|
|
float s = std::sqrt(trace + 1.0f) * 2.0f;
|
|
q.w = 0.25f * s;
|
|
q.x = (matrix.m[2][1] - matrix.m[1][2]) / s;
|
|
q.y = (matrix.m[0][2] - matrix.m[2][0]) / s;
|
|
q.z = (matrix.m[1][0] - matrix.m[0][1]) / s;
|
|
} else if (matrix.m[0][0] > matrix.m[1][1] && matrix.m[0][0] > matrix.m[2][2]) {
|
|
float s = std::sqrt(1.0f + matrix.m[0][0] - matrix.m[1][1] - matrix.m[2][2]) * 2.0f;
|
|
q.w = (matrix.m[2][1] - matrix.m[1][2]) / s;
|
|
q.x = 0.25f * s;
|
|
q.y = (matrix.m[0][1] + matrix.m[1][0]) / s;
|
|
q.z = (matrix.m[0][2] + matrix.m[2][0]) / s;
|
|
} else if (matrix.m[1][1] > matrix.m[2][2]) {
|
|
float s = std::sqrt(1.0f + matrix.m[1][1] - matrix.m[0][0] - matrix.m[2][2]) * 2.0f;
|
|
q.w = (matrix.m[0][2] - matrix.m[2][0]) / s;
|
|
q.x = (matrix.m[0][1] + matrix.m[1][0]) / s;
|
|
q.y = 0.25f * s;
|
|
q.z = (matrix.m[1][2] + matrix.m[2][1]) / s;
|
|
} else {
|
|
float s = std::sqrt(1.0f + matrix.m[2][2] - matrix.m[0][0] - matrix.m[1][1]) * 2.0f;
|
|
q.w = (matrix.m[1][0] - matrix.m[0][1]) / s;
|
|
q.x = (matrix.m[0][2] + matrix.m[2][0]) / s;
|
|
q.y = (matrix.m[1][2] + matrix.m[2][1]) / s;
|
|
q.z = 0.25f * s;
|
|
}
|
|
|
|
return q.Normalized();
|
|
}
|
|
|
|
Quaternion Quaternion::Slerp(const Quaternion& a, const Quaternion& b, float t) {
|
|
t = std::clamp(t, 0.0f, 1.0f);
|
|
float cosHalfTheta = a.w * b.w + a.x * b.x + a.y * b.y + a.z * b.z;
|
|
|
|
if (cosHalfTheta < 0.0f) {
|
|
return Slerp(Quaternion(-b.x, -b.y, -b.z, -b.w), Quaternion(a.x, a.y, a.z, a.w), 1.0f - t);
|
|
}
|
|
|
|
if (std::abs(cosHalfTheta) >= 1.0f) {
|
|
return a;
|
|
}
|
|
|
|
float halfTheta = std::acos(cosHalfTheta);
|
|
float sinHalfTheta = std::sqrt(1.0f - cosHalfTheta * cosHalfTheta);
|
|
|
|
if (std::abs(sinHalfTheta) < EPSILON) {
|
|
return Quaternion(
|
|
a.w * 0.5f + b.w * 0.5f,
|
|
a.x * 0.5f + b.x * 0.5f,
|
|
a.y * 0.5f + b.y * 0.5f,
|
|
a.z * 0.5f + b.z * 0.5f
|
|
);
|
|
}
|
|
|
|
float ratioA = std::sin((1.0f - t) * halfTheta) / sinHalfTheta;
|
|
float ratioB = std::sin(t * halfTheta) / sinHalfTheta;
|
|
|
|
return Quaternion(
|
|
a.x * ratioA + b.x * ratioB,
|
|
a.y * ratioA + b.y * ratioB,
|
|
a.z * ratioA + b.z * ratioB,
|
|
a.w * ratioA + b.w * ratioB
|
|
);
|
|
}
|
|
|
|
Quaternion Quaternion::LookRotation(const Vector3& forward, const Vector3& up) {
|
|
Vector3 f = Vector3::Normalize(forward);
|
|
|
|
if (std::abs(Vector3::Dot(f, Vector3::Up())) > 1.0f - EPSILON) {
|
|
return Quaternion::FromAxisAngle(Vector3::Right(), PI * 0.5f);
|
|
}
|
|
if (std::abs(Vector3::Dot(f, Vector3::Down())) > 1.0f - EPSILON) {
|
|
return Quaternion::FromAxisAngle(Vector3::Right(), -PI * 0.5f);
|
|
}
|
|
if (std::abs(Vector3::Dot(f, Vector3::Right())) > 1.0f - EPSILON) {
|
|
return Quaternion::FromAxisAngle(Vector3::Up(), -PI * 0.5f);
|
|
}
|
|
if (std::abs(Vector3::Dot(f, Vector3::Left())) > 1.0f - EPSILON) {
|
|
return Quaternion::FromAxisAngle(Vector3::Up(), PI * 0.5f);
|
|
}
|
|
|
|
Vector3 upVec = up;
|
|
if (std::abs(Vector3::Dot(f, upVec)) > 1.0f - EPSILON) {
|
|
upVec = (std::abs(f.y) < 1.0f - EPSILON) ? Vector3::Up() : Vector3::Right();
|
|
}
|
|
|
|
Vector3 r = Vector3::Normalize(Vector3::Cross(upVec, f));
|
|
Vector3 u = Vector3::Cross(f, r);
|
|
|
|
Matrix4 m;
|
|
m.m[0][0] = r.x; m.m[0][1] = r.y; m.m[0][2] = r.z;
|
|
m.m[1][0] = u.x; m.m[1][1] = u.y; m.m[1][2] = u.z;
|
|
m.m[2][0] = f.x; m.m[2][1] = f.y; m.m[2][2] = f.z;
|
|
m.m[0][3] = m[1][3] = m[2][3] = 0.0f;
|
|
m.m[3][0] = m[3][1] = m[3][2] = 0.0f;
|
|
m.m[3][3] = 1.0f;
|
|
|
|
return FromRotationMatrix(m);
|
|
}
|
|
|
|
Vector3 Quaternion::ToEulerAngles() const {
|
|
float sinrCosp = 2.0f * (w * x + y * z);
|
|
float cosrCosp = 1.0f - 2.0f * (x * x + y * y);
|
|
float roll = std::atan2(sinrCosp, cosrCosp);
|
|
|
|
float sinp = 2.0f * (w * y - z * x);
|
|
float pitch;
|
|
if (std::abs(sinp) >= 1.0f) {
|
|
pitch = std::copysign(PI * 0.5f, sinp);
|
|
} else {
|
|
pitch = std::asin(sinp);
|
|
}
|
|
|
|
float sinyCosp = 2.0f * (w * z + x * y);
|
|
float cosyCosp = 1.0f - 2.0f * (y * y + z * z);
|
|
float yaw = std::atan2(sinyCosp, cosyCosp);
|
|
|
|
return Vector3(roll, pitch, yaw);
|
|
}
|
|
|
|
Matrix4 Quaternion::ToMatrix4x4() const {
|
|
float xx = x * x;
|
|
float yy = y * y;
|
|
float zz = z * z;
|
|
float xy = x * y;
|
|
float xz = x * z;
|
|
float yz = y * z;
|
|
float wx = w * x;
|
|
float wy = w * y;
|
|
float wz = w * z;
|
|
|
|
Matrix4 result;
|
|
result.m[0][0] = 1.0f - 2.0f * (yy + zz);
|
|
result.m[0][1] = 2.0f * (xy - wz);
|
|
result.m[0][2] = 2.0f * (xz + wy);
|
|
|
|
result.m[1][0] = 2.0f * (xy + wz);
|
|
result.m[1][1] = 1.0f - 2.0f * (xx + zz);
|
|
result.m[1][2] = 2.0f * (yz - wx);
|
|
|
|
result.m[2][0] = 2.0f * (xz - wy);
|
|
result.m[2][1] = 2.0f * (yz + wx);
|
|
result.m[2][2] = 1.0f - 2.0f * (xx + yy);
|
|
|
|
result.m[3][3] = 1.0f;
|
|
|
|
return result;
|
|
}
|
|
|
|
Quaternion Quaternion::operator*(const Quaternion& other) const {
|
|
return Quaternion(
|
|
w * other.x + x * other.w + y * other.z - z * other.y,
|
|
w * other.y - x * other.z + y * other.w + z * other.x,
|
|
w * other.z + x * other.y - y * other.x + z * other.w,
|
|
w * other.w - x * other.x - y * other.y - z * other.z
|
|
);
|
|
}
|
|
|
|
Quaternion Quaternion::Inverse() const {
|
|
float norm = x * x + y * y + z * z + w * w;
|
|
if (norm > 0.0f) {
|
|
float invNorm = 1.0f / norm;
|
|
return Quaternion(-x * invNorm, -y * invNorm, -z * invNorm, w * invNorm);
|
|
}
|
|
return Identity();
|
|
}
|
|
|
|
float Quaternion::Dot(const Quaternion& other) const {
|
|
return x * other.x + y * other.y + z * other.z + w * other.w;
|
|
}
|
|
|
|
float Quaternion::Magnitude() const {
|
|
return std::sqrt(x * x + y * y + z * z + w * w);
|
|
}
|
|
|
|
Quaternion Quaternion::Normalized() const {
|
|
float mag = Magnitude();
|
|
if (mag > EPSILON) {
|
|
return Quaternion(x / mag, y / mag, z / mag, w / mag);
|
|
}
|
|
return Identity();
|
|
}
|
|
|
|
Quaternion Quaternion::Normalize(const Quaternion& q) {
|
|
return q.Normalized();
|
|
}
|
|
|
|
Vector3 operator*(const Quaternion& q, const Vector3& v) {
|
|
Vector3 qv(q.x, q.y, q.z);
|
|
Vector3 uv = Vector3::Cross(qv, v);
|
|
Vector3 uuv = Vector3::Cross(qv, uv);
|
|
return v + (uv * q.w + uuv) * 2.0f;
|
|
}
|
|
|
|
} // namespace Math
|
|
} // namespace XCEngine
|