@viamrobotics/motion-tools 1.11.0 → 1.12.0

package/dist/components/App.svelte

@@ -1,6 +1,7 @@
 <script lang="ts">
 	import type { Snippet } from 'svelte'
 	import { Canvas } from '@threlte/core'
+	import { PortalTarget } from '@threlte/extras'
 	import { SvelteQueryDevtools } from '@tanstack/svelte-query-devtools'
 	import { provideToast, ToastContainer } from '@viamrobotics/prime-core'
 	import type { Struct } from '@viamrobotics/sdk'
@@ -29,6 +30,7 @@
 	import Camera from './overlay/widgets/Camera.svelte'
 	import HoveredEntities from './hover/HoveredEntities.svelte'
 	import Settings from './overlay/settings/Settings.svelte'
+	import { useXR } from '@threlte/xr'
 
 	interface LocalConfigProps {
 		getLocalPartConfig: () => Struct
@@ -67,6 +69,7 @@
 	const settings = provideSettings()
 	const environment = provideEnvironment()
 	const currentRobotCameraWidgets = $derived(settings.current.openCameraWidgets[partID] || [])
+	const { isPresenting } = useXR()
 
 	$effect(() => {
 		settings.current.enableKeybindings = enableKeybindings
@@ -148,11 +151,13 @@
 						<ArmPositions />
 					{/if}
 
-					{#if !focus}
+					{#if !focus && !$isPresenting}
 						{#each currentRobotCameraWidgets as cameraName (cameraName)}
 							<Camera name={cameraName} />
 						{/each}
 					{/if}
+
+					<PortalTarget id="dom" />
 				</div>
 			{/snippet}
 		</SceneProviders>

package/dist/components/CameraControls.svelte

@@ -20,7 +20,7 @@
 			icon="camera-outline"
 			description="Reset camera"
 			onclick={() => {
-				cameraControls.current?.reset(true)
+				cameraControls.setInitialPose()
 			}}
 		/>
 	</fieldset>

package/dist/components/Focus.svelte

@@ -49,14 +49,7 @@
 	</TrackballControls>
 </Camera>
 
-<T
-	is={object3d}
-	bvh={{
-		enabled: object3d.type === 'Points',
-		maxDepth: 40,
-		maxLeafTris: 20,
-	}}
-/>
+<T is={object3d} />
 
 <T.BoxHelper
 	args={[object3d, 'red']}

package/dist/components/Geometry.svelte

@@ -3,7 +3,7 @@
 	import { type Snippet } from 'svelte'
 	import { meshBounds } from '@threlte/extras'
 	import { BufferGeometry, Color, DoubleSide, FrontSide, Group, Mesh } from 'three'
-	import { Line2, LineGeometry, LineMaterial } from 'three/examples/jsm/Addons.js'
+	import { Line2, LineMaterial } from 'three/examples/jsm/Addons.js'
 	import { CapsuleGeometry } from '../three/CapsuleGeometry'
 	import { colors, darkenColor } from '../color'
 	import AxesHelper from './AxesHelper.svelte'
@@ -11,6 +11,7 @@
 	import { traits, useTrait } from '../ecs'
 	import { poseToObject3d } from '../transform'
 	import type { Pose } from '@viamrobotics/sdk'
+	import LineGeometry from './LineGeometry.svelte'
 
 	interface Props extends ThrelteProps<Group> {
 		entity: Entity
@@ -47,6 +48,8 @@
 	const lineWidth = useTrait(() => entity, traits.LineWidth)
 	const center = useTrait(() => entity, traits.Center)
 	const showAxesHelper = useTrait(() => entity, traits.ShowAxesHelper)
+	const materialProps = useTrait(() => entity, traits.Material)
+	const renderOrder = useTrait(() => entity, traits.RenderOrder)
 
 	const geometryType = $derived.by(() => {
 		if (box.current) return 'box'
@@ -136,7 +139,7 @@
 			is={mesh}
 			name={entity}
 			userData.name={name}
-			bvh={{ enabled: geometryType === 'buffer' }}
+			renderOrder={renderOrder.current}
 		>
 			{#if model && renderMode.includes('model')}
 				<T is={model} />
@@ -144,14 +147,7 @@
 
 			{#if !model || renderMode.includes('colliders')}
 				{#if linePositions.current}
-					<T
-						is={LineGeometry}
-						oncreate={(ref) => {
-							if (linePositions.current) {
-								ref.setPositions(linePositions.current)
-							}
-						}}
-					/>
+					<LineGeometry positions={linePositions.current} />
 				{:else if box.current}
 					{@const { x, y, z } = box.current ?? { x: 0, y: 0, z: 0 }}
 					<T.BoxGeometry
@@ -179,16 +175,20 @@
 					is={LineMaterial}
 					{color}
 					width={lineWidth.current ? lineWidth.current * 0.001 : 0.5}
+					depthTest={materialProps.current?.depthTest}
 				/>
 			{:else}
+				{@const currentOpacity = opacity.current ?? 0.7}
 				<T.MeshToonMaterial
 					{color}
 					side={geometryType === 'buffer' ? DoubleSide : FrontSide}
-					transparent={(opacity.current ?? 0.7) < 1}
-					opacity={opacity.current ?? 0.7}
+					transparent={currentOpacity < 1}
+					depthWrite={currentOpacity === 1}
+					opacity={currentOpacity}
+					depthTest={materialProps.current?.depthTest}
 				/>
 
-				{#if geo && renderMode.includes('colliders')}
+				{#if geo && (renderMode.includes('colliders') || !model)}
 					<T.LineSegments
 						raycast={() => null}
 						bvh={{ enabled: false }}

package/dist/components/Lasso/Debug.svelte

@@ -0,0 +1,72 @@
+<!--
+@component
+
+Shows all steps for querying points within a lasso selection
+-->
+<script lang="ts">
+	import { T } from '@threlte/core'
+	import { Box3, BufferAttribute, BufferGeometry, Vector3 } from 'three'
+	import { useTrait, traits } from '../../ecs'
+	import type { Entity } from 'koota'
+	import * as lassoTraits from './traits'
+
+	const box3 = new Box3()
+	const min = new Vector3()
+	const max = new Vector3()
+
+	interface Props {
+		lasso: Entity
+	}
+
+	let { lasso }: Props = $props()
+
+	const indices = useTrait(() => lasso, lassoTraits.Indices)
+	const positions = useTrait(() => lasso, traits.LinePositions)
+	const box = useTrait(() => lasso, lassoTraits.Box)
+	const boxes = useTrait(() => lasso, lassoTraits.Boxes)
+
+	const geometry = new BufferGeometry()
+
+	$effect(() => {
+		if (indices.current) {
+			geometry.setIndex(new BufferAttribute(indices.current, 1))
+		}
+
+		if (positions.current) {
+			geometry.setAttribute('position', new BufferAttribute(positions.current, 3))
+		}
+	})
+</script>
+
+{#if positions.current && indices.current}
+	<T.Mesh>
+		<T is={geometry} />
+		<T.MeshBasicMaterial
+			wireframe
+			color="green"
+		/>
+	</T.Mesh>
+{/if}
+
+{#if boxes.current}
+	{#each boxes.current as box (box)}
+		<T.Box3Helper
+			args={[
+				new Box3().set(min.set(box.minX, box.minY, 0), max.set(box.maxX, box.maxY, 0)),
+				'lightgreen',
+			]}
+		/>
+	{/each}
+{/if}
+
+{#if box.current}
+	<T.Box3Helper
+		args={[
+			box3.set(
+				min.set(box.current.minX, box.current.minY, 0),
+				max.set(box.current.maxX, box.current.maxY, 0)
+			),
+			'red',
+		]}
+	/>
+{/if}

package/dist/components/Lasso/Debug.svelte.d.ts

@@ -0,0 +1,8 @@
+import type { Entity } from 'koota';
+interface Props {
+    lasso: Entity;
+}
+/** Shows all steps for querying points within a lasso selection */
+declare const Debug: import("svelte").Component<Props, {}, "">;
+type Debug = ReturnType<typeof Debug>;
+export default Debug;

package/dist/components/Lasso/Lasso.svelte

@@ -0,0 +1,299 @@
+<script lang="ts">
+	import { Raycaster, Box3, Vector3, Vector2, Plane, Triangle } from 'three'
+	import { useThrelte } from '@threlte/core'
+	import { Not } from 'koota'
+	import { useCameraControls } from '../../hooks/useControls.svelte'
+	import earcut from 'earcut'
+	import { traits, useQuery, useWorld } from '../../ecs'
+	import type { ShapecastCallbacks } from 'three-mesh-bvh'
+	import { createBufferGeometry } from '../../attribute'
+	import * as lassoTraits from './traits'
+	import Debug from './Debug.svelte'
+
+	interface Props {
+		debug?: boolean
+	}
+
+	let { debug = false }: Props = $props()
+
+	const world = useWorld()
+	const controls = useCameraControls()
+	const { scene, dom, camera } = useThrelte()
+
+	const box3 = new Box3()
+	const min = new Vector3()
+	const max = new Vector3()
+
+	const triangle = new Triangle()
+	const triangleBox = new Box3()
+	const a = new Vector3()
+	const b = new Vector3()
+	const c = new Vector3()
+
+	let frameScheduled = false
+	let drawing = false
+
+	const raycaster = new Raycaster()
+	const mouse = new Vector2()
+	const plane = new Plane(new Vector3(0, 0, 1), 0)
+	const point = new Vector3()
+
+	const raycast = (event: PointerEvent) => {
+		mouse.x = (event.clientX / window.innerWidth) * 2 - 1
+		mouse.y = -(event.clientY / window.innerHeight) * 2 + 1
+
+		raycaster.setFromCamera(mouse, camera.current)
+		raycaster.ray.intersectPlane(plane, point)
+		return point
+	}
+
+	const onpointerdown = (event: PointerEvent) => {
+		if (!event.shiftKey) return
+
+		const { x, y } = raycast(event)
+
+		drawing = true
+
+		world.spawn(
+			traits.LinePositions(new Float32Array([x, y, 0])),
+			traits.LineWidth(1.5),
+			traits.RenderOrder(999),
+			traits.Material({ depthTest: false }),
+			traits.Color({ r: 1, g: 0, b: 0 }),
+			lassoTraits.Box({ minX: x, minY: y, maxX: x, maxY: y }),
+			lassoTraits.Lasso
+		)
+
+		if (controls.current) {
+			controls.current.enabled = false
+		}
+	}
+
+	const onpointermove = (event: PointerEvent) => {
+		if (!drawing) return
+
+		let lasso = world.query(lassoTraits.Lasso).at(-1)
+
+		if (!lasso) return
+
+		if (frameScheduled) return
+
+		frameScheduled = true
+
+		/**
+		 * pointermove can execute at a rate much higher than screen
+		 * refresh, creating huge polygon vertex counts, so we cap it.
+		 */
+		requestAnimationFrame(() => {
+			frameScheduled = false
+
+			const { x, y } = raycast(event)
+			const positions = lasso.get(traits.LinePositions)
+			const box = lasso.get(lassoTraits.Box)
+
+			if (!positions || !box) return
+
+			const nextPositions = new Float32Array(positions.length + 3)
+			nextPositions.set(positions)
+			nextPositions[positions.length] = x
+			nextPositions[positions.length + 1] = y
+			lasso.set(traits.LinePositions, nextPositions)
+
+			if (x < box.minX) box.minX = x
+			else if (x > box.maxX) box.maxX = x
+
+			if (y < box.minY) box.minY = y
+			else if (y > box.maxY) box.maxY = y
+
+			lasso.set(lassoTraits.Box, box)
+		})
+	}
+
+	const onpointerleave = () => {
+		if (!drawing) return
+
+		onpointerup()
+	}
+
+	const onpointerup = () => {
+		if (!drawing) return
+
+		drawing = false
+
+		let lasso = world.query(lassoTraits.Lasso).at(-1)
+
+		if (!lasso) return
+
+		let positions = lasso.get(traits.LinePositions)
+
+		if (!positions) return
+
+		const [startX, startY] = positions
+
+		if (controls.current) {
+			controls.current.enabled = true
+		}
+
+		// Close the loop
+		const nextPositions = new Float32Array(positions.length + 3)
+		nextPositions.set(positions)
+		nextPositions[positions.length] = startX
+		nextPositions[positions.length + 1] = startY
+		lasso.set(traits.LinePositions, nextPositions)
+		positions = nextPositions
+
+		const indices = earcut(positions, undefined, 3)
+		if (debug) {
+			lasso.add(lassoTraits.Indices(new Uint16Array(indices)))
+		}
+
+		const getTriangleFromIndex = (i: number, triangle: Triangle) => {
+			const stride = 3
+			const ia = indices[i + 0] * stride
+			const ib = indices[i + 1] * stride
+			const ic = indices[i + 2] * stride
+			a.set(positions[ia + 0], positions[ia + 1], positions[ia + 2])
+			b.set(positions[ib + 0], positions[ib + 1], positions[ib + 2])
+			c.set(positions[ic + 0], positions[ic + 1], positions[ic + 2])
+			triangle.set(a, b, c)
+		}
+
+		const boxes: lassoTraits.AABB[] = []
+		for (let i = 0, l = indices.length; i < l; i += 3) {
+			getTriangleFromIndex(i, triangle)
+			box3.setFromPoints([triangle.a, triangle.b, triangle.c])
+			boxes.push({ minX: box3.min.x, minY: box3.min.y, maxX: box3.max.x, maxY: box3.max.y })
+		}
+		if (debug) {
+			lasso.add(lassoTraits.Boxes(boxes))
+		}
+
+		const lassoBox = lasso.get(lassoTraits.Box)
+
+		if (!lassoBox) return
+
+		min.set(lassoBox.minX, lassoBox.minY, Number.NEGATIVE_INFINITY)
+		max.set(lassoBox.maxX, lassoBox.maxY, Number.POSITIVE_INFINITY)
+		box3.set(min, max)
+
+		const enclosedPoints: number[] = []
+
+		for (const pointsEntity of world.query(traits.Points, Not(lassoTraits.LassoEnclosedPoints))) {
+			const geometry = pointsEntity.get(traits.BufferGeometry)
+
+			if (!geometry) return
+
+			const points = scene.getObjectByName(pointsEntity as unknown as string)
+
+			if (!points) {
+				return
+			}
+
+			geometry.boundsTree?.shapecast({
+				intersectsBounds: (box) => {
+					return box.intersectsBox(box3)
+				},
+
+				intersectsPoint: (point: Vector3) => {
+					for (let i = 0, j = 0, l = indices.length; i < l; i += 3, j += 1) {
+						const { minX, minY, maxX, maxY } = boxes[j]
+
+						min.set(minX, minY, Number.NEGATIVE_INFINITY)
+						max.set(maxX, maxY, Number.POSITIVE_INFINITY)
+						triangleBox.set(min, max)
+
+						if (triangleBox.containsPoint(point)) {
+							getTriangleFromIndex(i, triangle)
+
+							if (triangle.containsPoint(point)) {
+								enclosedPoints.push(point.x, point.y, point.z)
+							}
+						}
+					}
+				},
+				// intersectsPoint is not yet in typedef, this can be removed when it is added
+			} as ShapecastCallbacks)
+		}
+
+		const lassoResultGeometry = createBufferGeometry(new Float32Array(enclosedPoints))
+
+		world.spawn(
+			traits.Name('Lasso result'),
+			traits.BufferGeometry(lassoResultGeometry),
+			traits.Color({ r: 1, g: 0, b: 0 }),
+			traits.RenderOrder(999),
+			traits.Material({ depthTest: false }),
+			traits.Points,
+			traits.Removable,
+			lassoTraits.LassoEnclosedPoints,
+			lassoTraits.PointsCapturedBy(lasso)
+		)
+	}
+
+	const onkeydown = (event: KeyboardEvent) => {
+		if (event.key === 'Shift') {
+			dom.style.cursor = 'crosshair'
+		}
+	}
+
+	const onkeyup = (event: KeyboardEvent) => {
+		if (event.key === 'Shift') {
+			dom.style.removeProperty('cursor')
+		}
+	}
+
+	$effect(() => {
+		window.addEventListener('keydown', onkeydown)
+		window.addEventListener('keyup', onkeyup)
+		dom.addEventListener('pointerdown', onpointerdown)
+		dom.addEventListener('pointermove', onpointermove)
+		dom.addEventListener('pointerup', onpointerup)
+		dom.addEventListener('pointerleave', onpointerleave)
+
+		return () => {
+			window.removeEventListener('keydown', onkeydown)
+			window.removeEventListener('keyup', onkeyup)
+			dom.removeEventListener('pointerdown', onpointerdown)
+			dom.removeEventListener('pointermove', onpointermove)
+			dom.removeEventListener('pointerup', onpointerup)
+			dom.removeEventListener('pointerleave', onpointerleave)
+		}
+	})
+
+	const lassos = useQuery(lassoTraits.Lasso)
+
+	$effect(() => {
+		if (!controls.current) return
+
+		const currentControls = controls.current
+
+		const { minPolarAngle, maxPolarAngle } = currentControls
+
+		// Locks the camera to top down while this component is mounted
+		currentControls.polarAngle = 0
+		currentControls.minPolarAngle = 0
+		currentControls.maxPolarAngle = 0
+
+		return () => {
+			currentControls.minPolarAngle = minPolarAngle
+			currentControls.maxPolarAngle = maxPolarAngle
+		}
+	})
+
+	// On unmount, destroy all lasso related entities
+	$effect(() => {
+		return () => {
+			for (const entity of world.query(lassoTraits.LassoEnclosedPoints)) {
+				if (world.has(entity)) {
+					entity.destroy()
+				}
+			}
+		}
+	})
+</script>
+
+{#if debug}
+	{#each lassos.current as lasso (lasso)}
+		<Debug {lasso} />
+	{/each}
+{/if}

package/dist/components/Lasso/Lasso.svelte.d.ts

@@ -0,0 +1,6 @@
+interface Props {
+    debug?: boolean;
+}
+declare const Lasso: import("svelte").Component<Props, {}, "">;
+type Lasso = ReturnType<typeof Lasso>;
+export default Lasso;

package/dist/components/Lasso/Tool.svelte

@@ -0,0 +1,94 @@
+<script lang="ts">
+	import { Portal } from '@threlte/extras'
+	import { Button } from '@viamrobotics/prime-core'
+	import Lasso from './Lasso.svelte'
+	import DashboardButton from '../overlay/dashboard/Button.svelte'
+	import { useSettings } from '../../hooks/useSettings.svelte'
+	import FloatingPanel from '../overlay/FloatingPanel.svelte'
+	import { traits, useWorld } from '../../ecs'
+	import * as lassoTraits from './traits'
+	import { BufferGeometryUtils } from 'three/examples/jsm/Addons.js'
+	import { createBinaryPCD } from '../../pcd'
+	import type { BufferGeometry } from 'three'
+
+	interface Props {
+		/** Whether to auto-enable lasso mode when the component mounts */
+		enabled?: boolean
+
+		/** Fires when the user has committed to a lasso selection */
+		onSelection: (pcd: Blob) => void
+	}
+
+	let { enabled = false, onSelection }: Props = $props()
+
+	const world = useWorld()
+	const settings = useSettings()
+	const isLassoMode = $derived(settings.current.interactionMode === 'lasso')
+
+	const onCommitClick = () => {
+		const entities = world.query(lassoTraits.LassoEnclosedPoints)
+
+		const geometries: BufferGeometry[] = []
+		for (const entity of entities) {
+			const geometry = entity.get(traits.BufferGeometry)
+
+			if (geometry) {
+				geometries.push(geometry)
+			}
+		}
+
+		const mergedGeometry = BufferGeometryUtils.mergeGeometries(geometries)
+		const positions = mergedGeometry.getAttribute('position').array as Float32Array
+
+		const pcd = createBinaryPCD(positions)
+
+		onSelection(pcd)
+
+		for (const entity of entities) {
+			if (world.has(entity)) {
+				entity.destroy()
+			}
+		}
+	}
+
+	$effect(() => {
+		if (enabled) {
+			settings.current.interactionMode = 'lasso'
+		}
+	})
+</script>
+
+<Portal id="dashboard">
+	<fieldset>
+		<DashboardButton
+			active={isLassoMode}
+			icon="selection-drag"
+			description="{isLassoMode ? 'Disable' : 'Enable'} lasso selection"
+			onclick={() => {
+				settings.current.interactionMode = isLassoMode ? 'navigate' : 'lasso'
+			}}
+		/>
+	</fieldset>
+</Portal>
+
+{#if isLassoMode}
+	<Lasso />
+
+	<Portal id="dom">
+		<FloatingPanel
+			isOpen
+			exitable={false}
+			title="Lasso"
+			defaultSize={{ width: 445, height: 100 }}
+			defaultPosition={{ x: window.innerWidth / 2 - 200, y: window.innerHeight - 10 - 100 }}
+		>
+			<div class="flex items-center gap-4 p-4 text-xs">
+				Shift + click and drag to make a lasso selection.
+				<Button
+					onclick={onCommitClick}
+					variant="success">Commit selection</Button
+				>
+			</div>
+		</FloatingPanel>
+	</Portal>
+{/if}

package/dist/components/Lasso/Tool.svelte.d.ts

@@ -0,0 +1,9 @@
+interface Props {
+    /** Whether to auto-enable lasso mode when the component mounts */
+    enabled?: boolean;
+    /** Fires when the user has committed to a lasso selection */
+    onSelection: (pcd: Blob) => void;
+}
+declare const Tool: import("svelte").Component<Props, {}, "">;
+type Tool = ReturnType<typeof Tool>;
+export default Tool;

package/dist/components/Lasso/traits.d.ts

@@ -0,0 +1,21 @@
+export declare const Lasso: import("koota").Trait<() => boolean>;
+export declare const LassoEnclosedPoints: import("koota").Trait<() => boolean>;
+/**
+ * Captured points are removable, so we want to also destroy
+ * the source lasso every time a user deletes one.
+ */
+export declare const PointsCapturedBy: import("koota").Relation<import("koota").Trait<Record<string, never>>>;
+export interface AABB {
+    minX: number;
+    minY: number;
+    maxX: number;
+    maxY: number;
+}
+export declare const Box: import("koota").Trait<{
+    minX: number;
+    minY: number;
+    maxX: number;
+    maxY: number;
+}>;
+export declare const Indices: import("koota").Trait<() => Uint16Array<ArrayBuffer>>;
+export declare const Boxes: import("koota").Trait<() => AABB[]>;

package/dist/components/Lasso/traits.js

@@ -0,0 +1,16 @@
+import { relation, trait } from 'koota';
+export const Lasso = trait(() => true);
+export const LassoEnclosedPoints = trait(() => true);
+/**
+ * Captured points are removable, so we want to also destroy
+ * the source lasso every time a user deletes one.
+ */
+export const PointsCapturedBy = relation({ autoDestroy: 'target' });
+export const Box = trait({
+    minX: 0,
+    minY: 0,
+    maxX: 0,
+    maxY: 0,
+});
+export const Indices = trait(() => new Uint16Array());
+export const Boxes = trait(() => []);

package/dist/components/LineGeometry.svelte

@@ -0,0 +1,20 @@
+<script>
+	import { LineGeometry } from 'three/examples/jsm/Addons.js'
+	import { T } from '@threlte/core'
+	import { untrack } from 'svelte'
+
+	let { positions } = $props()
+
+	let geometry = $state.raw(new LineGeometry())
+
+	$effect(() => {
+		if (positions) {
+			untrack(() => {
+				geometry = new LineGeometry()
+				geometry.setPositions(positions)
+			})
+		}
+	})
+</script>
+
+<T is={geometry} />