diff --git a/scripts/package-boundaries.json b/scripts/package-boundaries.json index 8c774b97b..1a7242601 100644 --- a/scripts/package-boundaries.json +++ b/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", diff --git a/src/geo/index.js b/src/geo/index.js index f80ae09ac..fd3b1a841 100644 --- a/src/geo/index.js +++ b/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'; diff --git a/src/geo/quaternion.js b/src/geo/quaternion.js new file mode 100644 index 000000000..93fb61dbc --- /dev/null +++ b/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)); diff --git a/src/geo/quaternion.test.mjs b/src/geo/quaternion.test.mjs new file mode 100644 index 000000000..3b1fc1a0e --- /dev/null +++ b/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); +});