437 lines
15 KiB
TypeScript
437 lines
15 KiB
TypeScript
/**
|
|
* Renderer-independent solo vehicle controls for a route-relative simulation.
|
|
*
|
|
* Inputs are normalized action snapshots rather than DOM events, so keyboards,
|
|
* gamepads, touch controls, remote clients, and recorded replays all drive the
|
|
* same deterministic fixed-step state machine.
|
|
*/
|
|
|
|
import type { GeographicPoint, TransportPack } from "./types.ts";
|
|
import {
|
|
buildRoutePath,
|
|
sampleRoute,
|
|
type RoutePath,
|
|
type RouteSample,
|
|
} from "./vehicleSim.ts";
|
|
|
|
const MPH_TO_MPS = 0.44704;
|
|
const EARTH_RADIUS_M = 6_371_000;
|
|
const TWO_PI = Math.PI * 2;
|
|
|
|
export type VehicleControlMode = "assisted" | "manual";
|
|
export type VehicleModeRequest = "none" | VehicleControlMode;
|
|
|
|
/** Device-neutral actions sampled for one rendered or fixed simulation frame. */
|
|
export interface VehicleActionSnapshot {
|
|
/** Accelerator position in the inclusive range [0, 1]. */
|
|
throttle: number;
|
|
/** Service brake position in the inclusive range [0, 1]. */
|
|
brake: number;
|
|
/** Steering input where -1 is full left and 1 is full right. */
|
|
steering: number;
|
|
handbrake: boolean;
|
|
/** One-shot mode request. Meaningful even when all analogue axes are neutral. */
|
|
modeRequest: VehicleModeRequest;
|
|
/** One-shot request to restore the configured initial state. */
|
|
reset: boolean;
|
|
}
|
|
|
|
export const NEUTRAL_VEHICLE_ACTIONS: Readonly<VehicleActionSnapshot> = Object.freeze({
|
|
throttle: 0,
|
|
brake: 0,
|
|
steering: 0,
|
|
handbrake: false,
|
|
modeRequest: "none",
|
|
reset: false,
|
|
});
|
|
|
|
export interface VehicleControllerOptions {
|
|
routeId: string;
|
|
mode?: VehicleControlMode;
|
|
direction?: 1 | -1;
|
|
initialDistanceM?: number;
|
|
initialLateralOffsetM?: number;
|
|
initialSpeedMps?: number;
|
|
/** Defaults to 60 Hz and is clamped to a safe simulation range. */
|
|
fixedStepSeconds?: number;
|
|
/** Caps catch-up after a sleeping tab. Defaults to 0.25 seconds. */
|
|
maxFrameDeltaSeconds?: number;
|
|
maximumSpeedMps?: number;
|
|
assistedCruiseRatio?: number;
|
|
guardrailOffsetM?: number;
|
|
wheelRadiusM?: number;
|
|
/**
|
|
* Multiplies longitudinal route progress without changing acceleration or
|
|
* steering response. State-scale boards use compression; metre-scale roads
|
|
* leave this at 1. Defaults to 1.
|
|
*/
|
|
travelScale?: number;
|
|
}
|
|
|
|
export interface VehicleControllerState extends GeographicPoint {
|
|
routeId: string;
|
|
mode: VehicleControlMode;
|
|
direction: 1 | -1;
|
|
/** Distance from the route's declared start, wrapped to its total length. */
|
|
distanceM: number;
|
|
progress: number;
|
|
/** Signed offset from route centre; positive is to the driver's right. */
|
|
lateralOffsetM: number;
|
|
speedMps: number;
|
|
/** Smoothed normalized steering position, independent of input device. */
|
|
steering: number;
|
|
routeHeadingDeg: number;
|
|
headingDeg: number;
|
|
segmentId: string;
|
|
roadName: string;
|
|
speedLimitMph: number;
|
|
wheelRadians: number;
|
|
guardrailContact: boolean;
|
|
elapsedSteps: number;
|
|
}
|
|
|
|
export interface VehicleControllerSnapshot extends VehicleControllerState {}
|
|
|
|
/** A held input snapshot and its exact duration in fixed simulation steps. */
|
|
export interface TimedVehicleInputFrame {
|
|
steps: number;
|
|
actions?: Partial<VehicleActionSnapshot>;
|
|
}
|
|
|
|
export interface VehicleReplayResult {
|
|
/** Initial state followed by one snapshot after every simulated step. */
|
|
trajectory: readonly VehicleControllerSnapshot[];
|
|
final: VehicleControllerSnapshot;
|
|
}
|
|
|
|
interface ResolvedOptions {
|
|
routeId: string;
|
|
mode: VehicleControlMode;
|
|
direction: 1 | -1;
|
|
initialDistanceM: number;
|
|
initialLateralOffsetM: number;
|
|
initialSpeedMps: number;
|
|
fixedStepSeconds: number;
|
|
maxFrameDeltaSeconds: number;
|
|
maximumSpeedMps: number;
|
|
assistedCruiseRatio: number;
|
|
guardrailOffsetM: number;
|
|
wheelRadiusM: number;
|
|
travelScale: number;
|
|
}
|
|
|
|
function finiteOr(value: number | undefined, fallback: number): number {
|
|
return typeof value === "number" && Number.isFinite(value) ? value : fallback;
|
|
}
|
|
|
|
function clamp(value: number, min: number, max: number): number {
|
|
return Math.max(min, Math.min(max, value));
|
|
}
|
|
|
|
function wrap(value: number, modulus: number): number {
|
|
return ((value % modulus) + modulus) % modulus;
|
|
}
|
|
|
|
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;
|
|
}
|
|
|
|
/** Clamp and sanitize input from any adapter before it reaches simulation. */
|
|
export function normalizeVehicleActions(
|
|
actions: Partial<VehicleActionSnapshot> | undefined,
|
|
): VehicleActionSnapshot {
|
|
const modeRequest = actions?.modeRequest;
|
|
return {
|
|
throttle: clamp(finiteOr(actions?.throttle, 0), 0, 1),
|
|
brake: clamp(finiteOr(actions?.brake, 0), 0, 1),
|
|
steering: clamp(finiteOr(actions?.steering, 0), -1, 1),
|
|
handbrake: actions?.handbrake === true,
|
|
modeRequest: modeRequest === "manual" || modeRequest === "assisted" ? modeRequest : "none",
|
|
reset: actions?.reset === true,
|
|
};
|
|
}
|
|
|
|
function resolveOptions(options: VehicleControllerOptions): ResolvedOptions {
|
|
return {
|
|
routeId: options.routeId,
|
|
mode: options.mode === "manual" ? "manual" : "assisted",
|
|
direction: options.direction === -1 ? -1 : 1,
|
|
initialDistanceM: finiteOr(options.initialDistanceM, 0),
|
|
initialLateralOffsetM: finiteOr(options.initialLateralOffsetM, 0),
|
|
initialSpeedMps: Math.max(0, finiteOr(options.initialSpeedMps, 0)),
|
|
fixedStepSeconds: clamp(finiteOr(options.fixedStepSeconds, 1 / 60), 1 / 240, 0.1),
|
|
maxFrameDeltaSeconds: clamp(finiteOr(options.maxFrameDeltaSeconds, 0.25), 0.05, 1),
|
|
maximumSpeedMps: clamp(finiteOr(options.maximumSpeedMps, 58), 5, 100),
|
|
assistedCruiseRatio: clamp(finiteOr(options.assistedCruiseRatio, 0.92), 0.25, 1.1),
|
|
guardrailOffsetM: clamp(finiteOr(options.guardrailOffsetM, 5.4), 1, 20),
|
|
wheelRadiusM: clamp(finiteOr(options.wheelRadiusM, 0.36), 0.1, 1),
|
|
travelScale: clamp(finiteOr(options.travelScale, 1), 1, 10_000),
|
|
};
|
|
}
|
|
|
|
function hasManualIntent(actions: VehicleActionSnapshot): boolean {
|
|
return (
|
|
actions.modeRequest === "manual" ||
|
|
actions.handbrake ||
|
|
actions.throttle > 0.02 ||
|
|
actions.brake > 0.02 ||
|
|
Math.abs(actions.steering) > 0.08
|
|
);
|
|
}
|
|
|
|
function offsetPoint(sample: RouteSample, lateralOffsetM: number): GeographicPoint {
|
|
const heading = (sample.headingDeg * Math.PI) / 180;
|
|
// Right-hand normal to a compass bearing: south for eastbound, east for northbound.
|
|
const northM = -Math.sin(heading) * lateralOffsetM;
|
|
const eastM = Math.cos(heading) * lateralOffsetM;
|
|
const latitudeRadians = (sample.lat * Math.PI) / 180;
|
|
return {
|
|
lat: sample.lat + (northM / EARTH_RADIUS_M) * (180 / Math.PI),
|
|
lng:
|
|
sample.lng +
|
|
(eastM / (EARTH_RADIUS_M * Math.max(0.01, Math.cos(latitudeRadians)))) * (180 / Math.PI),
|
|
};
|
|
}
|
|
|
|
/**
|
|
* Deterministic route-relative driving state machine.
|
|
*
|
|
* `tick` adapts render time to a fixed clock. `stepFixed` is the authoritative
|
|
* primitive for tests, networking, and replay and always advances exactly once.
|
|
*/
|
|
export class VehicleController {
|
|
private readonly pack: TransportPack;
|
|
private readonly options: ResolvedOptions;
|
|
private path: RoutePath;
|
|
private accumulator = 0;
|
|
private readonly current: VehicleControllerState;
|
|
|
|
constructor(pack: TransportPack, options: VehicleControllerOptions) {
|
|
this.pack = pack;
|
|
this.options = resolveOptions(options);
|
|
this.path = buildRoutePath(pack, this.options.routeId);
|
|
const sample = sampleRoute(this.path, this.options.initialDistanceM, this.options.direction);
|
|
const point = offsetPoint(sample, 0);
|
|
this.current = {
|
|
...point,
|
|
routeId: this.path.route.id,
|
|
mode: this.options.mode,
|
|
direction: this.options.direction,
|
|
distanceM: 0,
|
|
progress: 0,
|
|
lateralOffsetM: 0,
|
|
speedMps: 0,
|
|
steering: 0,
|
|
routeHeadingDeg: sample.headingDeg,
|
|
headingDeg: sample.headingDeg,
|
|
segmentId: sample.segmentId,
|
|
roadName: sample.roadName,
|
|
speedLimitMph: sample.speedLimitMph,
|
|
wheelRadians: 0,
|
|
guardrailContact: false,
|
|
elapsedSteps: 0,
|
|
};
|
|
this.reset();
|
|
}
|
|
|
|
fixedStepSeconds(): number {
|
|
return this.options.fixedStepSeconds;
|
|
}
|
|
|
|
routeId(): string {
|
|
return this.path.route.id;
|
|
}
|
|
|
|
/** Stable state object for allocation-free polling. Treat it as read-only. */
|
|
state(): Readonly<VehicleControllerState> {
|
|
return this.current;
|
|
}
|
|
|
|
/** Detached state suitable for logs, network frames, and equality assertions. */
|
|
snapshot(): VehicleControllerSnapshot {
|
|
return { ...this.current };
|
|
}
|
|
|
|
/** Restore the configured spawn state and clear pending fractional time. */
|
|
reset(): void {
|
|
this.accumulator = 0;
|
|
const distanceM = wrap(this.options.initialDistanceM, this.path.lengthM);
|
|
const lateralOffsetM = clamp(
|
|
this.options.initialLateralOffsetM,
|
|
-this.options.guardrailOffsetM,
|
|
this.options.guardrailOffsetM,
|
|
);
|
|
const speedMps = clamp(this.options.initialSpeedMps, 0, this.options.maximumSpeedMps);
|
|
const sample = sampleRoute(this.path, distanceM, this.options.direction);
|
|
const point = offsetPoint(sample, lateralOffsetM);
|
|
Object.assign(this.current, point, {
|
|
routeId: this.path.route.id,
|
|
mode: this.options.mode,
|
|
direction: this.options.direction,
|
|
distanceM,
|
|
progress: distanceM / this.path.lengthM,
|
|
lateralOffsetM,
|
|
speedMps,
|
|
steering: 0,
|
|
routeHeadingDeg: sample.headingDeg,
|
|
headingDeg: sample.headingDeg,
|
|
segmentId: sample.segmentId,
|
|
roadName: sample.roadName,
|
|
speedLimitMph: sample.speedLimitMph,
|
|
wheelRadians: 0,
|
|
guardrailContact: false,
|
|
elapsedSteps: 0,
|
|
});
|
|
}
|
|
|
|
/**
|
|
* Change corridor as an explicit reset. Progress may optionally be preserved,
|
|
* which is useful for switching route variants without retaining stale metres.
|
|
*/
|
|
setRoute(routeId: string, preserveProgress = false): void {
|
|
if (routeId === this.path.route.id) return;
|
|
const previousProgress = this.current.progress;
|
|
this.path = buildRoutePath(this.pack, routeId);
|
|
this.options.routeId = routeId;
|
|
this.options.initialDistanceM = preserveProgress ? previousProgress * this.path.lengthM : 0;
|
|
this.reset();
|
|
}
|
|
|
|
/** Advance rendered seconds and return the number of fixed steps executed. */
|
|
tick(
|
|
deltaSeconds: number,
|
|
actions: Partial<VehicleActionSnapshot> = NEUTRAL_VEHICLE_ACTIONS,
|
|
): number {
|
|
if (!Number.isFinite(deltaSeconds) || deltaSeconds <= 0) return 0;
|
|
const normalized = normalizeVehicleActions(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;
|
|
}
|
|
|
|
/** Advance exactly one authoritative simulation step. */
|
|
stepFixed(actions: Partial<VehicleActionSnapshot> = NEUTRAL_VEHICLE_ACTIONS): void {
|
|
const normalized = normalizeVehicleActions(actions);
|
|
if (normalized.reset) {
|
|
this.reset();
|
|
return;
|
|
}
|
|
this.stepNormalized(normalized);
|
|
}
|
|
|
|
private stepNormalized(actions: VehicleActionSnapshot): void {
|
|
const dt = this.options.fixedStepSeconds;
|
|
const manualIntent = hasManualIntent(actions);
|
|
// Direct human input always wins, including over a simultaneous request to
|
|
// resume assistance. A neutral assisted request can re-engage on the next step.
|
|
if (manualIntent) this.current.mode = "manual";
|
|
else if (actions.modeRequest === "assisted") this.current.mode = "assisted";
|
|
|
|
let throttle = actions.throttle;
|
|
let brake = actions.brake;
|
|
let steeringTarget = actions.steering;
|
|
|
|
if (this.current.mode === "assisted") {
|
|
const roadTarget = this.current.speedLimitMph * MPH_TO_MPS * this.options.assistedCruiseRatio;
|
|
const targetSpeed = Math.min(roadTarget, this.options.maximumSpeedMps);
|
|
const speedError = targetSpeed - this.current.speedMps;
|
|
throttle = clamp(speedError / 5, 0, 1);
|
|
brake = clamp(-speedError / 7, 0, 1);
|
|
steeringTarget = clamp(-this.current.lateralOffsetM / 2.4, -1, 1);
|
|
}
|
|
|
|
this.current.steering = moveToward(this.current.steering, steeringTarget, 3.8 * dt);
|
|
|
|
const aeroDrag = this.current.speedMps * this.current.speedMps * 0.0018;
|
|
const rollingDrag = this.current.speedMps > 0 ? 0.12 : 0;
|
|
const engineFade = 1 - 0.55 * (this.current.speedMps / this.options.maximumSpeedMps);
|
|
const acceleration =
|
|
throttle * 5.4 * Math.max(0.2, engineFade) -
|
|
brake * 9.5 -
|
|
(actions.handbrake ? 13 : 0) -
|
|
aeroDrag -
|
|
rollingDrag;
|
|
this.current.speedMps = clamp(
|
|
this.current.speedMps + acceleration * dt,
|
|
0,
|
|
this.options.maximumSpeedMps,
|
|
);
|
|
|
|
const previousDistance = this.current.distanceM;
|
|
const physicalTravelled = this.current.speedMps * dt;
|
|
const routeTravelled = physicalTravelled * this.options.travelScale;
|
|
this.current.distanceM = wrap(
|
|
previousDistance + routeTravelled * this.current.direction,
|
|
this.path.lengthM,
|
|
);
|
|
|
|
let proposedLateral =
|
|
this.current.lateralOffsetM + this.current.steering * this.current.speedMps * 0.2 * dt;
|
|
if (this.current.mode === "assisted") {
|
|
// Assistance damps the final few centimetres without an abrupt lane snap.
|
|
proposedLateral *= Math.exp(-0.35 * dt);
|
|
}
|
|
this.current.guardrailContact = Math.abs(proposedLateral) > this.options.guardrailOffsetM;
|
|
if (this.current.guardrailContact) {
|
|
proposedLateral = clamp(
|
|
proposedLateral,
|
|
-this.options.guardrailOffsetM,
|
|
this.options.guardrailOffsetM,
|
|
);
|
|
this.current.speedMps = Math.min(this.current.speedMps * 0.78, 12);
|
|
this.current.steering *= 0.35;
|
|
}
|
|
this.current.lateralOffsetM = proposedLateral;
|
|
|
|
const sample = sampleRoute(this.path, this.current.distanceM, this.current.direction);
|
|
const point = offsetPoint(sample, this.current.lateralOffsetM);
|
|
const wheelDelta = physicalTravelled / this.options.wheelRadiusM;
|
|
Object.assign(this.current, point, {
|
|
progress: this.current.distanceM / this.path.lengthM,
|
|
routeHeadingDeg: sample.headingDeg,
|
|
headingDeg: sample.headingDeg + this.current.steering * 9,
|
|
segmentId: sample.segmentId,
|
|
roadName: sample.roadName,
|
|
speedLimitMph: sample.speedLimitMph,
|
|
wheelRadians: wrap(this.current.wheelRadians + wheelDelta, TWO_PI),
|
|
elapsedSteps: this.current.elapsedSteps + 1,
|
|
});
|
|
}
|
|
}
|
|
|
|
/** Execute an exact, renderer-independent input recording. */
|
|
export function replayVehicleInputs(
|
|
pack: TransportPack,
|
|
options: VehicleControllerOptions,
|
|
frames: readonly TimedVehicleInputFrame[],
|
|
): VehicleReplayResult {
|
|
const controller = new VehicleController(pack, options);
|
|
const trajectory: VehicleControllerSnapshot[] = [controller.snapshot()];
|
|
for (const frame of frames) {
|
|
const steps = Math.max(0, Math.floor(finiteOr(frame.steps, 0)));
|
|
const actions = normalizeVehicleActions(frame.actions);
|
|
for (let index = 0; index < steps; index += 1) {
|
|
// Reset and mode requests are edge-triggered at the start of a timed frame.
|
|
controller.stepFixed(
|
|
index === 0
|
|
? actions
|
|
: { ...actions, modeRequest: "none", reset: false },
|
|
);
|
|
trajectory.push(controller.snapshot());
|
|
}
|
|
}
|
|
const final = trajectory.at(-1) ?? controller.snapshot();
|
|
return { trajectory, final };
|
|
}
|