feat(math): Add Quat.slerpQuaternions() (#946)

src/math/Quat.js

@@ -278,6 +278,49 @@ export class Quat {
 		return this;
 	}
 
+	/**
+	 * Interpolates between `quatA` and `quatB` without modifying and returns a quaternion with the result.
+	 * @param {Quat} quatA
+	 * @param {Quat} quatB
+	 * @param {number} t
+	 */
+	static slerpQuaternions(quatA, quatB, t) {
+		// https://www.euclideanspace.com/maths/algebra/realNormedAlgebra/quaternions/slerp/
+		if (t == 0) {
+			return quatA.clone();
+		}
+		if (t == 1) {
+			return quatB.clone();
+		}
+
+		let cosHalfTheta = new Vec4(quatA).dot(quatB);
+		if (cosHalfTheta < 0) {
+			quatB.x = -quatB.x;
+			quatB.y = -quatB.y;
+			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) {
+			return quatA.clone();
+		}
+
+		// Calculate temporary values.
+		const halfTheta = Math.acos(cosHalfTheta);
+		const sinHalfTheta = Math.sqrt(1 - cosHalfTheta * cosHalfTheta);
+
+		const ratioA = Math.sin((1 - t) * halfTheta) / sinHalfTheta;
+		const ratioB = Math.sin(t * halfTheta) / sinHalfTheta;
+
+		return new Quat(
+			quatA.x * ratioA + quatB.x * ratioB,
+			quatA.y * ratioA + quatB.y * ratioB,
+			quatA.z * ratioA + quatB.z * ratioB,
+			quatA.w * ratioA + quatB.w * ratioB,
+		);
+	}
+
 	/**
 	 * @returns {[x: number, y: number, z: number, w: number]}
 	 */

src/math/Vec4.js

@@ -1,15 +1,17 @@
 import { Vec2 } from "./Vec2.js";
 import { Vec3 } from "./Vec3.js";
 import { Mat4 } from "./Mat4.js";
+import { Quat } from "./Quat.js";
 
 /**
  * @typedef {() => Vec4} vec4SetEmptySignature
  * @typedef {(vec: Vec2) => Vec4} vec4SetVec2Signature
  * @typedef {(vec: Vec3) => Vec4} vec4SetVec3Signature
  * @typedef {(vec: Vec4) => Vec4} vec4SetVec4Signature
+ * @typedef {(vec: Quat) => Vec4} vec4SetQuatSignature
  * @typedef {(x?: number, y?: number, z?: number, w?: number) => Vec4} vec4SetNumNumSignature
  * @typedef {(xyzw: number[]) => Vec4} vec4SetArraySignature
- * @typedef {import("./MathTypes.js").MergeParameters<vec4SetEmptySignature | vec4SetVec2Signature | vec4SetVec3Signature | vec4SetVec4Signature | vec4SetNumNumSignature | vec4SetArraySignature>} Vec4Parameters
+ * @typedef {import("./MathTypes.js").MergeParameters<vec4SetEmptySignature | vec4SetVec2Signature | vec4SetVec3Signature | vec4SetVec4Signature | vec4SetQuatSignature | vec4SetNumNumSignature | vec4SetArraySignature>} Vec4Parameters
  */
 
 /**
@@ -73,7 +75,7 @@ export class Vec4 {
 
 		if (args.length == 1) {
 			const arg = args[0];
-			if (arg instanceof Vec4) {
+			if (arg instanceof Vec4 || arg instanceof Quat) {
 				this._x = arg.x;
 				this._y = arg.y;
 				this._z = arg.z;

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

@@ -1,6 +1,6 @@
 import { assertEquals } from "std/testing/asserts.ts";
 import { Quat, Vec3 } from "../../../../src/mod.js";
-import { assertVecAlmostEquals } from "../../../../src/util/asserts.js";
+import { assertQuatAlmostEquals, assertVecAlmostEquals } from "../../../../src/util/asserts.js";
 
 Deno.test({
 	name: "rotateAxisAngle()",
@@ -26,3 +26,60 @@ Deno.test({
 		assertEquals(result, "Quat<1, 2, 3, 4>");
 	},
 });
+
+/**
+ * @param {Quat} a
+ * @param {Quat} b
+ * @param {number} t
+ * @param {Quat} expected
+ */
+function basicSlerpTest(a, b, t, expected) {
+	const result = Quat.slerpQuaternions(a, b, t);
+	assertQuatAlmostEquals(result, expected);
+}
+
+Deno.test({
+	name: "slerp two identity quaternions",
+	fn() {
+		basicSlerpTest(new Quat(), new Quat(), 0, new Quat());
+		basicSlerpTest(new Quat(), new Quat(), 0.123, new Quat());
+		basicSlerpTest(new Quat(), new Quat(), 0.2, new Quat());
+		basicSlerpTest(new Quat(), new Quat(), 0.5, new Quat());
+		basicSlerpTest(new Quat(), new Quat(), 1, new Quat());
+	},
+});
+
+Deno.test({
+	name: "basic 180 degree slerp",
+	fn() {
+		const a = new Quat();
+		const b = Quat.fromAxisAngle(0, 1, 0, Math.PI);
+		basicSlerpTest(a, b, 0, a);
+		basicSlerpTest(a, b, 0.1, Quat.fromAxisAngle(0, 1, 0, Math.PI * 0.1));
+		basicSlerpTest(a, b, 0.25, Quat.fromAxisAngle(0, 1, 0, Math.PI * 0.25));
+		basicSlerpTest(a, b, 0.5, Quat.fromAxisAngle(0, 1, 0, Math.PI * 0.5));
+		basicSlerpTest(a, b, 0.75, Quat.fromAxisAngle(0, 1, 0, Math.PI * 0.75));
+		basicSlerpTest(a, b, 0.9, Quat.fromAxisAngle(0, 1, 0, Math.PI * 0.9));
+		basicSlerpTest(a, b, 1, b);
+	},
+});
+
+Deno.test({
+	name: "slerp that results in a negative cosHalfTheta",
+	fn() {
+		const a = Quat.fromAxisAngle(0, 1, 0, 2);
+		const b = Quat.fromAxisAngle(0, 1, 0, -2);
+		basicSlerpTest(a, b, 0.219, Quat.fromAxisAngle(0, 1, 0, 2.5));
+		basicSlerpTest(a, b, 0.5, Quat.fromAxisAngle(0, 1, 0, Math.PI));
+	},
+});
+
+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(b, b, 0.5, b);
+	},
+});

