《游戏引擎实现原理》9.2 四元数运算与最短弧插值

q = ( a·sin(θ/2),  cos(θ/2) )

// file: src/core/quaternion.ts
// tier: L1
// verify: npx vitest run src/core/quaternion.test.ts
//
// helio 内核：单位四元数（x, y, z, w）的乘法、共轭、归一化与 slerp 最短弧插值。
// 约定 q = (x, y, z, w)，w 是标量部；q 与 -q 表示同一个旋转。

export type Quat = { x: number; y: number; z: number; w: number };
export type Vec3 = { x: number; y: number; z: number };

export const DEG2RAD = Math.PI / 180;
export const RAD2DEG = 180 / Math.PI;

/** 单位四元数（无旋转）。 */
export const QUAT_IDENTITY: Quat = { x: 0, y: 0, z: 0, w: 1 };

/** 绕单位轴 axis 旋转 angle（弧度）。 */
export function fromAxisAngle(axis: Vec3, angle: number): Quat {
  const h = angle * 0.5;
  const s = Math.sin(h);
  return { x: axis.x * s, y: axis.y * s, z: axis.z * s, w: Math.cos(h) };
}

export function quatDot(a: Quat, b: Quat): number {
  return a.x * b.x + a.y * b.y + a.z * b.z + a.w * b.w;
}

export function quatNegate(q: Quat): Quat {
  return { x: -q.x, y: -q.y, z: -q.z, w: -q.w };
}

/** Hamilton 乘积：先应用 b 再应用 a 的复合旋转。 */
export function quatMul(a: Quat, b: Quat): Quat {
  return {
    x: a.w * b.x + a.x * b.w + a.y * b.z - a.z * b.y,
    y: a.w * b.y - a.x * b.z + a.y * b.w + a.z * b.x,
    z: a.w * b.z + a.x * b.y - a.y * b.x + a.z * b.w,
    w: a.w * b.w - a.x * b.x - a.y * b.y - a.z * b.z,
  };
}

/** 共轭：单位四元数的共轭即逆旋转。 */
export function quatConjugate(q: Quat): Quat {
  return { x: -q.x, y: -q.y, z: -q.z, w: q.w };
}

export function quatLength(q: Quat): number {
  return Math.sqrt(quatDot(q, q));
}

export function quatNormalize(q: Quat): Quat {
  const len = quatLength(q);
  if (len === 0) return QUAT_IDENTITY;
  const inv = 1 / len;
  return { x: q.x * inv, y: q.y * inv, z: q.z * inv, w: q.w * inv };
}

/** 用单位四元数旋转向量：v' = q v q*。 */
export function rotateVec3(q: Quat, v: Vec3): Vec3 {
  const { x, y, z, w } = q;
  const tx = 2 * (y * v.z - z * v.y);
  const ty = 2 * (z * v.x - x * v.z);
  const tz = 2 * (x * v.y - y * v.x);
  return {
    x: v.x + w * tx + (y * tz - z * ty),
    y: v.y + w * ty + (z * tx - x * tz),
    z: v.z + w * tz + (x * ty - y * tx),
  };
}

/**
 * 球面线性插值。shortest = true 时先处理 dot < 0：
 * 把 b 取反成 -b（同一个旋转的另一半），使插值走「最短弧」而不是绕远路。
 */
export function quatSlerp(a: Quat, b: Quat, t: number, shortest = true): Quat {
  let cosom = quatDot(a, b);
  let target = b;
  if (shortest && cosom < 0) {
    cosom = -cosom;
    target = quatNegate(b);
  }
  if (cosom > 0.9995) {
    // 两旋转几乎相同：线性插值再归一化，避免 sin(theta) ≈ 0 时的除零
    return quatNormalize({
      x: a.x + (target.x - a.x) * t,
      y: a.y + (target.y - a.y) * t,
      z: a.z + (target.z - a.z) * t,
      w: a.w + (target.w - a.w) * t,
    });
  }
  const theta = Math.acos(cosom);
  const sinTheta = Math.sin(theta);
  const wa = Math.sin((1 - t) * theta) / sinTheta;
  const wb = Math.sin(t * theta) / sinTheta;
  return {
    x: wa * a.x + wb * target.x,
    y: wa * a.y + wb * target.y,
    z: wa * a.z + wb * target.z,
    w: wa * a.w + wb * target.w,
  };
}

