app-16292: shared ECS traits and helpers for entity pose plugins

.changeset/gizmo-foundation-utils.md

@@ -0,0 +1,5 @@
+---
+'@viamrobotics/motion-tools': patch
+---
+
+Add `writeMatrix` and `CustomDetails` ECS traits, a `firstHitOnly` raycaster option, and a `'gizmo'` interaction mode

src/lib/draw.ts

@@ -35,7 +35,7 @@ import { isPointCloud } from './geometry'
 const vec3 = new Vector3()
 const rgb = { r: 0, g: 0, b: 0 }
 
-const DEFAULT_LINE_WIDTH = 5
+export const DEFAULT_LINE_WIDTH = 5
 const DEFAULT_POINT_SIZE = 10
 const DEFAULT_NURBS_DEGREE = 3
 const DEFAULT_NURBS_WEIGHT = 1

src/lib/ecs/__tests__/writeMatrix.spec.ts

@@ -0,0 +1,98 @@
+import { createWorld } from 'koota'
+import { Matrix4 } from 'three'
+import { describe, expect, it, vi } from 'vitest'
+
+import { traits } from '$lib/ecs'
+import { createPose, matrixToPose, poseToMatrix } from '$lib/transform'
+
+import { writeMatrix } from '../traits'
+
+const matrix = () =>
+	poseToMatrix(
+		createPose({
+			x: 10,
+			y: 20,
+			z: 30,
+			oX: 0.6,
+			oY: 0.8,
+			oZ: 0,
+			theta: 45,
+		}),
+		new Matrix4()
+	)
+
+describe('writeMatrix', () => {
+	it('no-ops when the entity has no Matrix trait', () => {
+		const world = createWorld()
+		const entity = world.spawn()
+		expect(() => writeMatrix(entity, { x: 1 })).not.toThrow()
+	})
+
+	it('overwrites only the supplied position fields', () => {
+		const world = createWorld()
+		const entity = world.spawn(traits.Matrix(matrix()))
+		writeMatrix(entity, { x: 99 })
+		const pose = matrixToPose(entity.get(traits.Matrix)!, createPose())
+		expect(pose.x).toBeCloseTo(99)
+		expect(pose.y).toBeCloseTo(20)
+		expect(pose.z).toBeCloseTo(30)
+	})
+
+	it('overwrites only the supplied orientation fields', () => {
+		const world = createWorld()
+		const entity = world.spawn(traits.Matrix(matrix()))
+		writeMatrix(entity, { theta: 90 })
+		const pose = matrixToPose(entity.get(traits.Matrix)!, createPose())
+		expect(pose.x).toBeCloseTo(10)
+		expect(pose.theta).toBeCloseTo(90)
+		expect(pose.oX).toBeCloseTo(0.6)
+		expect(pose.oY).toBeCloseTo(0.8)
+		expect(pose.oZ).toBeCloseTo(0)
+	})
+
+	it('notifies subscribers via entity.changed', () => {
+		const world = createWorld()
+		const entity = world.spawn(traits.Matrix(matrix()))
+		const onChange = vi.fn()
+		world.onChange(traits.Matrix, onChange)
+		writeMatrix(entity, { x: 5 })
+		expect(onChange).toHaveBeenCalledWith(entity)
+	})
+
+	it('mutates the existing Matrix4 in place (does not allocate a new one)', () => {
+		const world = createWorld()
+		const entity = world.spawn(traits.Matrix(matrix()))
+		const before = entity.get(traits.Matrix)
+		writeMatrix(entity, { x: 5 })
+		const after = entity.get(traits.Matrix)
+		expect(after).toBe(before)
+	})
+
+	it('ignores explicitly undefined fields', () => {
+		const world = createWorld()
+		const entity = world.spawn(traits.Matrix(matrix()))
+		writeMatrix(entity, { x: undefined })
+		const pose = matrixToPose(entity.get(traits.Matrix)!, createPose())
+		expect(pose.x).toBeCloseTo(10)
+		expect(pose.y).toBeCloseTo(20)
+		expect(pose.z).toBeCloseTo(30)
+	})
+
+	it('does not notify subscribers when patch is empty', () => {
+		const world = createWorld()
+		const entity = world.spawn(traits.Matrix(matrix()))
+		const onChange = vi.fn()
+		world.onChange(traits.Matrix, onChange)
+		writeMatrix(entity, {})
+		expect(onChange).not.toHaveBeenCalled()
+	})
+
+	it('does not notify subscribers when all patch fields are undefined', () => {
+		const world = createWorld()
+		const entity = world.spawn(traits.Matrix(matrix()))
+		const onChange = vi.fn()
+		world.onChange(traits.Matrix, onChange)
+		writeMatrix(entity, { x: undefined, y: undefined })
+		expect(onChange).not.toHaveBeenCalled()
+	})
+})

src/lib/ecs/traits.ts

@@ -1,6 +1,6 @@
 import type { GLTF as ThreeGltf } from 'three/examples/jsm/loaders/GLTFLoader.js'
 
