Files
gods-eye-view/src/geo/quaternion.test.mjs
T
Sameh Khamis 11167fa608 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.
2026-11-05 14:15:13 -08:00

219 lines
6.8 KiB
JavaScript

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);
});