diff --git a/Quaternion.cpp b/Quaternion.cpp index e621bb0..0389ab2 100644 --- a/Quaternion.cpp +++ b/Quaternion.cpp @@ -3,11 +3,11 @@ // file, You can obtain one at https ://mozilla.org/MPL/2.0/. #include "Quaternion.h" +#include "Vector3.h" #include #include -#include "Vector3.h" -void CopyQuat(const Quat& q1, Quat& q2) { +void CopyQuat(const Quat &q1, Quat &q2) { q2.x = q1.x; q2.y = q1.y; q2.z = q1.z; @@ -42,9 +42,7 @@ Quaternion::~Quaternion() {} const Quaternion Quaternion::identity = Quaternion(0, 0, 0, 1); -Vector3 Quaternion::xyz() const { - return Vector3(x, y, z); -} +Vector3 Quaternion::xyz() const { return Vector3(x, y, z); } float Quaternion::GetLength() const { return sqrtf(x * x + y * y + z * z + w * w); @@ -53,7 +51,7 @@ float Quaternion::GetLength() const { float Quaternion::GetLengthSquared() const { return x * x + y * y + z * z + w * w; } -float Quaternion::GetLengthSquared(const Quaternion& q) { +float Quaternion::GetLengthSquared(const Quaternion &q) { return q.x * q.x + q.y * q.y + q.z * q.z + q.w * q.w; } @@ -65,7 +63,7 @@ void Quaternion::Normalize() { w /= length; } -Quaternion Quaternion::Normalize(const Quaternion& q) { +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); @@ -76,11 +74,11 @@ 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) { +Vector3 Quaternion::ToAngles(const Quaternion &q1) { float test = q1.x * q1.y + q1.z * q1.w; - if (test > 0.499f) { // singularity at north pole + 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 + } 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; @@ -96,7 +94,7 @@ Vector3 Quaternion::ToAngles(const Quaternion& q1) { } } -Quaternion Quaternion::operator*(const Quaternion& r2) const { +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, @@ -104,7 +102,7 @@ Quaternion Quaternion::operator*(const Quaternion& r2) const { -this->x * r2.x - this->y * r2.y - this->z * r2.z + this->w * r2.w); }; -Vector3 Quaternion::operator*(const Vector3& p) const { +Vector3 Quaternion::operator*(const Vector3 &p) const { float num = this->x * 2; float num2 = this->y * 2; float num3 = this->z * 2; @@ -127,7 +125,7 @@ Vector3 Quaternion::operator*(const Vector3& p) const { return result; } -bool Quaternion::operator==(const Quaternion& q) { +bool Quaternion::operator==(const Quaternion &q) { return (this->x == q.x && this->y == q.y && this->z == q.z && this->w == q.w); } @@ -136,11 +134,11 @@ Quaternion Quaternion::Inverse(Quaternion r) { return Quaternion(-r.x / n, -r.y / n, -r.z / n, r.w / n); } -Quaternion Quaternion::LookRotation(const Vector3& forward) { +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) { +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); @@ -201,8 +199,8 @@ Quaternion Quaternion::FromToRotation(Vector3 fromDirection, return rotation; } -Quaternion Quaternion::RotateTowards(const Quaternion& from, - const Quaternion& to, +Quaternion Quaternion::RotateTowards(const Quaternion &from, + const Quaternion &to, float maxDegreesDelta) { float num = Quaternion::Angle(from, to); if (num == 0) { @@ -212,7 +210,7 @@ Quaternion Quaternion::RotateTowards(const Quaternion& from, return SlerpUnclamped(from, to, t); } -Quaternion Quaternion::AngleAxis(float angle, const Vector3& axis) { +Quaternion Quaternion::AngleAxis(float angle, const Vector3 &axis) { if (Vector3::SqrMagnitude(axis) == 0.0f) return Quaternion(); @@ -234,27 +232,25 @@ float Quaternion::Angle(Quaternion a, Quaternion b) { return (float)acos(fmin(fabs(f), 1)) * 2 * Rad2Deg; } -void Quaternion::ToAngleAxis(float* angle, Vector3* axis) { +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) { +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 + *angle = 2.0f * acosf(q1.w); // angle float den = sqrtf(1.0F - q1.w * q1.w); if (den > 0.0001f) { - *axis = q1.xyz() / den; + *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, +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) { @@ -304,8 +300,7 @@ Quaternion Quaternion::SlerpUnclamped(const Quaternion& a, return Quaternion(); } -Quaternion Quaternion::Slerp(const Quaternion& a, - const Quaternion& b, +Quaternion Quaternion::Slerp(const Quaternion &a, const Quaternion &b, float t) { if (t > 1) t = 1; @@ -391,18 +386,16 @@ float Quaternion::GetAngleAround(Vector3 axis, Quaternion rotation) { } Quaternion Quaternion::GetRotationAround(Vector3 axis, Quaternion rotation) { - Vector3 ra = Vector3(rotation.x, rotation.y, rotation.z); // rotation axis + 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) + ra, axis); // return projection ra on to axis (parallel component) Quaternion twist = Quaternion(p.x, p.y, p.z, rotation.w); twist = Quaternion::Normalize(twist); return twist; } -void Quaternion::GetSwingTwist(Vector3 axis, - Quaternion rotation, - Quaternion* swing, - Quaternion* twist) { +void Quaternion::GetSwingTwist(Vector3 axis, Quaternion rotation, + Quaternion *swing, Quaternion *twist) { *twist = GetRotationAround(axis, rotation); *swing = rotation * Quaternion::Inverse(*twist); }