-import { Geometry as ViamGeometry } from '@viamrobotics/sdk'
+import { Pose, Geometry as ViamGeometry } from '@viamrobotics/sdk'
 import { type Entity, trait } from 'koota'
 import { Matrix4, BufferGeometry as ThreeBufferGeometry } from 'three'
 
@@ -9,6 +9,7 @@ import { ColorFormat } from '$lib/buf/draw/v1/metadata_pb'
 import { createBox, createCapsule, createSphere } from '$lib/geometry'
 import { parsePcdInWorker } from '$lib/loaders/pcd'
 import { parsePlyInput } from '$lib/ply'
+import { createPose, matrixToPose, poseToMatrix } from '$lib/transform'
 
 export const Name = trait(() => '')
 export const UUID = trait(() => '')
@@ -74,6 +75,14 @@ export const InstancedMatrix = trait(() => ({
 export const Hovered = trait(() => true)
 export const Invisible = trait(() => true)
 
+/**
+ * Suppresses the default frame-style world/local pose and parent-frame blocks
+ * in the details panel. Entities that render their own pose UI via the
+ * `details-extensions` portal target (e.g. gizmo plugin entities) opt in by
+ * adding this trait.
+ */
+export const CustomDetails = trait(() => true)
+
 /**
  * True when the entity itself, or any of its parents up the `ChildOf`
  * chain, has `Invisible`. Maintained by `provideInheritedInvisible`;
@@ -291,6 +300,25 @@ export const updateGeometryTrait = (entity: Entity, geometry?: ViamGeometry) =>
 	}
 }
 
+/**
+ * Patches an entity's `Matrix` trait in-place via the `Pose` round-trip
+ * (`matrixToPose` → merge → `poseToMatrix`), then signals `entity.changed(Matrix)`.
+ * No-ops silently if the entity has no `Matrix` trait.
+ */
+export const writeMatrix = (entity: Entity, patch: Partial<Pose>) => {
+	const matrix = entity.get(Matrix)
+	if (!matrix) return
+
+	const pose = matrixToPose(matrix, createPose())
+	const filtered = Object.fromEntries(
+		Object.entries(patch).filter(([, v]) => v !== undefined)
+	) as Partial<Pose>
+	if (Object.keys(filtered).length === 0) return
+	Object.assign(pose, filtered)
+	poseToMatrix(pose, matrix)
+	entity.changed(Matrix)
+}
+
 const updatePointCloud = (entity: Entity, pointCloud: Uint8Array): void => {
 	parsePcdInWorker(new Uint8Array(pointCloud))
 		.then((parsed) => {

src/lib/hooks/useMouseRaycaster.svelte.ts

@@ -14,11 +14,17 @@ interface RaycastEvent<T extends EventNames> {
 
 type Callback<T extends EventNames> = (event: RaycastEvent<T>) => void
 
-export const useMouseRaycaster = (getOptions?: () => { enabled: boolean }) => {
+interface MouseRaycasterOptions {
+	enabled?: boolean
+	firstHitOnly?: boolean
+}
+
+export const useMouseRaycaster = (getOptions?: () => MouseRaycasterOptions) => {
 	let intersections: Intersection[] = []
 
-	const options = $derived({
+	const options = $derived<Required<MouseRaycasterOptions>>({
 		enabled: true,
+		firstHitOnly: false,
 		...getOptions?.(),
 	})
 
@@ -105,6 +111,15 @@ export const useMouseRaycaster = (getOptions?: () => { enabled: boolean }) => {
 		intersections = currentIntersections
 	}
 
+	// firstHitOnly is set on the shared raycaster instance, so it applies to
+	// both onPointerMove (hover) and onPointerUp (click). This is intentional
+	// for gizmo-style consumers that only care about the closest hit.
+	// Kept in its own effect so toggling firstHitOnly doesn't tear down and
+	// re-attach the event listeners.
+	$effect(() => {
+		raycaster.firstHitOnly = options.firstHitOnly
+	})
+
 	$effect(() => {
 		if (!options.enabled) {
 			return

src/lib/hooks/useSettings.svelte.ts

@@ -7,7 +7,7 @@ const key = Symbol('dashboard-context')
 
 export interface Settings {
 	cameraMode: 'orthographic' | 'perspective'
-	interactionMode: 'navigate' | 'measure' | 'select'
+	interactionMode: 'navigate' | 'measure' | 'select' | 'gizmo'
 	refreshRates: {
 		poses: number
 		pointclouds: number

src/lib/three/arrow.ts

@@ -1,13 +1,16 @@
 import { BoxGeometry, type BufferGeometry, ConeGeometry } from 'three'
 import { mergeGeometries } from 'three/examples/jsm/utils/BufferGeometryUtils.js'
 
+/** Total length of the geometry produced by `createArrowGeometry`, in meters. */
+export const ARROW_LENGTH = 0.1
+
 /**
  * Returns one merged geometry for an arrow (box tail + cone head)
  *
  * Arrow points along +Y with its base at y = 0
  */
 export const createArrowGeometry = (): BufferGeometry => {
-	const length = 0.1
+	const length = ARROW_LENGTH
 	const headLength = length * 0.3
 	const headWidth = headLength * 0.3
 	const tailLength = length - headLength