test/unit/src/math/Vec4.test.js

@@ -1,5 +1,5 @@
 import { assertEquals, assertNotStrictEquals } from "std/testing/asserts.ts";
-import { Mat4, Vec2, Vec3, Vec4 } from "../../../../src/mod.js";
+import { Mat4, Quat, Vec2, Vec3, Vec4 } from "../../../../src/mod.js";
 import { assertAlmostEquals, assertVecAlmostEquals } from "../../../../src/util/asserts.js";
 
 Deno.test({
@@ -41,6 +41,16 @@ Deno.test({
 	},
 });
 
+Deno.test({
+	name: "Create with Quat",
+	fn() {
+		const quat = new Quat([1, 2, 3, 4]);
+		const vec = new Vec4(quat);
+
+		assertEquals(vec.toArray(), [1, 2, 3, 4]);
+	},
+});
+
 Deno.test({
 	name: "Create with one number",
 	fn() {
@@ -155,6 +165,17 @@ Deno.test({
 	},
 });
 
+Deno.test({
+	name: "Set with Quat",
+	fn() {
+		const quat = new Quat([1, 2, 3, 4]);
+		const vec = new Vec4();
+		vec.set(quat);
+
+		assertEquals(vec.toArray(), [1, 2, 3, 4]);
+	},
+});
+
 Deno.test({
 	name: "Set with two numbers",
 	fn() {