Add quaternions to src/geo

Ports Cesium's axis-angle, product, rotation-matrix and heading-pitch-roll
quaternions, with rotations given as frame axes. They match Cesium bit for
bit, including heading-pitch-roll frames on the polar axis. The model
convention is documented: at heading 0 the forward axis points east.

scripts/package-boundaries.json

@@ -2360,6 +2360,7 @@
       "src/geo/ellipsoid.js",
       "src/geo/frames.js",
       "src/geo/celestial.js",
+      "src/geo/quaternion.js",
       "src/layers/flights/icons.js",
       "src/data/aircraftIconLayer.js",
       "src/data/aircraftModelLayer.js",
@@ -5122,6 +5123,7 @@
       "src/geo/ellipsoid.js",
       "src/geo/frames.js",
       "src/geo/celestial.js",
+      "src/geo/quaternion.js",
       "src/agent/worldPacket.js",
       "src/data/layerSnapshot.js",
       "src/data/feedState.js",
@@ -5144,7 +5146,8 @@
       "src/geo/vector.js",
       "src/geo/ellipsoid.js",
       "src/geo/frames.js",
-      "src/geo/celestial.js"
+      "src/geo/celestial.js",
+      "src/geo/quaternion.js"
     ],
     "external": []
   },
@@ -5166,6 +5169,7 @@
       "src/geo/ellipsoid.js",
       "src/geo/frames.js",
       "src/geo/celestial.js",
+      "src/geo/quaternion.js",
       "src/render/stub/motion.js",
       "src/render/stub/camera.js",
       "src/render/stub/geodesic.js",

src/geo/index.js

@@ -1,11 +1,12 @@
 /**
  * @module geo
  * @description Renderer-neutral geodesy and vector maths: WGS84, ECEF and
- * geodetic conversions, east-north-up frames, and the sun and
- * moon. Plain numbers and `{x, y, z}` objects only, so code that needs maths
- * but not a renderer does not import one.
+ * geodetic conversions, east-north-up frames, quaternions, and the sun and
+ * moon. Plain numbers and `{x, y, z}` objects only, so code that needs
+ * maths but not a renderer does not import one.
  */
 export * from './vector.js';
 export * from './ellipsoid.js';
 export * from './frames.js';
 export * from './celestial.js';
+export * from './quaternion.js';

src/geo/quaternion.js

