@viamrobotics/motion-tools 0.19.2 → 1.0.2

package/dist/hooks/useDrawAPI.svelte.js

@@ -1,17 +1,18 @@
 import { getContext, setContext } from 'svelte';
-import { Color, MathUtils, Quaternion, Vector3, Vector4 } from 'three';
+import { Color, Vector3, Vector4 } from 'three';
 import { NURBSCurve } from 'three/addons/curves/NURBSCurve.js';
 import { UuidTool } from 'uuid-tool';
 import { parsePcdInWorker } from '../loaders/pcd';
 import { GLTFLoader } from 'three/examples/jsm/loaders/GLTFLoader.js';
-import { WorldObject } from '../WorldObject.svelte';
-import { useArrows } from './useArrows.svelte';
-import { createGeometry } from '../geometry';
 import { createPose, createPoseFromFrame } from '../transform';
 import { useCameraControls } from './useControls.svelte';
+import { useWorld, traits } from '../ecs';
 import { useThrelte } from '@threlte/core';
-import { OrientationVector } from '../three/OrientationVector';
+import {} from 'koota';
+import { parsePlyInput } from '../ply';
 import { useLogs } from './useLogs.svelte';
+import { createBox, createCapsule, createSphere } from '../geometry';
+const colorUtil = new Color();
 const bufferTypes = {
     DRAW_POINTS: 0,
     DRAW_POSES: 1,
@@ -19,9 +20,6 @@ const bufferTypes = {
     DRAW_PCD: 3,
     DRAW_GLTF: 4,
 };
-const axis = new Vector3();
-const quaternion = new Quaternion();
-const ov = new OrientationVector();
 const key = Symbol('draw-api-context-key');
 const tryParse = (json) => {
     try {
@@ -50,26 +48,25 @@ class Float32Reader {
     littleEndian = true;
     offset = 0;
     buffer = new ArrayBuffer();
+    array = new Float32Array();
     view = new DataView(this.buffer);
     header = { requestID: '', type: -1 };
     async init(data) {
         this.buffer = await data.arrayBuffer();
-        this.header = {
-            requestID: UuidTool.toString([...new Uint8Array(this.buffer.slice(0, 16))]),
-            type: new DataView(this.buffer).getFloat32(16, true),
-        };
-        // Slice away the request header and leave the body
+        this.header.requestID = UuidTool.toString([...new Uint8Array(this.buffer.slice(0, 16))]);
+        this.header.type = new Float32Array(this.buffer.slice(16, 20))[0];
         this.buffer = this.buffer.slice(20);
-        this.view = new DataView(this.buffer);
+        this.array = new Float32Array(this.buffer);
         return this;
     }
     read() {
-        const result = this.view.getFloat32(this.offset, this.littleEndian);
-        this.offset += 4;
+        const result = this.array[this.offset];
+        this.offset += 1;
         return result;
     }
 }
 export const provideDrawAPI = () => {
+    const world = useWorld();
     const logs = useLogs();
     const cameraControls = useCameraControls();
     const { invalidate } = useThrelte();
@@ -79,148 +76,134 @@ export const provideDrawAPI = () => {
     let reconnectDelay = 200;
     const maxReconnectDelay = 5_000;
     let ws;
-    const frames = $state([]);
-    const points = $state([]);
-    const lines = $state([]);
-    const meshes = $state([]);
-    const poses = $state([]);
-    const nurbs = $state([]);
-    const models = $state([]);
     let connectionStatus = $state('connecting');
-    const color = new Color();
     const direction = new Vector3();
     const origin = new Vector3();
     const loader = new GLTFLoader();
-    const batchedArrow = useArrows();
+    const entities = new Map();
     const sendResponse = (response) => {
         ws.send(JSON.stringify(response));
     };
     const drawFrames = async (data) => {
-        for (const frame of data) {
-            const name = frame.name || frame.id || '';
-            const pose = createPoseFromFrame(lowercaseKeys(frame));
-            const geometry = createGeometry();
-            if (frame.geometry?.type === 'box') {
-                geometry.label = `${name} geometry (box)`;
-                geometry.geometryType.case = 'box';
-                geometry.geometryType.value = {
-                    dimsMm: { x: frame.geometry.x, y: frame.geometry.y, z: frame.geometry.z },
-                };
+        for (const rawFrame of data) {
+            const frame = lowercaseKeys(rawFrame);
+            const pose = createPoseFromFrame(frame);
+            const name = frame.name ?? frame.id ?? '';
+            const parent = frame.parent;
+            const existing = entities.get(name);
+            if (existing) {
+                existing.set(traits.Pose, pose);
+                if (parent && parent !== 'world') {
+                    existing.set(traits.Parent, parent);
+                }
+                continue;
             }
-            else if (frame.geometry?.type === 'sphere') {
-                geometry.label = `${name} geometry (sphere)`;
-                geometry.geometryType.case = 'sphere';
-                geometry.geometryType.value = { radiusMm: frame.geometry.r };
+            const geometryTrait = () => {
+                if (frame.geometry?.type === 'box') {
+                    return traits.Box(frame.geometry);
+                }
+                else if (frame.geometry?.type === 'sphere') {
+                    return traits.Sphere(frame.geometry);
+                }
+                else if (frame.geometry?.type === 'capsule') {
+                    return traits.Capsule(frame.geometry);
+                }
+                return traits.ReferenceFrame;
+            };
+            const entityTraits = [];
+            if (parent && parent !== 'world') {
+                entityTraits.push(traits.Parent(parent));
             }
-            else if (frame.geometry?.type === 'capsule') {
-                geometry.label = `${name} geometry (capsule)`;
-                geometry.geometryType.case = 'capsule';
-                geometry.geometryType.value = { lengthMm: frame.geometry.l, radiusMm: frame.geometry.r };
+            if (frame.geometry) {
+                entityTraits.push(geometryTrait());
             }
-            const worldObject = new WorldObject(name, pose, frame.parent ?? 'world', frame.geometry ? geometry : undefined);
-            frames.push(worldObject);
+            entityTraits.push(traits.Name(name), traits.Pose(pose), traits.DrawAPI, traits.ReferenceFrame);
+            const entity = world.spawn(...entityTraits);
+            entities.set(name, entity);
         }
     };
     const drawPCD = async (buffer) => {
         const { positions, colors } = await parsePcdInWorker(new Uint8Array(buffer));
-        points.push(new WorldObject(`points ${++pointsIndex}`, undefined, undefined, {
-            center: undefined,
-            geometryType: {
-                case: 'points',
-                value: positions,
-            },
-        }, colors ? { colors } : undefined));
+        const entity = world.spawn(traits.Name(`Points ${++pointsIndex}`), traits.PointsGeometry(positions), traits.DrawAPI);
+        if (colors) {
+            entity.add(traits.VertexColors(colors));
+        }
     };
     // eslint-disable-next-line @typescript-eslint/no-explicit-any
     const drawGeometry = (data, color, parent) => {
-        const result = meshes.find((mesh) => mesh.name === data.label);
-        if (result) {
-            result.pose = createPose(data.center);
+        const name = data.label ?? `geometry ${++geometryIndex}`;
+        const pose = createPose(data.center);
+        const existing = entities.get(name);
+        if (existing) {
+            existing.set(traits.Pose, pose);
             return;
         }
-        const geometry = createGeometry();
-        if ('mesh' in data) {
-            geometry.geometryType.case = 'mesh';
-            geometry.geometryType.value = data.mesh;
-        }
-        else if ('box' in data) {
-            geometry.geometryType.case = 'box';
-            geometry.geometryType.value = data.box;
-        }
-        else if ('sphere' in data) {
-            geometry.geometryType.case = 'sphere';
-            geometry.geometryType.value = data.sphere;
-        }
-        else if ('capsule' in data) {
-            geometry.geometryType.case = 'capsule';
-            geometry.geometryType.value = data.capsule;
+        const geometryTrait = () => {
+            if ('mesh' in data) {
+                const geometry = parsePlyInput(data.mesh.mesh);
+                return traits.BufferGeometry(geometry);
+            }
+            else if ('box' in data) {
+                return traits.Box(createBox(data.box));
+            }
+            else if ('sphere' in data) {
+                return traits.Sphere(createSphere(data.sphere));
+            }
+            else if ('capsule' in data) {
+                return traits.Capsule(createCapsule(data.capsule));
+            }
+            return traits.ReferenceFrame;
+        };
+        const entityTraits = [];
+        if (parent && parent !== 'world') {
+            entityTraits.push(traits.Parent(parent));
         }
-        const object = new WorldObject(data.label ?? ++geometryIndex, createPose(data.center), parent, geometry, {
-            color: new Color(color),
-        });
-        meshes.push(object);
+        entityTraits.push(traits.Name(data.label ?? ++geometryIndex), traits.Pose(pose), traits.Color(colorUtil.set(color)), geometryTrait(), traits.DrawAPI);
+        const entity = world.spawn(...entityTraits);
+        entities.set(name, entity);
     };
     // eslint-disable-next-line @typescript-eslint/no-explicit-any
     const drawNurbs = (data, color) => {
-        const index = nurbs.findIndex(({ name }) => name === data.name);
-        if (index !== -1) {
-            nurbs.splice(index, 1);
-        }
+        const name = data.Name;
+        const existing = entities.get(name);
         const controlPoints = data.ControlPts.map((point) => new Vector4(point.x / 1000, point.y / 1000, point.z / 1000));
         const curve = new NURBSCurve(data.Degree, data.Knots, controlPoints);
-        const object = new WorldObject(data.name, data.pose, data.parent, { center: undefined, geometryType: { case: 'line', value: new Float32Array() } }, { color: new Color(color), points: curve.getPoints(200) });
-        nurbs.push(object);
+        const points = curve.getPoints(200);
+        if (existing) {
+            existing.set(traits.LineGeometry, points);
+            return;
+        }
+        const entity = world.spawn(traits.Name(name), traits.Color(colorUtil.set(color)), traits.LineGeometry(points), traits.DrawAPI);
+        entities.set(name, entity);
     };
+    const vec3 = new Vector3();
+    const pose = createPose();
     const drawPoses = async (reader) => {
         // Read counts
         const nPoints = reader.read();
         const nColors = reader.read();
         const arrowHeadAtPose = reader.read();
-        // Read positions
-        const nextPoses = new Float32Array(nPoints * 6);
-        for (let i = 0; i < nPoints * 6; i++) {
-            nextPoses[i] = reader.read();
-        }
-        // Read raw colors
-        const colors = new Float32Array(nColors * 3);
-        for (let i = 0; i < nColors * 3; i++) {
-            colors[i] = reader.read();
-        }
-        const length = 0.1;
-        for (let i = 0, j = 0, l = nextPoses.length; i < l; i += 6, j += 3) {
-            origin.set(nextPoses[i], nextPoses[i + 1], nextPoses[i + 2]).multiplyScalar(0.001);
-            direction.set(nextPoses[i + 3], nextPoses[i + 4], nextPoses[i + 5]);
-            color.set(colors[j], colors[j + 1], colors[j + 2]);
-            const arrowId = batchedArrow.addArrow(direction, origin, length, color, arrowHeadAtPose === 1);
-            const pose = createPose();
+        const entities = [];
+        for (let i = 0; i < nPoints; i += 1) {
+            origin.set(reader.read(), reader.read(), reader.read()).multiplyScalar(0.001);
+            direction.set(reader.read(), reader.read(), reader.read());
+            if (arrowHeadAtPose === 1) {
+                // Compute the base position so the arrow ends at the origin
+                origin.sub(vec3.copy(direction).multiplyScalar(/** arrow length */ 0.1));
+            }
             pose.x = origin.x;
             pose.y = origin.y;
             pose.z = origin.z;
-            if (direction.y > 0.99999) {
-                quaternion.set(0, 0, 0, 1);
-            }
-            else if (direction.y < -0.99999) {
-                quaternion.set(1, 0, 0, 0);
-            }
-            else {
-                axis.set(direction.z, 0, -direction.x).normalize();
-                const radians = Math.acos(direction.y);
-                quaternion.setFromAxisAngle(axis, radians);
-            }
-            ov.setFromQuaternion(quaternion);
-            pose.oX = ov.x;
-            pose.oY = ov.y;
-            pose.oZ = ov.z;
-            pose.theta = MathUtils.radToDeg(ov.th);
-            poses.push(new WorldObject(`pose ${++poseIndex}`, pose, 'world', undefined, {
-                getBoundingBoxAt(box3) {
-                    return batchedArrow.getBoundingBoxAt(arrowId, box3);
-                },
-                batched: {
-                    id: arrowId,
-                    object: batchedArrow.object3d,
-                },
-            }));
+            pose.oX = direction.x;
+            pose.oY = direction.y;
+            pose.oZ = direction.z;
+            const entity = world.spawn(traits.Name(`Pose ${++poseIndex}`), traits.Pose(pose), traits.Color, traits.DrawAPI, traits.Arrow);
+            entities.push(entity);
+        }
+        for (let i = 0; i < nColors; i += 1) {
+            const entity = entities[i];
+            colorUtil.set(reader.read(), reader.read(), reader.read());
+            entity.set(traits.Color, colorUtil);
         }
     };
     const drawPoints = async (reader) => {
@@ -230,10 +213,9 @@ export const provideDrawAPI = () => {
         for (let i = 0; i < labelLen; i++) {
             label += String.fromCharCode(reader.read());
         }
-        const index = points.findIndex(({ name }) => name === label);
-        if (index !== -1) {
-            points.splice(index, 1);
-        }
+        const entities = world.query(traits.DrawAPI);
+        const entity = entities.find((entity) => entity.get(traits.Name) === label);
+        entity?.destroy();
         // Read counts
         const nPoints = reader.read();
         const nColors = reader.read();
@@ -241,45 +223,22 @@ export const provideDrawAPI = () => {
         const r = reader.read();
         const g = reader.read();
         const b = reader.read();
-        // Read positions
-        const positions = new Float32Array(nPoints * 3);
-        for (let i = 0; i < nPoints * 3; i++) {
-            positions[i] = reader.read();
+        const nPointsElements = nPoints * 3;
+        const positions = reader.array.slice(reader.offset, reader.offset + nPointsElements);
+        reader.offset += nPointsElements;
+        const nColorsElements = nColors * 3;
+        const rawColors = reader.array.slice(reader.offset, reader.offset + nColorsElements);
+        reader.offset += nColorsElements;
+        const colors = new Float32Array(nPointsElements);
+        colors.set(rawColors);
+        // Cover the gap for any points not colored
+        for (let i = nColors; i < nPoints; i++) {
+            const offset = i * 3;
+            colors[offset] = r;
+            colors[offset + 1] = g;
+            colors[offset + 2] = b;
         }
-        const getColors = () => {
-            // Read raw colors
-            const rawColors = new Float32Array(nColors * 3);
-            for (let i = 0; i < nColors * 3; i++) {
-                rawColors[i] = reader.read();
-            }
-            const colors = new Float32Array(nPoints * 3);
-            colors.set(rawColors);
-            // Cover the gap for any points not colored
-            for (let i = nColors; i < nPoints; i++) {
-                const offset = i * 3;
-                colors[offset] = r;
-                colors[offset + 1] = g;
-                colors[offset + 2] = b;
-            }
-            return colors;
-        };
-        const metadata = nColors > 0
-            ? {
-                colors: getColors(),
-                color: new Color(r, g, b).convertLinearToSRGB(),
-            }
-            : r === -1
-                ? undefined
-                : {
-                    color: new Color(r, g, b).convertLinearToSRGB(),
-                };
-        points.push(new WorldObject(label, undefined, undefined, {
-            center: undefined,
-            geometryType: {
-                case: 'points',
-                value: positions,
-            },
-        }, metadata));
+        world.spawn(traits.Name(label), traits.Color(colorUtil.set(r, g, b)), traits.PointsGeometry(positions), traits.VertexColors(colors), traits.DrawAPI);
     };
     const drawLine = async (reader) => {
         // Read label length
@@ -288,39 +247,24 @@ export const provideDrawAPI = () => {
         for (let i = 0; i < labelLen; i++) {
             label += String.fromCharCode(reader.read());
         }
-        const index = lines.findIndex(({ name }) => name === label);
-        if (index !== -1) {
-            lines.splice(index, 1);
-        }
+        const entities = world.query(traits.DrawAPI);
+        const entity = entities.find((entity) => entity.get(traits.Name) === label);
+        entity?.destroy();
         // Read counts
         const nPoints = reader.read();
         // Read default color
-        const lineR = reader.read();
-        const lineG = reader.read();
-        const lineB = reader.read();
+        const r = reader.read();
+        const g = reader.read();
+        const b = reader.read();
         const dotR = reader.read();
         const dotG = reader.read();
         const dotB = reader.read();
         // Read positions
-        const positions = new Float32Array(nPoints * 3);
-        for (let i = 0; i < nPoints * 3; i++) {
-            positions[i] = reader.read();
-        }
         const points = [];
-        for (let i = 0; i < positions.length; i += 3) {
-            points.push(new Vector3(positions[i], positions[i + 1], positions[i + 2]));
+        for (let i = 0; i < nPoints * 3; i += 3) {
+            points.push(new Vector3(reader.read(), reader.read(), reader.read()));
         }
-        lines.push(new WorldObject(label, undefined, undefined, {
-            center: undefined,
-            geometryType: {
-                case: 'line',
-                value: positions,
-            },
-        }, {
-            points,
-            color: lineR === -1 ? undefined : new Color().setRGB(lineR, lineG, lineB),
-            lineDotColor: dotR === -1 ? undefined : new Color().setRGB(dotR, dotG, dotB),
-        }));
+        world.spawn(traits.Name(label), traits.Color({ r, g, b }), traits.LineGeometry(points), traits.DottedLineColor({ r: dotR, g: dotG, b: dotB }), traits.DrawAPI);
     };
     // eslint-disable-next-line @typescript-eslint/no-explicit-any
     const drawGeometries = (geometries, colors, parent) => {
@@ -334,67 +278,26 @@ export const provideDrawAPI = () => {
         const blob = new Blob([buffer], { type: 'model/gltf-binary' });
         const url = URL.createObjectURL(blob);
         const gltf = await loader.loadAsync(url);
-        models.push(new WorldObject(gltf.scene.name, undefined, undefined, undefined, { gltf }));
+        world.spawn(traits.Name(gltf.scene.name), traits.GLTF(gltf), traits.DrawAPI);
         URL.revokeObjectURL(url);
     };
     const remove = (names) => {
-        let index = -1;
         for (const name of names) {
-            index = frames.findIndex((frame) => frame.name === name);
-            if (index !== -1) {
-                frames.slice(index, 1);
-                continue;
-            }
-            index = points.findIndex((p) => p.name === name);
-            if (index !== -1) {
-                points.splice(index, 1);
-                continue;
-            }
-            index = meshes.findIndex((m) => m.name === name);
-            if (index !== -1) {
-                meshes.splice(index, 1);
-                continue;
-            }
-            index = poses.findIndex((p) => p.name === name);
-            if (index !== -1) {
-                const id = poses[index].metadata.batched?.id;
-                if (id) {
-                    batchedArrow.removeArrow(id);
-                    poses.splice(index, 1);
-                    continue;
+            for (const entity of world.query(traits.DrawAPI)) {
+                if (entity.get(traits.Name) === name) {
+                    entity.destroy();
                 }
             }
-            index = nurbs.findIndex((n) => n.name === name);
-            if (index !== -1) {
-                nurbs.splice(index, 1);
-                continue;
-            }
-            index = models.findIndex((m) => m.name === name);
-            if (index !== -1) {
-                models.splice(index, 1);
-                continue;
-            }
-            index = lines.findIndex((m) => m.name === name);
-            if (index !== -1) {
-                lines.splice(index, 1);
-                continue;
-            }
         }
     };
     const removeAll = () => {
-        frames.splice(0, frames.length);
-        points.splice(0, points.length);
-        lines.splice(0, lines.length);
-        meshes.splice(0, meshes.length);
-        nurbs.splice(0, nurbs.length);
-        models.splice(0, models.length);
-        poses.splice(0, poses.length);
-        batchedArrow.clear();
+        for (const entity of world.query(traits.DrawAPI)) {
+            entity.destroy();
+        }
         pointsIndex = 0;
         geometryIndex = 0;
         poseIndex = 0;
     };
-    const { BACKEND_IP, BUN_SERVER_PORT } = globalThis;
     const scheduleReconnect = () => {
         setTimeout(() => {
             reconnectDelay = Math.min(reconnectDelay * 2, maxReconnectDelay);
@@ -405,7 +308,7 @@ export const provideDrawAPI = () => {
     const onOpen = () => {
         connectionStatus = 'open';
         reconnectDelay = 1000;
-        logs.add(`Connected to drawing server at ${BACKEND_IP}:${BUN_SERVER_PORT}`);
+        logs.add(`Connected to drawing server at ${BACKEND_IP}:${WS_PORT}`);
     };
     const onClose = () => {
         connectionStatus = 'closed';
@@ -507,9 +410,9 @@ export const provideDrawAPI = () => {
         invalidate();
     };
     const connect = () => {
-        if (BACKEND_IP && BUN_SERVER_PORT) {
+        if (BACKEND_IP && WS_PORT) {
             const protocol = location.protocol === 'https:' ? 'wss' : 'ws';
-            ws = new WebSocket(`${protocol}://${BACKEND_IP}:${BUN_SERVER_PORT}/ws`);
+            ws = new WebSocket(`${protocol}://${BACKEND_IP}:${WS_PORT}/ws`);
             ws.onclose = onClose;
             ws.onerror = onError;
             ws.onopen = onOpen;
@@ -518,36 +421,9 @@ export const provideDrawAPI = () => {
     };
     connect();
     setContext(key, {
-        get frames() {
-            return frames;
-        },
-        get points() {
-            return points;
-        },
-        get lines() {
-            return lines;
-        },
-        get meshes() {
-            return meshes;
-        },
-        get poses() {
-            return poses;
-        },
-        get nurbs() {
-            return nurbs;
-        },
-        get models() {
-            return models;
-        },
         get connectionStatus() {
             return connectionStatus;
         },
-        addPoints(worldObject) {
-            points.push(worldObject);
-        },
-        addMesh(worldObject) {
-            meshes.push(worldObject);
-        },
     });
 };
 export const useDrawAPI = () => {

package/dist/hooks/useFramelessComponents.svelte.js

@@ -12,7 +12,7 @@ export const provideFramelessComponents = () => {
             .map((component) => component.name) ?? [];
         const fragmentComponentsWithNoFrame = [];
         for (const fragmentComponentName of Object.keys(partConfig.componentNameToFragmentId)) {
-            if (frames.current.find((worldObject) => worldObject.name === fragmentComponentName)) {
+            if (frames.current.find((frame) => frame.transform.referenceFrame === fragmentComponentName)) {
                 continue;
             }
             fragmentComponentsWithNoFrame.push(fragmentComponentName);

package/dist/hooks/useFrames.svelte.d.ts

@@ -1,6 +1,10 @@
-import { WorldObject } from '../WorldObject.svelte';
+import { Transform } from '@viamrobotics/sdk';
+interface FrameTransform {
+    type: 'machine' | 'config' | 'fragment';
+    transform: Transform;
+}
 interface FramesContext {
-    current: WorldObject[];
+    current: FrameTransform[];
     getParentFrameOptions: (componentName: string) => string[];
 }
 export declare const provideFrames: (partID: () => string) => void;