@viamrobotics/motion-tools 1.26.1 → 1.27.0

package/dist/plugins/bvh.svelte.js

@@ -17,11 +17,14 @@ export const bvh = (raycaster, options) => {
     injectPlugin('bvh', (args) => {
         const { props } = $derived(args);
         const opts = $derived(props.bvh ? { ...bvhOptions, ...props.bvh } : bvhOptions);
+        let computed = false;
+        let helper;
         $effect(() => {
             const { ref } = args;
-            if (opts.enabled === false) {
+            if (computed)
+                return;
+            if (opts.enabled === false)
                 return;
-            }
             if (isInstanceOf(ref, 'Points') &&
                 /**
                  * This check is necessary, there are some strange cases where points are coming in from PCDs without any position data
@@ -31,28 +34,12 @@ export const bvh = (raycaster, options) => {
                 ref.geometry.disposeBoundsTree = disposeBoundsTree;
                 ref.raycast = acceleratedRaycast;
                 computeBoundsTree.call(ref.geometry, { type: PointsBVH, ...opts });
-                const helper = opts.helper ? new BVHHelper(ref) : undefined;
-                if (helper)
-                    ref.add(helper);
-                return () => {
-                    ref.raycast = Points.prototype.raycast;
-                    if (helper)
-                        ref.remove(helper);
-                };
             }
             else if (isInstanceOf(ref, 'BatchedMesh')) {
                 /* @ts-expect-error Some sort of ambient type is conflicing here, likely from @threlte/extras */
                 ref.geometry.computeBoundsTree = computeBatchedBoundsTree;
                 ref.geometry.disposeBoundsTree = disposeBatchedBoundsTree;
                 ref.raycast = acceleratedRaycast;
-                const helper = opts.helper ? new BVHHelper(ref) : undefined;
-                if (helper)
-                    ref.add(helper);
-                return () => {
-                    ref.raycast = BatchedMesh.prototype.raycast;
-                    if (helper)
-                        ref.remove(helper);
-                };
             }
             else if (isInstanceOf(ref, 'Mesh') &&
                 /**
@@ -64,15 +51,39 @@ export const bvh = (raycaster, options) => {
                 ref.geometry.disposeBoundsTree = disposeBoundsTree;
                 ref.raycast = acceleratedRaycast;
                 computeBoundsTree.call(ref.geometry, opts);
-                const helper = opts.helper ? new BVHHelper(ref) : undefined;
-                if (helper)
-                    ref.add(helper);
-                return () => {
-                    ref.raycast = Mesh.prototype.raycast;
-                    if (helper)
-                        ref.remove(helper);
-                };
             }
+            else {
+                return;
+            }
+            if (opts.helper) {
+                helper = new BVHHelper(ref);
+                ref.add(helper);
+            }
+            computed = true;
+        });
+        $effect(() => {
+            const { ref } = args;
+            return () => {
+                if (!computed)
+                    return;
+                if (isInstanceOf(ref, 'Points')) {
+                    ref.geometry.disposeBoundsTree?.();
+                    ref.raycast = Points.prototype.raycast;
+                }
+                else if (isInstanceOf(ref, 'BatchedMesh')) {
+                    ref.geometry.disposeBoundsTree?.();
+                    ref.raycast = BatchedMesh.prototype.raycast;
+                }
+                else if (isInstanceOf(ref, 'Mesh')) {
+                    ref.geometry.disposeBoundsTree?.();
+                    ref.raycast = Mesh.prototype.raycast;
+                }
+                if (helper) {
+                    ref.remove(helper);
+                    helper = undefined;
+                }
+                computed = false;
+            };
         });
     });
 };

package/dist/transform.js

@@ -5,6 +5,20 @@ const euler = new Euler();
 const ov = new OrientationVector();
 const translation = new Vector3();
 const scale = new Vector3();
+const matA = new Matrix4();
+export const isPoseEqual = (a, b) => {
+    if (a === b)
+        return true;
+    if (!a || !b)
+        return false;
+    return (a.x === b.x &&
+        a.y === b.y &&
+        a.z === b.z &&
+        a.oX === b.oX &&
+        a.oY === b.oY &&
+        a.oZ === b.oZ &&
+        a.theta === b.theta);
+};
 export const createPose = (pose) => {
     // We should only default to the 0,0,1,0 orientation vector if the entire vector component is missing
     const oZ = pose?.oX === undefined && pose?.oY === undefined && pose?.oZ === undefined ? 1 : (pose?.oZ ?? 0);
@@ -84,20 +98,31 @@ export const poseToDirection = (pose) => {
     ov.set(pose.oX, pose.oY, pose.oZ, MathUtils.degToRad(pose.theta));
     return new Vector3(ov.x, ov.y, ov.z);
 };
-export const poseToMatrix = (pose) => {
+export const isFinitePose = (pose) => Number.isFinite(pose.x) &&
+    Number.isFinite(pose.y) &&
+    Number.isFinite(pose.z) &&
+    Number.isFinite(pose.oX) &&
+    Number.isFinite(pose.oY) &&
+    Number.isFinite(pose.oZ) &&
+    Number.isFinite(pose.theta);
+/**
+ * Build a TRS `Matrix4` (m) from a `Pose` (mm), writing into `matrix`.
+ */
+export const poseToMatrix = (pose, matrix) => {
     ov.set(pose.oX, pose.oY, pose.oZ, MathUtils.degToRad(pose.theta));
     ov.toQuaternion(quaternion);
-    const matrix = new Matrix4();
     matrix.makeRotationFromQuaternion(quaternion);
-    matrix.setPosition(pose.x, pose.y, pose.z);
+    matrix.setPosition(pose.x * 0.001, pose.y * 0.001, pose.z * 0.001);
     return matrix;
 };
-export const matrixToPose = (matrix) => {
-    const pose = createPose();
+/**
+ * Decompose a `Matrix4` (m) into a `Pose` (mm), writing into `pose`.
+ */
+export const matrixToPose = (matrix, pose) => {
     matrix.decompose(translation, quaternion, scale);
-    pose.x = translation.x;
-    pose.y = translation.y;
-    pose.z = translation.z;
+    pose.x = translation.x * 1000;
+    pose.y = translation.y * 1000;
+    pose.z = translation.z * 1000;
     ov.setFromQuaternion(quaternion);
     pose.oX = ov.x;
     pose.oY = ov.y;
@@ -105,16 +130,25 @@ export const matrixToPose = (matrix) => {
     pose.theta = MathUtils.radToDeg(ov.th);
     return pose;
 };
-export const composeRenderedPose = (livePose, baselinePose, editedPose) => matrixToPose(poseToMatrix(livePose)
-    .multiply(poseToMatrix(baselinePose).invert())
-    .multiply(poseToMatrix(editedPose)));
-export const composeEditedPoseForRenderedPose = (baselinePose, livePose, renderedPose) => matrixToPose(poseToMatrix(baselinePose)
-    .multiply(poseToMatrix(livePose).invert())
-    .multiply(poseToMatrix(renderedPose)));
-export const isFinitePose = (pose) => Number.isFinite(pose.x) &&
-    Number.isFinite(pose.y) &&
-    Number.isFinite(pose.z) &&
-    Number.isFinite(pose.oX) &&
-    Number.isFinite(pose.oY) &&
-    Number.isFinite(pose.oZ) &&
-    Number.isFinite(pose.theta);
+/**
+ * Compose the entity's local-to-parent transform: writes
+ * `live × baseline⁻¹ × edited` into `out`. Mirrors the formula
+ * `Frame.svelte` uses to blend live kinematics with user-staged edits;
+ * `worldMatrix.ts` premultiplies the result by the parent's `WorldMatrix`.
+ */
+export const composeLocalMatrix = (live, baseline, edited, out) => {
+    matA.copy(baseline).invert();
+    out.copy(live).multiply(matA).multiply(edited);
+    return out;
+};
+/**
+ * Inverse of `composeLocalMatrix` for the transform controls path:
+ * writes `baseline × live⁻¹ × target` into `out`. Solves for the
+ * `EditedMatrix` that, blended through `composeLocalMatrix`, renders
+ * to `target`.
+ */
+export const solveEditedMatrix = (baseline, live, target, out) => {
+    matA.copy(live).invert();
+    out.copy(baseline).multiply(matA).multiply(target);
+    return out;
+};

package/package.json

@@ -1,6 +1,6 @@
 {
   "name": "@viamrobotics/motion-tools",
-  "version": "1.26.1",
+  "version": "1.27.0",
   "description": "Motion visualization with Viam",
   "license": "Apache-2.0",
   "type": "module",
@@ -25,8 +25,8 @@
     "@testing-library/jest-dom": "6.8.0",
     "@testing-library/svelte": "5.2.8",
     "@testing-library/user-event": "^14.6.1",
-    "@threlte/core": "8.5.13",
-    "@threlte/extras": "9.15.2",
+    "@threlte/core": "8.5.14",
+    "@threlte/extras": "9.17.0",
     "@threlte/rapier": "3.4.1",
     "@threlte/xr": "1.6.0",
     "@types/bun": "1.2.21",

package/dist/components/overlay/left-pane/buildTree.d.ts

@@ -1,13 +0,0 @@
-import type { Entity, QueryResult, Trait } from 'koota';
-export interface TreeNode {
-    entity: Entity;
-    parent?: TreeNode;
-    children?: TreeNode[];
-}
-/**
- * Creates a tree representing parent child / relationships from a set of frames.
- */
-export declare const buildTreeNodes: (entities: QueryResult<[Trait]>) => {
-    rootNodes: TreeNode[];
-    nodeMap: Record<string, TreeNode | undefined>;
-};

package/dist/components/overlay/left-pane/buildTree.js

@@ -1,48 +0,0 @@
-import { hierarchy, traits } from '../../../ecs';
-function sortNodes(nodes) {
-    nodes.sort((a, b) => a.entity.get(traits.Name)?.localeCompare(b.entity.get(traits.Name) ?? '') ?? 0);
-}
-/**
- * Creates a tree representing parent child / relationships from a set of frames.
- */
-export const buildTreeNodes = (entities) => {
-    const nodeMap = {};
-    const rootNodes = [];
-    const childNodes = [];
-    for (const entity of entities) {
-        const parent = hierarchy.getParentName(entity);
-        const name = entity.get(traits.Name) ?? '';
-        const node = { entity };
-        nodeMap[name] = node;
-        if (!parent || parent === 'world') {
-            rootNodes.push(node);
-        }
-        else {
-            childNodes.push(node);
-        }
-    }
-    for (const node of childNodes) {
-        const parent = hierarchy.getParentName(node.entity);
-        if (parent) {
-            const parentNode = nodeMap[parent];
-            node.parent = parentNode;
-            if (parentNode) {
-                parentNode.children ??= [];
-                parentNode.children?.push(node);
-            }
-        }
-    }
-    for (const node of rootNodes) {
-        if (!node.children)
-            continue;
-        sortNodes(node.children);
-    }
-    for (const node of childNodes) {
-        if (!node.children)
-            continue;
-        sortNodes(node.children);
-    }
-    sortNodes(rootNodes);
-    sortNodes(childNodes);
-    return { rootNodes, nodeMap };
-};