// Small vector and quaternion helpers. Quaternions are [x, y, z, w]; transforms are [px, py, pz, qx, qy, qz, qw]. export type Vec3 = [number, number, number]; export type Quat = [number, number, number, number]; export type Xform = [number, number, number, number, number, number, number]; export const qmul = (a: Quat, b: Quat): Quat => [ a[3] * b[0] + a[0] * b[3] + a[1] * b[2] - a[2] * b[1], a[3] * b[1] - a[0] * b[2] + a[1] * b[3] + a[2] * b[0], a[3] * b[2] + a[0] * b[1] - a[1] * b[0] + a[2] * b[3], a[3] * b[3] - a[0] * b[0] - a[1] * b[1] - a[2] * b[2] ]; export const qconj = (q: Quat): Quat => [-q[0], -q[1], -q[2], q[3]]; export const qrot = (q: Quat, v: Vec3): Vec3 => { const [x, y, z, w] = q; const tx = 2 * (y * v[2] - z * v[1]); const ty = 2 * (z * v[0] - x * v[2]); const tz = 2 * (x * v[1] - y * v[0]); return [v[0] + w * tx + (y * tz - z * ty), v[1] + w * ty + (z * tx - x * tz), v[2] + w * tz + (x * ty - y * tx)]; }; export const qaxis = (axis: Vec3, angle: number): Quat => { const s = Math.sin(angle / 2); return [axis[0] * s, axis[1] * s, axis[2] * s, Math.cos(angle / 2)]; }; export const xmul = (a: Xform, b: Xform): Xform => { const qa: Quat = [a[3], a[4], a[5], a[6]]; const p = qrot(qa, [b[0], b[1], b[2]]); const q = qmul(qa, [b[3], b[4], b[5], b[6]]); return [a[0] + p[0], a[1] + p[1], a[2] + p[2], ...q]; }; export const xinv = (a: Xform): Xform => { const qi = qconj([a[3], a[4], a[5], a[6]]); const p = qrot(qi, [-a[0], -a[1], -a[2]]); return [p[0], p[1], p[2], ...qi]; }; /** Rotation vector (axis times angle) that takes orientation `from` to `to`, in world frame. */ export const rotError = (to: Quat, from: Quat): Vec3 => { let d = qmul(to, qconj(from)); if (d[3] < 0) d = [-d[0], -d[1], -d[2], -d[3]]; const s = Math.hypot(d[0], d[1], d[2]); if (s < 1e-9) return [0, 0, 0]; const angle = 2 * Math.atan2(s, d[3]); return [(d[0] / s) * angle, (d[1] / s) * angle, (d[2] / s) * angle]; }; /** Gripper pointing straight down, rotated by `yaw` about world Z. */ export const downQuat = (yaw: number): Quat => qmul(qaxis([0, 0, 1], yaw), [1, 0, 0, 0]);