California gets roads, traffic, and a car to follow
This commit is contained in:
@@ -0,0 +1,436 @@
|
||||
/**
|
||||
* 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 };
|
||||
}
|
||||
Reference in New Issue
Block a user