[RSDK-7089] Orientation format fix, add LengthCapsuleGeometry (viamro…

…botics#545)

packages/blocks/package.json

@@ -1,6 +1,6 @@
 {
   "name": "@viamrobotics/prime-blocks",
-  "version": "0.1.0",
+  "version": "0.1.1",
   "publishConfig": {
     "access": "public"
   },

packages/blocks/src/lib/capsule-geometry/capsule-geometry.ts

@@ -0,0 +1,28 @@
+/* eslint-disable max-classes-per-file */
+import { LatheGeometry, Path } from 'three';
+
+/**
+ * An alternate definition of a THREE.CapsuleGeometry: the length
+ * represents the entire length of the capsule, including the rounded ends,
+ * rather than just the midsection, which is the default THREE.CapsuleGeometry definition.
+ */
+export class LengthCapsuleGeometry extends LatheGeometry {
+  override type = 'CapsuleGeometry';
+
+  constructor(radius = 1, length = 1, capSegments = 4, radialSegments = 8) {
+    const path = new Path();
+    const midsectionLength = length - 2 * radius;
+
+    path.absarc(
+      0,
+      -midsectionLength / 2 - radius / 2,
+      radius,
+      Math.PI * 1.5,
+      0
+    );
+
+    path.absarc(0, midsectionLength / 2 + radius / 2, radius, 0, Math.PI * 0.5);
+
+    super(path.getPoints(capSegments), radialSegments);
+  }
+}

packages/blocks/src/lib/index.ts

@@ -1,5 +1,6 @@
 // Three.js components
 export { default as AxesHelper } from './axes-helper/axes-helper.svelte';
+export { LengthCapsuleGeometry } from './capsule-geometry/capsule-geometry';
 
 // MapLibre components
 export { LngLat, MercatorCoordinate } from 'maplibre-gl';

packages/blocks/src/lib/navigation-map/components/input/orientation.svelte

@@ -1,22 +1,24 @@
+<!--
+@component
+An editable 2d rotation input, presented to the user in degrees
+-->
 <script lang="ts">
-import { createEventDispatcher } from 'svelte';
+import { MathUtils } from 'three';
 import { SliderInput, Label } from '@viamrobotics/prime-core';
 
-/** The orientation angle along the z-axis. */
+/** The rotation in radians */
 export let th: number;
 
-const dispatch = createEventDispatcher<{
-  /** Fires when the orientation is edited. */
-  input: number;
-}>();
+/** Fires when orientation changes with the new value in radians */
+export let onChange: ((th: number) => void) | undefined = undefined;
 
 let input: HTMLInputElement;
 
 const handleInput = () => {
   const value = input.valueAsNumber;
 
   if (!Number.isNaN(value)) {
-    dispatch('input', value);
+    onChange?.(MathUtils.degToRad(value));
   }
 };
 </script>
@@ -27,8 +29,8 @@ const handleInput = () => {
     <SliderInput
       slot="input"
       bind:input
-      value={th}
-      placholder={0}
+      value={MathUtils.radToDeg(th)}
+      placeholder={0}
       on:blur={handleInput}
       on:input={handleInput}
       on:keydown={(event) => event.key === 'Enter' && handleInput()}

packages/blocks/src/lib/navigation-map/components/nav/obstacles.svelte

@@ -70,10 +70,9 @@ const handleGeometryInput =
   };
 
 const handleOrientationInput =
-  (name: string, geoIndex: number) => (event: CustomEvent<number>) => {
+  (name: string, geoIndex: number) => (value: number) => {
     const index = $obstacles.findIndex((obstacle) => obstacle.name === name);
-    $obstacles[index]!.geometries[geoIndex]!.pose.orientationVector.th =
-      event.detail;
+    $obstacles[index]!.geometries[geoIndex]!.pose.orientationVector.th = value;
     dispatch('update', $obstacles);
   };
 
@@ -213,7 +212,7 @@ $: debugMode = $environment === 'debug';
       />
       <OrientationInput
         th={geometry.pose.orientationVector.th}
-        on:input={handleOrientationInput(selectedObstacle.name, geoIndex)}
+        onChange={handleOrientationInput(selectedObstacle.name, geoIndex)}
       />
     {/each}
   </li>

packages/blocks/src/lib/navigation-map/components/obstacle.svelte

@@ -7,7 +7,13 @@ import type {
   MapLayerMouseEvent,
   MapLayerTouchEvent,
 } from 'maplibre-gl';
-import { useMapLibre, type Obstacle, useMapLibreEvent, AxesHelper } from '$lib';
+import {
+  useMapLibre,
+  type Obstacle,
+  useMapLibreEvent,
+  AxesHelper,
+  LengthCapsuleGeometry,
+} from '$lib';
 import { view, hovered, selected, environment, obstacles } from '../stores';
 
 /** The obstacle name. */
@@ -99,7 +105,7 @@ const handlePointerMove = (event: MapMouseEvent) => {
     pointermove.set(event.point.x, event.point.y);
     pointermove.sub(pointerdown);
     obstacle.geometries[0]!.pose.orientationVector.th =
-      pointerdownTheta + pointermove.y;
+      pointerdownTheta + pointermove.y / 10;
 
     // Scale
   } else if (event.originalEvent.altKey) {
@@ -221,7 +227,8 @@ useMapLibreEvent('mousedown', handleMapPointerDown);
         />
       {/if}
 
-      <T.CapsuleGeometry
+      <T
+        is={LengthCapsuleGeometry}
         computeBounding={name}
         args={[geometry.radius, geometry.length, 16, 32]}
         on:create={handleGeometryCreate}