fix(math): Fix slerpQuaternions results being wrong at certain angles (#961)

Author: jespertheend

src/math/Quat.js

@@ -285,31 +285,37 @@ export class Quat {
 	 * @param {number} t
 	 */
 	static slerpQuaternions(quatA, quatB, t) {
+		quatA = quatA.clone();
+		quatB = quatB.clone();
 		// https://www.euclideanspace.com/maths/algebra/realNormedAlgebra/quaternions/slerp/
-		if (t == 0) {
-			return quatA.clone();
-		}
-		if (t == 1) {
-			return quatB.clone();
-		}
+		if (t == 0) return quatA;
+		if (t == 1) return quatB;
 
 		let cosHalfTheta = new Vec4(quatA).dot(quatB);
 		if (cosHalfTheta < 0) {
 			quatB.x = -quatB.x;
 			quatB.y = -quatB.y;
+			quatB.z = -quatB.z;
 			quatB.w = -quatB.w;
 			cosHalfTheta = -cosHalfTheta;
 		}
 
 		// If quatA = quatB or quatA = -quatB then theta = 0 and we can return quatA
-		if (Math.abs(cosHalfTheta) >= 1) {
+		if (cosHalfTheta >= 1) {
 			return quatA.clone();
 		}
 
-		// Calculate temporary values.
 		const halfTheta = Math.acos(cosHalfTheta);
-		const sinHalfTheta = Math.sqrt(1 - cosHalfTheta * cosHalfTheta);
+		const sqrSinHalfTheta = 1 - cosHalfTheta * cosHalfTheta;
+
+		// When sinHalfTheta is very small, we run into precision errors.
+		// It's best to just linearly interpolate the two quaternions in that case
+		if (sqrSinHalfTheta <= Number.EPSILON) {
+			const vec = Vec4.lerp(quatA, quatB, t);
+			return new Quat(vec);
+		}
 
+		const sinHalfTheta = Math.sqrt(sqrSinHalfTheta);
 		const ratioA = Math.sin((1 - t) * halfTheta) / sinHalfTheta;
 		const ratioB = Math.sin(t * halfTheta) / sinHalfTheta;
 

test/unit/src/math/Quat.test.js

@@ -33,9 +33,9 @@ Deno.test({
  * @param {number} t
  * @param {Quat} expected
  */
-function basicSlerpTest(a, b, t, expected) {
+function basicSlerpTest(a, b, t, expected, tolerance = 0.00001) {
 	const result = Quat.slerpQuaternions(a, b, t);
-	assertQuatAlmostEquals(result, expected);
+	assertQuatAlmostEquals(result, expected, tolerance);
 }
 
 Deno.test({
@@ -64,6 +64,21 @@ Deno.test({
 	},
 });
 
+Deno.test({
+	name: "two quaternions that are very close to each other",
+	fn() {
+		const a = new Quat();
+		const b = Quat.fromAxisAngle(0, 1, 0, 0.00000003);
+		basicSlerpTest(a, b, 0, a, 0);
+		basicSlerpTest(a, b, 0.1, new Quat(0, 1.5e-9, 0, 1), 0);
+		basicSlerpTest(a, b, 0.25, new Quat(0, 3.75e-9, 0, 1), 0);
+		basicSlerpTest(a, b, 0.5, new Quat(0, 7.5e-9, 0, 1), 0);
+		basicSlerpTest(a, b, 0.75, new Quat(0, 1.1249999999999998e-8, 0, 0.9999999999999999), 0);
+		basicSlerpTest(a, b, 0.9, new Quat(0, 1.3499999999999998e-8, 0, 0.9999999999999999), 0);
+		basicSlerpTest(a, b, 1, b, 0);
+	},
+});
+
 Deno.test({
 	name: "slerp that results in a negative cosHalfTheta",
 	fn() {
@@ -74,12 +89,30 @@ Deno.test({
 	},
 });
 
+Deno.test({
+	name: "another slerp that results in a negative cosHalfTheta",
+	fn() {
+		const a = new Quat();
+		const b = Quat.fromAxisAngle(1, 1, 1, 4);
+		basicSlerpTest(a, b, 0.219, Quat.fromAxisAngle(1, 1, 1, -0.5));
+		basicSlerpTest(a, b, 0.5, Quat.fromAxisAngle(1, 1, 1, -1.14157));
+	},
+});
+
 Deno.test({
 	name: "slerp two quaternions that are the same",
 	fn() {
 		const a = new Quat(0, 0.2, 20, 1);
-		basicSlerpTest(a, a, 0.5, a);
 		const b = new Quat(12, 34, 56, 78);
+		basicSlerpTest(a, a, 0.01, a);
+		basicSlerpTest(b, b, 0.01, b);
+		basicSlerpTest(a, a, 0.1, a);
+		basicSlerpTest(b, b, 0.1, b);
+		basicSlerpTest(a, a, 0.5, a);
 		basicSlerpTest(b, b, 0.5, b);
+		basicSlerpTest(a, a, 0.9, a);
+		basicSlerpTest(b, b, 0.9, b);
+		basicSlerpTest(a, a, 0.99, a);
+		basicSlerpTest(b, b, 0.99, b);
 	},
 });