@@ -0,0 +1,150 @@
+/**
+ * @module geo/quaternion
+ * @description Unit quaternions `{x, y, z, w}` and the model
+ * heading-pitch-roll convention, in the east-north-up frame at a point: the
+ * forward axis is the body's x, which at heading 0 points east; heading
+ * turns it clockwise seen from above (heading π/2 points south), pitch
+ * raises it, roll turns about it. (The camera's heading, from north, is a
+ * different convention.)
+ *
+ * Each function follows Cesium's formula step by step, so results match it
+ * to the last bit. A rotation's axes are given as a frame's `x`, `y`, `z`
+ * columns (`geo/frames`), the form of Cesium's column-major `Matrix3`.
+ */
+import { eastNorthUp } from './frames.js';
+import { normalize, vec } from './vector.js';
+
+/** @typedef {{x: number, y: number, z: number, w: number}} Quaternion */
+
+export const quat = (x, y, z, w) => ({ x, y, z, w });
+
+export const IDENTITY_QUATERNION = Object.freeze(quat(0, 0, 0, 1));
+
+/**
+ * Rotation by `angle` radians (right-handed) about `axis`, normalised first.
+ * Throws a RangeError for an axis with no direction, where Cesium's debug
+ * build throws.
+ */
+export function fromAxisAngle(axis, angle) {
+  const halfAngle = angle / 2.0;
+  const s = Math.sin(halfAngle);
+  const n = normalize(axis);
+  if (Number.isNaN(n.x) || Number.isNaN(n.y) || Number.isNaN(n.z))
+    throw new RangeError('the axis has no direction');
+  return quat(n.x * s, n.y * s, n.z * s, Math.cos(halfAngle));
+}
+
+/** The product `left · right`: the rotation that applies `right`, then `left`. */
+export function multiplyQuaternions(left, right) {
+  const { x: lx, y: ly, z: lz, w: lw } = left;
+  const { x: rx, y: ry, z: rz, w: rw } = right;
+  return quat(
+    lw * rx + lx * rw + ly * rz - lz * ry,
+    lw * ry - lx * rz + ly * rw + lz * rx,
+    lw * rz + lx * ry - ly * rx + lz * rw,
+    lw * rw - lx * rx - ly * ry - lz * rz,
+  );
+}
+
+export const conjugateQuaternion = (q) => quat(-q.x, -q.y, -q.z, q.w);
+
+/** The rotation's axes: where it takes the unit x, y and z vectors. */
+export function toAxes(q) {
+  const x2 = q.x * q.x;
+  const xy = q.x * q.y;
+  const xz = q.x * q.z;
+  const xw = q.x * q.w;
+  const y2 = q.y * q.y;
+  const yz = q.y * q.z;
+  const yw = q.y * q.w;
+  const z2 = q.z * q.z;
+  const zw = q.z * q.w;
+  const w2 = q.w * q.w;
+  return {
+    x: vec(x2 - y2 - z2 + w2, 2.0 * (xy + zw), 2.0 * (xz - yw)),
+    y: vec(2.0 * (xy - zw), -x2 + y2 - z2 + w2, 2.0 * (yz + xw)),
+    z: vec(2.0 * (xz + yw), 2.0 * (yz - xw), -x2 - y2 + z2 + w2),
+  };
+}
+
+const NEXT = [1, 2, 0];
+
+/**
+ * The quaternion of a rotation given by its axes (orthonormal, right-handed:
+ * any frame's `x`, `y`, `z`). Its `w` is positive when the trace is. When
+ * the trace is not positive the result is Cesium's: the negative of the
+ * textbook quaternion (the same rotation). Keep that negation.
+ */
+export function fromAxes({ x: c0, y: c1, z: c2 }) {
+  // m[i][j]: row i of column j.
+  const m = [
+    [c0.x, c1.x, c2.x],
+    [c0.y, c1.y, c2.y],
+    [c0.z, c1.z, c2.z],
+  ];
+  const trace = m[0][0] + m[1][1] + m[2][2];
+  if (trace > 0.0) {
+    let root = Math.sqrt(trace + 1.0);
+    const w = 0.5 * root;
+    root = 0.5 / root;
+    return quat(
+      (m[2][1] - m[1][2]) * root,
+      (m[0][2] - m[2][0]) * root,
+      (m[1][0] - m[0][1]) * root,
+      w,
+    );
+  }
+  let i = 0;
+  if (m[1][1] > m[0][0]) i = 1;
+  if (m[2][2] > m[0][0] && m[2][2] > m[1][1]) i = 2;
+  const j = NEXT[i];
+  const k = NEXT[j];
+  let root = Math.sqrt(m[i][i] - m[j][j] - m[k][k] + 1.0);
+  const q = [0, 0, 0];
+  q[i] = 0.5 * root;
+  root = 0.5 / root;
+  // Cesium reads getElementIndex(column, row) here, i.e. m[row][column].
+  const w = (m[j][k] - m[k][j]) * root;
+  q[j] = (m[i][j] + m[j][i]) * root;
+  q[k] = (m[i][k] + m[k][i]) * root;
+  return quat(-q[0], -q[1], -q[2], w);
+}
+
+const UNIT_X = vec(1, 0, 0);
+const UNIT_Y = vec(0, 1, 0);
+const UNIT_Z = vec(0, 0, 1);
+
+/**
+ * Heading, pitch and roll (radians) as a rotation of the local frame: roll
+ * about x, then pitch about y, then heading about z. In an east-north-up
+ * frame x is east, so heading 0 faces east; see `headingPitchRollFrame`.
+ */
+export function fromHeadingPitchRoll({ heading = 0, pitch = 0, roll = 0 }) {
+  const rollQ = fromAxisAngle(UNIT_X, roll);
+  const pitchQ = fromAxisAngle(UNIT_Y, -pitch);
+  const headingQ = fromAxisAngle(UNIT_Z, -heading);
+  return multiplyQuaternions(headingQ, multiplyQuaternions(pitchQ, rollQ));
+}
+
+/**
+ * The ECEF axes of a body at `origin` (ECEF metres) turned by heading, pitch
+ * and roll in the east-north-up frame there: Cesium's
+ * `Transforms.headingPitchRollToFixedFrame`, as a frame.
+ */
+export function headingPitchRollFrame(origin, hpr) {
+  const enu = eastNorthUp(origin);
+  const r = toAxes(fromHeadingPitchRoll(hpr));
+  // enu · r, column by column, each sum in Cesium's Matrix4.multiply order,
+  // its zero translation product included: it turns a -0 sum into +0.
+  const col = (c) =>
+    vec(
+      enu.x.x * c.x + enu.y.x * c.y + enu.z.x * c.z + enu.origin.x * 0.0,
+      enu.x.y * c.x + enu.y.y * c.y + enu.z.y * c.z + enu.origin.y * 0.0,
+      enu.x.z * c.x + enu.y.z * c.y + enu.z.z * c.z + enu.origin.z * 0.0,
+    );
+  return { x: col(r.x), y: col(r.y), z: col(r.z), origin: enu.origin };
+}
+
+/** Cesium's `Transforms.headingPitchRollQuaternion`. */
+export const headingPitchRollQuaternion = (origin, hpr) =>
+  fromAxes(headingPitchRollFrame(origin, hpr));

