419 lines
13 KiB
C++
419 lines
13 KiB
C++
// This Source Code Form is subject to the terms of the Mozilla Public
|
|
// License, v. 2.0.If a copy of the MPL was not distributed with this
|
|
// file, You can obtain one at https ://mozilla.org/MPL/2.0/.
|
|
|
|
#include "Quaternion.h"
|
|
#include <float.h>
|
|
#include <math.h>
|
|
#include "Angle.h"
|
|
#include "Vector3.h"
|
|
|
|
void CopyQuat(const Quat& q1, Quat& q2) {
|
|
q2.x = q1.x;
|
|
q2.y = q1.y;
|
|
q2.z = q1.z;
|
|
q2.w = q1.w;
|
|
}
|
|
|
|
const float Deg2Rad = 0.0174532924F;
|
|
const float Rad2Deg = 57.29578F;
|
|
|
|
Quaternion::Quaternion() {
|
|
x = 0;
|
|
y = 0;
|
|
z = 0;
|
|
w = 1;
|
|
}
|
|
|
|
Quaternion::Quaternion(float _x, float _y, float _z, float _w) {
|
|
x = _x;
|
|
y = _y;
|
|
z = _z;
|
|
w = _w;
|
|
}
|
|
|
|
Quaternion::Quaternion(Quat q) {
|
|
x = q.x;
|
|
y = q.y;
|
|
z = q.z;
|
|
w = q.w;
|
|
}
|
|
|
|
Quaternion::~Quaternion() {}
|
|
|
|
const Quaternion Quaternion::identity = Quaternion(0, 0, 0, 1);
|
|
|
|
Vector3 Quaternion::xyz() const {
|
|
return Vector3(x, y, z);
|
|
}
|
|
|
|
float Quaternion::GetLength() const {
|
|
return sqrtf(x * x + y * y + z * z + w * w);
|
|
}
|
|
|
|
float Quaternion::GetLengthSquared() const {
|
|
return x * x + y * y + z * z + w * w;
|
|
}
|
|
float Quaternion::GetLengthSquared(const Quaternion& q) {
|
|
return q.x * q.x + q.y * q.y + q.z * q.z + q.w * q.w;
|
|
}
|
|
|
|
void Quaternion::Normalize() {
|
|
float length = GetLength();
|
|
x /= length;
|
|
y /= length;
|
|
z /= length;
|
|
w /= length;
|
|
}
|
|
|
|
Quaternion Quaternion::Normalize(const Quaternion& q) {
|
|
Quaternion result;
|
|
float length = q.GetLength();
|
|
result = Quaternion(q.x / length, q.y / length, q.z / length, q.w / length);
|
|
return result;
|
|
};
|
|
|
|
float Quaternion::Dot(Quaternion a, Quaternion b) {
|
|
return a.x * b.x + a.y * b.y + a.z * b.z + a.w * b.w;
|
|
}
|
|
|
|
Vector3 Quaternion::ToAngles(const Quaternion& q1) {
|
|
float test = q1.x * q1.y + q1.z * q1.w;
|
|
if (test > 0.499f) { // singularity at north pole
|
|
return Vector3(0, 2 * (float)atan2(q1.x, q1.w) * Rad2Deg, 90);
|
|
} else if (test < -0.499f) { // singularity at south pole
|
|
return Vector3(0, -2 * (float)atan2(q1.x, q1.w) * Rad2Deg, -90);
|
|
} else {
|
|
float sqx = q1.x * q1.x;
|
|
float sqy = q1.y * q1.y;
|
|
float sqz = q1.z * q1.z;
|
|
|
|
return Vector3(
|
|
atan2f(2 * q1.x * q1.w - 2 * q1.y * q1.z, 1 - 2 * sqx - 2 * sqz) *
|
|
Rad2Deg,
|
|
atan2f(2 * q1.y * q1.w - 2 * q1.x * q1.z, 1 - 2 * sqy - 2 * sqz) *
|
|
Rad2Deg,
|
|
asinf(2 * test) * Rad2Deg);
|
|
}
|
|
}
|
|
|
|
Quaternion Quaternion::operator*(const Quaternion& r2) const {
|
|
return Quaternion(
|
|
this->x * r2.w + this->y * r2.z - this->z * r2.y + this->w * r2.x,
|
|
-this->x * r2.z + this->y * r2.w + this->z * r2.x + this->w * r2.y,
|
|
this->x * r2.y - this->y * r2.x + this->z * r2.w + this->w * r2.z,
|
|
-this->x * r2.x - this->y * r2.y - this->z * r2.z + this->w * r2.w);
|
|
};
|
|
|
|
Vector3 Quaternion::operator*(const Vector3& p) const {
|
|
float num = this->x * 2;
|
|
float num2 = this->y * 2;
|
|
float num3 = this->z * 2;
|
|
float num4 = this->x * num;
|
|
float num5 = this->y * num2;
|
|
float num6 = this->z * num3;
|
|
float num7 = this->x * num2;
|
|
float num8 = this->x * num3;
|
|
float num9 = this->y * num3;
|
|
float num10 = this->w * num;
|
|
float num11 = this->w * num2;
|
|
float num12 = this->w * num3;
|
|
|
|
float px = p.Right();
|
|
float py = p.Up();
|
|
float pz = p.Forward();
|
|
// Vector3 result = Vector3::zero;
|
|
// result.x =
|
|
float rx =
|
|
(1 - (num5 + num6)) * px + (num7 - num12) * py + (num8 + num11) * pz;
|
|
// result.y =
|
|
float ry =
|
|
(num7 + num12) * px + (1 - (num4 + num6)) * py + (num9 - num10) * pz;
|
|
// result.z =
|
|
float rz =
|
|
(num8 - num11) * px + (num9 + num10) * py + (1 - (num4 + num5)) * pz;
|
|
Vector3 result = Vector3(rx, ry, rz);
|
|
return result;
|
|
}
|
|
|
|
bool Quaternion::operator==(const Quaternion& q) const {
|
|
return (this->x == q.x && this->y == q.y && this->z == q.z && this->w == q.w);
|
|
}
|
|
|
|
Quaternion Quaternion::Inverse(Quaternion r) {
|
|
float n = sqrtf(r.x * r.x + r.y * r.y + r.z * r.z + r.w * r.w);
|
|
return Quaternion(-r.x / n, -r.y / n, -r.z / n, r.w / n);
|
|
}
|
|
|
|
Quaternion Quaternion::LookRotation(const Vector3& forward) {
|
|
Vector3 up = Vector3(0, 1, 0);
|
|
return LookRotation(forward, up);
|
|
}
|
|
Quaternion Quaternion::LookRotation(const Vector3& forward, const Vector3& up) {
|
|
Vector3 nForward = Vector3::Normalize(forward);
|
|
Vector3 nRight = Vector3::Normalize(Vector3::Cross(up, nForward));
|
|
Vector3 nUp = Vector3::Cross(nForward, nRight);
|
|
float m00 = nRight.Right(); // x;
|
|
float m01 = nRight.Up(); // y;
|
|
float m02 = nRight.Forward(); // z;
|
|
float m10 = nUp.Right(); // x;
|
|
float m11 = nUp.Up(); // y;
|
|
float m12 = nUp.Forward(); // z;
|
|
float m20 = nForward.Right(); // x;
|
|
float m21 = nForward.Up(); // y;
|
|
float m22 = nForward.Forward(); // z;
|
|
|
|
float num8 = (m00 + m11) + m22;
|
|
Quaternion quaternion = Quaternion();
|
|
if (num8 > 0) {
|
|
float num = sqrtf(num8 + 1);
|
|
quaternion.w = num * 0.5f;
|
|
num = 0.5f / num;
|
|
quaternion.x = (m12 - m21) * num;
|
|
quaternion.y = (m20 - m02) * num;
|
|
quaternion.z = (m01 - m10) * num;
|
|
return quaternion;
|
|
}
|
|
if ((m00 >= m11) && (m00 >= m22)) {
|
|
float num7 = sqrtf(((1 + m00) - m11) - m22);
|
|
float num4 = 0.5F / num7;
|
|
quaternion.x = 0.5f * num7;
|
|
quaternion.y = (m01 + m10) * num4;
|
|
quaternion.z = (m02 + m20) * num4;
|
|
quaternion.w = (m12 - m21) * num4;
|
|
return quaternion;
|
|
}
|
|
if (m11 > m22) {
|
|
float num6 = sqrtf(((1 + m11) - m00) - m22);
|
|
float num3 = 0.5F / num6;
|
|
quaternion.x = (m10 + m01) * num3;
|
|
quaternion.y = 0.5F * num6;
|
|
quaternion.z = (m21 + m12) * num3;
|
|
quaternion.w = (m20 - m02) * num3;
|
|
return quaternion;
|
|
}
|
|
float num5 = sqrtf(((1 + m22) - m00) - m11);
|
|
float num2 = 0.5F / num5;
|
|
quaternion.x = (m20 + m02) * num2;
|
|
quaternion.y = (m21 + m12) * num2;
|
|
quaternion.z = 0.5F * num5;
|
|
quaternion.w = (m01 - m10) * num2;
|
|
return quaternion;
|
|
}
|
|
|
|
Quaternion Quaternion::FromToRotation(Vector3 fromDirection,
|
|
Vector3 toDirection) {
|
|
Vector3 axis = Vector3::Cross(fromDirection, toDirection);
|
|
axis = Vector3::Normalize(axis);
|
|
AngleOf<float> angle = Vector3::SignedAngle(fromDirection, toDirection, axis);
|
|
Quaternion rotation = Quaternion::AngleAxis(angle.InDegrees(), axis);
|
|
return rotation;
|
|
}
|
|
|
|
Quaternion Quaternion::RotateTowards(const Quaternion& from,
|
|
const Quaternion& to,
|
|
float maxDegreesDelta) {
|
|
float num = Quaternion::Angle(from, to);
|
|
if (num == 0) {
|
|
return to;
|
|
}
|
|
float t = (float)fmin(1, maxDegreesDelta / num);
|
|
return SlerpUnclamped(from, to, t);
|
|
}
|
|
|
|
Quaternion Quaternion::AngleAxis(float angle, const Vector3& axis) {
|
|
if (Vector3::SqrMagnitude(axis) == 0.0f)
|
|
return Quaternion();
|
|
|
|
Quaternion result = Quaternion();
|
|
float radians = angle * Deg2Rad;
|
|
radians *= 0.5f;
|
|
|
|
Vector3 axis2 = axis * (float)sin(radians);
|
|
result.x = axis2.Right(); // x;
|
|
result.y = axis2.Up(); // y;
|
|
result.z = axis2.Forward(); // z;
|
|
result.w = (float)cos(radians);
|
|
|
|
return Quaternion::Normalize(result);
|
|
}
|
|
|
|
float Quaternion::Angle(Quaternion a, Quaternion b) {
|
|
float f = Quaternion::Dot(a, b);
|
|
return (float)acos(fmin(fabs(f), 1)) * 2 * Rad2Deg;
|
|
}
|
|
|
|
void Quaternion::ToAngleAxis(float* angle, Vector3* axis) {
|
|
Quaternion::ToAxisAngleRad(*this, axis, angle);
|
|
*angle *= Rad2Deg;
|
|
}
|
|
|
|
void Quaternion::ToAxisAngleRad(const Quaternion& q,
|
|
Vector3* const axis,
|
|
float* angle) {
|
|
Quaternion q1 = (fabs(q.w) > 1.0f) ? Quaternion::Normalize(q) : q;
|
|
*angle = 2.0f * acosf(q1.w); // angle
|
|
float den = sqrtf(1.0F - q1.w * q1.w);
|
|
if (den > 0.0001f) {
|
|
*axis = Vector3::Normalize(q1.xyz() / den);
|
|
} else {
|
|
// This occurs when the angle is zero.
|
|
// Not a problem: just set an arbitrary normalized axis.
|
|
*axis = Vector3(1, 0, 0);
|
|
}
|
|
}
|
|
Quaternion Quaternion::SlerpUnclamped(const Quaternion& a,
|
|
const Quaternion& b,
|
|
float t) {
|
|
// if either input is zero, return the other.
|
|
if (Quaternion::GetLengthSquared(a) == 0.0f) {
|
|
if (Quaternion::GetLengthSquared(b) == 0.0f) {
|
|
return Quaternion();
|
|
}
|
|
return b;
|
|
} else if (Quaternion::GetLengthSquared(b) == 0.0f) {
|
|
return a;
|
|
}
|
|
|
|
const Vector3 axyz = a.xyz();
|
|
const Vector3 bxyz = b.xyz();
|
|
float cosHalfAngle = a.w * b.w + Vector3::Dot(axyz, bxyz);
|
|
|
|
Quaternion b2 = b;
|
|
if (cosHalfAngle >= 1.0f || cosHalfAngle <= -1.0f) {
|
|
// angle = 0.0f, so just return one input.
|
|
return a;
|
|
} else if (cosHalfAngle < 0.0f) {
|
|
b2.x = -b.x;
|
|
b2.y = -b.y;
|
|
b2.z = -b.z;
|
|
b2.w = -b.w;
|
|
cosHalfAngle = -cosHalfAngle;
|
|
}
|
|
|
|
float blendA;
|
|
float blendB;
|
|
if (cosHalfAngle < 0.99f) {
|
|
// do proper slerp for big angles
|
|
float halfAngle = acosf(cosHalfAngle);
|
|
float sinHalfAngle = sinf(halfAngle);
|
|
float oneOverSinHalfAngle = 1.0F / sinHalfAngle;
|
|
blendA = sinf(halfAngle * (1.0F - t)) * oneOverSinHalfAngle;
|
|
blendB = sinf(halfAngle * t) * oneOverSinHalfAngle;
|
|
} else {
|
|
// do lerp if angle is really small.
|
|
blendA = 1.0f - t;
|
|
blendB = t;
|
|
}
|
|
Vector3 v = axyz * blendA + b2.xyz() * blendB;
|
|
Quaternion result =
|
|
Quaternion(v.Right(), v.Up(), v.Forward(), blendA * a.w + blendB * b2.w);
|
|
if (result.GetLengthSquared() > 0.0f)
|
|
return Quaternion::Normalize(result);
|
|
else
|
|
return Quaternion();
|
|
}
|
|
|
|
Quaternion Quaternion::Slerp(const Quaternion& a,
|
|
const Quaternion& b,
|
|
float t) {
|
|
if (t > 1)
|
|
t = 1;
|
|
if (t < 0)
|
|
t = 0;
|
|
return Quaternion::SlerpUnclamped(a, b, t);
|
|
}
|
|
|
|
Quaternion Quaternion::Euler(float x, float y, float z) {
|
|
return Quaternion::Euler(Vector3(x, y, z));
|
|
}
|
|
Quaternion Quaternion::Euler(Vector3 euler) {
|
|
return Quaternion::FromEulerRad(euler * Deg2Rad);
|
|
}
|
|
|
|
Quaternion Quaternion::FromEulerRad(Vector3 euler) {
|
|
float yaw = euler.Right();
|
|
float pitch = euler.Up();
|
|
float roll = euler.Forward();
|
|
float rollOver2 = roll * 0.5f;
|
|
float sinRollOver2 = (float)sin((float)rollOver2);
|
|
float cosRollOver2 = (float)cos((float)rollOver2);
|
|
float pitchOver2 = pitch * 0.5f;
|
|
float sinPitchOver2 = (float)sin((float)pitchOver2);
|
|
float cosPitchOver2 = (float)cos((float)pitchOver2);
|
|
float yawOver2 = yaw * 0.5f;
|
|
float sinYawOver2 = (float)sin((float)yawOver2);
|
|
float cosYawOver2 = (float)cos((float)yawOver2);
|
|
Quaternion result;
|
|
result.w = cosYawOver2 * cosPitchOver2 * cosRollOver2 +
|
|
sinYawOver2 * sinPitchOver2 * sinRollOver2;
|
|
result.x = sinYawOver2 * cosPitchOver2 * cosRollOver2 +
|
|
cosYawOver2 * sinPitchOver2 * sinRollOver2;
|
|
result.y = cosYawOver2 * sinPitchOver2 * cosRollOver2 -
|
|
sinYawOver2 * cosPitchOver2 * sinRollOver2;
|
|
result.z = cosYawOver2 * cosPitchOver2 * sinRollOver2 -
|
|
sinYawOver2 * sinPitchOver2 * cosRollOver2;
|
|
return result;
|
|
}
|
|
|
|
Quaternion Quaternion::EulerXYZ(float x, float y, float z) {
|
|
return Quaternion::EulerXYZ(Vector3(x, y, z));
|
|
}
|
|
Quaternion Quaternion::EulerXYZ(Vector3 euler) {
|
|
return Quaternion::FromEulerRadXYZ(euler * Deg2Rad);
|
|
}
|
|
Quaternion Quaternion::FromEulerRadXYZ(Vector3 euler) {
|
|
float yaw = euler.Right(); // x;
|
|
float pitch = euler.Up(); // y;
|
|
float roll = euler.Forward(); // z;
|
|
float rollOver2 = roll * 0.5f;
|
|
float sinRollOver2 = (float)sin((float)rollOver2);
|
|
float cosRollOver2 = (float)cos((float)rollOver2);
|
|
float pitchOver2 = pitch * 0.5f;
|
|
float sinPitchOver2 = (float)sin((float)pitchOver2);
|
|
float cosPitchOver2 = (float)cos((float)pitchOver2);
|
|
float yawOver2 = yaw * 0.5f;
|
|
float sinYawOver2 = (float)sin((float)yawOver2);
|
|
float cosYawOver2 = (float)cos((float)yawOver2);
|
|
Quaternion result;
|
|
result.w = cosYawOver2 * cosPitchOver2 * cosRollOver2 +
|
|
sinYawOver2 * sinPitchOver2 * sinRollOver2;
|
|
result.x = sinYawOver2 * cosPitchOver2 * cosRollOver2 -
|
|
cosYawOver2 * sinPitchOver2 * sinRollOver2;
|
|
result.y = cosYawOver2 * sinPitchOver2 * cosRollOver2 +
|
|
sinYawOver2 * cosPitchOver2 * sinRollOver2;
|
|
result.z = cosYawOver2 * cosPitchOver2 * sinRollOver2 -
|
|
sinYawOver2 * sinPitchOver2 * cosRollOver2;
|
|
return result;
|
|
}
|
|
|
|
float Quaternion::GetAngleAround(Vector3 axis, Quaternion rotation) {
|
|
Quaternion secondaryRotation = GetRotationAround(axis, rotation);
|
|
float rotationAngle;
|
|
Vector3 rotationAxis;
|
|
secondaryRotation.ToAngleAxis(&rotationAngle, &rotationAxis);
|
|
|
|
// Do the axis point in opposite directions?
|
|
if (Vector3::Dot(axis, rotationAxis) < 0)
|
|
rotationAngle = -rotationAngle;
|
|
|
|
return rotationAngle;
|
|
}
|
|
|
|
Quaternion Quaternion::GetRotationAround(Vector3 axis, Quaternion rotation) {
|
|
Vector3 ra = Vector3(rotation.x, rotation.y, rotation.z); // rotation axis
|
|
Vector3 p = Vector3::Project(
|
|
ra, axis); // return projection ra on to axis (parallel component)
|
|
Quaternion twist = Quaternion(p.Right(), p.Up(), p.Forward(), rotation.w);
|
|
twist = Quaternion::Normalize(twist);
|
|
return twist;
|
|
}
|
|
|
|
void Quaternion::GetSwingTwist(Vector3 axis,
|
|
Quaternion rotation,
|
|
Quaternion* swing,
|
|
Quaternion* twist) {
|
|
*twist = GetRotationAround(axis, rotation);
|
|
*swing = rotation * Quaternion::Inverse(*twist);
|
|
}
|