diff --git a/Quaternion.cpp b/Quaternion.cpp index 9715911..c23016a 100644 --- a/Quaternion.cpp +++ b/Quaternion.cpp @@ -1,4 +1,3 @@ -//#include "stdafx.h" #include "pch.h" #include #include @@ -218,12 +217,10 @@ Quaternion Quaternion::LookRotation(const Vector3& forward, const Vector3& up) { 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); return rotation; - - // This does not work if from or to direction is Vector3::up - //return RotateTowards(LookRotation(fromDirection), LookRotation(toDirection), FLT_MAX); } Quaternion Quaternion::RotateTowards(const Quaternion& from, const Quaternion& to, float maxDegreesDelta) { @@ -242,7 +239,7 @@ Quaternion Quaternion::AngleAxis(float angle, const Vector3& axis) { Quaternion result = Quaternion(); float radians = angle * Deg2Rad; radians *= 0.5; - //Vector3::Normalize(axis); + Vector3 axis2 = axis * (float)sin(radians); result.x = axis2.x; result.y = axis2.y; @@ -330,17 +327,12 @@ Quaternion Quaternion::SlerpUnclamped(const Quaternion& a, const Quaternion& b, else return Quaternion(); } -//Quaternion Quaternion::SlerpUnclamped(const Quaternion& a, const Quaternion& b, float t) { -// return SlerpUnclamped(a, b, t); -//} + 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::Slerp(const Quaternion& a, const Quaternion& b, float t) { -// return Slerp(a, b, t); -//} Quaternion Quaternion::Euler(float x, float y, float z) { return Quaternion::Euler(Vector3(x, y, z)); @@ -390,3 +382,9 @@ Quaternion Quaternion::GetRotationAround(Vector3 axis, Quaternion rotation) { 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); +} + diff --git a/Quaternion.h b/Quaternion.h index dba74bd..a626f0e 100644 --- a/Quaternion.h +++ b/Quaternion.h @@ -61,6 +61,7 @@ public: static Quaternion FromEulerRad(Vector3 euler); static float GetAngleAround(Vector3 axis, Quaternion rotation); static Quaternion GetRotationAround(Vector3 axis, Quaternion rotation); + static void GetSwingTwist(Vector3 axis, Quaternion rotation, Quaternion* swing, Quaternion* twist); public: Vector3 xyz() const; }; diff --git a/Vector3.cpp b/Vector3.cpp index d044d58..fbe6062 100644 --- a/Vector3.cpp +++ b/Vector3.cpp @@ -1,4 +1,3 @@ -//#include "stdafx.h" #include "pch.h" #include #include "Vector3.h" @@ -6,6 +5,7 @@ const float Deg2Rad = 0.0174532924F; const float Rad2Deg = 57.29578F; +const float epsilon = 1E-05f; Vector3::Vector3() { x = 0; @@ -54,7 +54,7 @@ float Vector3::sqrMagnitude() const { Vector3 Vector3::Normalize(Vector3 v) { float num = Vector3::Magnitude(v); Vector3 result = Vector3::zero; - if (num > 1E-05f) { + if (num > epsilon) { result = v / num; } return result; @@ -63,7 +63,7 @@ Vector3 Vector3::Normalize(Vector3 v) { Vector3 Vector3::normalized() const { float num = this->magnitude(); Vector3 result = Vector3::zero; - if (num > 1E-05f) { + if (num > epsilon) { result = ((Vector3)*this) / num; } return result; @@ -72,12 +72,7 @@ Vector3 Vector3::normalized() const { Vector3 Vector3::operator -(const Vector3& v2) const { return Vector3(this->x - v2.x, this->y - v2.y, this->z - v2.z); } -//Vector operator -(const Vector& v1, const Vector& v2) { -// return Vector(v1.x - v2.x, v1.y - v2.y, v1.z - v2.z); -//} -//Vector Vector::operator -(const Vector& v1, const Vector& v2) { -// return Vector(v1.x - v2.x, v1.y - v2.y, v1.z - v2.z); -//} + Vector3 Vector3::operator -() { return Vector3(-this->x, -this->y, -this->z); } @@ -135,33 +130,39 @@ Vector3 Vector3::Cross(const Vector3& v1, const Vector3& v2) { // Projects a vector onto another vector. Vector3 Vector3::Project(Vector3 vector, Vector3 onNormal) { - float sqrMag = Dot(onNormal, onNormal); - if (sqrMag < 1E-05f) + float sqrMagnitude = Dot(onNormal, onNormal); + if (sqrMagnitude < epsilon) return Vector3::zero; - else - return onNormal * Dot(vector, onNormal) / sqrMag; + else { + float dot = Dot(vector, onNormal); + Vector3 r = onNormal * dot / sqrMagnitude; + return r; + } } // Projects a vector onto a plane defined by a normal orthogonal to the plane. Vector3 Vector3::ProjectOnPlane(Vector3 vector, Vector3 planeNormal) { - return vector - Project(vector, planeNormal); + Vector3 r = vector - Project(vector, planeNormal); + return r; } float clamp(float x, float lower, float upper) { - return fminf(upper, fmaxf(x, lower)); + float lowerClamp = fmaxf(x, lower); + float upperClamp = fminf(upper, lowerClamp); + return upperClamp; } // This function is buggy (0-1-0, 0-1-0) gives 180 degrees in return!!! float Vector3::Angle(Vector3 from, Vector3 to) { - // sqrt(a) * sqrt(b) = sqrt(a * b) -- valid for real numbers float denominator = sqrtf(from.sqrMagnitude() * to.sqrMagnitude()); - if (denominator < 1E-05f) + if (denominator < epsilon) return 0; float dot = Vector3::Dot(from, to); float fraction = dot / denominator; float cdot = clamp(fraction, -1.0, 1.0); - return ((float)acos(cdot)) * Rad2Deg; + float r = ((float)acos(cdot))* Rad2Deg; + return r; } float Vector3::SignedAngle(Vector3 from, Vector3 to, Vector3 axis) { @@ -172,7 +173,7 @@ float Vector3::SignedAngle(Vector3 from, Vector3 to, Vector3 axis) { float cross_y = from.z * to.x - from.x * to.z; float cross_z = from.x * to.y - from.y * to.x; float b = axis.x * cross_x + axis.y * cross_y + axis.z * cross_z; - float signd = b < 0 ? -1 : (b > 0 ? 1 : 0); + float signd = b < 0 ? -1.0F : (b > 0 ? 1.0F : 0.0F); // angle in [-179,180] float signed_angle = angle * signd; @@ -180,7 +181,6 @@ float Vector3::SignedAngle(Vector3 from, Vector3 to, Vector3 axis) { return signed_angle; } - void CopyVec3(const Vec3& v1, Vec3& v2) { v2.x = v1.x; v2.y = v1.y;