diff --git a/src/math/Quat.js b/src/math/Quat.js index 3c71a952..1d99c620 100644 --- a/src/math/Quat.js +++ b/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; diff --git a/test/unit/src/math/Quat.test.js b/test/unit/src/math/Quat.test.js index 3939e6e5..98b36edd 100644 --- a/test/unit/src/math/Quat.test.js +++ b/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); }, });