Fixed Vector3::Angle and Vector3::SignedAngle

http://gitlab.passervr.com/pascal/vectoralgebra/-/issues/1

Quaternion.cpp

@@ -152,17 +152,15 @@ 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, const Vector3 &upwards) {
-//	return LookRotation(forward, upwards);
-//}
-
-
-// This is not correct when forward == Vector3::up
+/// <summary>
+/// Creates a quaternion with the given forward direction
+/// </summary>
+/// <param name="forward">The diraction to look in</param>
+/// <returns>The rotation for this direction</returns>
+/// For the rotation, Vector::up is used for the up direction.
+/// Note: if the forward direction == Vector3::up, the result is Quaternion::identity
 Quaternion Quaternion::LookRotation(const Vector3& forward) {
 	Vector3 up = Vector3(0, 1, 0);
-	if ((Vector3)forward == up)
-		up = Vector3(0, 0, 1);
 	return LookRotation(forward, up);
 }
 Quaternion Quaternion::LookRotation(const Vector3& forward, const Vector3& up) {
@@ -219,7 +217,13 @@ Quaternion Quaternion::LookRotation(const Vector3& forward, const Vector3& up) {
 }
 
 Quaternion Quaternion::FromToRotation(Vector3 fromDirection, Vector3 toDirection) {
-	return RotateTowards(LookRotation(fromDirection), LookRotation(toDirection), FLT_MAX);
+	Vector3 axis = Vector3::Cross(fromDirection, toDirection);
+	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) {
@@ -232,9 +236,6 @@ Quaternion Quaternion::RotateTowards(const Quaternion& from, const Quaternion& t
 }
 
 Quaternion Quaternion::AngleAxis(float angle, const Vector3& axis) {
-	//	return AngleAxis(angle, axis);
-	//}
-	//Quaternion AngleAxis(float angle, Vector3& axis) {
 	if (Vector3::SqrMagnitude(axis) == 0.0)
 		return Quaternion();
 

Vector3.cpp

@@ -156,7 +156,7 @@ Vector3 Vector3::ProjectOnPlane(Vector3 vector, Vector3 planeNormal) {
 	return vector - Project(vector, planeNormal);
 }
 
-float clamp(float x, float upper, float lower) {
+float clamp(float x, float lower, float upper) {
 	return fminf(upper, fmaxf(x, lower));
 }
 
@@ -167,21 +167,25 @@ float Vector3::Angle(Vector3 from, Vector3 to) {
 	if (denominator < 1E-05f)
 		return 0;
 
-	float dot = clamp(Vector3::Dot(from, to) / denominator, -1.0, 1.0);
-	return ((float)acos(dot)) * Rad2Deg;
+	float dot = Vector3::Dot(from, to);
+	float fraction = dot / denominator;
+	float cdot = clamp(fraction, -1.0, 1.0);
+	return ((float)acos(cdot)) * Rad2Deg;
 }
 
 float Vector3::SignedAngle(Vector3 from, Vector3 to, Vector3 axis) {
 	// angle in [0,180]
 	float angle = Vector3::Angle(from, to);
-	float signd = signbit(Vector3::Dot(axis, Vector3::Cross(from, to)));
 
+	float cross_x = from.y * to.z - from.z * to.y;
+	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);
+	 
 	// angle in [-179,180]
 	float signed_angle = angle * signd;
 
-	// angle in [0,360] (not used but included here for completeness)
-	//float angle360 =  (signed_angle + 180) % 360;
-
 	return signed_angle;
 }
 
@@ -197,9 +201,3 @@ void Vec3_Constructor(Vec3* v, float _x, float _y, float _z) {
 	v->y = _y;
 	v->z = _z;
 }
-
-//Vec3 new_Vec3(float _x, float _y, float _z) {
-//	Vec3 v = Vec3();
-//	v.x = _x;
-//	return v;
-//}

Vector3.h

@@ -63,9 +63,6 @@ public:
 };
 
 void Vec3_Constructor(Vec3* v, float _x, float _y, float _z);
-Vec3 new_Vec3(float _x, float _y, float _z);
 
 
-//#include "Quaternion.h"
-
 #endif