feat(render): add squad quaternion-spline helper

Author: a-baran-orhan

packages/posecode-render/src/squad.ts

@@ -0,0 +1,88 @@
+/**
+ * Spherical-quadrangle (squad) quaternion interpolation — Shoemake's C1
+ * quaternion spline. Given a keyframe and its two neighbors, `squadControl`
+ * derives the intermediate control quaternion; `squad` blends one segment.
+ *
+ * All functions return NEW quaternions (or write into a caller `out`); the
+ * shared keyframe quaternions are never mutated.
+ */
+
+import * as THREE from "three";
+
+/** Ensure `b` is in the same hemisphere as `a` (shortest-path continuity). */
+function alignHemisphere(a: THREE.Quaternion, b: THREE.Quaternion): THREE.Quaternion {
+  const out = b.clone();
+  if (a.dot(out) < 0) out.set(-out.x, -out.y, -out.z, -out.w);
+  return out;
+}
+
+/** q^-1 for a UNIT quaternion is its conjugate. */
+function conjugate(q: THREE.Quaternion): THREE.Quaternion {
+  return new THREE.Quaternion(-q.x, -q.y, -q.z, q.w);
+}
+
+/** Natural log of a unit quaternion → a pure quaternion (w = 0). */
+function logUnit(q: THREE.Quaternion): THREE.Quaternion {
+  const v = new THREE.Vector3(q.x, q.y, q.z);
+  const vLen = v.length();
+  const w = THREE.MathUtils.clamp(q.w, -1, 1);
+  if (vLen < 1e-8) return new THREE.Quaternion(0, 0, 0, 0);
+  const theta = Math.atan2(vLen, w);
+  const k = theta / vLen;
+  return new THREE.Quaternion(v.x * k, v.y * k, v.z * k, 0);
+}
+
+/** Exp of a pure quaternion (w = 0) → a unit quaternion. */
+function expPure(q: THREE.Quaternion): THREE.Quaternion {
+  const v = new THREE.Vector3(q.x, q.y, q.z);
+  const theta = v.length();
+  if (theta < 1e-8) return new THREE.Quaternion(0, 0, 0, 1);
+  const s = Math.sin(theta) / theta;
+  return new THREE.Quaternion(v.x * s, v.y * s, v.z * s, Math.cos(theta));
+}
+
+function mul(a: THREE.Quaternion, b: THREE.Quaternion): THREE.Quaternion {
+  return a.clone().multiply(b);
+}
+
+/**
+ * Shoemake control quaternion for `cur`:
+ *   s = cur * exp( -( log(cur^-1 * next) + log(cur^-1 * prev) ) / 4 )
+ * Neighbors are hemisphere-aligned to `cur` first for shortest-path continuity.
+ */
+export function squadControl(
+  prev: THREE.Quaternion,
+  cur: THREE.Quaternion,
+  next: THREE.Quaternion,
+): THREE.Quaternion {
+  const p = alignHemisphere(cur, prev);
+  const n = alignHemisphere(cur, next);
+  const inv = conjugate(cur);
+  const logNext = logUnit(mul(inv, n));
+  const logPrev = logUnit(mul(inv, p));
+  const sum = new THREE.Quaternion(
+    -(logNext.x + logPrev.x) / 4,
+    -(logNext.y + logPrev.y) / 4,
+    -(logNext.z + logPrev.z) / 4,
+    0,
+  );
+  return mul(cur, expPure(sum)).normalize();
+}
+
+/**
+ * Squad blend of one segment: slerp(slerp(q0,q1,t), slerp(s0,s1,t), 2t(1-t)).
+ * Endpoints `q0`,`q1`; their controls `s0`,`s1`. Returns q0 at t=0, q1 at t=1.
+ */
+export function squad(
+  q0: THREE.Quaternion,
+  s0: THREE.Quaternion,
+  s1: THREE.Quaternion,
+  q1: THREE.Quaternion,
+  t: number,
+  out: THREE.Quaternion = new THREE.Quaternion(),
+): THREE.Quaternion {
+  const q1a = alignHemisphere(q0, q1);
+  const a = new THREE.Quaternion().slerpQuaternions(q0, q1a, t);
+  const b = new THREE.Quaternion().slerpQuaternions(s0, alignHemisphere(s0, s1), t);
+  return out.slerpQuaternions(a, alignHemisphere(a, b), 2 * t * (1 - t));
+}

packages/posecode-render/test/squad.test.ts

@@ -0,0 +1,47 @@
+import { describe, it, expect } from "vitest";
+import * as THREE from "three";
+import { squad, squadControl } from "../src/squad.js";
+
+const q = (x: number, y: number, z: number) =>
+  new THREE.Quaternion().setFromEuler(new THREE.Euler(x, y, z, "XYZ"));
+
+describe("squad", () => {
+  it("passes exactly through segment endpoints", () => {
+    const q0 = q(0, 0, 0);
+    const q1 = q(0, 1, 0);
+    const s0 = squadControl(q(0, -0.5, 0), q0, q1);
+    const s1 = squadControl(q0, q1, q(0, 1.5, 0));
+    const at0 = squad(q0, s0, s1, q1, 0);
+    const at1 = squad(q0, s0, s1, q1, 1);
+    expect(at0.angleTo(q0)).toBeLessThan(1e-6);
+    expect(at1.angleTo(q1)).toBeLessThan(1e-6);
+  });
+
+  it("is C1-continuous across a shared interior keyframe (slerp is not)", () => {
+    const k0 = q(0, 0, 0);
+    const k1 = q(0, 1, 0);
+    const k2 = q(0, 1.2, 0.8); // direction change at k1
+    const c_before = squadControl(k0, k1, k2); // control at k1 for both segs
+    const c0 = squadControl(q(0, -1, 0), k0, k1); // control at k0
+    const c2 = squadControl(k1, k2, q(0, 0.4, 1.6)); // control at k2
+
+    const eps = 1e-3;
+    const before = squad(k0, c0, c_before, k1, 1 - eps);
+    const atK1a = squad(k0, c0, c_before, k1, 1);
+    const atK1b = squad(k1, c_before, c2, k2, 0);
+    const after = squad(k1, c_before, c2, k2, eps);
+
+    const vBefore = atK1a.angleTo(before) / eps;
+    const vAfter = after.angleTo(atK1b) / eps;
+    expect(atK1a.angleTo(atK1b)).toBeLessThan(1e-6); // C0
+    expect(Math.abs(vBefore - vAfter)).toBeLessThan(0.15); // C1 within tolerance
+  });
+
+  it("falls back cleanly when neighbors are identical (no NaN)", () => {
+    const a = q(0, 0, 0);
+    const s = squadControl(a, a, a);
+    const mid = squad(a, s, s, a, 0.5);
+    expect(Number.isNaN(mid.x)).toBe(false);
+    expect(mid.angleTo(a)).toBeLessThan(1e-6);
+  });
+});