Skip to content

armgit / 67.js

Public Imported from GitHub
0

3D engine that AI agents drive

Add Ackermann steering, reverse legs and lights to the vehicle addon

Nguyen Xuan An274db53bfadb
diff --git a/bench/size/budgets.json b/bench/size/budgets.json
index 459b421b..9f0e6575 100644
--- a/bench/size/budgets.json
+++ b/bench/size/budgets.json
@@ -24,7 +24,7 @@
"entry:spume": 9000,
"entry:retarget": 17210,
"entry:skeleton-pose": 20640,
- "entry:vehicle": 14700,
+ "entry:vehicle": 15764,
"entry:vegetation": 18600,
"entry:terrain": 4100,
"entry:navmesh": 3400,
diff --git a/changes/322.md b/changes/322.md
new file mode 100644
index 00000000..6ec4338f
--- /dev/null
+++ b/changes/322.md
@@ -0,0 +1 @@
+- Add Ackermann steering, reverse legs and lights to `addons/vehicle` (issue #322). Each steerable wheel now turns about the shared turning centre from the measured wheelbase and track (`steering: 'shared'` keeps the old single angle), `driveAlongLegs` drives forward and reverse legs that stop at each end with the heading kept, and `pose.lights` carries headlight positions and directions that follow the body plus brake and reverse signals, with `applyVehicleLightsToParts` writing them to tail-light emission. The MCP `motion.drive` field is still open.
diff --git a/docs/vehicle.md b/docs/vehicle.md
index c730af9e..c7d3e152 100644
--- a/docs/vehicle.md
+++ b/docs/vehicle.md
@@ -48,6 +48,7 @@ renderer.updateParts(allParts);
| `forwardAxis` | `'+z'` | Model forward direction |
| `wheelbase` | measured front axle to rear axle | Metres, needed when the axles cannot be measured |
| `maxSteerAngle` | 35 degrees | Clamp for the steering angle |
+| `lights` | none | Headlights and tail lights, see Lights |
Per wheel: `side` is `left`, `right` or `center`, `axle` is `front` or `rear`; both are inferred from position when omitted.
`steerable` defaults to the front axle.
@@ -82,14 +83,48 @@ Speed is arc-length based. Give exactly one of:
Other options: `startTime`, `groundHeight(x, z)`, `maxSteerAngle`, `rollGain` (0.012 rad per m/s2), `pitchGain` (0.012), `bounceFrequency` (1.6 Hz), `dampingRatio` (0.45), `suspensionTravel` (0.2 m), `pathSamples` (400, for camera paths).
-`sample(seconds)` returns `{ time, distance, speed, position, heading, pitch, roll, steerAngle, wheels: [{ id, spinAngle, steerAngle, suspensionOffset }] }`.
+`sample(seconds)` returns `{ time, distance, speed, direction, position, heading, pitch, roll, steerAngle, wheels: [{ id, spinAngle, steerAngle, suspensionOffset }], lights }`.
- Wheel spin is `distance / radius`; inside wheels in a corner turn slower by their lateral offset times the heading change.
-- Steering uses a bicycle model: `atan(wheelbase * curvature)`, clamped.
+- Steering uses a bicycle model: `pose.steerAngle` is `atan(wheelbase * curvature)`, clamped. By default each steerable wheel gets its own Ackermann angle `atan2(ahead * tan, wheelbase - sideways * tan)` about the same turning centre, from its measured offsets (`rig.wheelbase`, `rig.trackWidth`): at full lock the inner front wheel turns `atan(wheelbase / (R - track / 2))` and the outer `atan(wheelbase / (R + track / 2))`, with `R = wheelbase / tan(maxSteerAngle)`. Pass `steering: 'shared'` for one angle on every steerable wheel.
- Roll follows lateral acceleration and pitch follows longitudinal acceleration through a spring and damper. The response is integrated at a fixed step when the drive is created, so sampling order does not matter.
- With `groundHeight`, the body height and its slope pitch and roll are fitted to the ground under the four wheels, and each wheel gets a `suspensionOffset` so it touches the ground. Without it, height and slope come from the path.
- The vehicle holds the end of the path with zero speed. Use `cruiseSpeed` for a natural stop; a constant `speed` stops instantly and the body pitches accordingly.
+## Reversing
+
+`driveAlongLegs(rig, legs, options)` drives `[{ path, direction: 'forward' | 'reverse', pause }]` in order and takes the same options as `driveAlongPath`, which is the one-leg case.
+The car stops at the end of every leg (only the first start and last end keep `startSpeed` and `endSpeed`), waits `pause` seconds (default 0), then starts the next leg.
+A reverse leg's path is the way the car travels, so list its points in travel order from where the previous leg ended.
+The car keeps facing the other way: heading is the path tangent plus a half turn, continuous at the stop, and steering and body roll flip so the same bend asks for the opposite lock.
+`pose.distance` and `pose.speed` are signed along the car's forward axis and `pose.direction` is `1` or `-1`, so wheel spin runs backwards while reversing and spin times radius still equals `pose.distance`.
+
+```js
+const drive = driveAlongLegs(rig, [
+ { path: [[0, 0, 0], [0, 0, 10]] },
+ { path: [[0, 0, 10], [0, 0, 4]], direction: 'reverse', pause: 0.5 },
+], { cruiseSpeed: 4, acceleration: 2, braking: 2 });
+```
+
+## Lights
+
+Describe lamps once in `createVehicleRig({ lights })`:
+
+| Field | Meaning |
+| --- | --- |
+| `headlights` | `[{ id, position, direction }]` in model space; `direction` defaults to the model forward axis |
+| `tailLights` | `[{ id, part \| node \| targets, signal: 'brake' \| 'reverse', off, on }]`, the lamp material's emission strength at signal 0 and 1 (defaults 0 and 4) |
+
+`pose.lights` is `{ brake, reverse, headlights: [{ id, position, direction }] }`.
+Headlight positions and unit directions are in world space and follow the body, including pitch and roll.
+`brake` is 0 to 1: the deceleration of the speed profile divided by `fullBrakeDeceleration` (default `braking`), so it is above 0 exactly while the car slows down, forwards or backwards.
+A constant `speed` never decelerates, so its brake signal stays 0.
+`reverse` is 1 while the car moves backwards.
+
+`computeVehicleLightEmissions(rig, pose)` returns `[{ id, emission, entries }]` for any source.
+`applyVehicleLightsToParts(rig, pose)` writes it to `part.material.emission` for a rig built from `parts` (give each lamp its own material, since parts sharing one material share the glow) and returns the parts; call `renderer.updateParts` after it.
+Use the headlight poses to place spot lights; the pose carries no renderer state.
+
## Applying a pose
| Function | Effect |
@@ -104,8 +139,9 @@ Skinned vehicles are not driven by this addon.
## Limits
-- Kinematic only: no collisions, tire slip, jumps or reversing. Heading follows the path tangent.
+- Kinematic only: no collisions, tire slip or jumps. Heading follows the path tangent (plus a half turn while reversing).
- Ground is sampled under the wheels as a plane fit, so sharp bumps narrower than the wheelbase show as suspension offsets, clamped to `suspensionTravel`.
-- All steerable wheels share one angle; there is no Ackermann split.
+- Rear-steered wheels get no Ackermann angle: the split is measured from the rear axle.
+- The MCP `motion.drive` field is not wired yet; the addon is code-only for now.
- Nodes with a nonzero scene-graph `pivot` are rejected.
- Wheel radius from geometry assumes the wheel is round in the plane across its axle.
diff --git a/src/vehicle.js b/src/vehicle.js
index bf05c131..92d75ad8 100644
--- a/src/vehicle.js
+++ b/src/vehicle.js
@@ -387,11 +387,52 @@ function assertNoPivot(entries) {
}
}
+function transformDirection(matrix, origin, direction) {
+ const tip = vec3.transformMat4(vec3.create(), vec3.add(vec3.create(), origin, direction), matrix);
+ const base = vec3.transformMat4(vec3.create(), origin, matrix);
+ return vec3.normalize(tip, vec3.subtract(tip, tip, base));
+}
+
+function assertVector3(value, message) {
+ if (!value || value.length !== 3 || !Array.from(value).every(Number.isFinite)) throw new TypeError(message);
+}
+
+/**
+ * `lights.headlights`: `[{ id, position, direction }]` in model space (direction defaults to forward).
+ * `lights.tailLights`: `[{ id, part|node|targets, signal: 'brake' | 'reverse', off, on }]`, emission strengths of the lamp material.
+ */
+function resolveLights(source, lights = {}, inverseRestFrame, forward) {
+ const headlights = (lights.headlights ?? []).map((light, index) => {
+ assertVector3(light.position, `Headlight ${index} needs a position of three finite numbers.`);
+ const direction = light.direction ?? forward;
+ assertVector3(direction, `Headlight ${index} direction must be three finite numbers.`);
+ const position = vec3.transformMat4(vec3.create(), light.position, inverseRestFrame);
+ return {
+ id: light.id ?? `headlight-${index}`,
+ positionLocal: Array.from(position),
+ directionLocal: Array.from(transformDirection(inverseRestFrame, light.position, direction)),
+ };
+ });
+ const tailLights = (lights.tailLights ?? []).map((light, index) => {
+ if (light.signal !== 'brake' && light.signal !== 'reverse') throw new RangeError(`Tail light ${index} signal must be brake or reverse.`);
+ const references = light.targets ?? light.parts ?? light.nodes ?? [light.target ?? light.part ?? light.node];
+ if (!Array.isArray(references) || references.length === 0 || references.includes(undefined)) {
+ throw new TypeError(`Tail light ${index} needs a part, node or targets list.`);
+ }
+ const { off = 0, on = 4 } = light;
+ if (!Number.isFinite(off) || !Number.isFinite(on) || off < 0 || on < 0) throw new RangeError(`Tail light ${index} off and on must be zero or positive.`);
+ const entries = references.map(reference => resolveEntry(source, reference));
+ return { id: light.id ?? entries[0].label, signal: light.signal, entries, off, on };
+ });
+ return { headlights: Object.freeze(headlights), tailLights: Object.freeze(tailLights) };
+}
+
/**
* Describes a vehicle: a rigid body plus wheels, found in `parts`, a scene `graph` or a loaded glTF `asset`.
* `wheels` is an array of `{ id, part|node|targets, side, axle, radius, center, steerable, spinSign }` or 'auto'.
+ * `lights` adds headlight positions and tail-light parts (see `resolveLights`).
*/
-export function createVehicleRig({ parts, graph, asset, body, wheels = 'auto', forwardAxis = '+z', wheelbase, maxSteerAngle = DEFAULT_MAX_STEER_ANGLE } = {}) {
+export function createVehicleRig({ parts, graph, asset, body, wheels = 'auto', forwardAxis = '+z', wheelbase, maxSteerAngle = DEFAULT_MAX_STEER_ANGLE, lights } = {}) {
const forward = FORWARD_AXES[forwardAxis];
if (!forward) throw new RangeError('forwardAxis must be one of +x, -x, +z or -z.');
assertFinitePositive(maxSteerAngle, 'maxSteerAngle must be positive radians.');
@@ -404,11 +445,17 @@ export function createVehicleRig({ parts, graph, asset, body, wheels = 'auto', f
const layout = resolveWheelLayout(measuredWheels, forward);
const bodyEntries = resolveBodyEntries(source, body, layout.wheels);
assertNoPivot([...bodyEntries, ...layout.wheels.flatMap(wheel => wheel.entries)]);
+ const resolvedWheelbase = resolveWheelbase(layout.wheels, wheelbase);
+ const referenceWheels = layout.wheels.some(wheel => wheel.axle === 'front') ? layout.wheels.filter(wheel => wheel.axle === 'front') : layout.wheels;
+ const lateralOffsets = referenceWheels.map(wheel => wheel.centerLocal[0]);
return Object.freeze({
kind: source.kind,
source,
forwardAxis,
- wheelbase: resolveWheelbase(layout.wheels, wheelbase),
+ wheelbase: resolvedWheelbase,
+ trackWidth: Math.max(...lateralOffsets) - Math.min(...lateralOffsets),
+ rearAxleZ: mean(referenceWheels.map(wheel => wheel.centerLocal[2])) - resolvedWheelbase,
+ ...resolveLights(source, lights, layout.inverseRestFrame, forward),
maxSteerAngle,
restFrame: layout.restFrame,
inverseRestFrame: layout.inverseRestFrame,
@@ -573,6 +620,7 @@ function createMotion(table, options) {
duration: startTime + duration,
distanceAt: time => clamp((time - startTime) * options.speed, 0, table.length),
speedAt: time => (time > startTime && time < startTime + duration ? options.speed : 0),
+ accelerationAt: () => 0,
};
}
const profile = createSpeedProfile(table, options);
@@ -598,6 +646,11 @@ function createMotion(table, options) {
const { index, elapsed, acceleration } = intervalAt(local);
return Math.max(0, profile.speeds[index] + acceleration * elapsed);
},
+ accelerationAt(time) {
+ const local = time - startTime;
+ if (local <= 0 || local >= profile.duration) return 0;
+ return intervalAt(local).acceleration;
+ },
};
}
@@ -610,8 +663,9 @@ function interpolateTable(values, spacing, distance) {
return Array.isArray(first) ? first.map((value, axis) => value + (second[axis] - value) * fraction) : first + (second - first) * fraction;
}
-function integrateBodyMotion(table, motion, options) {
- const steps = Math.ceil((motion.duration + SETTLE_SECONDS) / SIMULATION_STEP_SECONDS) + 1;
+// The timeline gives the signed forward speed and the curvature per metre driven forward at any time.
+function integrateBodyMotion(timeline, options) {
+ const steps = Math.ceil((timeline.duration + SETTLE_SECONDS) / SIMULATION_STEP_SECONDS) + 1;
const pitches = new Float64Array(steps);
const rolls = new Float64Array(steps);
const omega = 2 * Math.PI * options.bounceFrequency;
@@ -622,9 +676,9 @@ function integrateBodyMotion(table, motion, options) {
let rollRate = 0;
for (let step = 0; step < steps; step++) {
const time = step * SIMULATION_STEP_SECONDS;
- const speed = motion.speedAt(time);
- const longitudinal = (motion.speedAt(time + SIMULATION_STEP_SECONDS) - motion.speedAt(time - SIMULATION_STEP_SECONDS)) / (2 * SIMULATION_STEP_SECONDS);
- const curvature = interpolateTable(table.curvature, table.spacing, motion.distanceAt(time));
+ const speed = timeline.signedSpeedAt(time);
+ const longitudinal = (timeline.signedSpeedAt(time + SIMULATION_STEP_SECONDS) - timeline.signedSpeedAt(time - SIMULATION_STEP_SECONDS)) / (2 * SIMULATION_STEP_SECONDS);
+ const curvature = timeline.curvatureAt(time);
const pitchTarget = clamp(options.pitchGain * longitudinal, -MAX_BODY_ANGLE, MAX_BODY_ANGLE);
const rollTarget = clamp(options.rollGain * speed * speed * curvature, -MAX_BODY_ANGLE, MAX_BODY_ANGLE);
pitchRate += (omega ** 2 * (pitchTarget - pitch) - damping * pitchRate) * SIMULATION_STEP_SECONDS;
@@ -706,26 +760,90 @@ function suspensionOffsets(rig, bodyMatrix, groundHeight, travel) {
* `maxLateralAcceleration`. The vehicle holds the path end once it arrives.
*/
export function driveAlongPath(rig, path, options = {}) {
+ return driveAlongLegs(rig, [{ path }], options);
+}
+
+function buildLegs(legDescriptions, settings) {
+ const legs = [];
+ legDescriptions.forEach((description, index) => {
+ const { path, direction = 'forward', pause = 0 } = description ?? {};
+ if (direction !== 'forward' && direction !== 'reverse') throw new RangeError(`Leg ${index} direction must be forward or reverse.`);
+ if (!Number.isFinite(pause) || pause < 0) throw new RangeError(`Leg ${index} pause must be zero or positive seconds.`);
+ const previous = legs.at(-1);
+ const sign = direction === 'reverse' ? -1 : 1;
+ const startTime = previous ? previous.endTime + pause : settings.startTime;
+ const table = buildPathTable(path, settings.pathSamples);
+ // Every leg stops at both ends except the outer ones, which keep startSpeed and endSpeed.
+ const motion = createMotion(table, {
+ ...settings,
+ startTime,
+ startSpeed: previous ? 0 : settings.startSpeed,
+ endSpeed: index === legDescriptions.length - 1 ? settings.endSpeed : 0,
+ });
+ // A reversing car faces away from its direction of travel. The shift also keeps the heading continuous at a cusp.
+ let headingShift = direction === 'reverse' ? Math.PI : 0;
+ if (previous) {
+ const previousEnd = previous.headingShift + previous.table.headings.at(-1);
+ headingShift += Math.round((previousEnd - table.headings[0] - headingShift) / (2 * Math.PI)) * 2 * Math.PI;
+ }
+ legs.push({
+ table,
+ motion,
+ sign,
+ startTime,
+ endTime: motion.duration,
+ headingShift,
+ startForwardDistance: previous ? previous.startForwardDistance + previous.sign * previous.table.length : 0,
+ startHeadingChange: previous ? previous.startHeadingChange + previous.table.headings.at(-1) - previous.table.headings[0] : 0,
+ });
+ });
+ return legs;
+}
+
+/**
+ * Like `driveAlongPath` for a list of legs `[{ path, direction: 'forward' | 'reverse', pause }]` driven in order.
+ * The vehicle stops at the end of every leg. A reverse leg's path is the way the car travels; the car keeps facing the other way.
+ * `pose.distance` and `pose.speed` are signed along the car's forward axis, so they fall while reversing.
+ */
+export function driveAlongLegs(rig, legDescriptions, options = {}) {
if (!rig || !rig.wheels || !rig.restFrame) throw new TypeError('driveAlongPath needs a rig from createVehicleRig.');
+ if (!Array.isArray(legDescriptions) || legDescriptions.length === 0) throw new TypeError('legs must be a non-empty array of { path, direction, pause }.');
const settings = resolveDriveOptions(rig, options);
- const table = buildPathTable(path, settings.pathSamples);
- const motion = createMotion(table, settings);
- const dynamics = integrateBodyMotion(table, motion, settings);
+ const legs = buildLegs(legDescriptions, settings);
+ const duration = legs.at(-1).endTime;
+ const length = legs.reduce((total, leg) => total + leg.table.length, 0);
+ const legAt = time => legs.findLast(leg => leg.startTime <= time) ?? legs[0];
+ const timeline = {
+ duration,
+ signedSpeedAt(time) {
+ const leg = legAt(time);
+ return leg.sign * leg.motion.speedAt(time);
+ },
+ curvatureAt(time) {
+ const leg = legAt(time);
+ return leg.sign * interpolateTable(leg.table.curvature, leg.table.spacing, leg.motion.distanceAt(time));
+ },
+ };
+ const dynamics = integrateBodyMotion(timeline, settings);
const maxSteer = settings.maxSteerAngle;
function sample(seconds) {
if (!Number.isFinite(seconds)) throw new RangeError('Sample time must be finite.');
- const distance = motion.distanceAt(seconds);
- const heading = interpolateTable(table.headings, table.spacing, distance);
- const curvature = interpolateTable(table.curvature, table.spacing, distance);
- const headingChange = heading - table.headings[0];
+ const leg = legAt(seconds);
+ const { table, motion, sign } = leg;
+ const legDistance = motion.distanceAt(seconds);
+ const distance = leg.startForwardDistance + sign * legDistance;
+ const tableHeading = interpolateTable(table.headings, table.spacing, legDistance);
+ const heading = tableHeading + leg.headingShift;
+ const curvature = sign * interpolateTable(table.curvature, table.spacing, legDistance);
+ const headingChange = leg.startHeadingChange + tableHeading - table.headings[0];
const dynamicIndex = clamp(seconds, 0, (dynamics.pitches.length - 1) * SIMULATION_STEP_SECONDS) / SIMULATION_STEP_SECONDS;
const lower = Math.min(Math.floor(dynamicIndex), dynamics.pitches.length - 2);
const blend = dynamicIndex - lower;
const dynamicPitch = dynamics.pitches[lower] + (dynamics.pitches[lower + 1] - dynamics.pitches[lower]) * blend;
const dynamicRoll = dynamics.rolls[lower] + (dynamics.rolls[lower + 1] - dynamics.rolls[lower]) * blend;
- const position = interpolateTable(table.positions, table.spacing, distance);
- let slopePitch = interpolateTable(table.slopes, table.spacing, distance);
+ const position = interpolateTable(table.positions, table.spacing, legDistance);
+ let slopePitch = sign * interpolateTable(table.slopes, table.spacing, legDistance);
let slopeRoll = 0;
if (settings.groundHeight) {
const plane = groundPlane(rig, position, heading, settings.groundHeight);
@@ -738,10 +856,13 @@ export function driveAlongPath(rig, path, options = {}) {
const bodyMatrix = bodyWorldMatrix(position, heading, pitch, roll);
const offsets = suspensionOffsets(rig, bodyMatrix, settings.groundHeight, settings.suspensionTravel);
const steerAngle = clamp(Math.atan(rig.wheelbase * curvature), -maxSteer, maxSteer);
+ const wheelSteerAngles = settings.steering === 'ackermann' ? ackermannSteerAngles(rig, steerAngle) : null;
+ const speed = sign * motion.speedAt(seconds);
return {
time: seconds,
distance,
- speed: motion.speedAt(seconds),
+ speed,
+ direction: sign,
position,
heading,
pitch,
@@ -750,13 +871,17 @@ export function driveAlongPath(rig, path, options = {}) {
wheels: rig.wheels.map((wheel, index) => ({
id: wheel.id,
spinAngle: wheel.spinSign * (distance - wheel.centerLocal[0] * headingChange) / wheel.radius,
- steerAngle: wheel.steerable ? steerAngle : 0,
+ steerAngle: wheelSteerAngles ? wheelSteerAngles[index] : wheel.steerable ? steerAngle : 0,
suspensionOffset: offsets[index],
})),
+ lights: computeLightSignals(rig, bodyMatrix, {
+ braking: Math.max(0, -motion.accelerationAt(seconds)) / settings.fullBrakeDeceleration,
+ reversing: sign < 0 && speed !== 0,
+ }),
};
}
- return Object.freeze({ rig, length: table.length, duration: motion.duration, sample });
+ return Object.freeze({ rig, length, duration, sample });
}
function resolveDriveOptions(rig, options) {
@@ -764,7 +889,10 @@ function resolveDriveOptions(rig, options) {
speed, cruiseSpeed, acceleration = 3, braking = 4, startSpeed = 0, endSpeed = 0, maxLateralAcceleration = Infinity,
startTime = 0, groundHeight, maxSteerAngle = rig.maxSteerAngle, rollGain = 0.012, pitchGain = 0.012,
bounceFrequency = 1.6, dampingRatio = 0.45, suspensionTravel = 0.2, pathSamples = 400,
+ steering = 'ackermann', fullBrakeDeceleration = braking,
} = options;
+ if (steering !== 'ackermann' && steering !== 'shared') throw new RangeError("steering must be 'ackermann' or 'shared'.");
+ assertFinitePositive(fullBrakeDeceleration, 'fullBrakeDeceleration must be positive and finite.');
if ((speed === undefined) === (cruiseSpeed === undefined)) {
throw new TypeError('Give exactly one of speed (constant) or cruiseSpeed (with acceleration and braking).');
}
@@ -786,10 +914,59 @@ function resolveDriveOptions(rig, options) {
if (!Number.isInteger(pathSamples) || pathSamples < 8) throw new RangeError('pathSamples must be an integer of at least 8.');
return {
speed, cruiseSpeed, acceleration, braking, startSpeed, endSpeed, maxLateralAcceleration, startTime, groundHeight,
- maxSteerAngle, rollGain, pitchGain, bounceFrequency, dampingRatio, suspensionTravel, pathSamples,
+ maxSteerAngle, rollGain, pitchGain, bounceFrequency, dampingRatio, suspensionTravel, pathSamples, steering, fullBrakeDeceleration,
};
}
+// Each steerable wheel turns about the one centre the body turns about: tan(angle) = ahead / (turning radius - sideways).
+function ackermannSteerAngles(rig, centerAngle) {
+ const tangent = Math.tan(centerAngle);
+ return rig.wheels.map(wheel => {
+ if (!wheel.steerable) return 0;
+ const ahead = wheel.centerLocal[2] - rig.rearAxleZ;
+ return Math.atan2(ahead * tangent, rig.wheelbase - wheel.centerLocal[0] * tangent);
+ });
+}
+
+function computeLightSignals(rig, bodyMatrix, { braking, reversing }) {
+ return {
+ brake: clamp(braking, 0, 1),
+ reverse: reversing ? 1 : 0,
+ headlights: rig.headlights.map(light => ({
+ id: light.id,
+ position: Array.from(vec3.transformMat4(vec3.create(), light.positionLocal, bodyMatrix)),
+ direction: Array.from(transformDirection(bodyMatrix, light.positionLocal, light.directionLocal)),
+ })),
+ };
+}
+
+/** Emission strength per tail light for a pose: `off` plus `(on - off)` times the brake or reverse signal. */
+export function computeVehicleLightEmissions(rig, pose) {
+ if (!pose || !pose.lights) throw new TypeError('pose must come from drive.sample for this rig.');
+ return rig.tailLights.map(light => ({
+ id: light.id,
+ emission: light.off + (light.on - light.off) * pose.lights[light.signal],
+ entries: light.entries,
+ }));
+}
+
+/**
+ * Sets `material.emission` on the parts of every tail light for a rig built from `parts`. Give each lamp its own material:
+ * parts that share one material share the glow. Call `renderer.updateParts` afterwards. Returns the parts changed.
+ */
+export function applyVehicleLightsToParts(rig, pose) {
+ assertKind(rig, 'parts');
+ const changed = [];
+ for (const { id, emission, entries } of computeVehicleLightEmissions(rig, pose)) {
+ for (const { key: part } of entries) {
+ if (!part.material) throw new TypeError(`Tail light ${id} part needs a material to set emission on.`);
+ part.material.emission = emission;
+ changed.push(part);
+ }
+ }
+ return changed;
+}
+
function wheelMotionMatrix(wheel, wheelPose) {
const matrix = mat4.fromTranslation(createMatrix(), [0, wheelPose.suspensionOffset, 0]);
mat4.translate(matrix, matrix, wheel.centerLocal);
diff --git a/tests/vehicle-steering-lights.test.js b/tests/vehicle-steering-lights.test.js
new file mode 100644
index 00000000..ec95f540
--- /dev/null
+++ b/tests/vehicle-steering-lights.test.js
@@ -0,0 +1,219 @@
+import test from 'node:test';
+import assert from 'node:assert/strict';
+import { mat4 } from 'gl-matrix';
+import {
+ applyVehicleLightsToParts,
+ computeVehicleLightEmissions,
+ createVehicleRig,
+ driveAlongLegs,
+ driveAlongPath,
+} from '../src/vehicle.js';
+
+const WHEEL_RADIUS = 0.35;
+const HALF_TRACK = 0.8;
+const WHEELBASE = 2.4;
+const WHEEL_CORNERS = [
+ { id: 'FL', x: HALF_TRACK, z: WHEELBASE / 2 },
+ { id: 'FR', x: -HALF_TRACK, z: WHEELBASE / 2 },
+ { id: 'RL', x: HALF_TRACK, z: -WHEELBASE / 2 },
+ { id: 'RR', x: -HALF_TRACK, z: -WHEELBASE / 2 },
+];
+const HEADLIGHT_POSITION = [0.5, 0.7, 1.8];
+
+function boxGeometry(center, size) {
+ const positions = [];
+ for (const sx of [-1, 1]) {
+ for (const sy of [-1, 1]) {
+ for (const sz of [-1, 1]) {
+ positions.push(center[0] + sx * size[0] / 2, center[1] + sy * size[1] / 2, center[2] + sz * size[2] / 2);
+ }
+ }
+ }
+ return { positions: new Float32Array(positions) };
+}
+
+function wheelGeometry(center) {
+ const positions = [];
+ for (let step = 0; step < 24; step++) {
+ const angle = step / 24 * Math.PI * 2;
+ for (const side of [-1, 1]) {
+ positions.push(center[0] + side * 0.1, center[1] + Math.sin(angle) * WHEEL_RADIUS, center[2] + Math.cos(angle) * WHEEL_RADIUS);
+ }
+ }
+ return { positions: new Float32Array(positions) };
+}
+
+function createLitRig(steeringOptions = {}) {
+ const body = { name: 'Body', geometry: boxGeometry([0, 0.8, 0], [1.6, 0.5, 3.4]), matrix: mat4.create() };
+ const brakeLamp = { name: 'BrakeLamp', geometry: boxGeometry([0, 0.9, -1.7], [0.4, 0.1, 0.05]), matrix: mat4.create(), material: { emission: 0 } };
+ const reverseLamp = { name: 'ReverseLamp', geometry: boxGeometry([0.5, 0.9, -1.7], [0.1, 0.1, 0.05]), matrix: mat4.create(), material: { emission: 0 } };
+ const wheels = WHEEL_CORNERS.map(corner => ({
+ name: `Wheel_${corner.id}`,
+ geometry: wheelGeometry([corner.x, WHEEL_RADIUS, corner.z]),
+ matrix: mat4.create(),
+ }));
+ const rig = createVehicleRig({
+ parts: [body, brakeLamp, reverseLamp, ...wheels],
+ body: [body, brakeLamp, reverseLamp],
+ wheels: wheels.map((part, index) => ({ id: WHEEL_CORNERS[index].id, part })),
+ lights: {
+ headlights: [{ id: 'left', position: HEADLIGHT_POSITION }],
+ tailLights: [
+ { part: brakeLamp, signal: 'brake', off: 0.2, on: 5 },
+ { part: reverseLamp, signal: 'reverse', on: 2 },
+ ],
+ },
+ ...steeringOptions,
+ });
+ return { rig, brakeLamp, reverseLamp };
+}
+
+function tightArc(direction) {
+ const points = [];
+ for (let step = 0; step <= 16; step++) {
+ const angle = step / 16 * Math.PI / 2;
+ points.push([direction * 3 * (1 - Math.cos(angle)), 0, 3 * Math.sin(angle)]);
+ }
+ return points;
+}
+
+const REVERSING_LEGS = [
+ { path: [[0, 0, 0], [0, 0, 5], [0, 0, 10]] },
+ { path: [[0, 0, 10], [0, 0, 7], [0, 0, 4]], direction: 'reverse', pause: 0.5 },
+];
+const DRIVE_OPTIONS = { cruiseSpeed: 4, acceleration: 2, braking: 2 };
+
+test('full lock turns the inner front wheel by the Ackermann angle for the measured wheelbase and track', () => {
+ const { rig } = createLitRig();
+ assert.ok(Math.abs(rig.trackWidth - 2 * HALF_TRACK) < 1e-6);
+ const turningRadius = rig.wheelbase / Math.tan(rig.maxSteerAngle);
+ for (const direction of [1, -1]) {
+ const pose = driveAlongPath(rig, tightArc(direction), { speed: 2 }).sample(1.5);
+ assert.ok(Math.abs(Math.abs(pose.steerAngle) - rig.maxSteerAngle) < 1e-9, 'the path is tight enough to reach full lock');
+ const [frontLeft, frontRight] = pose.wheels;
+ const inner = direction > 0 ? frontLeft : frontRight;
+ const outer = direction > 0 ? frontRight : frontLeft;
+ assert.ok(Math.abs(inner.steerAngle) > Math.abs(outer.steerAngle));
+ assert.ok(Math.abs(Math.abs(inner.steerAngle) - Math.atan(rig.wheelbase / (turningRadius - rig.trackWidth / 2))) < 1e-6);
+ assert.ok(Math.abs(Math.abs(outer.steerAngle) - Math.atan(rig.wheelbase / (turningRadius + rig.trackWidth / 2))) < 1e-6);
+ assert.equal(pose.wheels[2].steerAngle, 0);
+ }
+});
+
+test('a reverse leg backs the car along its path with the heading kept and the wheels spinning backwards', () => {
+ const { rig } = createLitRig();
+ const drive = driveAlongLegs(rig, REVERSING_LEGS, DRIVE_OPTIONS);
+ const forwardLeg = driveAlongPath(rig, REVERSING_LEGS[0].path, DRIVE_OPTIONS);
+ const turnaround = drive.sample(forwardLeg.duration);
+ assert.equal(turnaround.speed, 0);
+ assert.equal(turnaround.lights.reverse, 0);
+ const earlier = drive.sample(forwardLeg.duration + 0.5 + 1.5);
+ const later = drive.sample(forwardLeg.duration + 0.5 + 2.5);
+ assert.ok(later.position[2] < earlier.position[2]);
+ assert.ok(earlier.position[2] < 10);
+ assert.ok(later.speed < 0);
+ assert.equal(later.lights.reverse, 1);
+ assert.ok(Math.abs(Math.sin(later.heading)) < 1e-6 && Math.cos(later.heading) > 0, 'still facing +z');
+ assert.ok(later.heading === earlier.heading);
+ for (let index = 0; index < 4; index++) {
+ assert.ok(later.wheels[index].spinAngle < earlier.wheels[index].spinAngle);
+ }
+ const end = drive.sample(drive.duration + 1);
+ assert.ok(Math.abs(end.position[2] - 4) < 1e-6);
+ assert.ok(Math.abs(end.distance - 4) < 1e-6);
+});
+
+test('a reversing car steers the opposite way for the same path curvature', () => {
+ const { rig } = createLitRig();
+ const bend = [[0, 0, 0], [0, 0, 5], [2, 0, 10]];
+ const forward = driveAlongLegs(rig, [{ path: bend }], { speed: 2 }).sample(2.5);
+ const reverse = driveAlongLegs(rig, [{ path: bend, direction: 'reverse' }], { speed: 2 }).sample(2.5);
+ assert.ok(forward.steerAngle * reverse.steerAngle < 0);
+ assert.ok(Math.abs(Math.abs(reverse.heading - forward.heading) - Math.PI) < 1e-9);
+});
+
+test('wheel spin times radius equals the distance driven, forwards and backwards', () => {
+ const { rig } = createLitRig();
+ const drive = driveAlongLegs(rig, REVERSING_LEGS, DRIVE_OPTIONS);
+ for (let time = 0.5; time < drive.duration; time += 0.37) {
+ const pose = drive.sample(time);
+ for (const wheel of pose.wheels) {
+ const rolled = wheel.spinAngle * WHEEL_RADIUS;
+ assert.ok(Math.abs(rolled - pose.distance) <= 0.02 * Math.max(Math.abs(pose.distance), 1e-9) + 1e-9, `t=${time}`);
+ }
+ }
+});
+
+test('the brake signal is on exactly while the car slows down', () => {
+ const { rig } = createLitRig();
+ const drive = driveAlongLegs(rig, REVERSING_LEGS, DRIVE_OPTIONS);
+ let slowing = 0;
+ let notSlowing = 0;
+ for (let time = 0.05; time < drive.duration; time += 0.05) {
+ const rate = (Math.abs(drive.sample(time + 0.01).speed) - Math.abs(drive.sample(time - 0.01).speed)) / 0.02;
+ const brake = drive.sample(time).lights.brake;
+ assert.ok(brake >= 0 && brake <= 1);
+ if (rate < -0.5) {
+ slowing++;
+ assert.ok(brake > 0, `t=${time}`);
+ } else if (rate > -0.01) {
+ notSlowing++;
+ assert.equal(brake, 0, `t=${time}`);
+ }
+ }
+ assert.ok(slowing > 10 && notSlowing > 10);
+});
+
+test('headlights follow the body position and heading', () => {
+ const { rig } = createLitRig();
+ const drive = driveAlongPath(rig, [[0, 0, 0], [0, 0, 8], [6, 0, 14]], { speed: 3 });
+ const straight = driveAlongPath(rig, [[0, 0, 0], [0, 0, 8]], { speed: 3 });
+ const start = straight.sample(0).lights.headlights[0];
+ start.position.forEach((value, axis) => assert.ok(Math.abs(value - HEADLIGHT_POSITION[axis]) < 0.02));
+ assert.ok(Math.abs(start.direction[2] - 1) < 0.02);
+ assert.ok(Math.abs(straight.sample(1).lights.headlights[0].position[2] - (HEADLIGHT_POSITION[2] + 3)) < 0.02);
+ const pose = drive.sample(3.5);
+ const light = pose.lights.headlights[0];
+ assert.ok(Math.abs(light.direction[0] - Math.sin(pose.heading)) < 0.05);
+ assert.ok(Math.abs(light.direction[2] - Math.cos(pose.heading)) < 0.05);
+ const offsetX = light.position[0] - pose.position[0];
+ const offsetZ = light.position[2] - pose.position[2];
+ assert.ok(Math.abs(Math.hypot(offsetX, offsetZ) - Math.hypot(HEADLIGHT_POSITION[0], HEADLIGHT_POSITION[2])) < 0.05);
+});
+
+test('tail-light emission follows the brake and reverse signals and can be applied to parts', () => {
+ const { rig, brakeLamp, reverseLamp } = createLitRig();
+ const drive = driveAlongLegs(rig, REVERSING_LEGS, DRIVE_OPTIONS);
+ const cruising = drive.sample(1.8);
+ assert.equal(cruising.lights.brake, 0);
+ assert.equal(applyVehicleLightsToParts(rig, cruising).length, 2);
+ assert.equal(brakeLamp.material.emission, 0.2);
+ assert.equal(reverseLamp.material.emission, 0);
+ let brakingTime = 0;
+ while (drive.sample(brakingTime).lights.brake === 0) brakingTime += 0.05;
+ const braking = drive.sample(brakingTime);
+ applyVehicleLightsToParts(rig, braking);
+ assert.ok(brakeLamp.material.emission > 0.2);
+ const reversing = drive.sample(drive.duration - 1.5);
+ const emissions = computeVehicleLightEmissions(rig, reversing);
+ assert.equal(emissions.find(item => item.id === 'ReverseLamp').emission, 2);
+});
+
+test('legs and lights sample the same pose whatever order they are read in', () => {
+ const { rig } = createLitRig();
+ const drive = driveAlongLegs(rig, REVERSING_LEGS, DRIVE_OPTIONS);
+ const times = [0, 1.1, 2.4, 3.3, 4.6, 6, 9, 20];
+ const inOrder = times.map(time => drive.sample(time));
+ const shuffled = [...times].reverse().map(time => drive.sample(time)).reverse();
+ assert.deepEqual(shuffled, inOrder);
+ const again = driveAlongLegs(rig, REVERSING_LEGS, DRIVE_OPTIONS);
+ assert.deepEqual(again.sample(4.6), inOrder[4]);
+});
+
+test('bad steering, light and leg input produces specific errors', () => {
+ const { rig } = createLitRig();
+ assert.throws(() => driveAlongPath(rig, [[0, 0, 0], [0, 0, 5]], { speed: 1, steering: 'both' }), /steering must be/);
+ assert.throws(() => driveAlongLegs(rig, [], { speed: 1 }), /non-empty array of/);
+ assert.throws(() => driveAlongLegs(rig, [{ path: REVERSING_LEGS[0].path, direction: 'sideways' }], { speed: 1 }), /direction must be/);
+ assert.throws(() => createVehicleRig({ parts: [{ name: 'Wheel_FL', matrix: mat4.create() }], lights: { tailLights: [{ part: 0, signal: 'fog' }] } }), /at least two wheels|signal/);
+});
diff --git a/tests/vehicle.test.js b/tests/vehicle.test.js
index 0c5c5072..45cecddc 100644
--- a/tests/vehicle.test.js
+++ b/tests/vehicle.test.js
@@ -128,7 +128,10 @@ test('steering turns the front wheels toward the turn and leaves the rear straig
assert.ok(leftPose.heading > 0.1);
assert.ok(rightPose.heading < -0.1);
assert.equal(leftPose.wheels[2].steerAngle, 0);
- assert.equal(leftPose.wheels[0].steerAngle, leftPose.steerAngle);
+ assert.ok(leftPose.wheels[0].steerAngle > leftPose.steerAngle);
+ const sharedPose = driveAlongPath(rig, arcPath(10, 1), { speed: 5, steering: 'shared' }).sample(1.5);
+ assert.equal(sharedPose.wheels[0].steerAngle, sharedPose.steerAngle);
+ assert.equal(sharedPose.wheels[1].steerAngle, sharedPose.steerAngle);
});
test('front wheel matrices yaw toward the turn relative to the body', () => {