[util] Calculate quaternion to rotate between two vectors

The calculation fails (produces NaN) if the vectors are anti-parallel,
but works for all other combinations. I came up with this implementation
when I discovered Unity's Quaternion.FromToRotation could did not work
with very small angles. This implementation will produce a usable
quaternion below 0.00255 degrees (though it will be slightly larger than
unit). Unity's failed such that I could see KSP's skybox snap while it
rotated around my test vessel.

include/QF/math/quaternion.h

@@ -164,6 +164,7 @@ extern const vec_t *const quat_origin;
 
 void QuatMult (const quat_t q1, const quat_t q2, quat_t out);
 void QuatMultVec (const quat_t q, const vec3_t v, vec3_t out);
+void QuatRotation (const vec3_t a, const vec3_t b, quat_t out);
 void QuatInverse (const quat_t in, quat_t out);
 void QuatExp (const quat_t a, quat_t b);
 void QuatToMatrix (const quat_t q, vec_t *m, int homogenous, int vertical);

libs/util/mathlib.c

@@ -272,6 +272,24 @@ QuatMultVec (const quat_t q, const vec3_t v, vec3_t out)
 	VectorMultAdd (tv, s * s - dqq, v, out);
 }
 
+VISIBLE void
+QuatRotation(const vec3_t a, const vec3_t b, quat_t out)
+{
+	vec_t       ma, mb;
+	vec_t       den, mba_mab;
+	vec3_t      t;
+
+	ma = VectorLength(a);
+	mb = VectorLength(b);
+	den = 2 * ma * mb;
+	VectorScale (a, mb, t);
+	VectorMultAdd(t, ma, b, t);
+	mba_mab = VectorLength(t);
+	CrossProduct (a, b, t);
+	VectorScale(t, 1 / mba_mab, out);
+	out[3] = mba_mab / den;
+}
+
 VISIBLE void
 QuatInverse (const quat_t in, quat_t out)
 {

libs/util/test/test-quat.c

@@ -29,6 +29,45 @@ static vec3_t test_angles[] = {
 };
 #define num_angle_tests (sizeof (test_angles) / sizeof (test_angles[0]))
 
+static struct {
+	vec3_t      a;
+	vec3_t      b;
+	quat_t      expect;
+} quat_vector_rotation_tests[] = {
+	{{1, 0, 0}, {1, 0, 0}, {0, 0, 0, 1}},
+	{{0, 1, 0}, {0, 1, 0}, {0, 0, 0, 1}},
+	{{0, 0, 1}, {0, 0, 1}, {0, 0, 0, 1}},
+	{{1, 0, 0}, {8, 0, 0}, {0, 0, 0, 1}},
+	{{0, 8, 0}, {0, 1, 0}, {0, 0, 0, 1}},
+	{{0, 0, 8}, {0, 0, 8}, {0, 0, 0, 1}},
+	{{1, 0, 0}, {-1, 0, 0}, {0, 0, 0, 0}}, // x, y, z = NaN
+	{{0, -1, 0}, {0, 1, 0}, {0, 0, 0, 0}}, // x, y, z = NaN
+	{{0, 0, 1}, {0, 0, -1}, {0, 0, 0, 0}}, // x, y, z = NaN
+	{{-1, 0, 0}, {8, 0, 0}, {0, 0, 0, 0}}, // x, y, z = NaN
+	{{0, 8, 0}, {0, -1, 0}, {0, 0, 0, 0}}, // x, y, z = NaN
+	{{0, 0, -8}, {0, 0, 8}, {0, 0, 0, 0}}, // x, y, z = NaN
+	// excessive for float, but if vec_t becomes double...
+	// 1/50 second orbiting JNSQ Kerbin at 120km altitude. While this has
+	// nothing to do with quakeforge (yet?), it came up when testing camera
+	// rotation in KSP and Unity failed miserably. I don't remember the exact
+	// details, but I could see significant snapping in the rotation (I thought
+	// it was a few times per second, but these numbers indicate it must have
+	// been every few seconds).
+	// The quaternion is unit to 9 sigfigs (a little larger)
+	{{1720000, 0, 0}, {1719999.9983028059, 76.409082595828366, 0}, {0, 0, 2.22119434e-5, 1}},
+	// 1/20 second, same situation
+	{{1720000, 0, 0}, {1719999.9893925365, 191.02270615971355, 0}, {0, 0, 5.55298575e-5, 1}},
+	// 1/5 second, same situation
+	{{1720000, 0, 0}, {1719999.8302805868, 764.09080107761736, 0}, {0, 0, 2.2211943e-4, 1}},
+	// 1/4 second, same situation
+	{{1720000, 0, 0}, {1719999.7348134194, 955.11348367609469, 0}, {0, 0, 2.77649262e-4, 1}},
+	// 1/3 second, same situation. This is (float) 1 ulp in w: about 0.0424
+	// degrees.
+	// The quaternion is unit to 9 sigfigs (a little larger)
+	{{1720000, 0, 0}, {1719999.5285571995, 1273.4845939975546, 0}, {0, 0, 3.70199094e-4, 0.99999994}},
+};
+#define num_quat_vector_rotation_tests (sizeof (quat_vector_rotation_tests) / sizeof (quat_vector_rotation_tests[0]))
+
 // return true if a and b are close enough (yay, floats)
 static int
 compare (vec_t a, vec_t b)
@@ -200,6 +239,60 @@ fail:
 	return 0;
 }
 
