From 4d49c75f3b323f8971e839bc22ae71f1373cff86 Mon Sep 17 00:00:00 2001
From: Micheal Parks <103450731+micheal-parks@users.noreply.github.com>
Date: Thu, 25 Jul 2024 12:11:54 -0400
Subject: [PATCH] [RSDK-7089] Orientation format fix, add LengthCapsuleGeometry
 (#545)

---
 packages/blocks/package.json                  |  2 +-
 .../lib/capsule-geometry/capsule-geometry.ts  | 28 +++++++++++++++++++
 packages/blocks/src/lib/index.ts              |  1 +
 .../components/input/orientation.svelte       | 20 +++++++------
 .../components/nav/obstacles.svelte           |  7 ++---
 .../navigation-map/components/obstacle.svelte | 13 +++++++--
 6 files changed, 54 insertions(+), 17 deletions(-)
 create mode 100644 packages/blocks/src/lib/capsule-geometry/capsule-geometry.ts

diff --git a/packages/blocks/package.json b/packages/blocks/package.json
index a19d880b..b8ba1840 100644
--- a/packages/blocks/package.json
+++ b/packages/blocks/package.json
@@ -1,6 +1,6 @@
 {
   "name": "@viamrobotics/prime-blocks",
-  "version": "0.1.0",
+  "version": "0.1.1",
   "publishConfig": {
     "access": "public"
   },
diff --git a/packages/blocks/src/lib/capsule-geometry/capsule-geometry.ts b/packages/blocks/src/lib/capsule-geometry/capsule-geometry.ts
new file mode 100644
index 00000000..0494ae40
--- /dev/null
+++ b/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);
+  }
+}
diff --git a/packages/blocks/src/lib/index.ts b/packages/blocks/src/lib/index.ts
index 9287fbec..aa5d64f5 100644
--- a/packages/blocks/src/lib/index.ts
+++ b/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';
diff --git a/packages/blocks/src/lib/navigation-map/components/input/orientation.svelte b/packages/blocks/src/lib/navigation-map/components/input/orientation.svelte
index 6574bb13..c0a30529 100644
--- a/packages/blocks/src/lib/navigation-map/components/input/orientation.svelte
+++ b/packages/blocks/src/lib/navigation-map/components/input/orientation.svelte
@@ -1,14 +1,16 @@
+<!--
+@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;
 
@@ -16,7 +18,7 @@ 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()}
diff --git a/packages/blocks/src/lib/navigation-map/components/nav/obstacles.svelte b/packages/blocks/src/lib/navigation-map/components/nav/obstacles.svelte
index 9c874ee5..525daec3 100644
--- a/packages/blocks/src/lib/navigation-map/components/nav/obstacles.svelte
+++ b/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>
diff --git a/packages/blocks/src/lib/navigation-map/components/obstacle.svelte b/packages/blocks/src/lib/navigation-map/components/obstacle.svelte
index 3780355a..45a32ab3 100644
--- a/packages/blocks/src/lib/navigation-map/components/obstacle.svelte
+++ b/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}