mirror of
https://github.com/bilawalsidhu/gods-eye-view.git
synced 2026-11-04 18:34:02 +00:00
344 lines
15 KiB
JavaScript
344 lines
15 KiB
JavaScript
// src/data/motionModel.test.mjs
|
||
import { test } from 'node:test';
|
||
import assert from 'node:assert/strict';
|
||
import * as Cesium from 'cesium';
|
||
import {
|
||
norm360, norm180,
|
||
courseBetweenCartesians, limitCourseStep,
|
||
estimateTurnRateDps, turnRateFromFixHistory, arcOffsetEnu,
|
||
speedRamp, lerpAngleDeg, courseSlewCapDps, displayedKinematics, staleCoastLimitSeconds,
|
||
liftRepeatedGroundFix, synthesizeForwardKinematicsFix,
|
||
projectGroundArcLatLon, corridorPathLatLon,
|
||
COURSE_TRACK_ONLY_MPS, COURSE_CHORD_ONLY_MPS, COURSE_MIN_DPS, TURN_MIN_SPEED_MPS,
|
||
} from './motionModel.js';
|
||
|
||
const AUSTIN = { lat: 30.2672, lon: -97.7431, alt: 9000 };
|
||
const cart = (latOff, lonOff) =>
|
||
Cesium.Cartesian3.fromDegrees(AUSTIN.lon + lonOff, AUSTIN.lat + latOff, AUSTIN.alt);
|
||
|
||
test('norm helpers', () => {
|
||
assert.equal(norm360(-90), 270);
|
||
assert.equal(norm180(350), -10);
|
||
});
|
||
|
||
test('displayedKinematics prefers motion derived from the rendered fix segment', () => {
|
||
assert.deepEqual(displayedKinematics({
|
||
derivedSpeedMps: 112,
|
||
derivedTrackDeg: 271,
|
||
reportedSpeedMps: 0,
|
||
reportedTrackDeg: 90,
|
||
}), { speedMps: 112, trackDeg: 271 });
|
||
});
|
||
|
||
test('displayedKinematics preserves a derived stop and falls back when unavailable', () => {
|
||
assert.deepEqual(displayedKinematics({
|
||
derivedSpeedMps: 0,
|
||
derivedTrackDeg: 0,
|
||
reportedSpeedMps: 80,
|
||
reportedTrackDeg: 180,
|
||
}), { speedMps: 0, trackDeg: 0 });
|
||
assert.deepEqual(displayedKinematics({
|
||
reportedSpeedMps: 80,
|
||
reportedTrackDeg: 180,
|
||
}), { speedMps: 80, trackDeg: 180 });
|
||
});
|
||
|
||
test('stale coasting follows last source contact without allowing indefinite drift', () => {
|
||
const fix = 1_000_000;
|
||
assert.equal(staleCoastLimitSeconds({ fixEpochMs: fix }), 60);
|
||
assert.equal(staleCoastLimitSeconds({
|
||
fixEpochMs: fix,
|
||
lastContactEpochMs: fix + 85_000,
|
||
}), 145);
|
||
assert.equal(staleCoastLimitSeconds({
|
||
fixEpochMs: fix,
|
||
lastContactEpochMs: fix + 600_000,
|
||
}), 300);
|
||
assert.equal(staleCoastLimitSeconds({
|
||
fixEpochMs: fix,
|
||
lastContactEpochMs: fix - 10_000,
|
||
}), 60);
|
||
});
|
||
|
||
test('courseBetweenCartesians: due north ≈ 0°, due east ≈ 90°', () => {
|
||
const from = cart(0, 0);
|
||
const north = cart(1000 / 111320, 0);
|
||
const east = cart(0, 1000 / (111320 * Math.cos(AUSTIN.lat * Math.PI / 180)));
|
||
assert.ok(Math.abs(norm180(courseBetweenCartesians(from, north) - 0)) < 0.5);
|
||
assert.ok(Math.abs(norm180(courseBetweenCartesians(from, east) - 90)) < 0.5);
|
||
});
|
||
|
||
test('courseBetweenCartesians: sub-threshold chord returns null', () => {
|
||
const from = cart(0, 0);
|
||
const near = cart(5 / 111320, 0); // 5 m — below default 25 m
|
||
assert.equal(courseBetweenCartesians(from, near), null);
|
||
});
|
||
|
||
test('limitCourseStep: passes small deltas, clamps big ones, wraps shortest way', () => {
|
||
assert.equal(limitCourseStep(null, 123, 60, 0.08), 123); // seed
|
||
assert.equal(limitCourseStep(10, 12, 60, 1), 12); // within rate
|
||
assert.equal(limitCourseStep(0, 90, 60, 0.5), 30); // clamped to 30°
|
||
const wrapped = limitCourseStep(350, 10, 60, 1); // +20 through 360, not −340
|
||
assert.ok(Math.abs(norm180(wrapped - 10)) < 1e-9);
|
||
});
|
||
|
||
test('estimateTurnRateDps: 2°/s ramp detected; noise floored; clamped', () => {
|
||
const ramp = [
|
||
{ tSec: 0, trackDeg: 0 }, { tSec: 30, trackDeg: 60 }, { tSec: 60, trackDeg: 120 },
|
||
];
|
||
assert.ok(Math.abs(estimateTurnRateDps(ramp) - 2) < 1e-9);
|
||
const noise = [{ tSec: 0, trackDeg: 0 }, { tSec: 30, trackDeg: 3 }]; // 0.1°/s < floor
|
||
assert.equal(estimateTurnRateDps(noise), 0);
|
||
const wild = [{ tSec: 0, trackDeg: 0 }, { tSec: 10, trackDeg: 300 }]; // −6°/s unwrapped
|
||
assert.equal(estimateTurnRateDps(wild), -4); // clamped
|
||
});
|
||
|
||
test('turnRateFromFixHistory adapts JulianDate history', () => {
|
||
const t0 = Cesium.JulianDate.now();
|
||
const h = [0, 30, 60].map((s, i) => ({
|
||
time: Cesium.JulianDate.addSeconds(t0, s, new Cesium.JulianDate()),
|
||
track: i * 60,
|
||
}));
|
||
assert.ok(Math.abs(turnRateFromFixHistory(h) - 2) < 1e-9);
|
||
});
|
||
|
||
test('arcOffsetEnu: zero turn = straight line; 90° arc hits R,R; endCourse advances', () => {
|
||
const out = { east: 0, north: 0, endCourseDeg: 0 };
|
||
arcOffsetEnu(200, 90, 0, 10, out); // east at 200 m/s for 10 s
|
||
assert.ok(Math.abs(out.east - 2000) < 1e-6 && Math.abs(out.north) < 1e-6);
|
||
assert.equal(out.endCourseDeg, 90);
|
||
|
||
const w = 3; // °/s, dt 30 s → 90° of arc, R = v/ω
|
||
arcOffsetEnu(200, 0, w, 30, out);
|
||
const R = 200 / (w * Math.PI / 180);
|
||
assert.ok(Math.abs(out.east - R) < 1e-6);
|
||
assert.ok(Math.abs(out.north - R) < 1e-6);
|
||
assert.equal(out.endCourseDeg, 90);
|
||
});
|
||
|
||
test('speedRamp: track-only below the gate, chord-only above, ramp between, legacy for unknown', () => {
|
||
assert.equal(speedRamp(0), 0);
|
||
assert.equal(speedRamp(COURSE_TRACK_ONLY_MPS), 0);
|
||
assert.equal(speedRamp(COURSE_CHORD_ONLY_MPS), 1);
|
||
assert.equal(speedRamp(300), 1);
|
||
const mid = speedRamp((COURSE_TRACK_ONLY_MPS + COURSE_CHORD_ONLY_MPS) / 2);
|
||
assert.ok(Math.abs(mid - 0.5) < 1e-9);
|
||
assert.equal(speedRamp(NaN), 1); // unknown speed → legacy chord behavior
|
||
assert.equal(speedRamp(null), 1);
|
||
});
|
||
|
||
test('lerpAngleDeg: wraps the shortest way', () => {
|
||
assert.equal(lerpAngleDeg(10, 30, 0.5), 20);
|
||
assert.equal(lerpAngleDeg(350, 10, 0.5), 0); // through 360, not backward 180
|
||
assert.equal(lerpAngleDeg(350, 10, 0), 350);
|
||
assert.equal(lerpAngleDeg(350, 10, 1), 10);
|
||
});
|
||
|
||
test('courseSlewCapDps: COURSE_MIN_DPS at low speed easing to the fleet cap at cruise', () => {
|
||
assert.equal(courseSlewCapDps(0, 60), COURSE_MIN_DPS);
|
||
assert.equal(courseSlewCapDps(COURSE_TRACK_ONLY_MPS, 60), COURSE_MIN_DPS);
|
||
assert.equal(courseSlewCapDps(COURSE_CHORD_ONLY_MPS, 60), 60);
|
||
assert.equal(courseSlewCapDps(NaN, 60), 60); // unknown speed → legacy cap
|
||
});
|
||
|
||
test('turn-rate guard: hover fix-track jitter manufactures NO turn rate; a real slow turn still does', () => {
|
||
const t0 = Cesium.JulianDate.now();
|
||
const mk = (specs) => specs.map(([s, track, velocity]) => ({
|
||
time: Cesium.JulianDate.addSeconds(t0, s, new Cesium.JulianDate()),
|
||
track, velocity,
|
||
}));
|
||
// Hovering heli: 1 m/s drift, reported track flipping ±45° — used to yield ±°/s
|
||
// that the extrapolation arc integrated into a pirouette. Now 0.
|
||
const hover = mk([[0, 90, 1], [15, 135, 1], [30, 45, 1], [45, 135, 1], [60, 45, 1]]);
|
||
assert.equal(turnRateFromFixHistory(hover), 0);
|
||
// A 25 kt (12.9 m/s) plane really turning 3°/s keeps its estimate (above the floor).
|
||
assert.ok(12.9 > TURN_MIN_SPEED_MPS);
|
||
const slowTurn = mk([[0, 0, 12.9], [15, 45, 12.9], [30, 90, 12.9]]);
|
||
assert.ok(Math.abs(turnRateFromFixHistory(slowTurn) - 3) < 1e-9);
|
||
// estimateTurnRateDps skips only the low-speed intervals when a floor is given.
|
||
const mixed = [
|
||
{ tSec: 0, trackDeg: 0, speedMps: 1 }, { tSec: 15, trackDeg: 90, speedMps: 1 }, // noise interval (skipped)
|
||
{ tSec: 30, trackDeg: 90, speedMps: 100 }, { tSec: 45, trackDeg: 120, speedMps: 100 }, // real 2°/s
|
||
];
|
||
assert.ok(Math.abs(estimateTurnRateDps(mixed, 0.4, 4, 5) - 2) < 1e-9);
|
||
// Samples WITHOUT a speed keep the legacy behavior even with a floor set.
|
||
const noSpeed = [{ tSec: 0, trackDeg: 0 }, { tSec: 30, trackDeg: 60 }];
|
||
assert.ok(Math.abs(estimateTurnRateDps(noSpeed, 0.4, 4, 5) - 2) < 1e-9);
|
||
});
|
||
|
||
test('arcOffsetEnu: backward dt retraces the forward arc (warm-up symmetry)', () => {
|
||
const fwd = { east: 0, north: 0, endCourseDeg: 0 };
|
||
const back = { east: 0, north: 0, endCourseDeg: 0 };
|
||
arcOffsetEnu(200, 30, 2, 20, fwd);
|
||
// Going backward from the arc end at its end-course must land at the start.
|
||
arcOffsetEnu(200, fwd.endCourseDeg, 2, -20, back);
|
||
assert.ok(Math.abs(fwd.east + back.east) < 1e-6);
|
||
assert.ok(Math.abs(fwd.north + back.north) < 1e-6);
|
||
});
|
||
|
||
test('repeated kinematics apply only from a forward synthetic fix', () => {
|
||
const epochMs = 1_800_000_000_000;
|
||
const originalPosition = Cesium.Cartesian3.fromDegrees(-97.7431, 30.2672, 9000);
|
||
const newest = {
|
||
time: Cesium.JulianDate.fromDate(new Date(epochMs)),
|
||
epochMs,
|
||
position: Cesium.Cartesian3.clone(originalPosition),
|
||
velocity: 100,
|
||
track: 90,
|
||
};
|
||
const synthetic = synthesizeForwardKinematicsFix(newest, {
|
||
epochMs: epochMs + 10_000,
|
||
velocity: 220,
|
||
track: 15,
|
||
});
|
||
assert.ok(synthetic);
|
||
assert.equal(newest.velocity, 100);
|
||
assert.equal(newest.track, 90);
|
||
assert.ok(Cesium.Cartesian3.equals(newest.position, originalPosition));
|
||
assert.equal(synthetic.velocity, 220);
|
||
assert.equal(synthetic.track, 15);
|
||
assert.ok(Math.abs(Cesium.Cartesian3.distance(newest.position, synthetic.position) - 1000) < 0.5);
|
||
});
|
||
|
||
test('liftRepeatedGroundFix raises only grounded history and preserves the stored coordinates', () => {
|
||
const newest = {
|
||
time: Cesium.JulianDate.now(),
|
||
epochMs: 123,
|
||
position: Cesium.Cartesian3.fromDegrees(-98.04, 29.71, -30),
|
||
velocity: 0,
|
||
track: 270,
|
||
};
|
||
const lifted = Cesium.Cartesian3.fromDegrees(-97.5, 30.1, 320);
|
||
assert.equal(liftRepeatedGroundFix(newest, lifted, true), true);
|
||
const carto = Cesium.Cartographic.fromCartesian(newest.position);
|
||
assert.ok(Math.abs(Cesium.Math.toDegrees(carto.longitude) - (-98.04)) < 1e-7);
|
||
assert.ok(Math.abs(Cesium.Math.toDegrees(carto.latitude) - 29.71) < 1e-7);
|
||
assert.ok(Math.abs(carto.height - 320) < 1e-5);
|
||
assert.equal(newest.epochMs, 123);
|
||
assert.equal(newest.velocity, 0);
|
||
assert.equal(newest.track, 270);
|
||
|
||
const airborne = { position: Cesium.Cartesian3.fromDegrees(-98.04, 29.71, -30) };
|
||
assert.equal(liftRepeatedGroundFix(airborne, lifted, false), false);
|
||
const lower = Cesium.Cartesian3.fromDegrees(-98.04, 29.71, 100);
|
||
assert.equal(liftRepeatedGroundFix(newest, lower, true), false);
|
||
assert.ok(Math.abs(Cesium.Cartographic.fromCartesian(newest.position).height - 320) < 1e-5);
|
||
});
|
||
|
||
// --- Forward ground projection (display-floor corridor) --------------------
|
||
// While a grounded contact COASTS past its newest fix, the displayed position
|
||
// travels AWAY from that fix. The corridor needs the ground it is heading FOR
|
||
// — and, because the dead-reckon integrates a constant-rate turn, that ground
|
||
// is an ARC whenever the contact is turning. A straight tangent leaves the
|
||
// real path cold.
|
||
|
||
test('projectGroundArcLatLon walks due north for course 0 with no turn', () => {
|
||
const p = projectGroundArcLatLon(30.2, -97.66, 0, 111.32, 0, 10); // 1113.2 m
|
||
assert.ok(Math.abs(p.lat - 30.21) < 1e-4, `lat ${p.lat}`);
|
||
assert.ok(Math.abs(p.lon + 97.66) < 1e-9, 'no easting on a due-north leg');
|
||
});
|
||
|
||
test('projectGroundArcLatLon walks due east for course 90 (longitude scaled by latitude)', () => {
|
||
const p = projectGroundArcLatLon(30.2, -97.66, 90, 100, 0, 10); // 1000 m
|
||
assert.ok(Math.abs(p.lat - 30.2) < 1e-9, 'no northing on a due-east leg');
|
||
const expected = -97.66 + 1000 / (111320 * Math.cos(30.2 * Math.PI / 180));
|
||
assert.ok(Math.abs(p.lon - expected) < 1e-6, `lon ${p.lon} vs ${expected}`);
|
||
});
|
||
|
||
test('projectGroundArcLatLon curves away from the tangent under a sustained turn', () => {
|
||
// 12 m/s, 3 deg/s right turn for 30 s = 90 deg of arc.
|
||
const arc = projectGroundArcLatLon(30.2, -97.66, 0, 12, 3, 30);
|
||
const tangent = projectGroundArcLatLon(30.2, -97.66, 0, 12, 0, 30);
|
||
// The tangent runs 360 m due north; the quarter-circle ends ~229 m north and
|
||
// ~229 m east of the start. Hundreds of metres apart — many cells apart.
|
||
assert.ok(arc.lon > tangent.lon + 0.001, `arc must swing east: ${arc.lon} vs ${tangent.lon}`);
|
||
assert.ok(arc.lat < tangent.lat - 0.001, `arc must fall short north: ${arc.lat} vs ${tangent.lat}`);
|
||
});
|
||
|
||
test('projectGroundArcLatLon turns left for a negative rate', () => {
|
||
const right = projectGroundArcLatLon(30.2, -97.66, 0, 12, 3, 30);
|
||
const left = projectGroundArcLatLon(30.2, -97.66, 0, 12, -3, 30);
|
||
assert.ok(left.lon < -97.66 && right.lon > -97.66, `left ${left.lon} right ${right.lon}`);
|
||
});
|
||
|
||
test('projectGroundArcLatLon returns the origin for a zero-length leg', () => {
|
||
assert.deepEqual(projectGroundArcLatLon(30.2, -97.66, 45, 10, 0, 0), { lat: 30.2, lon: -97.66 });
|
||
assert.deepEqual(projectGroundArcLatLon(30.2, -97.66, 45, 0, 0, 30), { lat: 30.2, lon: -97.66 });
|
||
});
|
||
|
||
test('projectGroundArcLatLon returns the origin for non-finite inputs', () => {
|
||
assert.deepEqual(projectGroundArcLatLon(30.2, -97.66, NaN, 10, 0, 30), { lat: 30.2, lon: -97.66 });
|
||
assert.deepEqual(projectGroundArcLatLon(30.2, -97.66, 45, 10, 0, NaN), { lat: 30.2, lon: -97.66 });
|
||
});
|
||
|
||
test('projectGroundArcLatLon does not blow up at the pole', () => {
|
||
const p = projectGroundArcLatLon(89.9999, 0, 90, 100, 0, 10);
|
||
assert.ok(Number.isFinite(p.lat) && Number.isFinite(p.lon), `got ${JSON.stringify(p)}`);
|
||
});
|
||
|
||
// --- corridorPathLatLon: the path the display will actually walk ------------
|
||
|
||
test('corridorPathLatLon is a straight two-point chord while interpolating', () => {
|
||
// The dead-reckon LERPS between fixes, so the chord IS the display path.
|
||
const path = corridorPathLatLon({
|
||
extrapolating: false,
|
||
displayLat: 30.200, displayLon: -97.660, courseDeg: 0, speedMps: 10, turnRateDps: 3,
|
||
fixLat: 30.203, fixLon: -97.658, lookaheadSec: 60,
|
||
});
|
||
assert.deepEqual(path, [{ lat: 30.2, lon: -97.66 }, { lat: 30.203, lon: -97.658 }]);
|
||
});
|
||
|
||
test('corridorPathLatLon leads FORWARD, past the fix, while coasting', () => {
|
||
const path = corridorPathLatLon({
|
||
extrapolating: true,
|
||
displayLat: 30.200, displayLon: -97.660, courseDeg: 0, speedMps: 10, turnRateDps: 0,
|
||
fixLat: 30.197, fixLon: -97.660, lookaheadSec: 60,
|
||
});
|
||
const end = path[path.length - 1];
|
||
assert.ok(end.lat > 30.200, `must lead the contact, got ${end.lat}`);
|
||
assert.ok(end.lat > 30.197, 'never the backtrail behind the stale fix');
|
||
assert.ok(Math.abs(end.lat - (30.2 + 600 / 111320)) < 1e-5, `end lat ${end.lat}`);
|
||
});
|
||
|
||
test('corridorPathLatLon samples the ARC, not a chord, for a turning coaster', () => {
|
||
const path = corridorPathLatLon({
|
||
extrapolating: true,
|
||
displayLat: 30.200, displayLon: -97.660, courseDeg: 0, speedMps: 12, turnRateDps: 3,
|
||
fixLat: 30.199, fixLon: -97.660, lookaheadSec: 30,
|
||
});
|
||
assert.ok(path.length > 2, 'a turning path needs intermediate samples');
|
||
// Mid-path must bulge off the straight line joining the ends — that bulge is
|
||
// the ground a chord-only corridor would leave cold.
|
||
const a = path[0];
|
||
const z = path[path.length - 1];
|
||
const mid = path[Math.floor(path.length / 2)];
|
||
const t = (mid.lat - a.lat) / (z.lat - a.lat);
|
||
const chordLon = a.lon + (z.lon - a.lon) * t;
|
||
assert.ok(Math.abs(mid.lon - chordLon) > 1e-4,
|
||
`arc midpoint ${mid.lon} should not sit on the chord ${chordLon}`);
|
||
});
|
||
|
||
test('corridorPathLatLon on a stationary coaster collapses to the display point', () => {
|
||
const path = corridorPathLatLon({
|
||
extrapolating: true,
|
||
displayLat: 30.200, displayLon: -97.660, courseDeg: 45, speedMps: 0, lookaheadSec: 60,
|
||
fixLat: 30.200, fixLon: -97.660,
|
||
});
|
||
for (const p of path) assert.deepEqual(p, { lat: 30.2, lon: -97.66 });
|
||
});
|
||
|
||
test('corridorPathLatLon falls back to projection when the fix is unusable', () => {
|
||
const path = corridorPathLatLon({
|
||
extrapolating: false,
|
||
displayLat: 30.200, displayLon: -97.660, courseDeg: 90, speedMps: 10, turnRateDps: 0,
|
||
fixLat: NaN, fixLon: NaN, lookaheadSec: 60,
|
||
});
|
||
assert.ok(path[path.length - 1].lon > -97.660, 'still produces a usable endpoint');
|
||
});
|
||
|
||
test('corridorPathLatLon returns nothing without a display position', () => {
|
||
assert.deepEqual(corridorPathLatLon({
|
||
extrapolating: true, displayLat: NaN, displayLon: -97.66, courseDeg: 0, speedMps: 10, lookaheadSec: 60,
|
||
}), []);
|
||
});
|