diff --git a/.changeset/gizmo-foundation-utils.md b/.changeset/gizmo-foundation-utils.md new file mode 100644 index 000000000..175894a35 --- /dev/null +++ b/.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 diff --git a/src/lib/draw.ts b/src/lib/draw.ts index dd433f1df..27620a26b 100644 --- a/src/lib/draw.ts +++ b/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 diff --git a/src/lib/ecs/__tests__/writeMatrix.spec.ts b/src/lib/ecs/__tests__/writeMatrix.spec.ts new file mode 100644 index 000000000..158fc005a --- /dev/null +++ b/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() + }) +}) diff --git a/src/lib/ecs/traits.ts b/src/lib/ecs/traits.ts index d1531b5a5..d48983c53 100644 --- a/src/lib/ecs/traits.ts +++ b/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 { type 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`; @@ -296,6 +305,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) => { + 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 + 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) => { diff --git a/src/lib/hooks/useMouseRaycaster.svelte.ts b/src/lib/hooks/useMouseRaycaster.svelte.ts index 65e78fefe..b0f2c2063 100644 --- a/src/lib/hooks/useMouseRaycaster.svelte.ts +++ b/src/lib/hooks/useMouseRaycaster.svelte.ts @@ -14,10 +14,14 @@ interface RaycastEvent { type Callback = (event: RaycastEvent) => void -export const useMouseRaycaster = (getOptions?: () => { enabled: boolean }) => { +interface MouseRaycasterOptions { + enabled?: boolean +} + +export const useMouseRaycaster = (getOptions?: () => MouseRaycasterOptions) => { let intersections: Intersection[] = [] - const options = $derived({ + const options = $derived>({ enabled: true, ...getOptions?.(), }) @@ -110,6 +114,8 @@ export const useMouseRaycaster = (getOptions?: () => { enabled: boolean }) => { return } + raycaster.firstHitOnly = true + dom.addEventListener('pointermove', onPointerMove, { passive: true }) dom.addEventListener('pointerdown', onPointerDown, { passive: true }) dom.addEventListener('pointerup', onPointerUp, { passive: true }) diff --git a/src/lib/hooks/useSettings.svelte.ts b/src/lib/hooks/useSettings.svelte.ts index fea0e6079..bd68c77d5 100644 --- a/src/lib/hooks/useSettings.svelte.ts +++ b/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 diff --git a/src/lib/three/arrow.ts b/src/lib/three/arrow.ts index 4798d5e69..2a6e35439 100644 --- a/src/lib/three/arrow.ts +++ b/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