src/geo/quaternion.test.mjs

@@ -0,0 +1,218 @@
+import assert from 'node:assert/strict';
+import test from 'node:test';
+import * as Cesium from 'cesium';
+import {
+  conjugateQuaternion,
+  fromAxes,
+  fromAxisAngle,
+  fromDegrees,
+  fromHeadingPitchRoll,
+  headingPitchRollFrame,
+  headingPitchRollQuaternion,
+  multiplyQuaternions,
+  toAxes,
+  toRadians,
+} from './index.js';
+
+function random(seed) {
+  let s = seed >>> 0;
+  return () => {
+    s = (s * 1664525 + 1013904223) >>> 0;
+    return s / 2 ** 32;
+  };
+}
+
+const next = random(20261105);
+const angle = () => (next() * 2 - 1) * 2 * Math.PI;
+const axis = () => ({
+  x: next() * 2 - 1,
+  y: next() * 2 - 1,
+  z: next() * 2 - 1,
+});
+
+/** Random rotations, plus ones near a half turn (trace ≤ 0, the other branch). */
+function rotations(count = 400) {
+  const out = [];
+  for (let i = 0; i < count; i++) out.push([axis(), angle()]);
+  for (const a of [
+    { x: 1, y: 0, z: 0 },
+    { x: 0, y: 1, z: 0 },
+    { x: 0, y: 0, z: 1 },
+    { x: 1, y: 1e-3, z: -2e-3 },
+  ])
+    for (const t of [Math.PI, Math.PI - 1e-9, -Math.PI, 2.5, 0, 1e-12])
+      out.push([a, t]);
+  return out;
+}
+
+const same = (a, b, message) => {
+  for (const k of ['x', 'y', 'z', 'w'])
+    if (k in b) assert.equal(a[k], b[k], `${message} ${k}`);
+};
+const columns = (m) =>
+  [0, 1, 2].map((c) => Cesium.Matrix3.getColumn(m, c, new Cesium.Cartesian3()));
+
+test("axis-angle, product and axes are bit for bit the default renderer's", () => {
+  let previous = Cesium.Quaternion.IDENTITY;
+  let previousOurs = { x: 0, y: 0, z: 0, w: 1 };
+  let otherBranch = 0;
+  for (const [a, t] of rotations()) {
+    const ours = fromAxisAngle(a, t);
+    const theirs = Cesium.Quaternion.fromAxisAngle(
+      new Cesium.Cartesian3(a.x, a.y, a.z),
+      t,
+    );
+    same(ours, theirs, `fromAxisAngle(${JSON.stringify(a)}, ${t})`);
+
+    same(
+      multiplyQuaternions(ours, previousOurs),
+      Cesium.Quaternion.multiply(theirs, previous, new Cesium.Quaternion()),
+      'multiply',
+    );
+    previous = theirs;
+    previousOurs = ours;
+
+    const m = Cesium.Matrix3.fromQuaternion(theirs);
+    const axes = toAxes(ours);
+    const [c0, c1, c2] = columns(m);
+    same(axes.x, c0, 'toAxes x');
+    same(axes.y, c1, 'toAxes y');
+    same(axes.z, c2, 'toAxes z');
+
+    if (m[0] + m[4] + m[8] <= 0) otherBranch += 1;
+    same(fromAxes(axes), Cesium.Quaternion.fromRotationMatrix(m), 'fromAxes');
+  }
+  assert.ok(otherBranch >= 20, `only ${otherBranch} rotations had trace ≤ 0`);
+});
+
+test('fromAxes inverts toAxes up to sign', () => {
+  for (const [a, t] of rotations(100)) {
+    const q = fromAxisAngle(a, t);
+    const back = fromAxes(toAxes(q));
+    const s = Math.sign(
+      back.w * q.w + back.x * q.x + back.y * q.y + back.z * q.z,
+    );
+    for (const k of ['x', 'y', 'z', 'w'])
+      assert.ok(
+        Math.abs(back[k] * s - q[k]) < 1e-12,
+        `${k}: ${back[k]} vs ${q[k]}`,
+      );
+  }
+});
+
+test('conjugateQuaternion undoes a rotation', () => {
+  const q = fromAxisAngle({ x: 1, y: 2, z: 3 }, 0.7);
+  const id = multiplyQuaternions(q, conjugateQuaternion(q));
+  assert.ok(Math.abs(id.w - 1) < 1e-15 && Math.hypot(id.x, id.y, id.z) < 1e-15);
+});
+
+test("heading-pitch-roll matches the default renderer's", () => {
+  for (let i = 0; i < 400; i++) {
+    const hpr = { heading: angle(), pitch: angle() / 4, roll: angle() / 2 };
+    same(
+      fromHeadingPitchRoll(hpr),
+      Cesium.Quaternion.fromHeadingPitchRoll(
+        new Cesium.HeadingPitchRoll(hpr.heading, hpr.pitch, hpr.roll),
+      ),
+      `fromHeadingPitchRoll(${JSON.stringify(hpr)})`,
+    );
+
+    const lon = next() * 360 - 180;
+    const lat = i < 4 ? [90, -90, 90, -90][i] : next() * 180 - 90;
+    const height = next() * 2e7 - 1e3;
+    const origin = fromDegrees(lon, lat, height);
+    const cesiumHpr = new Cesium.HeadingPitchRoll(
+      hpr.heading,
+      hpr.pitch,
+      hpr.roll,
+    );
+    const cesiumOrigin = Cesium.Cartesian3.fromDegrees(lon, lat, height);
+    const frame = headingPitchRollFrame(origin, hpr);
+    const m = Cesium.Transforms.headingPitchRollToFixedFrame(
+      cesiumOrigin,
+      cesiumHpr,
+    );
+    const cols = [0, 1, 2].map((c) =>
+      Cesium.Matrix4.getColumn(m, c, new Cesium.Cartesian4()),
+    );
+    for (const [name, ours, theirs] of [
+      ['x', frame.x, cols[0]],
+      ['y', frame.y, cols[1]],
+      ['z', frame.z, cols[2]],
+    ])
+      for (const k of ['x', 'y', 'z'])
+        assert.ok(
+          Math.abs(ours[k] - theirs[k]) <= 1e-12,
+          `frame ${name}.${k} at ${lon},${lat}: ${ours[k]} vs ${theirs[k]}`,
+        );
+
+    const q = headingPitchRollQuaternion(origin, hpr);
+    const qc = Cesium.Transforms.headingPitchRollQuaternion(
+      cesiumOrigin,
+      cesiumHpr,
+    );
+    // Either sign is the same rotation; near a zero trace the ENU frame's
+    // last-bit differences can pick the other.
+    const sign = q.x * qc.x + q.y * qc.y + q.z * qc.z + q.w * qc.w < 0 ? -1 : 1;
+    for (const k of ['x', 'y', 'z', 'w'])
+      assert.ok(
+        Math.abs(q[k] * sign - qc[k]) <= 1e-12,
+        `quaternion ${k}: ${q[k]} vs ${qc[k]}`,
+      );
+  }
+});
+
+test('heading-pitch-roll frames are exact on the polar axis, origin included', () => {
+  const hprs = [
+    { heading: 0, pitch: 0, roll: -Math.PI },
+    { heading: 1.1, pitch: -0.4, roll: 0.3 },
+    { heading: -2.5, pitch: 1.2, roll: 3 },
+    {},
+  ];
+  for (const z of [6356752.3142451793, -6356752.3142451793, 1e7, -7e6])
+    for (const hpr of hprs) {
+      const origin = { x: 0, y: 0, z };
+      const cesiumHpr = new Cesium.HeadingPitchRoll(
+        hpr.heading ?? 0,
+        hpr.pitch ?? 0,
+        hpr.roll ?? 0,
+      );
+      const cesiumOrigin = new Cesium.Cartesian3(0, 0, z);
+      const frame = headingPitchRollFrame(origin, hpr);
+      const m = Cesium.Transforms.headingPitchRollToFixedFrame(
+        cesiumOrigin,
+        cesiumHpr,
+      );
+      ['x', 'y', 'z', 'origin'].forEach((name, c) =>
+        ['x', 'y', 'z'].forEach((k, r) =>
+          assert.ok(
+            Object.is(frame[name][k], m[c * 4 + r]),
+            `${name}.${k} at z=${z} ${JSON.stringify(hpr)}: ${frame[name][k]} vs ${m[c * 4 + r]}`,
+          ),
+        ),
+      );
+      same(
+        headingPitchRollQuaternion(origin, hpr),
+        Cesium.Transforms.headingPitchRollQuaternion(cesiumOrigin, cesiumHpr),
+        `quaternion at z=${z}`,
+      );
+    }
+});
+
+test('the forward axis faces east at heading 0 and south at heading π/2', () => {
+  const origin = fromDegrees(10, 45, 0);
+  const at = (heading) => headingPitchRollFrame(origin, { heading }).x;
+  const east = {
+    x: -Math.sin(toRadians(10)),
+    y: Math.cos(toRadians(10)),
+    z: 0,
+  };
+  const dot = (a, b) => a.x * b.x + a.y * b.y + a.z * b.z;
+  assert.ok(Math.abs(dot(at(0), east) - 1) < 1e-12);
+  assert.ok(Math.abs(dot(at(Math.PI / 2), east)) < 1e-12);
+  assert.ok(at(Math.PI / 2).z < 0, 'heading π/2 turns towards the equator');
+});
+
+test('an axis with no direction throws', () => {
+  assert.throws(() => fromAxisAngle({ x: 0, y: 0, z: 0 }, 1), RangeError);
+});