opendance-web/web-server/src/preprocess/math3d.ts

export type Vec3 = [number, number, number];

/** 3×3 行主序矩阵 */
export type Mat3 = [number, number, number, number, number, number, number, number, number];

export const IDENTITY3: Mat3 = [1, 0, 0, 0, 1, 0, 0, 0, 1];

export function vAdd(a: Vec3, b: Vec3): Vec3 {
  return [a[0] + b[0], a[1] + b[1], a[2] + b[2]];
}

export function vSub(a: Vec3, b: Vec3): Vec3 {
  return [a[0] - b[0], a[1] - b[1], a[2] - b[2]];
}

export function vScale(a: Vec3, s: number): Vec3 {
  return [a[0] * s, a[1] * s, a[2] * s];
}

export function vMid(a: Vec3, b: Vec3): Vec3 {
  return [(a[0] + b[0]) / 2, (a[1] + b[1]) / 2, (a[2] + b[2]) / 2];
}

export function vLen(a: Vec3): number {
  return Math.hypot(a[0], a[1], a[2]);
}

export function vNorm(a: Vec3): Vec3 {
  const len = vLen(a);
  if (len < 1e-9) return [0, 0, 0];
  return [a[0] / len, a[1] / len, a[2] / len];
}

export function vDot(a: Vec3, b: Vec3): number {
  return a[0] * b[0] + a[1] * b[1] + a[2] * b[2];
}

/** 向量 ba 与 bc 夹角的余弦，落在 [-1, 1] —— 契约 skeleton-topology.md §5 */
export function angleCos(a: Vec3, b: Vec3, c: Vec3): number {
  const ba = vSub(a, b);
  const bc = vSub(c, b);
  const denom = vLen(ba) * vLen(bc);
  if (denom < 1e-9) return 0;
  return Math.max(-1, Math.min(1, vDot(ba, bc) / denom));
}

export function matMul(a: Mat3, b: Mat3): Mat3 {
  const out = new Array<number>(9);
  for (let r = 0; r < 3; r += 1) {
    for (let c = 0; c < 3; c += 1) {
      out[r * 3 + c] = a[r * 3] * b[c] + a[r * 3 + 1] * b[3 + c] + a[r * 3 + 2] * b[6 + c];
    }
  }
  return out as Mat3;
}

/**
 * 四元数 (x, y, z, w) → 3×3 行主序旋转矩阵。
 * 输入会自动归一化；零四元数视为单位四元数。
 */
export function quatToMat3(q: readonly [number, number, number, number]): Mat3 {
  let [x, y, z, w] = q;
  const len = Math.hypot(x, y, z, w);
  if (len < 1e-9) return IDENTITY3;
  x /= len;
  y /= len;
  z /= len;
  w /= len;
  return [
    1 - 2 * (y * y + z * z), 2 * (x * y - z * w), 2 * (x * z + y * w),
    2 * (x * y + z * w), 1 - 2 * (x * x + z * z), 2 * (y * z - x * w),
    2 * (x * z - y * w), 2 * (y * z + x * w), 1 - 2 * (x * x + y * y),
  ];
}

export function matTranspose(m: Mat3): Mat3 {
  return [m[0], m[3], m[6], m[1], m[4], m[7], m[2], m[5], m[8]];
}

export function vCross(a: Vec3, b: Vec3): Vec3 {
  return [a[1] * b[2] - a[2] * b[1], a[2] * b[0] - a[0] * b[2], a[0] * b[1] - a[1] * b[0]];
}

export function matVec(m: Mat3, v: Vec3): Vec3 {
  return [
    m[0] * v[0] + m[1] * v[1] + m[2] * v[2],
    m[3] * v[0] + m[4] * v[1] + m[5] * v[2],
    m[6] * v[0] + m[7] * v[1] + m[8] * v[2],
  ];
}

const DEG2RAD = Math.PI / 180;

/** 绕单轴旋转矩阵，axis ∈ {X, Y, Z} */
export function rotationMatrix(axis: string, degrees: number): Mat3 {
  const rad = degrees * DEG2RAD;
  const c = Math.cos(rad);
  const s = Math.sin(rad);
  switch (axis.toUpperCase()) {
    case 'X':
      return [1, 0, 0, 0, c, -s, 0, s, c];
    case 'Y':
      return [c, 0, s, 0, 1, 0, -s, 0, c];
    case 'Z':
      return [c, -s, 0, s, c, 0, 0, 0, 1];
    default:
      return IDENTITY3;
  }
}

/**
 * 由通道序列合成局部旋转矩阵。
 * BVH 约定：按通道声明顺序依次左乘，即 R = R1 × R2 × R3（内旋）。
 */
export function composeRotation(channels: ReadonlyArray<{ axis: string; value: number }>): Mat3 {
  let out: Mat3 = IDENTITY3;
  for (const { axis, value } of channels) {
    out = matMul(out, rotationMatrix(axis, value));
  }
  return out;
}

/** 世界变换：旋转 + 平移 */
export interface Transform {
  r: Mat3;
  t: Vec3;
}

export const IDENTITY_TRANSFORM: Transform = { r: IDENTITY3, t: [0, 0, 0] };

export function applyTransform(parent: Transform, offset: Vec3, localR: Mat3, extraT?: Vec3): Transform {
  return {
    r: matMul(parent.r, localR),
    t: vAdd(parent.t, matVec(parent.r, vAdd(offset, extraT ?? [0, 0, 0]))),
  };
}