/** 该四元数代表的旋转角（弧度，恒取非负的较小值）。 */
export function rotationAngle(q: Quat): number {
  const w = Math.min(1, Math.abs(q.w));
  return 2 * Math.acos(w);
}

/** 两个旋转之间的夹角（弧度）：2·acos(|dot|)。 */
export function angleBetween(a: Quat, b: Quat): number {
  const d = Math.min(1, Math.abs(quatDot(a, b)));
  return 2 * Math.acos(d);
}

// file: src/core/quaternion.test.ts
// tier: L1
// verify: npx vitest run src/core/quaternion.test.ts

import { describe, expect, it } from 'vitest';
import {
  DEG2RAD,
  QUAT_IDENTITY,
  RAD2DEG,
  angleBetween,
  fromAxisAngle,
  quatConjugate,
  quatDot,
  quatMul,
  quatNormalize,
  quatSlerp,
  rotateVec3,
  rotationAngle,
} from './quaternion.ts';

const Y = { x: 0, y: 1, z: 0 };
const X = { x: 1, y: 0, z: 0 };
const deg = (x: number) => x * DEG2RAD;
const toDeg = (x: number) => x * RAD2DEG;
const close = (a: number, b: number, eps = 1e-9) => Math.abs(a - b) < eps;

describe('四元数基本运算', () => {
  it('单位四元数旋转向量不变', () => {
    const v = rotateVec3(QUAT_IDENTITY, { x: 1, y: 2, z: 3 });
    expect(close(v.x, 1) && close(v.y, 2) && close(v.z, 3)).toBe(true);
  });

  it('绕 Y 转 90° 把 +X 转到 -Z', () => {
    const q = fromAxisAngle(Y, deg(90));
    const v = rotateVec3(q, { x: 1, y: 0, z: 0 });
    expect(close(v.x, 0) && close(v.z, -1)).toBe(true);
  });

  it('乘法复合：绕 Y 转 90° 两次等于 180°', () => {
    const q90 = fromAxisAngle(Y, deg(90));
    const q180 = quatMul(q90, q90);
    const v = rotateVec3(q180, { x: 1, y: 0, z: 0 });
    expect(close(v.x, -1) && close(v.z, 0)).toBe(true);
  });

  it('共轭即逆旋转：q · q* = 单位', () => {
    const q = fromAxisAngle(X, deg(37));
    const p = quatMul(q, quatConjugate(q));
    expect(close(p.w, 1) && close(p.x, 0) && close(p.y, 0) && close(p.z, 0)).toBe(true);
  });

  it('归一化后长度为 1', () => {
    const q = quatNormalize({ x: 1, y: 2, z: 3, w: 4 });
    expect(close(quatDot(q, q), 1)).toBe(true);
  });
});

describe('slerp 最短弧：dot < 0 时必须取反', () => {
  const q0 = QUAT_IDENTITY; // 0°
  const q350 = fromAxisAngle(Y, deg(350)); // 350°（等价于 -10°）

  it('q0 与 q350 的点积为负', () => {
    expect(quatDot(q0, q350)).toBeLessThan(0);
  });

  it('不处理 dot < 0：t = 0.5 时走了 175° 的远弧', () => {
    const far = quatSlerp(q0, q350, 0.5, false);
    const angle = toDeg(rotationAngle(far));
    expect(close(angle, 175, 1e-6)).toBe(true);
  });

  it('处理 dot < 0：t = 0.5 时走最短弧，只转 5°', () => {
    const near = quatSlerp(q0, q350, 0.5, true);
    const angle = toDeg(rotationAngle(near));
    expect(close(angle, 5, 1e-6)).toBe(true);
  });

  it('两条路径在 t = 0.5 的结果相差 180°', () => {
    const far = quatSlerp(q0, q350, 0.5, false);
    const near = quatSlerp(q0, q350, 0.5, true);
    expect(close(toDeg(angleBetween(far, near)), 180, 1e-6)).toBe(true);
  });

  it('最短弧在 t = 1 时精确到达目标旋转', () => {
    const end = quatSlerp(q0, q350, 1, true);
    expect(close(toDeg(angleBetween(end, q350)), 0, 1e-6)).toBe(true);
  });
});