diff --git a/Quaternion.cpp b/Quaternion.cpp index 8f76a71..8d22db8 100644 --- a/Quaternion.cpp +++ b/Quaternion.cpp @@ -3,11 +3,12 @@ // file, You can obtain one at https ://mozilla.org/MPL/2.0/. #include "Quaternion.h" -#include "Vector3.h" #include #include +#include "Angle.h" +#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,7 +43,9 @@ 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); @@ -51,7 +54,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; } @@ -63,7 +66,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); @@ -74,11 +77,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; @@ -94,7 +97,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, @@ -102,7 +105,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; @@ -133,7 +136,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); } @@ -142,23 +145,23 @@ 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); - 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 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(); @@ -202,13 +205,13 @@ Quaternion Quaternion::FromToRotation(Vector3 fromDirection, Vector3 toDirection) { Vector3 axis = Vector3::Cross(fromDirection, toDirection); axis = Vector3::Normalize(axis); - float angle = Vector3::SignedAngle(fromDirection, toDirection, axis); - Quaternion rotation = Quaternion::AngleAxis(angle, axis); + AngleOf angle = Vector3::SignedAngle(fromDirection, toDirection, axis); + Quaternion rotation = Quaternion::AngleAxis(angle.ToFloat(), axis); 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) { @@ -218,7 +221,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(); @@ -227,9 +230,9 @@ Quaternion Quaternion::AngleAxis(float angle, const Vector3 &axis) { 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.x = axis2.Right(); // x; + result.y = axis2.Up(); // y; + result.z = axis2.Forward(); // z; result.w = (float)cos(radians); return Quaternion::Normalize(result); @@ -240,15 +243,16 @@ 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 = Vector3::Normalize(q1.xyz() / den); @@ -258,7 +262,8 @@ void Quaternion::ToAxisAngleRad(const Quaternion &q, Vector3 *const 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) { @@ -309,7 +314,8 @@ Quaternion Quaternion::SlerpUnclamped(const Quaternion &a, const Quaternion &b, 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; @@ -357,9 +363,9 @@ 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 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); @@ -395,16 +401,18 @@ 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.Right(), p.Up(), p.Forward(), 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); } diff --git a/Vector3.cpp b/Vector3.cpp index b8f20ff..36c2b3b 100644 --- a/Vector3.cpp +++ b/Vector3.cpp @@ -188,7 +188,7 @@ float clamp(float x, float lower, float upper) { return upperClamp; } -float Vector3::Angle(const Vector3& v1, const Vector3& v2) { +AngleOf Vector3::Angle(const Vector3& v1, const Vector3& v2) { float denominator = sqrtf(v1.sqrMagnitude() * v2.sqrMagnitude()); if (denominator < epsilon) return 0; @@ -200,23 +200,23 @@ float Vector3::Angle(const Vector3& v1, const Vector3& v2) { float cdot = clamp(fraction, -1.0, 1.0); float r = ((float)acos(cdot)) * Rad2Deg; - return r; + return AngleOf(r); } -float Vector3::SignedAngle(const Vector3& v1, - const Vector3& v2, - const Vector3& axis) { +AngleOf Vector3::SignedAngle(const Vector3& v1, + const Vector3& v2, + const Vector3& axis) { // angle in [0,180] - float angle = Vector3::Angle(v1, v2); + AngleOf angle = Vector3::Angle(v1, v2); Vector3 cross = Vector3::Cross(v1, v2); float b = Vector3::Dot(axis, cross); float signd = b < 0 ? -1.0F : (b > 0 ? 1.0F : 0.0F); // angle in [-179,180] - float signed_angle = angle * signd; + AngleOf signed_angle = angle * signd; - return signed_angle; + return AngleOf(signed_angle); } Vector3 Vector3::Lerp(const Vector3& v1, const Vector3& v2, float f) { diff --git a/Vector3.h b/Vector3.h index f17eae8..4f8aa1d 100644 --- a/Vector3.h +++ b/Vector3.h @@ -204,15 +204,15 @@ struct Vector3 : Vec3 { /// @remark This reterns an unsigned angle which is the shortest distance /// between the two vectors. Use Vector3::SignedAngle if a signed angle is /// needed. - static float Angle(const Vector3& v1, const Vector3& v2); + static AngleOf Angle(const Vector3& v1, const Vector3& v2); /// @brief The signed angle between two vectors /// @param v1 The starting vector /// @param v2 The ending vector /// @param axis The axis to rotate around /// @return The signed angle between the two vectors - static float SignedAngle(const Vector3& v1, - const Vector3& v2, - const Vector3& axis); + static AngleOf SignedAngle(const Vector3& v1, + const Vector3& v2, + const Vector3& axis); /// @brief Lerp (linear interpolation) between two vectors /// @param v1 The starting vector diff --git a/test/Vector3_test.cc b/test/Vector3_test.cc index 4e5003c..c7ba095 100644 --- a/test/Vector3_test.cc +++ b/test/Vector3_test.cc @@ -1,7 +1,7 @@ #if GTEST #include -#include #include +#include #include "Vector3.h" @@ -488,29 +488,29 @@ TEST(Vector3, ProjectOnPlane) { TEST(Vector3, Angle) { Vector3 v1 = Vector3(4, 5, 6); Vector3 v2 = Vector3(1, 2, 3); - float f = 0; + AngleOf f = 0; bool r = false; f = Vector3::Angle(v1, v2); - EXPECT_FLOAT_EQ(f, 12.9331388F) << "Angle(4 5 6, 1 2 3)"; + EXPECT_FLOAT_EQ(f.ToFloat(), 12.9331388F) << "Angle(4 5 6, 1 2 3)"; v2 = Vector3(-1, -2, -3); f = Vector3::Angle(v1, v2); - EXPECT_FLOAT_EQ(f, 167.066864F) << "Angle(4 5 6, -1 -2 -3)"; + EXPECT_FLOAT_EQ(f.ToFloat(), 167.066864F) << "Angle(4 5 6, -1 -2 -3)"; v2 = Vector3(0, 0, 0); f = Vector3::Angle(v1, v2); - EXPECT_FLOAT_EQ(f, 0) << "Angle(4 5 6, 0 0 0)"; + EXPECT_FLOAT_EQ(f.ToFloat(), 0) << "Angle(4 5 6, 0 0 0)"; if (std::numeric_limits::is_iec559) { v2 = Vector3(FLOAT_INFINITY, FLOAT_INFINITY, FLOAT_INFINITY); f = Vector3::Angle(v1, v2); - r = isnan(f); + r = isnan(f.ToFloat()); EXPECT_TRUE(r) << "Angle(4 5 6, INFINITY INFINITY INFINITY)"; v2 = Vector3(-FLOAT_INFINITY, -FLOAT_INFINITY, -FLOAT_INFINITY); f = Vector3::Angle(v1, v2); - r = isnan(f); + r = isnan(f.ToFloat()); EXPECT_TRUE(r) << "Angle(4 5 6, -INFINITY -INFINITY -INFINITY)"; } } @@ -519,39 +519,42 @@ TEST(Vector3, SignedAngle) { Vector3 v1 = Vector3(4, 5, 6); Vector3 v2 = Vector3(1, 2, 3); Vector3 v3 = Vector3(7, 8, -9); - float f = 0; + AngleOf f = 0; bool r = false; f = Vector3::SignedAngle(v1, v2, v3); - EXPECT_FLOAT_EQ(f, -12.9331388F) << "SignedAngle(4 5 6, 1 2 3, 7 8 -9)"; + EXPECT_FLOAT_EQ(f.ToFloat(), -12.9331388F) + << "SignedAngle(4 5 6, 1 2 3, 7 8 -9)"; v2 = Vector3(-1, -2, -3); f = Vector3::SignedAngle(v1, v2, v3); - EXPECT_FLOAT_EQ(f, 167.066864F) << "SignedAngle(4 5 6, -1 -2 -3, 7 8 -9)"; + EXPECT_FLOAT_EQ(f.ToFloat(), 167.066864F) + << "SignedAngle(4 5 6, -1 -2 -3, 7 8 -9)"; v2 = Vector3(0, 0, 0); f = Vector3::SignedAngle(v1, v2, v3); - EXPECT_FLOAT_EQ(f, 0) << "SignedAngle(4 5 6, 0 0 0, 7 8 -9 )"; + EXPECT_FLOAT_EQ(f.ToFloat(), 0) << "SignedAngle(4 5 6, 0 0 0, 7 8 -9 )"; v2 = Vector3(1, 2, 3); v3 = Vector3(-7, -8, 9); f = Vector3::SignedAngle(v1, v2, v3); - EXPECT_FLOAT_EQ(f, 12.9331388F) << "SignedAngle(4 5 6, 1 2 3, -7 -8 9)"; + EXPECT_FLOAT_EQ(f.ToFloat(), 12.9331388F) + << "SignedAngle(4 5 6, 1 2 3, -7 -8 9)"; v3 = Vector3(0, 0, 0); f = Vector3::SignedAngle(v1, v2, v3); - EXPECT_FLOAT_EQ(f, 0) << "SignedAngle(4 5 6, 1 2 3, 0 0 0)"; + EXPECT_FLOAT_EQ(f.ToFloat(), 0) << "SignedAngle(4 5 6, 1 2 3, 0 0 0)"; if (std::numeric_limits::is_iec559) { v2 = Vector3(FLOAT_INFINITY, FLOAT_INFINITY, FLOAT_INFINITY); f = Vector3::SignedAngle(v1, v2, v3); - r = isnan(f); + r = isnan(f.ToFloat()); EXPECT_TRUE(r) << "SignedAngle(4 5 6, INFINITY INFINITY INFINITY)"; v2 = Vector3(-FLOAT_INFINITY, -FLOAT_INFINITY, -FLOAT_INFINITY); f = Vector3::SignedAngle(v1, v2, v3); - r = isnan(f); + r = isnan(f.ToFloat()); EXPECT_TRUE(r) << "SignedAngle(4 5 6, -INFINITY -INFINITY -INFINITY)"; } }