247 lines
14 KiB
JavaScript
247 lines
14 KiB
JavaScript
import { fnv1a64 } from "./replay.mjs";
|
|
|
|
const POSE_DOUBLES = 9;
|
|
const SEGMENT_DOUBLES = POSE_DOUBLES * 2 + 3 + 3 + 2;
|
|
const SEGMENT_INT_OFFSET = SEGMENT_DOUBLES * 2;
|
|
const QUEUE_HEADER_BYTES = 32;
|
|
const STATE_POSITION_BYTES = 72;
|
|
const MOTION_QUEUE_SCHEMA = "sim_motion_queue_state.v1";
|
|
const DEFAULT_CONTROLS = Object.freeze({ profile: 0, lookahead: 1, acceleration_limit: 1e9, jerk_limit: 1e12, paused: false });
|
|
|
|
function call(core, name, returnType, argTypes, args) {
|
|
const fn = core[`_${name}`] ?? core[name];
|
|
if (typeof fn === "function") return fn(...args);
|
|
if (typeof core.ccall === "function") {
|
|
try { return core.ccall(name, returnType, argTypes, args); } catch (error) { throw new Error(`missing core export: ${name}`, { cause: error }); }
|
|
}
|
|
throw new Error(`missing core export: ${name}`);
|
|
}
|
|
|
|
function optionalCall(core, name, returnType, argTypes, args) {
|
|
const fn = core[`_${name}`] ?? core[name];
|
|
if (typeof fn === "function") return fn(...args);
|
|
if (typeof core.ccall === "function") {
|
|
try { return core.ccall(name, returnType, argTypes, args); } catch { return null; }
|
|
}
|
|
return null;
|
|
}
|
|
|
|
function poseToArray(pose = {}) {
|
|
return ["x", "y", "z", "a", "b", "c", "u", "v", "w"].map((axis) => Number(pose[axis] ?? 0));
|
|
}
|
|
|
|
function bytesToHex(bytes) {
|
|
let output = "";
|
|
for (const byte of bytes) output += byte.toString(16).padStart(2, "0");
|
|
return output;
|
|
}
|
|
|
|
function hexToBytes(value) {
|
|
if (typeof value !== "string" || value.length % 2 !== 0 || !/^[0-9a-f]*$/i.test(value)) throw new Error("wasm_motion_state_bytes_invalid");
|
|
const bytes = new Uint8Array(value.length / 2);
|
|
for (let index = 0; index < bytes.length; index += 1) bytes[index] = Number.parseInt(value.slice(index * 2, index * 2 + 2), 16);
|
|
return bytes;
|
|
}
|
|
|
|
function controls(value = {}) {
|
|
const result = {
|
|
profile: value.profile ?? DEFAULT_CONTROLS.profile,
|
|
lookahead: value.lookahead ?? DEFAULT_CONTROLS.lookahead,
|
|
acceleration_limit: value.acceleration_limit ?? DEFAULT_CONTROLS.acceleration_limit,
|
|
jerk_limit: value.jerk_limit ?? DEFAULT_CONTROLS.jerk_limit,
|
|
paused: value.paused ?? DEFAULT_CONTROLS.paused,
|
|
};
|
|
if (![0, 1].includes(result.profile) || !Number.isSafeInteger(result.lookahead) || result.lookahead < 1 || !Number.isFinite(result.acceleration_limit) || result.acceleration_limit <= 0 || !Number.isFinite(result.jerk_limit) || result.jerk_limit <= 0 || typeof result.paused !== "boolean") throw new Error("wasm_motion_controls_invalid");
|
|
return result;
|
|
}
|
|
|
|
export class WasmMotionQueue {
|
|
constructor(core, { cycleTime = 0.01, velocityLimit = 1e9 } = {}) {
|
|
this.core = core;
|
|
this.available = ["sim_motion_size", "sim_motion_command_size", "sim_motion_state_size", "sim_motion_init", "sim_motion_enqueue", "sim_motion_step", "sim_motion_read_state"].every((name) => typeof core?.[`_${name}`] === "function" || typeof core?.[name] === "function");
|
|
this.queue = 0;
|
|
this.command = 0;
|
|
this.state = 0;
|
|
this.commandCapacity = 0;
|
|
this.stateCapacity = 0;
|
|
this.queueSize = 0;
|
|
this.queueCapacity = 0;
|
|
this.cycleTime = cycleTime;
|
|
this.velocityLimit = velocityLimit;
|
|
this.controls = controls();
|
|
if (!this.available) return;
|
|
const queueSize = call(core, "sim_motion_size", "number", [], []);
|
|
this.queueSize = queueSize;
|
|
this.commandCapacity = call(core, "sim_motion_command_size", "number", [], []);
|
|
this.stateCapacity = call(core, "sim_motion_state_size", "number", [], []);
|
|
if (![queueSize, this.commandCapacity, this.stateCapacity].every((value) => Number.isSafeInteger(value) && value > 0)) throw new Error("wasm_motion_queue_size_invalid");
|
|
this.queueCapacity = (queueSize - QUEUE_HEADER_BYTES - this.stateCapacity) / this.commandCapacity;
|
|
if (!Number.isSafeInteger(this.queueCapacity) || this.queueCapacity < 1) throw new Error("wasm_motion_queue_capacity_invalid");
|
|
this.queue = core._malloc(queueSize);
|
|
this.command = core._malloc(this.commandCapacity);
|
|
this.state = core._malloc(this.stateCapacity);
|
|
if (!this.queue || !this.command || !this.state) throw new Error("wasm_motion_queue_allocation_failed");
|
|
this.core.HEAPU8.fill(0, this.queue, this.queue + queueSize);
|
|
const result = call(core, "sim_motion_init", "number", ["number", "number", "number"], [this.queue, cycleTime, velocityLimit]);
|
|
if (result !== 0) throw new Error(`wasm_motion_init_${result}`);
|
|
}
|
|
|
|
enqueue(segment) {
|
|
if (!this.available) return { status: "skipped", code: "wasm_motion_unavailable" };
|
|
if (!segment) return { status: "skipped" };
|
|
const heap = this.core.HEAPU8;
|
|
heap.fill(0, this.command, this.command + this.commandCapacity);
|
|
const doubles = this.core.HEAPF64;
|
|
const ints = this.core.HEAP32;
|
|
const base = this.command / 8;
|
|
[...poseToArray(segment.start), ...poseToArray(segment.end), Number(segment.center?.x ?? 0), Number(segment.center?.y ?? 0), Number(segment.center?.z ?? 0), Number(segment.normal?.x ?? 0), Number(segment.normal?.y ?? 0), Number(segment.normal?.z ?? 0), Number(segment.velocity ?? 0), Number(segment.acceleration ?? 0)].forEach((value, index) => { doubles[base + index] = value; });
|
|
ints[this.command / 4 + SEGMENT_INT_OFFSET] = Number(segment.motion_type ?? 1);
|
|
ints[this.command / 4 + SEGMENT_INT_OFFSET + 1] = Number(segment.turn ?? 0);
|
|
// sim_motion_command layout is segment (216 bytes), id (4), tag (4).
|
|
// Keep the WASM switchkins mode in the tag field; the C queue assigns id.
|
|
ints[this.command / 4 + SEGMENT_INT_OFFSET + 3] = Number(segment.kinematics_mode ?? segment.tag ?? 0);
|
|
const result = call(this.core, "sim_motion_enqueue", "number", ["number", "number", "number"], [this.queue, this.command, 0]);
|
|
return result === 0 ? { status: "verified", source_line: segment.source_line ?? null } : { status: "error", code: `wasm_motion_enqueue_${result}` };
|
|
}
|
|
|
|
reset() {
|
|
if (!this.available) return this.read();
|
|
this.core.HEAPU8.fill(0, this.queue, this.queue + this.queueSize);
|
|
const result = call(this.core, "sim_motion_init", "number", ["number", "number", "number"], [this.queue, this.cycleTime, this.velocityLimit]);
|
|
if (result !== 0) throw new Error(`wasm_motion_reset_${result}`);
|
|
this.controls = controls();
|
|
return this.read();
|
|
}
|
|
|
|
setProfile(profile) {
|
|
const next = controls({ ...this.controls, profile });
|
|
if (!this.available) { this.controls = next; return this.read(); }
|
|
const result = optionalCall(this.core, "sim_motion_set_profile", "number", ["number", "number"], [this.queue, next.profile]);
|
|
if (result !== null && result !== 0) throw new Error(`wasm_motion_profile_${result}`);
|
|
this.controls = next;
|
|
return this.read();
|
|
}
|
|
|
|
setLookahead(lookahead) {
|
|
const next = controls({ ...this.controls, lookahead });
|
|
if (!this.available) { this.controls = next; return this.read(); }
|
|
const result = optionalCall(this.core, "sim_motion_set_lookahead", "number", ["number", "number"], [this.queue, next.lookahead]);
|
|
if (result !== null && result !== 0) throw new Error(`wasm_motion_lookahead_${result}`);
|
|
this.controls = next;
|
|
return this.read();
|
|
}
|
|
|
|
setDynamics(accelerationLimit, jerkLimit) {
|
|
const next = controls({ ...this.controls, acceleration_limit: accelerationLimit, jerk_limit: jerkLimit });
|
|
if (!this.available) { this.controls = next; return this.read(); }
|
|
const result = optionalCall(this.core, "sim_motion_set_dynamics", "number", ["number", "number", "number"], [this.queue, next.acceleration_limit, next.jerk_limit]);
|
|
if (result !== null && result !== 0) throw new Error(`wasm_motion_dynamics_${result}`);
|
|
this.controls = next;
|
|
return this.read();
|
|
}
|
|
|
|
setPaused(paused) {
|
|
const next = controls({ ...this.controls, paused: Boolean(paused) });
|
|
if (!this.available) { this.controls = next; return this.read(); }
|
|
const result = optionalCall(this.core, "sim_motion_set_paused", "number", ["number", "number"], [this.queue, next.paused ? 1 : 0]);
|
|
if (result !== null && result !== 0) throw new Error(`wasm_motion_pause_${result}`);
|
|
this.controls = next;
|
|
return this.read();
|
|
}
|
|
|
|
abort() {
|
|
if (!this.available) return this.read();
|
|
const current = this.read();
|
|
if (current.status !== "verified") throw new Error("wasm_motion_abort_state");
|
|
const previous = this.core.HEAPU8.slice(this.queue, this.queue + this.queueSize);
|
|
const headerOffset = this.queueCapacity * this.commandCapacity;
|
|
const stateOffset = headerOffset + QUEUE_HEADER_BYTES;
|
|
const previousView = new DataView(previous.buffer, previous.byteOffset, previous.byteLength);
|
|
const nextId = previousView.getUint32(headerOffset + 8, true);
|
|
this.reset();
|
|
const bytes = new Uint8Array(this.queueSize);
|
|
const view = new DataView(bytes.buffer);
|
|
// sim_motion_queue stores commands first, followed by its 32-byte header.
|
|
// Preserve cycle/velocity configuration from that actual header offset.
|
|
bytes.set(previous.subarray(headerOffset + 16, headerOffset + QUEUE_HEADER_BYTES), headerOffset + 16);
|
|
view.setUint32(headerOffset + 8, nextId, true);
|
|
view.setUint32(headerOffset + 12, current.state.tick, true);
|
|
bytes.set(previous.subarray(stateOffset, stateOffset + STATE_POSITION_BYTES), stateOffset);
|
|
view.setFloat64(stateOffset + 72, 0, true);
|
|
view.setFloat64(stateOffset + 80, 0, true);
|
|
view.setUint32(stateOffset + 88, current.state.tick, true);
|
|
view.setUint32(stateOffset + 92, 0, true);
|
|
view.setUint32(stateOffset + 96, 0, true);
|
|
view.setUint32(stateOffset + 100, 0, true);
|
|
view.setInt32(stateOffset + 104, 0, true);
|
|
view.setInt32(stateOffset + 108, 0, true);
|
|
this.core.HEAPU8.set(bytes, this.queue);
|
|
return this.read();
|
|
}
|
|
|
|
step() {
|
|
if (!this.available) return { status: "error", code: "wasm_motion_step_-605" };
|
|
if (this.controls.paused) return { status: "error", code: "wasm_motion_step_-605" };
|
|
const result = call(this.core, "sim_motion_step", "number", ["number"], [this.queue]);
|
|
if (result !== 0) return { status: "error", code: `wasm_motion_step_${result}` };
|
|
return this.read();
|
|
}
|
|
|
|
read() {
|
|
if (!this.available) return { status: "blocked", state: { position: { x: 0, y: 0, z: 0 }, velocity: 0, progress: 0, tick: 0, queue_depth: 0, active_id: 0, active_tag: 0, motion_type: 0, status: -605, source: "wasm_motion_queue_unavailable" } };
|
|
const result = call(this.core, "sim_motion_read_state", "number", ["number", "number"], [this.queue, this.state]);
|
|
if (result !== 0) return { status: "error", code: `wasm_motion_read_${result}` };
|
|
const doubles = this.core.HEAPF64.subarray(this.state / 8, this.state / 8 + 11);
|
|
const ints = this.core.HEAPU32.subarray(this.state / 4 + 22, this.state / 4 + 26);
|
|
return { status: "verified", state: { position: { x: doubles[0], y: doubles[1], z: doubles[2], a: doubles[3], b: doubles[4], c: doubles[5], u: doubles[6], v: doubles[7], w: doubles[8] }, velocity: this.controls.paused ? 0 : doubles[9], progress: doubles[10], tick: ints[0], queue_depth: ints[1], active_id: ints[2], active_tag: ints[3], motion_type: this.core.HEAP32[this.state / 4 + 26], status: this.core.HEAP32[this.state / 4 + 27], source: "wasm_motion_queue" } };
|
|
}
|
|
|
|
exportState() {
|
|
if (!this.available) {
|
|
const unsigned = { schema: MOTION_QUEUE_SCHEMA, available: false, controls: { ...this.controls }, state: this.read().state };
|
|
return { ...unsigned, state_hash: fnv1a64(unsigned) };
|
|
}
|
|
const current = this.read();
|
|
if (current.status !== "verified") throw new Error("wasm_motion_state_unavailable");
|
|
const unsigned = {
|
|
schema: MOTION_QUEUE_SCHEMA,
|
|
queue_size: this.queueSize,
|
|
command_size: this.commandCapacity,
|
|
state_size: this.stateCapacity,
|
|
queue_capacity: this.queueCapacity,
|
|
cycle_time: this.cycleTime,
|
|
velocity_limit: this.velocityLimit,
|
|
controls: { ...this.controls },
|
|
state: current.state,
|
|
bytes: bytesToHex(this.core.HEAPU8.slice(this.queue, this.queue + this.queueSize)),
|
|
};
|
|
return { ...unsigned, state_hash: fnv1a64(unsigned) };
|
|
}
|
|
|
|
restoreState(value) {
|
|
if (!this.available) throw new Error("wasm_motion_restore_unavailable");
|
|
if (!value || value.schema !== MOTION_QUEUE_SCHEMA) throw new Error("wasm_motion_restore_schema");
|
|
const unsigned = structuredClone(value);
|
|
delete unsigned.state_hash;
|
|
if (value.state_hash !== fnv1a64(unsigned)) throw new Error("wasm_motion_restore_hash");
|
|
if (value.queue_size !== this.queueSize || value.command_size !== this.commandCapacity || value.state_size !== this.stateCapacity || value.queue_capacity !== this.queueCapacity || value.cycle_time !== this.cycleTime || value.velocity_limit !== this.velocityLimit) throw new Error("wasm_motion_restore_layout");
|
|
const bytes = hexToBytes(value.bytes);
|
|
if (bytes.length !== this.queueSize) throw new Error("wasm_motion_restore_bytes");
|
|
const nextControls = controls(value.controls);
|
|
this.core.HEAPU8.fill(0, this.queue, this.queue + this.queueSize);
|
|
const initialized = call(this.core, "sim_motion_init", "number", ["number", "number", "number"], [this.queue, this.cycleTime, this.velocityLimit]);
|
|
if (initialized !== 0) throw new Error(`wasm_motion_restore_init_${initialized}`);
|
|
this.controls = controls();
|
|
this.setProfile(nextControls.profile);
|
|
this.setLookahead(nextControls.lookahead);
|
|
this.setDynamics(nextControls.acceleration_limit, nextControls.jerk_limit);
|
|
this.setPaused(nextControls.paused);
|
|
this.core.HEAPU8.set(bytes, this.queue);
|
|
const actual = this.read();
|
|
if (actual.status !== "verified" || fnv1a64(actual.state) !== fnv1a64(value.state)) throw new Error("wasm_motion_restore_state_mismatch");
|
|
return this.exportState();
|
|
}
|
|
|
|
destroy() { for (const pointer of [this.queue, this.command, this.state]) if (pointer && typeof this.core._free === "function") this.core._free(pointer); this.queue = this.command = this.state = 0; }
|
|
}
|