3D engine that AI agents drive
JavaScript 3d3d-engineagentic-aiai-agentsblenderclaude-codegame-enginegaussian-splattinggltfjavascriptmcpmcp-servermodel-context-protocolpath-tracerpath-tracingpbrrenderingthreejs-alternativewebgl2webgpu
import { mat4, vec3 } from 'gl-matrix';
import { invalidField } from './field-error.js';
export { crashImpulse, dentDepth, dentGeometry, detachedPartFlight, detachedPartIds, findDriveImpact, hingedDoorSwing } from './vehicle-damage.js';
// Local vehicle frame: x is left, y is up, z is forward, origin on the ground under the wheel centroid.
const FORWARD_AXES = Object.freeze({
'+x': [1, 0, 0],
'-x': [-1, 0, 0],
'+z': [0, 0, 1],
'-z': [0, 0, -1],
});
const UP_AXIS = Object.freeze([0, 1, 0]);
const DEFAULT_MAX_STEER_ANGLE = 35 * Math.PI / 180;
const MAX_BODY_ANGLE = 0.35;
const SIMULATION_STEP_SECONDS = 1 / 240;
const SETTLE_SECONDS = 2;
// The body-motion integrator keeps one step per 1/240 s of the drive, so a drive of hours would allocate gigabytes.
const MAX_DRIVE_SECONDS = 3600;
const PATH_SPACING_METRES = 0.1;
const MAX_PATH_SAMPLES = 20000;
const MINIMUM_SPEED_LIMIT = 0.5;
const WHEEL_WORDS = new Map([
['wheel', 'wheel'], ['wheels', 'wheel'],
['tire', 'tire'], ['tires', 'tire'], ['tyre', 'tire'], ['tyres', 'tire'],
['rim', 'rim'], ['rims', 'rim'], ['hub', 'rim'], ['hubcap', 'rim'], ['hubcaps', 'rim'],
]);
const NON_ROLLING_WORDS = new Set([
'steering', 'spare', 'fender', 'fenders', 'mudguard', 'arch', 'arches', 'well', 'wells',
'guard', 'brake', 'brakes', 'caliper', 'rotor', 'disc', 'housing', 'cover', 'shield',
]);
const CORNER_WORDS = new Map([
['fl', ['front', 'left']], ['fr', ['front', 'right']], ['rl', ['rear', 'left']], ['rr', ['rear', 'right']],
['bl', ['rear', 'left']], ['br', ['rear', 'right']], ['lf', ['front', 'left']], ['rf', ['front', 'right']],
['lr', ['rear', 'left']], ['lb', ['rear', 'left']], ['rb', ['rear', 'right']],
]);
const AXLE_WORDS = new Map([['front', 'front'], ['rear', 'rear'], ['back', 'rear']]);
const SIDE_WORDS = new Map([['left', 'left'], ['l', 'left'], ['right', 'right'], ['r', 'right']]);
function createMatrix() {
return mat4.identity(new Float64Array(16));
}
function assertFinitePositive(value, path, example = '1') {
if (!Number.isFinite(value) || value <= 0) throw invalidField(path, value, `a finite number above 0, such as ${example}`);
}
function assertFiniteNumber(value, path, example) {
if (!Number.isFinite(value)) throw invalidField(path, value, `a finite number, such as ${example}`);
}
function clamp(value, minimum, maximum) {
return Math.min(maximum, Math.max(minimum, value));
}
function nameTokens(name) {
return name
.replace(/([a-z0-9])([A-Z])/g, '$1 $2')
.replace(/([A-Z]+)([A-Z][a-z])/g, '$1 $2')
.toLowerCase()
.split(/[^a-z0-9]+/)
.filter(Boolean);
}
function cornerFromTokens(tokens) {
let axle = null;
let side = null;
for (const token of tokens) {
const corner = CORNER_WORDS.get(token);
if (corner) {
axle ??= corner[0];
side ??= corner[1];
}
axle ??= AXLE_WORDS.get(token) ?? null;
side ??= SIDE_WORDS.get(token) ?? null;
}
return { axle, side };
}
/**
* Reports which names look like rolling wheels and which wheel-like names were rejected, so an agent can confirm.
* Each match carries `kind` (wheel, tire or rim), `side` and `axle` when the name states them.
*/
export function findWheelCandidates(names) {
if (!Array.isArray(names) || names.some(name => typeof name !== 'string')) {
throw new TypeError('findWheelCandidates needs an array of name strings.');
}
const wheels = [];
const rejected = [];
names.forEach((name, index) => {
const tokens = nameTokens(name);
const wheelToken = tokens.find(token => WHEEL_WORDS.has(token));
if (!wheelToken) return;
const blocker = tokens.find(token => NON_ROLLING_WORDS.has(token));
if (blocker) {
rejected.push({ index, name, reason: `contains "${blocker}", which does not roll with the wheel` });
return;
}
const { axle, side } = cornerFromTokens(tokens);
const corner = axle && side ? `${axle}-${side}` : null;
wheels.push({ index, name, kind: WHEEL_WORDS.get(wheelToken), axle, side, corner });
});
return { wheels, rejected };
}
function createPartsSource(parts) {
if (!Array.isArray(parts) || parts.length === 0) {
throw new TypeError('parts must be a non-empty array of parts with a 16-value matrix.');
}
const entries = parts.map((part, index) => {
if (!part || !part.matrix || part.matrix.length !== 16) {
throw new TypeError(`Part ${index} needs a 16-value matrix.`);
}
const restWorld = Float64Array.from(part.matrix);
return {
key: part,
label: String(part.name ?? `part-${index}`),
parentKey: null,
restWorld,
meshes: part.geometry ? [{ geometry: part.geometry, matrix: restWorld }] : [],
};
});
return finishSource('parts', entries);
}
function createGraphSource(graph) {
if (!graph || !graph.nodes || graph.nodes.length === 0) {
throw new TypeError('graph must be a scene graph with nodes.');
}
const entries = Array.from(graph.nodes, node => {
const restWorld = Float64Array.from(node.worldMatrix);
return {
key: node.name,
label: node.name,
parentKey: node.parent,
restWorld,
pivot: node.pivot,
meshes: node.geometry ? [{ geometry: node.geometry, matrix: restWorld }] : [],
};
});
return finishSource('graph', entries);
}
function composeNodeMatrix(metadata) {
const matrix = createMatrix();
mat4.fromRotationTranslationScale(matrix, metadata.rotation, metadata.translation, metadata.scale);
return matrix;
}
function createAssetSource(asset) {
if (!asset || !asset.nodes || !asset.parts || typeof asset.stagePose !== 'function') {
throw new TypeError('asset must be a loadGltf result with nodes, parts and stagePose.');
}
const metadataById = new Map(asset.nodes.map(metadata => [metadata.id, metadata]));
const worlds = new Map();
function worldOf(id) {
if (worlds.has(id)) return worlds.get(id);
const metadata = metadataById.get(id);
const local = composeNodeMatrix(metadata);
const world = metadata.parent === null ? local : mat4.multiply(createMatrix(), worldOf(metadata.parent), local);
worlds.set(id, world);
return world;
}
const entries = asset.nodes.map(metadata => ({
key: metadata.id,
label: metadata.label,
parentKey: metadata.parent,
restWorld: worldOf(metadata.id),
editable: metadata.editable,
meshes: (metadata.meshIds ?? []).map(meshId => {
const part = asset.parts[Number(meshId.slice(5))];
return { geometry: part.geometry, matrix: Float64Array.from(part.matrix) };
}),
}));
return finishSource('asset', entries);
}
function finishSource(kind, entries) {
const byKey = new Map(entries.map(entry => [entry.key, entry]));
for (const entry of entries) entry.childKeys = [];
for (const entry of entries) {
if (entry.parentKey !== null && byKey.has(entry.parentKey)) byKey.get(entry.parentKey).childKeys.push(entry.key);
}
return { kind, entries, byKey };
}
function createSource({ parts, graph, asset }) {
const supplied = [parts, graph, asset].filter(value => value !== undefined);
if (supplied.length !== 1) {
throw new TypeError('createVehicleRig needs exactly one of parts, graph or asset.');
}
if (parts !== undefined) return createPartsSource(parts);
if (graph !== undefined) return createGraphSource(graph);
return createAssetSource(asset);
}
function resolveEntry(source, reference, path = 'target') {
if (source.byKey.has(reference)) return source.byKey.get(reference);
if (Number.isInteger(reference) && source.kind === 'parts' && source.entries[reference]) return source.entries[reference];
if (typeof reference === 'string') {
const matches = source.entries.filter(entry => entry.label === reference);
if (matches.length === 1) return matches[0];
if (matches.length > 1) throw invalidField(path, reference, `a ${source.kind} name that is used once; this name matches ${matches.length}, such as an id from scene_inspect`);
}
throw invalidField(path, reference?.name ?? reference, `the name of a ${source.kind === 'parts' ? 'part' : 'node'} in the asset, such as one scene_inspect lists`);
}
function collectMeshes(source, entry, meshes = []) {
meshes.push(...entry.meshes);
for (const childKey of entry.childKeys) collectMeshes(source, source.byKey.get(childKey), meshes);
return meshes;
}
function meshBounds(meshes) {
const minimum = [Infinity, Infinity, Infinity];
const maximum = [-Infinity, -Infinity, -Infinity];
const point = vec3.create();
for (const { geometry, matrix } of meshes) {
const positions = geometry.positions;
for (let offset = 0; offset + 2 < positions.length; offset += 3) {
vec3.set(point, positions[offset], positions[offset + 1], positions[offset + 2]);
vec3.transformMat4(point, point, matrix);
for (let axis = 0; axis < 3; axis++) {
minimum[axis] = Math.min(minimum[axis], point[axis]);
maximum[axis] = Math.max(maximum[axis], point[axis]);
}
}
}
return Number.isFinite(minimum[0]) ? { minimum, maximum } : null;
}
function extentAlong(bounds, axisVector) {
let extent = 0;
for (let axis = 0; axis < 3; axis++) {
extent += Math.abs(axisVector[axis]) * (bounds.maximum[axis] - bounds.minimum[axis]);
}
return extent;
}
function measureWheel(bounds, forward) {
const center = bounds.minimum.map((value, axis) => (value + bounds.maximum[axis]) / 2);
const radius = Math.max(extentAlong(bounds, UP_AXIS), extentAlong(bounds, forward)) / 2;
return { center, radius };
}
function isDescendantOfAny(source, entry, keys) {
let parentKey = entry.parentKey;
while (parentKey !== null && source.byKey.has(parentKey)) {
if (keys.has(parentKey)) return true;
parentKey = source.byKey.get(parentKey).parentKey;
}
return false;
}
function detectWheelGroups(source, forward) {
const labels = source.entries.map(entry => entry.label);
const { wheels: candidates, rejected } = findWheelCandidates(labels);
const candidateKeys = new Set(candidates.map(candidate => source.entries[candidate.index].key));
const independent = candidates.filter(candidate => !isDescendantOfAny(source, source.entries[candidate.index], candidateKeys));
const groups = new Map();
const loose = [];
for (const candidate of independent) {
if (candidate.corner) {
const group = groups.get(candidate.corner) ?? { id: candidate.corner, axle: candidate.axle, side: candidate.side, entries: [] };
group.entries.push(source.entries[candidate.index]);
groups.set(candidate.corner, group);
} else {
loose.push(source.entries[candidate.index]);
}
}
const measured = entry => {
const bounds = meshBounds(collectMeshes(source, entry));
return bounds ? { entry, ...measureWheel(bounds, forward) } : { entry, center: null, radius: 0 };
};
const clusters = Array.from(groups.values()).map(group => {
const largest = group.entries.map(measured).sort((first, second) => second.radius - first.radius)[0];
return { ...group, center: largest.center, radius: largest.radius };
});
for (const item of loose.map(measured).sort((first, second) => second.radius - first.radius)) {
const home = item.center && clusters.find(cluster => cluster.center && vec3.distance(cluster.center, item.center) < cluster.radius * 0.6);
if (home) {
home.entries.push(item.entry);
} else {
clusters.push({ id: `wheel-${clusters.length}`, axle: null, side: null, entries: [item.entry], center: item.center, radius: item.radius });
}
}
return { groups: clusters, rejected };
}
function normalizeWheelSpec(source, spec, index) {
const path = `wheels[${index}]`;
const references = spec.targets ?? spec.parts ?? spec.nodes ?? [spec.target ?? spec.part ?? spec.node];
if (!Array.isArray(references) || references.length === 0 || references.includes(undefined)) {
throw invalidField(`${path}.node`, spec.node, 'a node, part or targets list, such as "wheel-0"', TypeError);
}
const entries = references.map(reference => resolveEntry(source, reference, `${path}.node`));
return { id: spec.id ?? entries[0].label, index, entries, axle: spec.axle, side: spec.side, spec };
}
function describeWheels(source, wheelOption, forward) {
if (wheelOption === undefined || wheelOption === 'auto') {
const { groups, rejected } = detectWheelGroups(source, forward);
return { described: groups.map((group, index) => ({ id: group.id, index, entries: group.entries, axle: group.axle, side: group.side, spec: {} })), rejected };
}
if (!Array.isArray(wheelOption)) throw invalidField('wheels', wheelOption, `"auto", or an array of wheel descriptions, such as "auto"`, TypeError);
return { described: wheelOption.map((spec, index) => normalizeWheelSpec(source, spec, index)), rejected: [] };
}
function measureDescribedWheel(source, described, forward) {
const bounds = meshBounds(described.entries.flatMap(entry => collectMeshes(source, entry)));
const measured = bounds ? measureWheel(bounds, forward) : null;
const center = described.spec.center ?? measured?.center;
const radius = described.spec.radius ?? measured?.radius;
if (!center || radius === undefined) {
throw invalidField(`wheels[${described.index}].radius`, radius, 'a radius with a center, or geometry to measure them from, such as 0.35');
}
if (center.length !== 3 || !Array.from(center).every(Number.isFinite)) {
throw invalidField(`wheels[${described.index}].center`, center, 'an array of 3 finite numbers, such as [0.8, 0.35, 1.2]');
}
assertFinitePositive(radius, `wheels[${described.index}].radius`, '0.35');
return { ...described, center: Array.from(center), radius };
}
function createRestFrame(left, forward, anchor) {
const frame = createMatrix();
frame.set([...left, 0, ...UP_AXIS, 0, ...forward, 0, ...anchor, 1]);
return frame;
}
function mean(values) {
return values.reduce((total, value) => total + value, 0) / values.length;
}
function resolveWheelLayout(measuredWheels, forward) {
const left = vec3.cross(vec3.create(), UP_AXIS, forward);
const centroid = [0, 1, 2].map(axis => mean(measuredWheels.map(wheel => wheel.center[axis])));
const anchor = vec3.scaleAndAdd(vec3.create(), centroid, UP_AXIS, -mean(measuredWheels.map(wheel => wheel.radius)));
const restFrame = createRestFrame(left, forward, anchor);
const inverseRestFrame = mat4.invert(createMatrix(), restFrame);
const wheels = measuredWheels.map((wheel, index) => {
const centerLocal = vec3.transformMat4(vec3.create(), wheel.center, inverseRestFrame);
const side = wheel.side ?? (Math.abs(centerLocal[0]) < 1e-4 ? 'center' : centerLocal[0] > 0 ? 'left' : 'right');
const axle = wheel.axle ?? (centerLocal[2] >= 0 ? 'front' : 'rear');
if (!['left', 'right', 'center'].includes(side)) throw invalidField(`wheels[${wheel.index}].side`, side, 'one of left, right, center');
if (!['front', 'rear'].includes(axle)) throw invalidField(`wheels[${wheel.index}].axle`, axle, 'one of front, rear');
const spinSign = wheel.spec.spinSign ?? 1;
if (spinSign !== 1 && spinSign !== -1) throw invalidField(`wheels[${wheel.index}].spinSign`, spinSign, 'one of 1, -1');
return {
id: wheel.id || `wheel-${index}`,
entries: wheel.entries,
side,
axle,
radius: wheel.radius,
steerable: wheel.spec.steerable ?? axle === 'front',
spinSign,
centerLocal: Array.from(centerLocal),
};
});
return { wheels, restFrame, inverseRestFrame };
}
function resolveWheelbase(wheels, wheelbaseOption) {
if (wheelbaseOption !== undefined) {
assertFinitePositive(wheelbaseOption, 'wheelbase', '2.7');
return wheelbaseOption;
}
const front = wheels.filter(wheel => wheel.axle === 'front');
const rear = wheels.filter(wheel => wheel.axle === 'rear');
if (front.length && rear.length) {
const distance = mean(front.map(wheel => wheel.centerLocal[2])) - mean(rear.map(wheel => wheel.centerLocal[2]));
if (distance > 1e-6) return distance;
}
if (wheels.some(wheel => wheel.steerable)) {
throw invalidField('wheelbase', undefined, 'a wheelbase in metres, such as 2.7, or wheels marked with axle front and rear so it can be measured');
}
return 0;
}
function resolveBodyEntries(source, bodyOption, wheels) {
if (bodyOption !== undefined) {
if (!Array.isArray(bodyOption) || bodyOption.length === 0) throw invalidField('body', bodyOption, 'a non-empty array of parts or nodes, such as ["body"]', TypeError);
return bodyOption.map((reference, index) => resolveEntry(source, reference, `body[${index}]`));
}
const wheelKeys = new Set(wheels.flatMap(wheel => wheel.entries.map(entry => entry.key)));
const bodyEntries = source.entries.filter(entry => !wheelKeys.has(entry.key) && !isDescendantOfAny(source, entry, wheelKeys) &&
(source.kind === 'parts' || entry.parentKey === null));
if (bodyEntries.length === 0) throw invalidField('body', undefined, 'a list of the body nodes, such as ["body"], since no body parts remain after removing the wheels');
return bodyEntries;
}
function assertNoPivot(entries) {
for (const entry of entries) {
if (entry.pivot && entry.pivot.some(component => component !== 0)) {
throw invalidField('body', entry.label, 'nodes with a zero pivot, since vehicle poses do not support a nonzero one; pick another node');
}
}
}
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, path) {
if (!value || value.length !== 3 || !Array.from(value).every(Number.isFinite)) throw invalidField(path, value, 'an array of 3 finite numbers, such as [0.5, 0.6, 2]', TypeError);
}
/**
* `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, `lights.headlights[${index}].position`);
const direction = light.direction ?? forward;
assertVector3(direction, `lights.headlights[${index}].direction`);
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 invalidField(`lights.tailLights[${index}].signal`, light.signal, 'one of brake, 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 invalidField(`lights.tailLights[${index}].node`, light.node, 'a node, part or targets list, such as "tail"', TypeError);
}
const { off = 0, on = 4 } = light;
if (!Number.isFinite(off) || !Number.isFinite(on) || off < 0 || on < 0) throw invalidField(`lights.tailLights[${index}].${Number.isFinite(off) && off >= 0 ? 'on' : 'off'}`, Number.isFinite(off) && off >= 0 ? on : off, 'a number of 0 or more, such as 4');
const entries = references.map(reference => resolveEntry(source, reference, `lights.tailLights[${index}].node`));
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, lights } = {}) {
const forward = FORWARD_AXES[forwardAxis];
if (!forward) throw invalidField('forwardAxis', forwardAxis, 'one of +x, -x, +z, -z');
assertFinitePositive(maxSteerAngle, 'maxSteerAngle', '0.6');
const source = createSource({ parts, graph, asset });
const { described, rejected } = describeWheels(source, wheels, forward);
if (described.length < 2) {
throw invalidField('wheels', described.length, 'at least 2 wheels, such as "auto" on a car with 4 wheel nodes, or a list of wheel nodes');
}
const measuredWheels = described.map(item => measureDescribedWheel(source, item, forward));
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: 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,
bodyEntries,
wheels: Object.freeze(layout.wheels),
rejectedNames: rejected,
});
}
function validatePoint(point, index, pathField) {
if ((!Array.isArray(point) && !ArrayBuffer.isView(point)) || point.length !== 3 || !Array.from(point).every(Number.isFinite)) {
throw invalidField(`${pathField}[${index}]`, point, 'an array of 3 finite numbers, such as [0, 0, 10]', TypeError);
}
}
function pointsFromPath(path, pathSamples, pathField) {
if (Array.isArray(path)) return path.map(point => Array.from(point));
if (path && typeof path.sample === 'function' && Number.isFinite(path.duration) && path.duration > 0) {
const points = [];
for (let index = 0; index <= pathSamples; index++) {
points.push(Array.from(path.sample(path.duration * index / pathSamples).eye));
}
return points;
}
throw invalidField(pathField, path, 'an array of [x, y, z] points, such as [[0, 0, 0], [0, 0, 10]], or a camera path with duration and sample(time)', TypeError);
}
function removeRepeatedPoints(points) {
return points.filter((point, index) => index === 0 || vec3.distance(point, points[index - 1]) > 1e-6);
}
function centripetalPoint(points, parameters, time) {
const blend = (first, second, firstTime, secondTime) => {
const span = secondTime - firstTime;
return first.map((value, axis) => (secondTime - time) / span * value + (time - firstTime) / span * second[axis]);
};
const [point0, point1, point2, point3] = points;
const [time0, time1, time2, time3] = parameters;
const first = blend(point0, point1, time0, time1);
const second = blend(point1, point2, time1, time2);
const third = blend(point2, point3, time2, time3);
return blend(blend(first, second, time0, time2), blend(second, third, time1, time3), time1, time2);
}
function splinePolyline(points) {
if (points.length === 2) return points;
const reflected = (inner, outer) => inner.map((value, axis) => 2 * value - outer[axis]);
const padded = [reflected(points[0], points[1]), ...points, reflected(points.at(-1), points.at(-2))];
const polyline = [points[0]];
for (let segment = 1; segment < padded.length - 2; segment++) {
const control = padded.slice(segment - 1, segment + 3);
const parameters = [0];
for (let index = 1; index < 4; index++) {
parameters.push(parameters[index - 1] + Math.sqrt(vec3.distance(control[index - 1], control[index])));
}
const count = clamp(Math.ceil(vec3.distance(control[1], control[2]) / 0.05), 8, 2000);
for (let step = 1; step <= count; step++) {
polyline.push(centripetalPoint(control, parameters, parameters[1] + (parameters[2] - parameters[1]) * step / count));
}
}
return polyline;
}
function resampleUniformly(polyline, pathField) {
const lengths = [0];
for (let index = 1; index < polyline.length; index++) {
lengths.push(lengths[index - 1] + vec3.distance(polyline[index - 1], polyline[index]));
}
const total = lengths.at(-1);
if (!(total > 1e-6)) throw invalidField(pathField, 'a path of zero length', 'points that are not all the same, such as [[0, 0, 0], [0, 0, 10]]');
const intervals = Math.max(2, Math.ceil(total / Math.max(PATH_SPACING_METRES, total / MAX_PATH_SAMPLES)));
const spacing = total / intervals;
const positions = [];
let segment = 0;
for (let index = 0; index <= intervals; index++) {
const distance = index * spacing;
while (segment < polyline.length - 2 && lengths[segment + 1] < distance) segment++;
const span = lengths[segment + 1] - lengths[segment];
const fraction = span > 0 ? clamp((distance - lengths[segment]) / span, 0, 1) : 0;
positions.push(polyline[segment].map((value, axis) => value + (polyline[segment + 1][axis] - value) * fraction));
}
return { positions, spacing, length: total };
}
function smoothed(values, radius) {
return values.map((_, index) => {
const first = Math.max(0, index - radius);
const last = Math.min(values.length - 1, index + radius);
let total = 0;
for (let neighbour = first; neighbour <= last; neighbour++) total += values[neighbour];
return total / (last - first + 1);
});
}
function buildPathTable(path, pathSamples, pathField) {
const points = pointsFromPath(path, pathSamples, pathField);
points.forEach((point, index) => validatePoint(point, index, pathField));
const distinct = removeRepeatedPoints(points);
if (distinct.length < 2) throw invalidField(pathField, `${distinct.length} distinct point`, 'at least 2 distinct points, such as [[0, 0, 0], [0, 0, 10]]');
const { positions, spacing, length } = resampleUniformly(splinePolyline(distinct), pathField);
const headings = [];
const slopes = [];
positions.forEach((_, index) => {
const before = positions[Math.max(0, index - 1)];
const after = positions[Math.min(positions.length - 1, index + 1)];
const horizontal = Math.hypot(after[0] - before[0], after[2] - before[2]);
let heading = horizontal > 1e-9 ? Math.atan2(after[0] - before[0], after[2] - before[2]) : (headings.at(-1) ?? 0);
if (index > 0) heading += Math.round((headings[index - 1] - heading) / (2 * Math.PI)) * 2 * Math.PI;
headings.push(heading);
slopes.push(horizontal > 1e-9 ? Math.atan2(after[1] - before[1], horizontal) : 0);
});
const rawCurvature = headings.map((heading, index) => {
const before = Math.max(0, index - 1);
const after = Math.min(headings.length - 1, index + 1);
return (headings[after] - headings[before]) / ((after - before) * spacing);
});
return { positions, headings, slopes, curvature: smoothed(rawCurvature, 5), spacing, length };
}
function createSpeedProfile(table, options) {
const { cruiseSpeed, acceleration, braking, startSpeed, endSpeed, maxLateralAcceleration } = options;
const count = table.positions.length;
const limitSquared = table.curvature.map(curvature => {
const lateralLimit = maxLateralAcceleration / Math.max(Math.abs(curvature), 1e-9);
return Math.min(cruiseSpeed ** 2, Math.max(lateralLimit, MINIMUM_SPEED_LIMIT ** 2));
});
const speedSquared = Float64Array.from(limitSquared);
speedSquared[0] = Math.min(startSpeed ** 2, limitSquared[0]);
for (let index = 1; index < count; index++) {
speedSquared[index] = Math.min(speedSquared[index], speedSquared[index - 1] + 2 * acceleration * table.spacing);
}
speedSquared[count - 1] = Math.min(speedSquared[count - 1], endSpeed ** 2);
for (let index = count - 2; index >= 0; index--) {
speedSquared[index] = Math.min(speedSquared[index], speedSquared[index + 1] + 2 * braking * table.spacing);
}
const speeds = speedSquared.map(Math.sqrt);
const times = new Float64Array(count);
for (let index = 1; index < count; index++) {
const meanSpeed = (speeds[index - 1] + speeds[index]) / 2;
if (meanSpeed < 1e-9) throw invalidField('acceleration', 'a speed profile that never leaves rest', 'a higher acceleration or startSpeed, such as acceleration 3');
times[index] = times[index - 1] + table.spacing / meanSpeed;
}
return { speeds, times, duration: times[count - 1] };
}
function findInterval(times, time) {
let low = 0;
let high = times.length - 1;
while (high - low > 1) {
const middle = (low + high) >> 1;
if (times[middle] <= time) low = middle;
else high = middle;
}
return low;
}
function createMotion(table, options) {
const startTime = options.startTime;
if (options.speed !== undefined) {
const duration = table.length / options.speed;
return {
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);
const spacing = table.spacing;
function intervalAt(time) {
const index = findInterval(profile.times, time);
const startSpeedSquared = profile.speeds[index] ** 2;
const endSpeedSquared = profile.speeds[index + 1] ** 2;
return { index, elapsed: time - profile.times[index], acceleration: (endSpeedSquared - startSpeedSquared) / (2 * spacing) };
}
return {
duration: startTime + profile.duration,
distanceAt(time) {
const local = time - startTime;
if (local <= 0) return 0;
if (local >= profile.duration) return table.length;
const { index, elapsed, acceleration } = intervalAt(local);
return index * spacing + profile.speeds[index] * elapsed + 0.5 * acceleration * elapsed ** 2;
},
speedAt(time) {
const local = time - startTime;
if (local <= 0 || local >= profile.duration) return 0;
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;
},
};
}
function interpolateTable(values, spacing, distance) {
const position = clamp(distance / spacing, 0, values.length - 1);
const index = Math.min(Math.floor(position), values.length - 2);
const fraction = position - index;
const first = values[index];
const second = values[index + 1];
return Array.isArray(first) ? first.map((value, axis) => value + (second[axis] - value) * fraction) : first + (second - first) * fraction;
}
// 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;
const damping = 2 * options.dampingRatio * omega;
let pitch = 0;
let roll = 0;
let pitchRate = 0;
let rollRate = 0;
for (let step = 0; step < steps; step++) {
const time = step * SIMULATION_STEP_SECONDS;
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;
rollRate += (omega ** 2 * (rollTarget - roll) - damping * rollRate) * SIMULATION_STEP_SECONDS;
pitch += pitchRate * SIMULATION_STEP_SECONDS;
roll += rollRate * SIMULATION_STEP_SECONDS;
pitches[step] = pitch;
rolls[step] = roll;
}
return { pitches, rolls };
}
function groundPlane(rig, position, heading, groundHeight) {
const cosine = Math.cos(heading);
const sine = Math.sin(heading);
const samples = rig.wheels.map(wheel => {
const [left, , forward] = wheel.centerLocal;
const x = position[0] + left * cosine + forward * sine;
const z = position[2] - left * sine + forward * cosine;
return { left, forward, height: readGroundHeight(groundHeight, x, z) };
});
const meanLeft = mean(samples.map(sample => sample.left));
const meanForward = mean(samples.map(sample => sample.forward));
const meanHeight = mean(samples.map(sample => sample.height));
let leftLeft = 0;
let forwardForward = 0;
let leftForward = 0;
let leftHeight = 0;
let forwardHeight = 0;
for (const sample of samples) {
const left = sample.left - meanLeft;
const forward = sample.forward - meanForward;
const height = sample.height - meanHeight;
leftLeft += left * left;
forwardForward += forward * forward;
leftForward += left * forward;
leftHeight += left * height;
forwardHeight += forward * height;
}
const determinant = leftLeft * forwardForward - leftForward ** 2;
const wellPosed = determinant > 1e-9 * Math.max(1, leftLeft * forwardForward);
const leftSlope = wellPosed ? (leftHeight * forwardForward - forwardHeight * leftForward) / determinant : 0;
const forwardSlope = wellPosed ? (forwardHeight * leftLeft - leftHeight * leftForward) / determinant : 0;
return {
height: meanHeight - leftSlope * meanLeft - forwardSlope * meanForward,
pitch: Math.atan(forwardSlope),
roll: Math.atan(leftSlope),
};
}
function readGroundHeight(groundHeight, x, z) {
const height = groundHeight(x, z);
if (!Number.isFinite(height)) throw new TypeError('groundHeight must return a finite number.');
return height;
}
function bodyWorldMatrix(position, heading, pitch, roll) {
const matrix = mat4.fromTranslation(createMatrix(), position);
mat4.rotateY(matrix, matrix, heading);
mat4.rotateX(matrix, matrix, -pitch);
mat4.rotateZ(matrix, matrix, roll);
return matrix;
}
function suspensionOffsets(rig, bodyMatrix, groundHeight, travel) {
const point = vec3.create();
const uprightness = Math.max(0.5, bodyMatrix[5]);
return rig.wheels.map(wheel => {
if (!groundHeight) return 0;
vec3.transformMat4(point, wheel.centerLocal, bodyMatrix);
const wanted = readGroundHeight(groundHeight, point[0], point[2]) + wheel.radius;
return clamp((wanted - point[1]) / uprightness, -travel, travel);
});
}
/**
* Builds a deterministic drive along a path. `sample(seconds)` returns the same pose for the same time.
* Speed is `speed` (constant, m/s) or a profile: `cruiseSpeed`, `acceleration`, `braking`, `startSpeed`, `endSpeed`,
* `maxLateralAcceleration`. The vehicle holds the path end once it arrives.
*/
export function driveAlongPath(rig, path, options = {}) {
return driveAlongLegs(rig, [{ path }], options, () => 'path');
}
function buildLegs(legDescriptions, settings, pathFieldOf) {
const legs = [];
legDescriptions.forEach((description, index) => {
const { path, direction = 'forward', pause = 0 } = description ?? {};
if (direction !== 'forward' && direction !== 'reverse') throw invalidField(`legs[${index}].direction`, direction, 'one of forward, reverse');
if (!Number.isFinite(pause) || pause < 0) throw invalidField(`legs[${index}].pause`, pause, 'seconds of 0 or more, such as 2');
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, pathFieldOf(index));
// 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 = {}, pathFieldOf = index => `legs[${index}].path`) {
if (!rig || !rig.wheels || !rig.restFrame) throw new TypeError('driveAlongPath needs a rig from createVehicleRig.');
if (!Array.isArray(legDescriptions) || legDescriptions.length === 0) throw invalidField('legs', legDescriptions, 'a non-empty array of { path, direction, pause }, such as [{ "path": [[0, 0, 0], [0, 0, 10]] }]', TypeError);
const settings = resolveDriveOptions(rig, options);
const legs = buildLegs(legDescriptions, settings, pathFieldOf);
const duration = legs.at(-1).endTime;
if (!(duration <= MAX_DRIVE_SECONDS)) {
const causes = settings.speed === undefined ? 'cruiseSpeed, acceleration, a leg pause or startTime' : 'speed, a leg pause or startTime';
throw invalidField('duration', `${duration} seconds`, `a drive of at most ${MAX_DRIVE_SECONDS} seconds; raise ${causes} or shorten the path`);
}
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 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, legDistance);
let slopePitch = sign * interpolateTable(table.slopes, table.spacing, legDistance);
let slopeRoll = 0;
if (settings.groundHeight) {
const plane = groundPlane(rig, position, heading, settings.groundHeight);
position[1] = plane.height;
slopePitch = plane.pitch;
slopeRoll = plane.roll;
}
const pitch = slopePitch + dynamicPitch;
const roll = slopeRoll + dynamicRoll;
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,
direction: sign,
position,
heading,
pitch,
roll,
steerAngle,
wheels: rig.wheels.map((wheel, index) => ({
id: wheel.id,
spinAngle: wheel.spinSign * (distance - wheel.centerLocal[0] * headingChange) / wheel.radius,
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, duration, sample });
}
function resolveDriveOptions(rig, options) {
const {
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 invalidField('steering', steering, 'one of ackermann, shared');
assertFinitePositive(fullBrakeDeceleration, 'fullBrakeDeceleration', '4');
if ((speed === undefined) === (cruiseSpeed === undefined)) {
throw invalidField(speed === undefined ? 'speed' : 'cruiseSpeed', speed ?? cruiseSpeed, 'exactly one of speed (constant) or cruiseSpeed (with acceleration and braking), such as speed 8', TypeError);
}
if (speed !== undefined) assertFinitePositive(speed, 'speed', '8');
if (cruiseSpeed !== undefined) {
assertFinitePositive(cruiseSpeed, 'cruiseSpeed', '8');
assertFinitePositive(acceleration, 'acceleration', '3');
assertFinitePositive(braking, 'braking', '4');
}
if (!Number.isFinite(startTime) || startTime < 0) throw invalidField('startTime', startTime, 'seconds of 0 or more, such as 0');
if (groundHeight !== undefined && typeof groundHeight !== 'function') throw new TypeError('groundHeight must be a function (x, z) => height.');
assertFinitePositive(maxSteerAngle, 'maxSteerAngle', '0.6');
assertFinitePositive(bounceFrequency, 'bounceFrequency', '1.6');
assertFinitePositive(dampingRatio, 'dampingRatio', '0.45');
assertFiniteNumber(rollGain, 'rollGain', '0.012');
assertFiniteNumber(pitchGain, 'pitchGain', '0.012');
if (!Number.isFinite(suspensionTravel) || suspensionTravel < 0) {
throw invalidField('suspensionTravel', suspensionTravel, 'a distance in metres of 0 or more, such as 0.2');
}
if (!Number.isFinite(startSpeed) || startSpeed < 0 || !Number.isFinite(endSpeed) || endSpeed < 0) {
const badSpeedField = !Number.isFinite(startSpeed) || startSpeed < 0 ? 'startSpeed' : 'endSpeed';
throw invalidField(badSpeedField, badSpeedField === 'startSpeed' ? startSpeed : endSpeed, 'a speed of 0 or more, such as 0');
}
if (!(maxLateralAcceleration > 0)) throw invalidField('maxLateralAcceleration', maxLateralAcceleration, 'a number above 0, such as 6');
if (!Number.isInteger(pathSamples) || pathSamples < 8) throw invalidField('pathSamples', pathSamples, 'a whole number of 8 or more, such as 400');
return {
speed, cruiseSpeed, acceleration, braking, startSpeed, endSpeed, maxLateralAcceleration, startTime, groundHeight,
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);
mat4.rotateY(matrix, matrix, wheelPose.steerAngle);
mat4.rotateX(matrix, matrix, wheelPose.spinAngle);
mat4.translate(matrix, matrix, wheel.centerLocal.map(value => -value));
return matrix;
}
/** World matrices for every moved entry, keyed by the entry key (part object, node name or glTF node id). */
function computeTargetWorlds(rig, pose) {
if (!pose || !Array.isArray(pose.wheels) || pose.wheels.length !== rig.wheels.length) {
throw new TypeError('pose must come from drive.sample for this rig.');
}
const bodyMatrix = bodyWorldMatrix(pose.position, pose.heading, pose.pitch, pose.roll);
const bodyFromModel = mat4.multiply(createMatrix(), bodyMatrix, rig.inverseRestFrame);
const worlds = new Map();
for (const entry of rig.bodyEntries) {
worlds.set(entry.key, mat4.multiply(createMatrix(), bodyFromModel, entry.restWorld));
}
rig.wheels.forEach((wheel, index) => {
const wheelFromModel = mat4.multiply(createMatrix(), bodyMatrix, wheelMotionMatrix(wheel, pose.wheels[index]));
mat4.multiply(wheelFromModel, wheelFromModel, rig.inverseRestFrame);
for (const entry of wheel.entries) {
worlds.set(entry.key, mat4.multiply(createMatrix(), wheelFromModel, entry.restWorld));
}
});
return worlds;
}
function assertKind(rig, kind) {
if (rig.kind !== kind) throw new TypeError(`This rig was built from ${rig.kind}, not ${kind}.`);
}
/** Writes sampled world matrices into the part.matrix of a rig built from `parts`. Returns the moved parts. */
export function applyVehiclePoseToParts(rig, pose) {
assertKind(rig, 'parts');
const worlds = computeTargetWorlds(rig, pose);
const moved = [];
for (const [part, world] of worlds) {
part.matrix.set(world);
moved.push(part);
}
return moved;
}
function createLocalMatrixResolver(rig, worlds) {
const { byKey } = rig.source;
const memo = new Map();
function newWorld(key) {
if (worlds.has(key)) return worlds.get(key);
if (memo.has(key)) return memo.get(key);
const entry = byKey.get(key);
let world = entry.restWorld;
if (entry.parentKey !== null && byKey.has(entry.parentKey)) {
const parentRest = byKey.get(entry.parentKey).restWorld;
const restLocal = mat4.multiply(createMatrix(), mat4.invert(createMatrix(), parentRest), entry.restWorld);
world = mat4.multiply(createMatrix(), newWorld(entry.parentKey), restLocal);
}
memo.set(key, world);
return world;
}
return function localMatrix(entry, world) {
if (entry.parentKey === null || !byKey.has(entry.parentKey)) return Float64Array.from(world);
const inverseParent = mat4.invert(createMatrix(), newWorld(entry.parentKey));
return mat4.multiply(createMatrix(), inverseParent, world);
};
}
/** Writes sampled poses into the localMatrix of scene-graph nodes and calls graph.update(). Returns graph.parts. */
export function applyVehiclePoseToSceneGraph(rig, pose, graph) {
assertKind(rig, 'graph');
const worlds = computeTargetWorlds(rig, pose);
const localMatrix = createLocalMatrixResolver(rig, worlds);
for (const [name, world] of worlds) {
graph.getNode(name).localMatrix.set(localMatrix(rig.source.byKey.get(name), world));
}
return graph.update();
}
/** Node overrides for `asset.stagePose({ nodeOverrides })` on a rig built from a loadGltf asset. */
export function createVehicleNodeOverrides(rig, pose) {
assertKind(rig, 'asset');
const worlds = computeTargetWorlds(rig, pose);
const localMatrix = createLocalMatrixResolver(rig, worlds);
const overrides = [];
for (const [id, world] of worlds) {
const entry = rig.source.byKey.get(id);
if (entry.editable === false) throw new RangeError(`Imported node ${entry.label} has an authored pose that cannot be overridden.`);
const local = localMatrix(entry, world);
overrides.push({
id,
translation: Array.from(mat4.getTranslation(vec3.create(), local)),
rotation: Array.from(mat4.getRotation(new Float64Array(4), local)),
scale: Array.from(mat4.getScaling(vec3.create(), local)),
});
}
return overrides;
}
/** Stages and commits the sampled pose on a loadGltf asset, then returns what `commit()` returned. */
export function applyVehiclePoseToAsset(rig, pose, asset) {
const staged = asset.stagePose({ nodeOverrides: createVehicleNodeOverrides(rig, pose) });
return staged.commit();
}