596 lines
23 KiB
TypeScript
596 lines
23 KiB
TypeScript
/** Deterministic, renderer-independent fixed-wing flight over California. */
|
|
|
|
const EARTH_RADIUS_M = 6_371_000;
|
|
const TWO_PI = Math.PI * 2;
|
|
|
|
export type AircraftControlMode = "assisted" | "manual";
|
|
export type AircraftModeRequest = "none" | AircraftControlMode;
|
|
|
|
export interface AircraftGeographicPoint {
|
|
lat: number;
|
|
lng: number;
|
|
}
|
|
|
|
export interface AircraftWaypoint extends AircraftGeographicPoint {
|
|
id: string;
|
|
altitudeM: number;
|
|
}
|
|
|
|
export interface AircraftActionSnapshot {
|
|
throttle: number;
|
|
yaw: number;
|
|
pitch: number;
|
|
roll: number;
|
|
modeRequest: AircraftModeRequest;
|
|
reset: boolean;
|
|
}
|
|
|
|
export const NEUTRAL_AIRCRAFT_ACTIONS: Readonly<AircraftActionSnapshot> = Object.freeze({
|
|
throttle: 0,
|
|
yaw: 0,
|
|
pitch: 0,
|
|
roll: 0,
|
|
modeRequest: "none",
|
|
reset: false,
|
|
});
|
|
|
|
export interface CaliforniaFlightEnvelope {
|
|
minLat: number;
|
|
maxLat: number;
|
|
minLng: number;
|
|
maxLng: number;
|
|
minAltitudeM: number;
|
|
maxAltitudeM: number;
|
|
}
|
|
|
|
export const DEFAULT_CALIFORNIA_FLIGHT_ENVELOPE: Readonly<CaliforniaFlightEnvelope> =
|
|
Object.freeze({
|
|
minLat: 32.4,
|
|
maxLat: 42.1,
|
|
minLng: -124.6,
|
|
maxLng: -114.0,
|
|
minAltitudeM: 75,
|
|
maxAltitudeM: 6_000,
|
|
});
|
|
|
|
export interface AircraftControllerOptions {
|
|
initialPosition?: Partial<AircraftGeographicPoint>;
|
|
initialAltitudeM?: number;
|
|
initialHeadingDeg?: number;
|
|
initialSpeedMps?: number;
|
|
mode?: AircraftControlMode;
|
|
route?: readonly AircraftWaypoint[];
|
|
envelope?: CaliforniaFlightEnvelope;
|
|
minimumSpeedMps?: number;
|
|
maximumSpeedMps?: number;
|
|
assistedCruiseMps?: number;
|
|
assistedAltitudeM?: number;
|
|
/** Deterministic scenario wind, positive north/east in metres per second. */
|
|
windNorthMps?: number;
|
|
windEastMps?: number;
|
|
/** Deterministic sinusoidal gust amplitude. Zero disables turbulence. */
|
|
turbulenceMps?: number;
|
|
stallSpeedMps?: number;
|
|
batteryCapacityWh?: number;
|
|
/** Terrain/runway elevation callback used for ground contact and landing. */
|
|
terrainElevationM?: (lat: number, lng: number) => number;
|
|
fixedStepSeconds?: number;
|
|
maxFrameDeltaSeconds?: number;
|
|
}
|
|
|
|
export interface AircraftControllerState extends AircraftGeographicPoint {
|
|
altitudeM: number;
|
|
headingDeg: number;
|
|
pitchDeg: number;
|
|
rollDeg: number;
|
|
speedMps: number;
|
|
verticalSpeedMps: number;
|
|
mode: AircraftControlMode;
|
|
routeWaypointIndex: number;
|
|
routeWaypointId: string | null;
|
|
throttle: number;
|
|
yawInput: number;
|
|
pitchInput: number;
|
|
rollInput: number;
|
|
fanRadians: number;
|
|
angleOfAttackDeg: number;
|
|
liftCoefficient: number;
|
|
loadFactorG: number;
|
|
stalled: boolean;
|
|
windNorthMps: number;
|
|
windEastMps: number;
|
|
batteryWh: number;
|
|
energyUsedWh: number;
|
|
groundClearanceM: number;
|
|
onGround: boolean;
|
|
hardLanding: boolean;
|
|
envelopeContact: boolean;
|
|
elapsedSteps: number;
|
|
}
|
|
|
|
export interface AircraftControllerSnapshot extends AircraftControllerState {}
|
|
|
|
export interface TimedAircraftInputFrame {
|
|
steps: number;
|
|
actions?: Partial<AircraftActionSnapshot>;
|
|
}
|
|
|
|
export interface AircraftReplayResult {
|
|
trajectory: readonly AircraftControllerSnapshot[];
|
|
final: AircraftControllerSnapshot;
|
|
}
|
|
|
|
export interface AircraftCameraPose {
|
|
position: AircraftGeographicPoint & { altitudeM: number };
|
|
target: AircraftGeographicPoint & { altitudeM: number };
|
|
rollDeg: number;
|
|
}
|
|
|
|
interface ResolvedOptions {
|
|
initialPosition: AircraftGeographicPoint;
|
|
initialAltitudeM: number;
|
|
initialHeadingDeg: number;
|
|
initialSpeedMps: number;
|
|
mode: AircraftControlMode;
|
|
route: readonly AircraftWaypoint[];
|
|
envelope: CaliforniaFlightEnvelope;
|
|
minimumSpeedMps: number;
|
|
maximumSpeedMps: number;
|
|
assistedCruiseMps: number;
|
|
assistedAltitudeM: number;
|
|
windNorthMps: number;
|
|
windEastMps: number;
|
|
turbulenceMps: number;
|
|
stallSpeedMps: number;
|
|
batteryCapacityWh: number;
|
|
terrainElevationM: (lat: number, lng: number) => number;
|
|
fixedStepSeconds: number;
|
|
maxFrameDeltaSeconds: number;
|
|
}
|
|
|
|
function clamp(value: number, min: number, max: number): number {
|
|
return Math.max(min, Math.min(max, value));
|
|
}
|
|
|
|
function finiteOr(value: number | undefined, fallback: number): number {
|
|
return typeof value === "number" && Number.isFinite(value) ? value : fallback;
|
|
}
|
|
|
|
function moveToward(value: number, target: number, maximumDelta: number): number {
|
|
if (value < target) return Math.min(value + maximumDelta, target);
|
|
if (value > target) return Math.max(value - maximumDelta, target);
|
|
return value;
|
|
}
|
|
|
|
function wrapDegrees(value: number): number {
|
|
return ((value % 360) + 360) % 360;
|
|
}
|
|
|
|
function signedAngleDegrees(from: number, to: number): number {
|
|
return ((to - from + 540) % 360) - 180;
|
|
}
|
|
|
|
function distanceM(a: AircraftGeographicPoint, b: AircraftGeographicPoint): number {
|
|
const mean = ((a.lat + b.lat) / 2) * Math.PI / 180;
|
|
const north = (b.lat - a.lat) * Math.PI / 180 * EARTH_RADIUS_M;
|
|
const east = (b.lng - a.lng) * Math.PI / 180 * Math.cos(mean) * EARTH_RADIUS_M;
|
|
return Math.hypot(north, east);
|
|
}
|
|
|
|
function bearingDeg(a: AircraftGeographicPoint, b: AircraftGeographicPoint): number {
|
|
const mean = ((a.lat + b.lat) / 2) * Math.PI / 180;
|
|
return wrapDegrees(Math.atan2((b.lng - a.lng) * Math.cos(mean), b.lat - a.lat) * 180 / Math.PI);
|
|
}
|
|
|
|
function checkedEnvelope(value: CaliforniaFlightEnvelope | undefined): CaliforniaFlightEnvelope {
|
|
const envelope = { ...(value ?? DEFAULT_CALIFORNIA_FLIGHT_ENVELOPE) };
|
|
if (
|
|
!Number.isFinite(envelope.minLat) || !Number.isFinite(envelope.maxLat) ||
|
|
!Number.isFinite(envelope.minLng) || !Number.isFinite(envelope.maxLng) ||
|
|
!Number.isFinite(envelope.minAltitudeM) || !Number.isFinite(envelope.maxAltitudeM) ||
|
|
envelope.minLat >= envelope.maxLat || envelope.minLng >= envelope.maxLng ||
|
|
envelope.minAltitudeM >= envelope.maxAltitudeM
|
|
) throw new RangeError("aircraft flight envelope must be finite and ordered");
|
|
return envelope;
|
|
}
|
|
|
|
function checkedRoute(
|
|
route: readonly AircraftWaypoint[] | undefined,
|
|
envelope: CaliforniaFlightEnvelope,
|
|
): readonly AircraftWaypoint[] {
|
|
if (!route) return [];
|
|
const ids = new Set<string>();
|
|
return route.map((point) => {
|
|
if (
|
|
typeof point.id !== "string" || point.id.length === 0 || ids.has(point.id) ||
|
|
!Number.isFinite(point.lat) || !Number.isFinite(point.lng) ||
|
|
!Number.isFinite(point.altitudeM) ||
|
|
point.lat < envelope.minLat || point.lat > envelope.maxLat ||
|
|
point.lng < envelope.minLng || point.lng > envelope.maxLng ||
|
|
point.altitudeM < envelope.minAltitudeM || point.altitudeM > envelope.maxAltitudeM
|
|
) throw new RangeError(
|
|
"aircraft route waypoints must be finite, unique, and inside the flight envelope",
|
|
);
|
|
ids.add(point.id);
|
|
return { ...point };
|
|
});
|
|
}
|
|
|
|
function resolveOptions(value: AircraftControllerOptions): ResolvedOptions {
|
|
const envelope = checkedEnvelope(value.envelope);
|
|
const minimumSpeedMps = clamp(finiteOr(value.minimumSpeedMps, 20), 5, 100);
|
|
const maximumSpeedMps = clamp(finiteOr(value.maximumSpeedMps, 95), minimumSpeedMps, 250);
|
|
const assistedAltitudeM = clamp(
|
|
finiteOr(value.assistedAltitudeM, 1_500),
|
|
envelope.minAltitudeM,
|
|
envelope.maxAltitudeM,
|
|
);
|
|
const stallSpeedMps = clamp(
|
|
finiteOr(value.stallSpeedMps, Math.max(18, minimumSpeedMps + 3)),
|
|
minimumSpeedMps,
|
|
maximumSpeedMps * 0.8,
|
|
);
|
|
const terrainElevationM = value.terrainElevationM ?? (() => envelope.minAltitudeM);
|
|
if (typeof terrainElevationM !== "function") {
|
|
throw new RangeError("aircraft terrainElevationM must be a function");
|
|
}
|
|
return {
|
|
initialPosition: {
|
|
lat: clamp(finiteOr(value.initialPosition?.lat, 34.0522), envelope.minLat, envelope.maxLat),
|
|
lng: clamp(finiteOr(value.initialPosition?.lng, -118.2437), envelope.minLng, envelope.maxLng),
|
|
},
|
|
initialAltitudeM: clamp(finiteOr(value.initialAltitudeM, assistedAltitudeM), envelope.minAltitudeM, envelope.maxAltitudeM),
|
|
initialHeadingDeg: wrapDegrees(finiteOr(value.initialHeadingDeg, 320)),
|
|
initialSpeedMps: clamp(finiteOr(value.initialSpeedMps, 55), minimumSpeedMps, maximumSpeedMps),
|
|
mode: value.mode === "manual" ? "manual" : "assisted",
|
|
route: checkedRoute(value.route, envelope),
|
|
envelope,
|
|
minimumSpeedMps,
|
|
maximumSpeedMps,
|
|
assistedCruiseMps: clamp(finiteOr(value.assistedCruiseMps, 62), minimumSpeedMps, maximumSpeedMps),
|
|
assistedAltitudeM,
|
|
windNorthMps: clamp(finiteOr(value.windNorthMps, 0), -80, 80),
|
|
windEastMps: clamp(finiteOr(value.windEastMps, 0), -80, 80),
|
|
turbulenceMps: clamp(finiteOr(value.turbulenceMps, 0), 0, 30),
|
|
stallSpeedMps,
|
|
batteryCapacityWh: clamp(finiteOr(value.batteryCapacityWh, 54_000), 1_000, 500_000),
|
|
terrainElevationM,
|
|
fixedStepSeconds: clamp(finiteOr(value.fixedStepSeconds, 1 / 60), 1 / 240, 0.1),
|
|
maxFrameDeltaSeconds: clamp(finiteOr(value.maxFrameDeltaSeconds, 0.25), 0.05, 1),
|
|
};
|
|
}
|
|
|
|
export function normalizeAircraftActions(
|
|
value: Partial<AircraftActionSnapshot> | undefined,
|
|
): AircraftActionSnapshot {
|
|
return {
|
|
throttle: clamp(finiteOr(value?.throttle, 0), 0, 1),
|
|
yaw: clamp(finiteOr(value?.yaw, 0), -1, 1),
|
|
pitch: clamp(finiteOr(value?.pitch, 0), -1, 1),
|
|
roll: clamp(finiteOr(value?.roll, 0), -1, 1),
|
|
modeRequest: value?.modeRequest === "manual" || value?.modeRequest === "assisted"
|
|
? value.modeRequest
|
|
: "none",
|
|
reset: value?.reset === true,
|
|
};
|
|
}
|
|
|
|
function hasManualIntent(actions: AircraftActionSnapshot): boolean {
|
|
return (
|
|
actions.modeRequest === "manual" || actions.throttle > 0.02 ||
|
|
Math.abs(actions.yaw) > 0.06 || Math.abs(actions.pitch) > 0.06 || Math.abs(actions.roll) > 0.06
|
|
);
|
|
}
|
|
|
|
export class AircraftController {
|
|
private readonly options: ResolvedOptions;
|
|
private accumulator = 0;
|
|
private readonly current: AircraftControllerState;
|
|
|
|
constructor(options: AircraftControllerOptions = {}) {
|
|
this.options = resolveOptions(options);
|
|
this.current = {
|
|
...this.options.initialPosition,
|
|
altitudeM: this.options.initialAltitudeM,
|
|
headingDeg: this.options.initialHeadingDeg,
|
|
pitchDeg: 0,
|
|
rollDeg: 0,
|
|
speedMps: this.options.initialSpeedMps,
|
|
verticalSpeedMps: 0,
|
|
mode: this.options.mode,
|
|
routeWaypointIndex: 0,
|
|
routeWaypointId: null,
|
|
throttle: 0,
|
|
yawInput: 0,
|
|
pitchInput: 0,
|
|
rollInput: 0,
|
|
fanRadians: 0,
|
|
angleOfAttackDeg: 0,
|
|
liftCoefficient: 1,
|
|
loadFactorG: 1,
|
|
stalled: false,
|
|
windNorthMps: this.options.windNorthMps,
|
|
windEastMps: this.options.windEastMps,
|
|
batteryWh: this.options.batteryCapacityWh,
|
|
energyUsedWh: 0,
|
|
groundClearanceM: 0,
|
|
onGround: false,
|
|
hardLanding: false,
|
|
envelopeContact: false,
|
|
elapsedSteps: 0,
|
|
};
|
|
this.reset();
|
|
}
|
|
|
|
fixedStepSeconds(): number {
|
|
return this.options.fixedStepSeconds;
|
|
}
|
|
|
|
state(): Readonly<AircraftControllerState> {
|
|
return this.current;
|
|
}
|
|
|
|
snapshot(): AircraftControllerSnapshot {
|
|
return { ...this.current };
|
|
}
|
|
|
|
/** Restore a trusted JSON snapshot for deterministic environment checkpointing. */
|
|
restore(snapshot: AircraftControllerSnapshot): void {
|
|
const numeric = Object.entries(snapshot)
|
|
.filter(([, value]) => typeof value === "number")
|
|
.every(([, value]) => Number.isFinite(value));
|
|
const envelope = this.options.envelope;
|
|
if (
|
|
!numeric || (snapshot.mode !== "manual" && snapshot.mode !== "assisted") ||
|
|
snapshot.lat < envelope.minLat || snapshot.lat > envelope.maxLat ||
|
|
snapshot.lng < envelope.minLng || snapshot.lng > envelope.maxLng ||
|
|
snapshot.altitudeM < envelope.minAltitudeM || snapshot.altitudeM > envelope.maxAltitudeM ||
|
|
snapshot.speedMps < this.options.minimumSpeedMps ||
|
|
snapshot.speedMps > this.options.maximumSpeedMps ||
|
|
!Number.isSafeInteger(snapshot.elapsedSteps) || snapshot.elapsedSteps < 0
|
|
) throw new RangeError("aircraft snapshot is incompatible or invalid");
|
|
Object.assign(this.current, snapshot);
|
|
this.accumulator = 0;
|
|
}
|
|
|
|
reset(): void {
|
|
this.accumulator = 0;
|
|
Object.assign(this.current, this.options.initialPosition, {
|
|
altitudeM: this.options.initialAltitudeM,
|
|
headingDeg: this.options.initialHeadingDeg,
|
|
pitchDeg: 0,
|
|
rollDeg: 0,
|
|
speedMps: this.options.initialSpeedMps,
|
|
verticalSpeedMps: 0,
|
|
mode: this.options.mode,
|
|
routeWaypointIndex: 0,
|
|
routeWaypointId: this.options.route[0]?.id ?? null,
|
|
throttle: 0,
|
|
yawInput: 0,
|
|
pitchInput: 0,
|
|
rollInput: 0,
|
|
fanRadians: 0,
|
|
angleOfAttackDeg: 0,
|
|
liftCoefficient: 1,
|
|
loadFactorG: 1,
|
|
stalled: false,
|
|
windNorthMps: this.options.windNorthMps,
|
|
windEastMps: this.options.windEastMps,
|
|
batteryWh: this.options.batteryCapacityWh,
|
|
energyUsedWh: 0,
|
|
groundClearanceM: Math.max(0, this.options.initialAltitudeM - this.groundElevationM(
|
|
this.options.initialPosition.lat,
|
|
this.options.initialPosition.lng,
|
|
)),
|
|
onGround: false,
|
|
hardLanding: false,
|
|
envelopeContact: false,
|
|
elapsedSteps: 0,
|
|
});
|
|
}
|
|
|
|
private groundElevationM(lat: number, lng: number): number {
|
|
const value = this.options.terrainElevationM(lat, lng);
|
|
if (!Number.isFinite(value)) {
|
|
throw new RangeError("aircraft terrainElevationM must return a finite elevation");
|
|
}
|
|
return clamp(value, this.options.envelope.minAltitudeM, this.options.envelope.maxAltitudeM);
|
|
}
|
|
|
|
tick(deltaSeconds: number, actions: Partial<AircraftActionSnapshot> = NEUTRAL_AIRCRAFT_ACTIONS): number {
|
|
if (!Number.isFinite(deltaSeconds) || deltaSeconds <= 0) return 0;
|
|
const normalized = normalizeAircraftActions(actions);
|
|
if (normalized.reset) {
|
|
this.reset();
|
|
return 0;
|
|
}
|
|
this.accumulator += Math.min(deltaSeconds, this.options.maxFrameDeltaSeconds);
|
|
let steps = 0;
|
|
while (this.accumulator + Number.EPSILON >= this.options.fixedStepSeconds) {
|
|
this.stepNormalized(normalized);
|
|
this.accumulator -= this.options.fixedStepSeconds;
|
|
steps += 1;
|
|
}
|
|
return steps;
|
|
}
|
|
|
|
stepFixed(actions: Partial<AircraftActionSnapshot> = NEUTRAL_AIRCRAFT_ACTIONS): void {
|
|
const normalized = normalizeAircraftActions(actions);
|
|
if (normalized.reset) this.reset();
|
|
else this.stepNormalized(normalized);
|
|
}
|
|
|
|
private stepNormalized(actions: AircraftActionSnapshot): void {
|
|
const dt = this.options.fixedStepSeconds;
|
|
const manual = hasManualIntent(actions);
|
|
if (manual) this.current.mode = "manual";
|
|
else if (actions.modeRequest === "assisted") this.current.mode = "assisted";
|
|
|
|
let throttle = actions.throttle;
|
|
let yaw = actions.yaw;
|
|
let pitch = actions.pitch;
|
|
let roll = actions.roll;
|
|
if (this.current.mode === "assisted") {
|
|
throttle = clamp(0.5 + (this.options.assistedCruiseMps - this.current.speedMps) / 18, 0, 1);
|
|
const target = this.options.route[this.current.routeWaypointIndex];
|
|
if (target && distanceM(this.current, target) < 3_000 && this.options.route.length > 1) {
|
|
this.current.routeWaypointIndex = (this.current.routeWaypointIndex + 1) % this.options.route.length;
|
|
}
|
|
const waypoint = this.options.route[this.current.routeWaypointIndex];
|
|
this.current.routeWaypointId = waypoint?.id ?? null;
|
|
const desiredHeading = waypoint ? bearingDeg(this.current, waypoint) : this.options.initialHeadingDeg;
|
|
const headingError = signedAngleDegrees(this.current.headingDeg, desiredHeading);
|
|
roll = clamp(headingError / 38, -1, 1);
|
|
yaw = clamp(headingError / 90, -0.45, 0.45);
|
|
const altitudeTarget = waypoint?.altitudeM ?? this.options.assistedAltitudeM;
|
|
pitch = clamp((altitudeTarget - this.current.altitudeM) / 350, -0.65, 0.65);
|
|
}
|
|
|
|
this.current.throttle = moveToward(this.current.throttle, throttle, 0.8 * dt);
|
|
this.current.yawInput = moveToward(this.current.yawInput, yaw, 2.5 * dt);
|
|
this.current.pitchInput = moveToward(this.current.pitchInput, pitch, 2.2 * dt);
|
|
this.current.rollInput = moveToward(this.current.rollInput, roll, 2.8 * dt);
|
|
|
|
const gustPhase = this.current.elapsedSteps * dt * 0.73;
|
|
const gustNorth = Math.sin(gustPhase) * this.options.turbulenceMps;
|
|
const gustEast = Math.sin(gustPhase * 0.61 + 1.7) * this.options.turbulenceMps * 0.72;
|
|
this.current.windNorthMps = this.options.windNorthMps + gustNorth;
|
|
this.current.windEastMps = this.options.windEastMps + gustEast;
|
|
|
|
const targetRoll = this.current.rollInput * 58 + gustEast * 0.22;
|
|
const targetPitch = this.current.pitchInput * 22 + gustNorth * 0.08;
|
|
this.current.rollDeg = moveToward(this.current.rollDeg, targetRoll, 55 * dt);
|
|
this.current.pitchDeg = moveToward(this.current.pitchDeg, targetPitch, 28 * dt);
|
|
const flightPathDeg = Math.atan2(
|
|
this.current.verticalSpeedMps,
|
|
Math.max(1, this.current.speedMps),
|
|
) * 180 / Math.PI;
|
|
this.current.angleOfAttackDeg = this.current.pitchDeg - flightPathDeg;
|
|
this.current.liftCoefficient = clamp(1 + this.current.angleOfAttackDeg * 0.055, 0.05, 1.6);
|
|
this.current.stalled = this.current.speedMps < this.options.stallSpeedMps ||
|
|
Math.abs(this.current.angleOfAttackDeg) > 19;
|
|
const stallDrag = this.current.stalled ? 3.8 : 0;
|
|
const batteryFactor = clamp(this.current.batteryWh / Math.max(1, this.options.batteryCapacityWh * 0.08), 0, 1);
|
|
const thrust = this.current.throttle * 8.5 * batteryFactor;
|
|
const drag = 1.2 + this.current.speedMps * this.current.speedMps * 0.00075 + stallDrag;
|
|
this.current.speedMps = clamp(
|
|
this.current.speedMps + (thrust - drag) * dt,
|
|
this.options.minimumSpeedMps,
|
|
this.options.maximumSpeedMps,
|
|
);
|
|
const bankTurn = Math.sin(this.current.rollDeg * Math.PI / 180) * 24;
|
|
this.current.headingDeg = wrapDegrees(
|
|
this.current.headingDeg + (bankTurn + this.current.yawInput * 20) * dt,
|
|
);
|
|
const liftAuthority = clamp(
|
|
this.current.liftCoefficient * (this.current.speedMps / Math.max(1, this.options.assistedCruiseMps)),
|
|
0.08,
|
|
1.35,
|
|
);
|
|
const commandedVerticalSpeed = Math.sin(this.current.pitchDeg * Math.PI / 180) *
|
|
this.current.speedMps * liftAuthority;
|
|
const stallSinkMps = this.current.stalled
|
|
? clamp((this.options.stallSpeedMps - this.current.speedMps) * 0.7 + 2.5, 2.5, 14)
|
|
: 0;
|
|
this.current.verticalSpeedMps = moveToward(
|
|
this.current.verticalSpeedMps,
|
|
commandedVerticalSpeed - stallSinkMps,
|
|
(this.current.stalled ? 8 : 5) * dt,
|
|
);
|
|
this.current.loadFactorG = clamp(
|
|
liftAuthority / Math.max(0.25, Math.cos(this.current.rollDeg * Math.PI / 180)),
|
|
0,
|
|
3.5,
|
|
);
|
|
|
|
const heading = this.current.headingDeg * Math.PI / 180;
|
|
const horizontalSpeed = Math.cos(this.current.pitchDeg * Math.PI / 180) * this.current.speedMps;
|
|
const northM = (Math.cos(heading) * horizontalSpeed + this.current.windNorthMps) * dt;
|
|
const eastM = (Math.sin(heading) * horizontalSpeed + this.current.windEastMps) * dt;
|
|
const nextLat = this.current.lat + northM / EARTH_RADIUS_M * 180 / Math.PI;
|
|
const nextLng = this.current.lng + eastM /
|
|
(EARTH_RADIUS_M * Math.max(0.01, Math.cos(this.current.lat * Math.PI / 180))) * 180 / Math.PI;
|
|
let nextAltitude = this.current.altitudeM + this.current.verticalSpeedMps * dt;
|
|
const groundElevationM = this.groundElevationM(nextLat, nextLng);
|
|
const wasVerticalSpeedMps = this.current.verticalSpeedMps;
|
|
this.current.onGround = nextAltitude <= groundElevationM + 0.75;
|
|
this.current.hardLanding = this.current.onGround && wasVerticalSpeedMps < -4.5;
|
|
if (this.current.onGround) {
|
|
nextAltitude = groundElevationM;
|
|
this.current.verticalSpeedMps = 0;
|
|
this.current.rollDeg = moveToward(this.current.rollDeg, 0, 70 * dt);
|
|
this.current.pitchDeg = moveToward(this.current.pitchDeg, 0, 45 * dt);
|
|
if (this.current.throttle < 0.55) {
|
|
this.current.speedMps = Math.max(this.options.minimumSpeedMps, this.current.speedMps - 4 * dt);
|
|
} else if (
|
|
this.current.speedMps > this.options.stallSpeedMps * 1.08 &&
|
|
this.current.pitchInput > 0.2
|
|
) {
|
|
this.current.onGround = false;
|
|
nextAltitude = groundElevationM + 0.8;
|
|
this.current.verticalSpeedMps = 0.8;
|
|
}
|
|
}
|
|
const envelope = this.options.envelope;
|
|
this.current.envelopeContact =
|
|
nextLat < envelope.minLat || nextLat > envelope.maxLat ||
|
|
nextLng < envelope.minLng || nextLng > envelope.maxLng ||
|
|
nextAltitude < envelope.minAltitudeM || nextAltitude > envelope.maxAltitudeM;
|
|
this.current.lat = clamp(nextLat, envelope.minLat, envelope.maxLat);
|
|
this.current.lng = clamp(nextLng, envelope.minLng, envelope.maxLng);
|
|
this.current.altitudeM = clamp(nextAltitude, envelope.minAltitudeM, envelope.maxAltitudeM);
|
|
this.current.groundClearanceM = Math.max(0, this.current.altitudeM - groundElevationM);
|
|
if (this.current.envelopeContact) {
|
|
this.current.speedMps = Math.max(this.options.minimumSpeedMps, this.current.speedMps * 0.92);
|
|
this.current.pitchDeg = moveToward(this.current.pitchDeg, 0, 90 * dt);
|
|
this.current.rollDeg = moveToward(this.current.rollDeg, 0, 90 * dt);
|
|
}
|
|
this.current.fanRadians = (this.current.fanRadians + (30 + this.current.throttle * 180) * dt) % TWO_PI;
|
|
const electricalPowerW = 8_000 + this.current.throttle * 122_000 +
|
|
Math.abs(this.current.verticalSpeedMps) * 420;
|
|
const usedWh = Math.min(this.current.batteryWh, electricalPowerW * dt / 3_600);
|
|
this.current.batteryWh -= usedWh;
|
|
this.current.energyUsedWh += usedWh;
|
|
this.current.elapsedSteps += 1;
|
|
}
|
|
}
|
|
|
|
export function replayAircraftInputs(
|
|
options: AircraftControllerOptions,
|
|
frames: readonly TimedAircraftInputFrame[],
|
|
): AircraftReplayResult {
|
|
const controller = new AircraftController(options);
|
|
const trajectory: AircraftControllerSnapshot[] = [controller.snapshot()];
|
|
for (const frame of frames) {
|
|
const steps = Number.isFinite(frame.steps) ? Math.max(0, Math.floor(frame.steps)) : 0;
|
|
const held = normalizeAircraftActions(frame.actions);
|
|
for (let index = 0; index < steps; index += 1) {
|
|
controller.stepFixed(index === 0 ? held : { ...held, modeRequest: "none", reset: false });
|
|
trajectory.push(controller.snapshot());
|
|
}
|
|
}
|
|
return { trajectory, final: trajectory.at(-1) ?? controller.snapshot() };
|
|
}
|
|
|
|
/** Renderer-neutral geographic chase camera derived from an authoritative pose. */
|
|
export function aircraftChaseCameraPose(
|
|
state: Readonly<AircraftControllerState>,
|
|
distanceBehindM = 24,
|
|
heightAboveM = 8,
|
|
lookAheadM = 35,
|
|
): AircraftCameraPose {
|
|
const heading = state.headingDeg * Math.PI / 180;
|
|
const geographicOffset = (northM: number, eastM: number): AircraftGeographicPoint => ({
|
|
lat: state.lat + northM / EARTH_RADIUS_M * 180 / Math.PI,
|
|
lng: state.lng + eastM /
|
|
(EARTH_RADIUS_M * Math.max(0.01, Math.cos(state.lat * Math.PI / 180))) * 180 / Math.PI,
|
|
});
|
|
const behind = geographicOffset(-Math.cos(heading) * distanceBehindM, -Math.sin(heading) * distanceBehindM);
|
|
const ahead = geographicOffset(Math.cos(heading) * lookAheadM, Math.sin(heading) * lookAheadM);
|
|
return {
|
|
position: { ...behind, altitudeM: state.altitudeM + heightAboveM },
|
|
target: { ...ahead, altitudeM: state.altitudeM + state.verticalSpeedMps * 0.4 },
|
|
rollDeg: state.rollDeg * 0.2,
|
|
};
|
|
}
|