mirror of
https://github.com/bilawalsidhu/gods-eye-view.git
synced 2026-11-16 08:12:24 +00:00
The trail no longer joins a day of history into one line. It lifts the pen across a gap of more than 48 minutes, an implied speed over 1,500 kt, or a report without altitude (a parked aircraft within 0.1 nm for under 3 h stays joined), and every history point is kept as its own segment. History never gets an invented altitude. The live end is the path the aircraft was drawn along, thinned to its shape, so it no longer cuts corners through turns; a late report trims what was drawn after it. Old segments are frozen in chunks, so a 2,000-point trail costs about 0.1 ms a frame. A tracked aircraft that misses an update shows STALE and is dimmed. It keeps flying on its last speed and heading for five minutes (at most 1,500 kt), then stops, dimmed, labelled SIGNAL LOST with the time since its last report, and is kept for ten minutes after that report so the camera keeps its target. Aircraft on or near known ground hold at their last report instead of coasting. Military flights follow the same rules.
676 lines
26 KiB
JavaScript
676 lines
26 KiB
JavaScript
// src/data/motionModel.js
|
||
/**
|
||
* Pure display-motion math for the aircraft layers. No layer state, no DOM —
|
||
* importable under `node --test`.
|
||
*
|
||
* Turn-rate estimation + constant-rate-turn arc integration are adapted from
|
||
* skylight (https://github.com/cpaczek/skylight, MIT) shared/src/aim.ts.
|
||
*/
|
||
import * as Cesium from 'cesium';
|
||
|
||
const DEG = Math.PI / 180;
|
||
const _mmCarto = new Cesium.Cartographic();
|
||
const _mmCandidateCarto = new Cesium.Cartographic();
|
||
|
||
export function norm360(deg) {
|
||
return ((deg % 360) + 360) % 360;
|
||
}
|
||
|
||
/**
|
||
* Lift a repeated grounded fix to a newly resolved render floor without
|
||
* manufacturing a new source timestamp or changing its reported lat/lon.
|
||
* Airborne fixes and downward candidates are deliberately left untouched.
|
||
*
|
||
* @param {{position: Cesium.Cartesian3}|null} newest
|
||
* @param {Cesium.Cartesian3|null} candidatePosition
|
||
* @param {boolean} onGround
|
||
* @returns {boolean} True when the stored position was lifted.
|
||
*/
|
||
export function liftRepeatedGroundFix(newest, candidatePosition, onGround) {
|
||
if (!onGround || !newest?.position || !candidatePosition) return false;
|
||
const stored = Cesium.Cartographic.fromCartesian(
|
||
newest.position,
|
||
Cesium.Ellipsoid.WGS84,
|
||
_mmCarto,
|
||
);
|
||
const candidate = Cesium.Cartographic.fromCartesian(
|
||
candidatePosition,
|
||
Cesium.Ellipsoid.WGS84,
|
||
_mmCandidateCarto,
|
||
);
|
||
if (!stored || !candidate || candidate.height <= stored.height) return false;
|
||
Cesium.Cartesian3.fromRadians(
|
||
stored.longitude,
|
||
stored.latitude,
|
||
candidate.height,
|
||
Cesium.Ellipsoid.WGS84,
|
||
newest.position,
|
||
);
|
||
return true;
|
||
}
|
||
|
||
/** Normalize to (−180, 180]. */
|
||
export function norm180(deg) {
|
||
const n = norm360(deg);
|
||
return n > 180 ? n - 360 : n;
|
||
}
|
||
|
||
/**
|
||
* Course over ground (deg from north, clockwise) of the chord from → to,
|
||
* measured in `from`'s local ENU frame. Returns null when the chord is shorter
|
||
* than `minChordM` or the motion is vertical-only (direction unreliable —
|
||
* callers fall back to the reported track).
|
||
*/
|
||
export function courseBetweenCartesians(from, to, minChordM = 25) {
|
||
const dx = to.x - from.x;
|
||
const dy = to.y - from.y;
|
||
const dz = to.z - from.z;
|
||
if (dx * dx + dy * dy + dz * dz < minChordM * minChordM) return null;
|
||
const carto = Cesium.Cartographic.fromCartesian(
|
||
from,
|
||
Cesium.Ellipsoid.WGS84,
|
||
_mmCarto,
|
||
);
|
||
if (!carto) return null;
|
||
const sLat = Math.sin(carto.latitude);
|
||
const cLat = Math.cos(carto.latitude);
|
||
const sLon = Math.sin(carto.longitude);
|
||
const cLon = Math.cos(carto.longitude);
|
||
const e = -sLon * dx + cLon * dy;
|
||
const n = -sLat * cLon * dx - sLat * sLon * dy + cLat * dz;
|
||
if (e * e + n * n < 1) return null; // < 1 m horizontal — vertical-only motion
|
||
return norm360(Math.atan2(e, n) / DEG);
|
||
}
|
||
|
||
// ---------------------------------------------------------------------------
|
||
// Low-speed course tuning (2026-07-03 field test: slow planes + helicopters).
|
||
// The interpolation chord is only trustworthy when the segment actually covers
|
||
// ground: at hover the fix-to-fix chord is a few metres of GPS jitter (its
|
||
// direction a pure random walk), and on a slow tight turn the chord STEPS by
|
||
// the whole per-segment turn at each poll boundary (a 60°/s whip, then frozen).
|
||
// Below the gate the reported per-fix track takes over; below the hold floor
|
||
// nothing is trustworthy and callers keep their previous display course.
|
||
// ---------------------------------------------------------------------------
|
||
|
||
/** At/below this displayed ground speed the course source is the reported
|
||
* track only (chord weight 0). ≈30 kt. */
|
||
export const COURSE_TRACK_ONLY_MPS = 15.4;
|
||
/** At/above this displayed ground speed the course source is the chord only
|
||
* (weight 1) — the regime the owner field-approved; behavior unchanged there.
|
||
* ≈50 kt. */
|
||
export const COURSE_CHORD_ONLY_MPS = 25.7;
|
||
/** Below this displayed speed neither chord nor reported track means anything
|
||
* (hover / GPS drift — a hovering heli's track is velocity-vector noise):
|
||
* callers HOLD their previous display course. ≈3 kt. */
|
||
export const COURSE_HOLD_SPEED_MPS = 1.5;
|
||
/** Course slew cap at/below the track-only gate. Slow aircraft turn a few °/s;
|
||
* whipping their nose at the fleet's 60°/s cap reads as a snap. */
|
||
export const COURSE_MIN_DPS = 12;
|
||
/** Below this fix ground speed, fix-track deltas are GPS-vector noise, not a
|
||
* turn — the turn-rate estimator ignores those intervals (a hovering heli's
|
||
* jitter used to manufacture a fake ±°/s rate that the extrapolation arc then
|
||
* integrated into a slow pirouette). ≈10 kt. */
|
||
export const TURN_MIN_SPEED_MPS = 5;
|
||
|
||
/** 0 at/below COURSE_TRACK_ONLY_MPS, 1 at/above COURSE_CHORD_ONLY_MPS, linear
|
||
* between. Non-finite speed (unknown) → 1, the legacy chord behavior. */
|
||
export function speedRamp(speedMps) {
|
||
if (!Number.isFinite(speedMps)) return 1;
|
||
if (speedMps <= COURSE_TRACK_ONLY_MPS) return 0;
|
||
if (speedMps >= COURSE_CHORD_ONLY_MPS) return 1;
|
||
return (
|
||
(speedMps - COURSE_TRACK_ONLY_MPS) /
|
||
(COURSE_CHORD_ONLY_MPS - COURSE_TRACK_ONLY_MPS)
|
||
);
|
||
}
|
||
|
||
/** Shortest-arc angle lerp: from → to by t (t=0 → from, t=1 → to). Also the
|
||
* course blender (blend chord over track by the speed ramp). */
|
||
export function lerpAngleDeg(fromDeg, toDeg, t) {
|
||
return norm360(fromDeg + norm180(toDeg - fromDeg) * t);
|
||
}
|
||
|
||
/** Speed-scaled slew cap for limitCourseStep: COURSE_MIN_DPS at low speed
|
||
* easing to `maxDps` at cruise, so slow aircraft glide through segment
|
||
* boundaries instead of whipping at the fleet cap. */
|
||
export function courseSlewCapDps(speedMps, maxDps, minDps = COURSE_MIN_DPS) {
|
||
return minDps + (maxDps - minDps) * speedRamp(speedMps);
|
||
}
|
||
|
||
/**
|
||
* Resolve the kinematics that belong to the position currently being drawn.
|
||
*
|
||
* A feed may transiently report zero or missing ground speed while consecutive
|
||
* fixes still describe clear movement. The delayed renderer already derives a
|
||
* matching segment speed and course from those fixes, so tracking and cockpit
|
||
* consumers should prefer that pair over stale poll fields.
|
||
*/
|
||
export function displayedKinematics({
|
||
derivedSpeedMps,
|
||
derivedTrackDeg,
|
||
reportedSpeedMps,
|
||
reportedTrackDeg,
|
||
} = {}) {
|
||
return {
|
||
speedMps: Number.isFinite(derivedSpeedMps)
|
||
? Math.max(0, derivedSpeedMps)
|
||
: Number.isFinite(reportedSpeedMps)
|
||
? Math.max(0, reportedSpeedMps)
|
||
: null,
|
||
trackDeg: Number.isFinite(derivedTrackDeg)
|
||
? derivedTrackDeg
|
||
: Number.isFinite(reportedTrackDeg)
|
||
? reportedTrackDeg
|
||
: null,
|
||
};
|
||
}
|
||
|
||
/**
|
||
* Bound forward coasting by the freshest source contact, not only by the last
|
||
* position-fix epoch.
|
||
*
|
||
* ADS-B state vectors often keep receiving velocity/track messages after the
|
||
* last position timestamp. A fixed "60 s after position" ceiling therefore
|
||
* alternates between a frozen icon and a catch-up jump even while the source
|
||
* still hears the aircraft. This horizon permits one grace window after the
|
||
* latest actual contact, while `maximumSec` prevents a cached/stale feed from
|
||
* drifting an aircraft indefinitely.
|
||
*/
|
||
export function staleCoastLimitSeconds({
|
||
fixEpochMs,
|
||
lastContactEpochMs,
|
||
minimumSec = 60,
|
||
contactGraceSec = 60,
|
||
maximumSec = 300,
|
||
} = {}) {
|
||
const floor = Math.max(0, Number.isFinite(minimumSec) ? minimumSec : 60);
|
||
const ceiling = Math.max(
|
||
floor,
|
||
Number.isFinite(maximumSec) ? maximumSec : 300,
|
||
);
|
||
if (!Number.isFinite(fixEpochMs) || !Number.isFinite(lastContactEpochMs))
|
||
return floor;
|
||
const contactLeadSec = Math.max(0, (lastContactEpochMs - fixEpochMs) / 1000);
|
||
const grace = Math.max(
|
||
0,
|
||
Number.isFinite(contactGraceSec) ? contactGraceSec : 60,
|
||
);
|
||
return Math.min(ceiling, Math.max(floor, contactLeadSec + grace));
|
||
}
|
||
|
||
/** @constant {number} Dead reckoning never moves a contact faster than this
|
||
* (1,500 kt, the trail's continuity limit), so a corrupt reported speed
|
||
* cannot fly a coasting plane hundreds of kilometres. */
|
||
export const COAST_MAX_SPEED_MPS = (1_500 * 1852) / 3600;
|
||
|
||
/** @constant {number} A contact with no fresh report coasts at most this long past it, whatever its source limit; there is no distance cap. */
|
||
export const STALE_COAST_MAX_SEC = 300;
|
||
|
||
/** @constant {number} A report this close to a known ground height holds
|
||
* instead of coasting. */
|
||
export const STALE_HOLD_MAX_HEIGHT_M = 150;
|
||
|
||
/**
|
||
* Whether a contact whose reports have stopped holds at its last reported
|
||
* position instead of coasting: it reported on the ground, or a known ground
|
||
* floor puts it within STALE_HOLD_MAX_HEIGHT_M of the ground (landing or
|
||
* taking off). A plane that low has no evidence of being anywhere but where
|
||
* it was last heard. Where the ground is unknown, barometric altitude is not
|
||
* read as height above it; the contact coasts under the stale cap.
|
||
* @param {object} report
|
||
* @param {boolean} [report.onGround]
|
||
* @param {number} [report.heightM] - Render height (ellipsoidal metres).
|
||
* @param {number} [report.groundM] - Ellipsoidal ground under the report, if known.
|
||
* @returns {boolean}
|
||
*/
|
||
export function staleHoldsAtLastReport({ onGround, heightM, groundM } = {}) {
|
||
if (onGround === true) return true;
|
||
return (
|
||
Number.isFinite(heightM) &&
|
||
Number.isFinite(groundM) &&
|
||
heightM - groundM < STALE_HOLD_MAX_HEIGHT_M
|
||
);
|
||
}
|
||
|
||
/**
|
||
* The hold-or-coast decision for a report, made once and kept on the report,
|
||
* so a ground floor that resolves while the contact coasts cannot flip it and
|
||
* snap the icon back. Height and position come from the report itself.
|
||
* @param {{position: Cesium.Cartesian3, staleHolds?: boolean}} fix - History
|
||
* fix; `staleHolds` is stored on it.
|
||
* @param {object} args
|
||
* @param {boolean} [args.onGround] - The contact's ground flag.
|
||
* @param {(latDeg: number, lonDeg: number) => number|null} [args.cachedGroundFloor]
|
||
* @returns {boolean}
|
||
*/
|
||
export function staleHoldsForFix(fix, { onGround, cachedGroundFloor } = {}) {
|
||
if (typeof fix.staleHolds === 'boolean') return fix.staleHolds;
|
||
const carto = Cesium.Cartographic.fromCartesian(
|
||
fix.position,
|
||
Cesium.Ellipsoid.WGS84,
|
||
_holdCarto,
|
||
);
|
||
const groundM =
|
||
carto && typeof cachedGroundFloor === 'function'
|
||
? cachedGroundFloor(
|
||
Cesium.Math.toDegrees(carto.latitude),
|
||
Cesium.Math.toDegrees(carto.longitude),
|
||
)
|
||
: null;
|
||
fix.staleHolds = staleHoldsAtLastReport({
|
||
onGround,
|
||
heightM: carto?.height,
|
||
groundM,
|
||
});
|
||
return fix.staleHolds;
|
||
}
|
||
|
||
const _holdCarto = new Cesium.Cartographic();
|
||
|
||
/**
|
||
* Seconds a contact coasts past `newest` at a display time `elapsedSec`
|
||
* after it, counting any coast a synthetic fix already took since the real
|
||
* report it was built from (`reportEpochMs`), so a chain of synthetic fixes
|
||
* never coasts further than one report would.
|
||
* @param {object} args
|
||
* @param {{epochMs: number, reportEpochMs?: number, velocity?: number}} args.newest
|
||
* @param {number} args.elapsedSec - Display time past `newest`.
|
||
* @param {number} args.coastLimitSec - Source limit past `newest`.
|
||
* @param {boolean} args.holds
|
||
* @returns {number}
|
||
*/
|
||
export function staleCoastSecondsFrom({
|
||
newest,
|
||
elapsedSec,
|
||
coastLimitSec,
|
||
holds,
|
||
}) {
|
||
const alreadySec = Number.isFinite(newest?.reportEpochMs)
|
||
? Math.max(0, (newest.epochMs - newest.reportEpochMs) / 1000)
|
||
: 0;
|
||
return Math.max(
|
||
0,
|
||
staleCoastSeconds({
|
||
elapsedSec: elapsedSec + alreadySec,
|
||
coastLimitSec: coastLimitSec + alreadySec,
|
||
holds,
|
||
}) - alreadySec,
|
||
);
|
||
}
|
||
|
||
/** @constant {number} A coasted position sits at least this far (m) above the ground. */
|
||
export const COAST_FLOOR_LIFT_M = 1.5;
|
||
|
||
/**
|
||
* Lift a coasted position to COAST_FLOOR_LIFT_M above the ground where the
|
||
* ground is known; a coast runs level from the report and terrain may rise
|
||
* under it.
|
||
* @param {Cesium.Cartesian3} position - Mutated in place.
|
||
* @param {(latDeg: number, lonDeg: number) => number|null} [cachedGroundFloor]
|
||
* @returns {Cesium.Cartesian3}
|
||
*/
|
||
export function floorCoastedPosition(position, cachedGroundFloor) {
|
||
if (typeof cachedGroundFloor !== 'function') return position;
|
||
const carto = Cesium.Cartographic.fromCartesian(
|
||
position,
|
||
Cesium.Ellipsoid.WGS84,
|
||
_coastCarto,
|
||
);
|
||
if (!carto) return position;
|
||
const ground = cachedGroundFloor(
|
||
Cesium.Math.toDegrees(carto.latitude),
|
||
Cesium.Math.toDegrees(carto.longitude),
|
||
);
|
||
if (!Number.isFinite(ground) || carto.height >= ground + COAST_FLOOR_LIFT_M)
|
||
return position;
|
||
return Cesium.Cartesian3.fromRadians(
|
||
carto.longitude,
|
||
carto.latitude,
|
||
ground + COAST_FLOOR_LIFT_M,
|
||
Cesium.Ellipsoid.WGS84,
|
||
position,
|
||
);
|
||
}
|
||
|
||
const _coastCarto = new Cesium.Cartographic();
|
||
|
||
/**
|
||
* Seconds to coast past the last report: the elapsed time, bounded by the
|
||
* source's coast limit and STALE_COAST_MAX_SEC; none for a contact that
|
||
* holds.
|
||
* @param {object} args
|
||
* @param {number} args.elapsedSec - Display time past the last report.
|
||
* @param {number} args.coastLimitSec - From `staleCoastLimitSeconds`.
|
||
* @param {boolean} args.holds - From `staleHoldsAtLastReport`.
|
||
* @returns {number}
|
||
*/
|
||
export function staleCoastSeconds({ elapsedSec, coastLimitSec, holds }) {
|
||
if (holds || !(elapsedSec > 0)) return 0;
|
||
return Math.max(0, Math.min(elapsedSec, coastLimitSec, STALE_COAST_MAX_SEC));
|
||
}
|
||
|
||
/**
|
||
* Shortest-arc rate limiter: step `prevDeg` toward `targetDeg` by at most
|
||
* `maxDegPerSec * dtSec`. Seeds directly on the target when prev is null.
|
||
* Smooths the once-per-fix course step at interpolation-segment boundaries
|
||
* without lagging real turns (real aircraft turn ≤ ~4°/s; limiter default 60°/s).
|
||
*/
|
||
export function limitCourseStep(prevDeg, targetDeg, maxDegPerSec, dtSec) {
|
||
if (prevDeg == null || !Number.isFinite(prevDeg)) return norm360(targetDeg);
|
||
const d = norm180(targetDeg - prevDeg);
|
||
const maxStep = Math.max(0, maxDegPerSec * Math.max(0, dtSec));
|
||
const step = Math.abs(d) <= maxStep ? d : Math.sign(d) * maxStep;
|
||
return norm360(prevDeg + step);
|
||
}
|
||
|
||
/**
|
||
* Turn-rate estimate (deg/s) from track samples, with angle unwrapping.
|
||
* Below `noiseFloorDps` returns 0 (treat as straight); clamped to ±maxDps
|
||
* (standard-rate turn is 3°/s). dt sanity window matches our 15–30 s poll
|
||
* cadence (2..120 s). When `minSpeedMps` > 0, intervals whose endpoints carry
|
||
* a FINITE speed below it are skipped — at hover/taxi speed the reported track
|
||
* is GPS-vector noise, not a turn (samples without a speed keep the legacy
|
||
* behavior: no way to tell, so they still count).
|
||
*/
|
||
export function estimateTurnRateDps(
|
||
samples,
|
||
noiseFloorDps = 0.4,
|
||
maxDps = 4,
|
||
minSpeedMps = 0,
|
||
) {
|
||
if (!samples || samples.length < 2) return 0;
|
||
let sum = 0;
|
||
let n = 0;
|
||
for (let i = 1; i < samples.length; i++) {
|
||
const dt = samples[i].tSec - samples[i - 1].tSec;
|
||
if (dt < 2 || dt > 120) continue;
|
||
if (
|
||
!Number.isFinite(samples[i].trackDeg) ||
|
||
!Number.isFinite(samples[i - 1].trackDeg)
|
||
)
|
||
continue;
|
||
if (minSpeedMps > 0) {
|
||
const v0 = samples[i - 1].speedMps;
|
||
const v1 = samples[i].speedMps;
|
||
if (
|
||
(Number.isFinite(v0) && v0 < minSpeedMps) ||
|
||
(Number.isFinite(v1) && v1 < minSpeedMps)
|
||
)
|
||
continue;
|
||
}
|
||
sum += norm180(samples[i].trackDeg - samples[i - 1].trackDeg) / dt;
|
||
n += 1;
|
||
}
|
||
if (!n) return 0;
|
||
const rate = sum / n;
|
||
if (Math.abs(rate) < noiseFloorDps) return 0;
|
||
return Math.max(-maxDps, Math.min(maxDps, rate));
|
||
}
|
||
|
||
/** Adapter: fix history [{time: JulianDate, track, velocity}] →
|
||
* estimateTurnRateDps, guarded by TURN_MIN_SPEED_MPS (low-speed fix-track
|
||
* jitter must not manufacture a turn rate). */
|
||
export function turnRateFromFixHistory(history) {
|
||
if (!history || history.length < 2) return 0;
|
||
const t0 = history[0].time;
|
||
const samples = [];
|
||
for (const s of history) {
|
||
if (!Number.isFinite(s.track)) continue;
|
||
samples.push({
|
||
tSec: Cesium.JulianDate.secondsDifference(s.time, t0),
|
||
trackDeg: s.track,
|
||
speedMps: s.velocity,
|
||
});
|
||
}
|
||
return estimateTurnRateDps(samples, undefined, undefined, TURN_MIN_SPEED_MPS);
|
||
}
|
||
|
||
/** @constant {number} Metres per degree of latitude (spherical approximation). */
|
||
const METRES_PER_DEG = 111320;
|
||
/** @constant {number} Ground distance between samples along a projected arc
|
||
* (m). Half a coarse floor cell: the corridor's cell walk joins consecutive
|
||
* samples with a straight chord, and at this spacing the chord's departure
|
||
* from the arc is centimetres even on a tight taxi turn. A FIXED sample count
|
||
* cannot promise that — its spacing grows with speed, so a fast contact's
|
||
* chords cut corners off its own path. */
|
||
export const CORRIDOR_SAMPLE_SPACING_M = 55;
|
||
/** @constant {number} Longest arc a single corridor projects (m). The cell
|
||
* budget truncates at ~12 cells ≈ 1.3 km anyway, so projecting further just
|
||
* builds samples the walk will discard. Ground past this is picked up on a
|
||
* later poll as the display advances into it — the same honest truncation the
|
||
* straight walk makes. */
|
||
export const CORRIDOR_MAX_LENGTH_M = 1300;
|
||
/** Scratch for the shared arc integrator (see arcOffsetEnu). */
|
||
const _scratchProjectArc = { east: 0, north: 0, endCourseDeg: 0 };
|
||
|
||
/**
|
||
* Where a ground contact will be after `dtSec`, using the SAME kinematics the
|
||
* dead-reckon uses: `arcOffsetEnu`, which integrates a constant-rate turn
|
||
* rather than running a straight tangent. A taxiing contact in a sustained
|
||
* turn leaves the tangent within a few hundred metres, so a straight
|
||
* projection would aim the display-floor corridor at ground it never crosses
|
||
* while its actual arc stays cold.
|
||
*
|
||
* Flat-earth ENU→lat/lon on purpose: the only consumer quantises the result to
|
||
* ~111 m cells over ≤1 km legs, where the spherical error is centimetres. The
|
||
* TURN, not the earth model, is what mattered.
|
||
*
|
||
* @param {number} lat @param {number} lon - Start, degrees.
|
||
* @param {number} courseDeg - Instantaneous course (0 = north, 90 = east).
|
||
* @param {number} speedMps - Ground speed.
|
||
* @param {number} turnRateDps - Signed turn rate; 0 gives a straight leg.
|
||
* @param {number} dtSec - Seconds ahead; 0 or negative returns the start.
|
||
* @returns {{lat: number, lon: number}} Projected coordinate, degrees.
|
||
*/
|
||
export function projectGroundArcLatLon(
|
||
lat,
|
||
lon,
|
||
courseDeg,
|
||
speedMps,
|
||
turnRateDps,
|
||
dtSec,
|
||
) {
|
||
if (!Number.isFinite(lat) || !Number.isFinite(lon)) return { lat, lon };
|
||
if (
|
||
!Number.isFinite(courseDeg) ||
|
||
!Number.isFinite(speedMps) ||
|
||
!Number.isFinite(dtSec) ||
|
||
speedMps <= 0 ||
|
||
dtSec <= 0
|
||
) {
|
||
return { lat, lon };
|
||
}
|
||
const off = arcOffsetEnu(
|
||
speedMps,
|
||
courseDeg,
|
||
turnRateDps || 0,
|
||
dtSec,
|
||
_scratchProjectArc,
|
||
);
|
||
// Guard the pole: cos(lat) → 0 would send the longitude to infinity.
|
||
const cosLat = Math.max(Math.cos(lat * DEG), 1e-6);
|
||
return {
|
||
lat: lat + off.north / METRES_PER_DEG,
|
||
lon: lon + off.east / (METRES_PER_DEG * cosLat),
|
||
};
|
||
}
|
||
|
||
/**
|
||
* The PATH a grounded contact's display is about to walk — the ground its floor
|
||
* needs to be warm for, as a polyline rather than a single endpoint.
|
||
*
|
||
* Two regimes, matching what the dead-reckon actually does:
|
||
* - INTERPOLATING: the display LERPS a straight chord between two fixes, so a
|
||
* two-point corridor to the newest fix is exact.
|
||
* - EXTRAPOLATING (coasting past the newest fix, or the pre-history warm-up):
|
||
* the display integrates a constant-rate turn away from that fix. A single
|
||
* projected endpoint would still be joined to the start by a straight chord,
|
||
* so a sustained turn's arc — where the contact actually goes — would be
|
||
* left cold. Sampling the arc keeps corridor and display on one path.
|
||
*
|
||
* @param {object} p
|
||
* @param {boolean} p.extrapolating - Whether the display position was extrapolated.
|
||
* @param {number} p.displayLat @param {number} p.displayLon - Where it renders now.
|
||
* @param {number} p.courseDeg - Course of the displayed motion.
|
||
* @param {number} p.speedMps - Displayed ground speed.
|
||
* @param {number} [p.turnRateDps] - The contact's estimated turn rate.
|
||
* @param {number} p.fixLat @param {number} p.fixLon - The newest fix.
|
||
* @param {number} p.lookaheadSec - How far ahead to project when extrapolating.
|
||
* The sample COUNT derives from the arc's length so spacing stays at
|
||
* CORRIDOR_SAMPLE_SPACING_M whatever the speed, and the arc is truncated at
|
||
* CORRIDOR_MAX_LENGTH_M rather than thinned — the same discipline the straight
|
||
* walk uses. A fixed count silently widened its spacing as speed rose, and the
|
||
* chords between widely spaced samples cut corners off the contact's own path,
|
||
* leaving cells it genuinely occupies cold.
|
||
*
|
||
* @returns {Array<{lat: number, lon: number}>} Polyline, display position first.
|
||
*/
|
||
export function corridorPathLatLon({
|
||
extrapolating,
|
||
displayLat,
|
||
displayLon,
|
||
courseDeg,
|
||
speedMps,
|
||
turnRateDps = 0,
|
||
fixLat,
|
||
fixLon,
|
||
lookaheadSec,
|
||
}) {
|
||
if (!Number.isFinite(displayLat) || !Number.isFinite(displayLon)) return [];
|
||
const start = { lat: displayLat, lon: displayLon };
|
||
if (!extrapolating && Number.isFinite(fixLat) && Number.isFinite(fixLon)) {
|
||
// The dead-reckon LERPS a straight chord between two fixes, so two points
|
||
// ARE the display path here; the corridor's own walk fills the cells.
|
||
return [start, { lat: fixLat, lon: fixLon }];
|
||
}
|
||
const speed = Number.isFinite(speedMps) ? speedMps : 0;
|
||
const seconds = Number.isFinite(lookaheadSec) ? lookaheadSec : 0;
|
||
if (speed <= 0 || seconds <= 0) return [start];
|
||
const cappedSec = Math.min(seconds, CORRIDOR_MAX_LENGTH_M / speed);
|
||
const arcLengthM = speed * cappedSec;
|
||
const n = Math.max(1, Math.ceil(arcLengthM / CORRIDOR_SAMPLE_SPACING_M));
|
||
const out = [start];
|
||
for (let i = 1; i <= n; i++) {
|
||
out.push(
|
||
projectGroundArcLatLon(
|
||
displayLat,
|
||
displayLon,
|
||
courseDeg,
|
||
speed,
|
||
turnRateDps,
|
||
cappedSec * (i / n),
|
||
),
|
||
);
|
||
}
|
||
return out;
|
||
}
|
||
|
||
/**
|
||
* Dead-reckon offset in the local ENU frame: along-track at ground speed,
|
||
* integrating a constant-rate turn when turnRateDps is significant. Writes
|
||
* {east, north, endCourseDeg} into `result` (no allocation) and returns it.
|
||
* Works for negative dt (backward extrapolation — the warm-up path).
|
||
*/
|
||
export function arcOffsetEnu(speedMps, trackDeg, turnRateDps, dtSec, result) {
|
||
const tr = trackDeg * DEG;
|
||
const w = (turnRateDps || 0) * DEG; // rad/s
|
||
if (Math.abs(w) < 1e-4) {
|
||
result.east = speedMps * Math.sin(tr) * dtSec;
|
||
result.north = speedMps * Math.cos(tr) * dtSec;
|
||
result.endCourseDeg = norm360(trackDeg);
|
||
return result;
|
||
}
|
||
result.east = (speedMps / w) * (Math.cos(tr) - Math.cos(tr + w * dtSec));
|
||
result.north = (speedMps / w) * (Math.sin(tr + w * dtSec) - Math.sin(tr));
|
||
result.endCourseDeg = norm360(trackDeg + turnRateDps * dtSec);
|
||
return result;
|
||
}
|
||
|
||
const _forwardFixArc = { east: 0, north: 0, endCourseDeg: 0 };
|
||
const _forwardFixEnu = new Cesium.Matrix4();
|
||
const _forwardFixOffset = new Cesium.Cartesian3();
|
||
|
||
/**
|
||
* Create a forward-only synthetic fix when a repeated position epoch carries
|
||
* newer velocity or course data. The prior fix remains immutable: its own
|
||
* kinematics project it to `epochMs`, and only the returned fix adopts the new
|
||
* kinematics. This prevents a late course change from reprojecting the entire
|
||
* stale interval and snapping the rendered contact by kilometres.
|
||
*
|
||
* The same stale rules as the display apply: no fix for a contact that holds
|
||
* at its report (`staleHoldsForFix`, decided before calling), a coast of at
|
||
* most STALE_COAST_MAX_SEC counted from the real report
|
||
* (`reportEpochMs` carries it along a chain), and never below known ground.
|
||
* The result is a motion fix only, never a report for the trail.
|
||
*
|
||
* @param {{time: Cesium.JulianDate, epochMs?: number, position: Cesium.Cartesian3,
|
||
* velocity?: number, track?: number, staleHolds?: boolean, reportEpochMs?: number}} newest
|
||
* Existing immutable history fix.
|
||
* @param {{epochMs: number, velocity: number, track: number, turnRateDps?: number,
|
||
* cachedGroundFloor?: Function}} next
|
||
* New kinematics and the wall/source epoch at which they become authoritative.
|
||
* @returns {{time: Cesium.JulianDate, epochMs: number, position: Cesium.Cartesian3,
|
||
* velocity: number, track: number, staleHolds?: boolean, reportEpochMs: number}|null}
|
||
* Forward synthetic fix, or null.
|
||
*/
|
||
export function synthesizeForwardKinematicsFix(newest, next = {}) {
|
||
if (!newest?.position || !newest?.time || !Number.isFinite(next.epochMs))
|
||
return null;
|
||
const startEpochMs = Number.isFinite(newest.epochMs)
|
||
? newest.epochMs
|
||
: Cesium.JulianDate.toDate(newest.time).getTime();
|
||
if (!Number.isFinite(startEpochMs) || next.epochMs <= startEpochMs)
|
||
return null;
|
||
if (newest.staleHolds === true) return null;
|
||
|
||
const previousVelocity = Number.isFinite(newest.velocity)
|
||
? Math.max(0, newest.velocity)
|
||
: 0;
|
||
const previousTrack = Number.isFinite(newest.track) ? newest.track : 0;
|
||
const dtSec = staleCoastSecondsFrom({
|
||
newest: { epochMs: startEpochMs, reportEpochMs: newest.reportEpochMs },
|
||
elapsedSec: (next.epochMs - startEpochMs) / 1000,
|
||
coastLimitSec: Infinity,
|
||
holds: false,
|
||
});
|
||
arcOffsetEnu(
|
||
previousVelocity,
|
||
previousTrack,
|
||
Number.isFinite(next.turnRateDps) ? next.turnRateDps : 0,
|
||
dtSec,
|
||
_forwardFixArc,
|
||
);
|
||
Cesium.Cartesian3.fromElements(
|
||
_forwardFixArc.east,
|
||
_forwardFixArc.north,
|
||
0,
|
||
_forwardFixOffset,
|
||
);
|
||
const enu = Cesium.Transforms.eastNorthUpToFixedFrame(
|
||
newest.position,
|
||
Cesium.Ellipsoid.WGS84,
|
||
_forwardFixEnu,
|
||
);
|
||
const position = floorCoastedPosition(
|
||
Cesium.Matrix4.multiplyByPoint(
|
||
enu,
|
||
_forwardFixOffset,
|
||
new Cesium.Cartesian3(),
|
||
),
|
||
next.cachedGroundFloor,
|
||
);
|
||
return {
|
||
time: Cesium.JulianDate.fromDate(new Date(next.epochMs)),
|
||
epochMs: next.epochMs,
|
||
reportEpochMs: Number.isFinite(newest.reportEpochMs)
|
||
? newest.reportEpochMs
|
||
: startEpochMs,
|
||
staleHolds: newest.staleHolds,
|
||
position,
|
||
velocity: Number.isFinite(next.velocity)
|
||
? Math.max(0, next.velocity)
|
||
: previousVelocity,
|
||
track: Number.isFinite(next.track) ? next.track : previousTrack,
|
||
};
|
||
}
|