mirror of
https://github.com/bilawalsidhu/gods-eye-view.git
synced 2026-11-17 00:32:21 +00:00
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.
This commit is contained in:
1 parent
5a7c7f3d57
commit
11167fa608
4 files changed
+377
-4
No files matched your search
@@ -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);
|
||||
});
|
||||
Reference in new issue
Block a user