+static int
+test_rotation4 (const vec3_t a, const vec3_t b, const quat_t expect)
+{
+	int         i;
+	quat_t      quat;
+	vec3_t      t;
+	vec_t       d = 0;
+
+	VectorZero (t);
+
+	// find the rotation vector between a and b
+	QuatRotation (a, b, quat);
+
+	if (quat[3] == 0) {
+		if (expect[3] != 0) {
+			goto fail;
+		}
+		// expect NaN for the vector components because the vectors are
+		// anti-parallel and thus the rotation axis is undefined
+		if (!(isnan(quat[0]) && isnan(quat[1]) && isnan(quat[2]))) {
+			goto fail;
+		}
+	} else {
+		// the vectors are not anti-parallel and thus the rotation axis is
+		// defined, so NaN is invalid
+		if (isnan(quat[0]) || isnan(quat[1]) || isnan(quat[2])) {
+			goto fail;
+		}
+		for (i = 0; i < 4; i++) {
+			// yes, float precision will make it difficult to set up expect
+			// but it is at least consistent (ie, the "errors" are not at all
+			// random and thus will be the same from run to run)
+			if (quat[i] != expect[i]) {
+				goto fail;
+			}
+		}
+		QuatMultVec(quat, a, t);
+
+		d = DotProduct (t, b) / (VectorLength (t) * VectorLength (b)) - 1;
+		if (d * d > 1e-8) {
+			goto fail;
+		}
+	}
+	return 1;
+fail:
+	printf ("\ntest_rotation4\n");
+	printf ("%11.9g %11.9g %11.9g\n", VectorExpand(a));
+	printf ("%11.9g %11.9g %11.9g\n", VectorExpand(b));
+	printf ("%11.9g %11.9g %11.9g %11.9g\n", QuatExpand(quat));
+	printf ("%11.9g %11.9g %11.9g\n", VectorExpand(t));
+	printf ("%11.9g\n", d);
+	return 0;
+}
+
 #define s05 0.70710678118654757
 
 static struct {
@@ -285,6 +378,15 @@ main (int argc, const char **argv)
 			res = 1;
 	}
 
+	for (i = 0; i < num_quat_vector_rotation_tests; i++) {
+		vec_t      *a = quat_vector_rotation_tests[i].a;
+		vec_t      *b = quat_vector_rotation_tests[i].b;
+		vec_t      *expect = quat_vector_rotation_tests[i].expect;
+		if (!test_rotation4 (a, b, expect)) {
+			res = 1;
+		}
+	}
+
 	for (i = 0; i < num_quat_mat_tests; i ++) {
 		vec_t      *q = quat_mat_tests[i].q;
 		vec_t      *expect = quat_mat_tests[i].expect;