@viamrobotics/motion-tools 1.26.0 → 1.26.2

package/dist/components/Entities/hooks/useEntityEvents.svelte.js

@@ -1,9 +1,18 @@
 import { useCursor } from '@threlte/extras';
-import { Vector2 } from 'three';
+import { MathUtils, Matrix4, Quaternion, Vector2 } from 'three';
 import { traits, useTrait } from '../../../ecs';
 import { useFocusedEntity, useSelectedEntity } from '../../../hooks/useSelection.svelte';
 import { updateHoverInfo } from '../../../HoverUpdater.svelte';
-import { createPose, matrixToPose, poseToMatrix } from '../../../transform';
+import { OrientationVector } from '../../../three/OrientationVector';
+const tempHoverMatrix = new Matrix4();
+const hoverQuat = new Quaternion();
+const hoverOv = new OrientationVector();
+const infoToLocalMatrix = (info, out) => {
+    hoverOv.set(info.oX, info.oY, info.oZ, MathUtils.degToRad(info.theta));
+    hoverOv.toQuaternion(hoverQuat);
+    out.makeRotationFromQuaternion(hoverQuat);
+    out.setPosition(info.x, info.y, info.z);
+};
 export const useEntityEvents = (entity) => {
     const down = new Vector2();
     const selectedEntity = useSelectedEntity();
@@ -19,15 +28,18 @@ export const useEntityEvents = (entity) => {
         if (currentEntity && !currentEntity.has(traits.Hovered)) {
             const hoverInfo = updateHoverInfo(currentEntity, event);
             if (hoverInfo) {
-                currentEntity.add(traits.InstancedPose({
+                infoToLocalMatrix(hoverInfo, tempHoverMatrix);
+                const worldMatrix = currentEntity.get(traits.WorldMatrix);
+                const composed = new Matrix4();
+                if (worldMatrix) {
+                    composed.copy(worldMatrix).multiply(tempHoverMatrix);
+                }
+                else {
+                    composed.copy(tempHoverMatrix);
+                }
+                currentEntity.add(traits.InstancedMatrix({
+                    matrix: composed,
                     index: hoverInfo.index,
-                    x: hoverInfo.x,
-                    y: hoverInfo.y,
-                    z: hoverInfo.z,
-                    oX: hoverInfo.oX,
-                    oY: hoverInfo.oY,
-                    oZ: hoverInfo.oZ,
-                    theta: hoverInfo.theta,
                 }));
             }
             currentEntity.add(traits.Hovered);
@@ -40,34 +52,21 @@ export const useEntityEvents = (entity) => {
         const currentEntity = entity();
         if (currentEntity?.has(traits.Hovered)) {
             const hoverInfo = updateHoverInfo(currentEntity, event);
-            const hoverPose = createPose(hoverInfo
-                ? {
-                    x: hoverInfo.x,
-                    y: hoverInfo.y,
-                    z: hoverInfo.z,
-                    oX: 0,
-                    oY: 0,
-                    oZ: 1,
-                    theta: 0,
-                }
-                : undefined);
-            const worldPose = currentEntity.get(traits.WorldPose) ?? createPose();
-            const hoverPoseMatrix = poseToMatrix(hoverPose);
-            const worldPoseMatrix = poseToMatrix(worldPose);
-            const resultMatrix = worldPoseMatrix.multiply(hoverPoseMatrix);
-            const resultPose = matrixToPose(resultMatrix);
-            if (hoverInfo) {
-                currentEntity.set(traits.InstancedPose, {
-                    index: hoverInfo.index,
-                    x: resultPose.x,
-                    y: resultPose.y,
-                    z: resultPose.z,
-                    oX: resultPose.oX,
-                    oY: resultPose.oY,
-                    oZ: resultPose.oZ,
-                    theta: resultPose.theta,
-                });
+            if (!hoverInfo)
+                return;
+            infoToLocalMatrix(hoverInfo, tempHoverMatrix);
+            const instanced = currentEntity.get(traits.InstancedMatrix);
+            if (!instanced)
+                return;
+            const worldMatrix = currentEntity.get(traits.WorldMatrix);
+            if (worldMatrix) {
+                instanced.matrix.copy(worldMatrix).multiply(tempHoverMatrix);
+            }
+            else {
+                instanced.matrix.copy(tempHoverMatrix);
             }
+            instanced.index = hoverInfo.index;
+            currentEntity.changed(traits.InstancedMatrix);
         }
     };
     const onpointerleave = (event) => {
@@ -77,8 +76,8 @@ export const useEntityEvents = (entity) => {
         if (currentEntity?.has(traits.Hovered)) {
             currentEntity.remove(traits.Hovered);
         }
-        if (currentEntity?.has(traits.InstancedPose)) {
-            currentEntity.remove(traits.InstancedPose);
+        if (currentEntity?.has(traits.InstancedMatrix)) {
+            currentEntity.remove(traits.InstancedMatrix);
         }
     };
     const ondblclick = (event) => {
@@ -109,8 +108,8 @@ export const useEntityEvents = (entity) => {
             if (currentEntity?.has(traits.Hovered)) {
                 currentEntity.remove(traits.Hovered);
             }
-            if (currentEntity?.has(traits.InstancedPose)) {
-                currentEntity.remove(traits.InstancedPose);
+            if (currentEntity?.has(traits.InstancedMatrix)) {
+                currentEntity.remove(traits.InstancedMatrix);
             }
         }
     });

package/dist/components/SceneProviders.svelte

@@ -1,7 +1,7 @@
 <script lang="ts">
 	import type { Snippet } from 'svelte'
 
-	import { provideHierarchy } from '../ecs'
+	import { provideHierarchy, provideWorldMatrix } from '../ecs'
 	import { provide3DModels } from '../hooks/use3DModels.svelte'
 	import { provideArmClient } from '../hooks/useArmClient.svelte'
 	import { provideArmKinematics } from '../hooks/useArmKinematics.svelte'
@@ -43,6 +43,7 @@
 	provideLogs()
 
 	provideHierarchy()
+	provideWorldMatrix()
 	provideOrigin()
 	provideDrawAPI()
 	provideRelationships()

package/dist/components/SelectedTransformControls.svelte

@@ -1,6 +1,6 @@
 <script lang="ts">
 	import { TransformControls } from '@threlte/extras'
-	import { Quaternion, Vector3 } from 'three'
+	import { Matrix4, Quaternion, Vector3 } from 'three'
 
 	import type { FrameEditSession } from '../editing/FrameEditSession'
 
@@ -11,9 +11,11 @@
 	import { useSelectedEntity, useSelectedObject3d } from '../hooks/useSelection.svelte'
 	import { useSettings } from '../hooks/useSettings.svelte'
 	import {
-		composeEditedPoseForRenderedPose,
 		createPose,
+		matrixToPose,
+		poseToMatrix,
 		quaternionToPose,
+		solveEditedMatrix,
 		vector3ToPose,
 	} from '../transform'
 
@@ -27,8 +29,8 @@
 	const mode = $derived(settings.current.transformMode)
 	const entity = $derived(selectedEntity.current)
 	const transformable = useTrait(() => entity, traits.Transformable)
-	const networkPose = useTrait(() => entity, traits.Pose)
-	const livePose = useTrait(() => entity, traits.LivePose)
+	const configMatrix = useTrait(() => entity, traits.Matrix)
+	const liveMatrix = useTrait(() => entity, traits.LiveMatrix)
 	const box = useTrait(() => entity, traits.Box)
 	const sphere = useTrait(() => entity, traits.Sphere)
 	const capsule = useTrait(() => entity, traits.Capsule)
@@ -42,10 +44,12 @@
 	// the geometry inside it.
 	const ref = $derived(selectedObject3d.current?.parent ?? selectedObject3d.current)
 
-	const activeMode = $derived.by(() => {
-		if (mode === 'none' || !transformable.current) return undefined
+	const activeMode = $derived.by<'translate' | 'rotate' | 'scale' | undefined>(() => {
+		if (mode === 'none' || !transformable.current) return
+
 		// Scale only does anything for primitive geometries the gizmo can size.
-		if (mode === 'scale' && !hasScalableGeometry) return undefined
+		if (mode === 'scale' && !hasScalableGeometry) return
+
 		return mode
 	})
 	const isSphereScale = $derived(activeMode === 'scale' && sphere.current !== undefined)
@@ -54,6 +58,9 @@
 	const quaternion = new Quaternion()
 	const vector3 = new Vector3()
 	const refPose = createPose()
+	const tempRefMatrix = new Matrix4()
+	const tempEditedMatrix = new Matrix4()
+	const tempPose = createPose()
 
 	let session: FrameEditSession | undefined
 	let scaleStart:
@@ -93,7 +100,9 @@
 		if (entity?.has(traits.FramesAPI)) {
 			session = sessions.begin([entity])
 		}
+
 		captureScaleStart()
+
 		environment.current.viewerMode = 'edit'
 		transformControls.setActive(true)
 	}
@@ -109,15 +118,17 @@
 			if (isFrameEntity) {
 				stageFrameTransform()
 			} else {
-				const pose = entity.get(traits.Pose)
-				if (pose) {
+				const matrix = entity.get(traits.Matrix)
+				if (matrix) {
+					matrixToPose(matrix, tempPose)
 					if (activeMode === 'translate') {
-						vector3ToPose(ref.getWorldPosition(vector3), pose)
+						vector3ToPose(ref.getWorldPosition(vector3), tempPose)
 					} else {
-						quaternionToPose(ref.getWorldQuaternion(quaternion), pose)
+						quaternionToPose(ref.getWorldQuaternion(quaternion), tempPose)
 						ref.quaternion.copy(quaternion)
 					}
-					entity.set(traits.Pose, pose)
+					poseToMatrix(tempPose, matrix)
+					entity.changed(traits.Matrix)
 				}
 			}
 		} else {
@@ -127,11 +138,14 @@
 				captureScaleStart()
 			}
 
+			// Clamp at 0 — the gizmo can produce negative scale factors when
+			// dragged past the origin, which would yield negative dimensions
+			// and a degenerate OBB.
 			if (scaleStart?.type === 'box') {
 				const next = {
-					x: scaleStart.x * ref.scale.x,
-					y: scaleStart.y * ref.scale.y,
-					z: scaleStart.z * ref.scale.z,
+					x: Math.max(0, scaleStart.x * ref.scale.x),
+					y: Math.max(0, scaleStart.y * ref.scale.y),
+					z: Math.max(0, scaleStart.z * ref.scale.z),
 				}
 				if (isFrameEntity) {
 					session?.stageGeometry(entity, { type: 'box', ...next })
@@ -139,14 +153,17 @@
 					entity.set(traits.Box, next)
 				}
 			} else if (scaleStart?.type === 'sphere') {
-				const next = { r: scaleStart.r * ref.scale.x }
+				const next = { r: Math.max(0, scaleStart.r * ref.scale.x) }
 				if (isFrameEntity) {
 					session?.stageGeometry(entity, { type: 'sphere', ...next })
 				} else {
 					entity.set(traits.Sphere, next)
 				}
 			} else if (scaleStart?.type === 'capsule') {
-				const next = { r: scaleStart.r * ref.scale.x, l: scaleStart.l * ref.scale.y }
+				const next = {
+					r: Math.max(0, scaleStart.r * ref.scale.x),
+					l: Math.max(0, scaleStart.l * ref.scale.y),
+				}
 				if (isFrameEntity) {
 					session?.stageGeometry(entity, { type: 'capsule', ...next })
 				} else {
@@ -165,29 +182,31 @@
 		transformControls.setActive(false)
 	}
 
-	// Pose.svelte renders frame entities through the live blend
-	//   render = M(live) × M(network)⁻¹ × M(edited)
-	// so for the user's drag to render where they pulled the gizmo to, EditedPose
-	// must satisfy
-	//   M(edited) = M(network) × M(live)⁻¹ × M(ref)
-	// where M(ref) is the gizmo-driven group's parent-relative matrix in mm.
-	// When live ≈ network (no kinematic offset), this collapses to M(edited) =
-	// M(ref) — the same as the naive writeback. When they diverge (e.g. an arm
-	// whose joints have moved away from its config pose), this composition is
-	// what keeps the rendering anchored to the user's pointer instead of
-	// shearing through the live × baseline⁻¹ offset.
+	/**
+	 * Frame.svelte renders frame entities by blending M(live) × M(config)⁻¹ × M(edited)
+	 * so for the user's drag to render where they pulled the gizmo to,
+	 * EditedMatrix must satisfy M(edited) = M(config) × M(live)⁻¹ × M(ref)
+	 * where M(ref) is the gizmo-driven group's parent-relative matrix in mm.
+	 *
+	 * When live ≈ config (no kinematic offset), this collapses to
+	 * M(edited) = M(ref) — the same as the naive writeback. When they diverge
+	 * (e.g. an arm whose joints have moved away from its config pose), this
+	 * composition is what keeps the rendering anchored to the user's pointer
+	 * instead of shearing through the live × baseline⁻¹ offset.
+	 */
+
 	const stageFrameTransform = () => {
 		if (!ref || !entity) return
 
 		vector3ToPose(ref.position, refPose)
 		quaternionToPose(ref.quaternion, refPose)
 
-		const live = livePose.current
-		const network = networkPose.current
+		const live = liveMatrix.current
+		const config = configMatrix.current
 
-		if (!live || !network) {
-			// No live pose available — Pose.svelte's blend short-circuits to
-			// editedPose, so naive writeback is correct.
+		if (!live || !config) {
+			// No live matrix available — Frame.svelte's blend short-circuits to
+			// editedMatrix, so naive writeback is correct.
 			if (activeMode === 'translate') {
 				session?.stagePose(entity, {
 					x: refPose.x,
@@ -205,7 +224,11 @@
 			return
 		}
 
-		session?.stagePose(entity, composeEditedPoseForRenderedPose(network, live, refPose))
+		poseToMatrix(refPose, tempRefMatrix)
+
+		solveEditedMatrix(config, live, tempRefMatrix, tempEditedMatrix)
+		matrixToPose(tempEditedMatrix, tempPose)
+		session?.stagePose(entity, { ...tempPose })
 	}
 </script>
 

package/dist/components/StaticGeometries.svelte

@@ -29,7 +29,7 @@
 	keys.onKeys('=', () => {
 		const entity = world.spawn(
 			traits.Name(`custom geometry ${++index}`),
-			traits.Pose,
+			traits.Matrix,
 			traits.Box({ x: 100, y: 100, z: 100 }),
 			traits.Removable,
 			traits.Transformable

package/dist/components/hover/HoveredEntity.svelte

@@ -1,6 +1,11 @@
 <script lang="ts">
+	import { MathUtils, Quaternion, Vector3 } from 'three'
+
+	import type { HoverInfo } from '../../HoverUpdater.svelte'
+
 	import { traits, useTrait } from '../../ecs'
 	import { useFocusedEntity, useSelectedEntity } from '../../hooks/useSelection.svelte'
+	import { OrientationVector } from '../../three/OrientationVector'
 
 	import HoveredEntityTooltip from './HoveredEntityTooltip.svelte'
 
@@ -8,9 +13,34 @@
 	const focusedEntity = useFocusedEntity()
 
 	const displayEntity = $derived(selectedEntity.current ?? focusedEntity.current)
-	const hoverInfo = useTrait(() => displayEntity, traits.InstancedPose)
+	const instancedMatrix = useTrait(() => displayEntity, traits.InstancedMatrix)
+
+	// Pool: InstancedMatrix's `Matrix4` is in metres (matches Three.js).
+	// Decompose for the tooltip's display, which expects metres for position
+	// and OV+theta for orientation.
+	const translation = new Vector3()
+	const quaternion = new Quaternion()
+	const scaleVec = new Vector3()
+	const ov = new OrientationVector()
+
+	const hoverInfo = $derived.by((): HoverInfo | undefined => {
+		const data = instancedMatrix.current
+		if (!data) return undefined
+		data.matrix.decompose(translation, quaternion, scaleVec)
+		ov.setFromQuaternion(quaternion)
+		return {
+			index: data.index,
+			x: translation.x,
+			y: translation.y,
+			z: translation.z,
+			oX: ov.x,
+			oY: ov.y,
+			oZ: ov.z,
+			theta: MathUtils.radToDeg(ov.th),
+		}
+	})
 </script>
 
-{#if hoverInfo.current}
-	<HoveredEntityTooltip hoverInfo={hoverInfo.current} />
+{#if hoverInfo}
+	<HoveredEntityTooltip {hoverInfo} />
 {/if}

package/dist/components/hover/LinkedHoveredEntity.svelte

@@ -3,8 +3,7 @@
 
 	import { compileExpression } from 'filtrex'
 
-	import { relations, traits } from '../../ecs'
-	import { useTrait } from '../../ecs'
+	import { relations, traits, useTrait } from '../../ecs'
 	import { SubEntityLinkType } from '../../ecs/relations'
 	import { useSelectedEntity } from '../../hooks/useSelection.svelte'
 	import { useFocusedEntity } from '../../hooks/useSelection.svelte'
@@ -22,7 +21,7 @@
 	const focusedEntity = useFocusedEntity()
 	const displayEntity = $derived(selectedEntity.current ?? focusedEntity.current)
 
-	const displayedHoverInfo = useTrait(() => displayEntity, traits.InstancedPose)
+	const displayedHoverInfo = useTrait(() => displayEntity, traits.InstancedMatrix)
 
 	let hoverInfo = $state.raw<HoverInfo | null>(null)