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.jsonindex 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.mdnew file mode 100644index 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.mdindex 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.jsindex 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', fconst 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.jsnew file mode 100644index 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.jsindex 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 straigassert.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', () => {