mirror of
https://github.com/bilawalsidhu/gods-eye-view.git
synced 2026-11-17 00:32:21 +00:00
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.
219 lines
6.8 KiB
JavaScript
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);
|
|
});
|