Added Quaternion::GetSwingTwist,

Code cleanup

Quaternion.cpp

@@ -1,4 +1,3 @@
-//#include "stdafx.h"
 #include "pch.h"
 #include <math.h>
 #include <float.h>
@@ -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);
+}
+

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;
 };

Vector3.cpp

@@ -1,4 +1,3 @@
-//#include "stdafx.h"
 #include "pch.h"
 #include <math.h>
 #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;