From 5f19b74d2fd0f8e2bb46281dc9f411c327f8683a Mon Sep 17 00:00:00 2001
From: Youwen Wu <youwenw@gmail.com>
Date: Mon, 26 Feb 2024 00:59:23 -0800
Subject: [PATCH] feat: add robot movement animation

---
 .../Dashboard/Visualization/Controls.svelte   | 131 +++++-------------
 .../lib/Dashboard/Visualization/Scene.svelte  | 112 ++++++++-------
 .../Visualization/smoothMotionController.ts   |  54 ++++++++
 3 files changed, 156 insertions(+), 141 deletions(-)
 create mode 100644 client/src/lib/Dashboard/Visualization/smoothMotionController.ts

diff --git a/client/src/lib/Dashboard/Visualization/Controls.svelte b/client/src/lib/Dashboard/Visualization/Controls.svelte
index 50b237c..f78fd68 100644
--- a/client/src/lib/Dashboard/Visualization/Controls.svelte
+++ b/client/src/lib/Dashboard/Visualization/Controls.svelte
@@ -1,10 +1,19 @@
 <script lang="ts">
   import { createEventDispatcher, onDestroy } from 'svelte'
-  import { Camera, Vector2, Vector3, Quaternion } from 'three'
+  import {
+    Camera,
+    Vector2,
+    Vector3,
+    Quaternion,
+    Object3D,
+    type Object3DEventMap,
+    Group,
+  } from 'three'
   import { useThrelte, useParent, useTask } from '@threlte/core'
 
-  export let object: Camera
+  export let object: Group<Object3DEventMap>
   export let rotateSpeed = 1.0
+  export let shouldOrbit: boolean
 
   $: if (object) {
     // console.log(object)
@@ -55,88 +64,52 @@
     )
   }
 
-  // domElement.addEventListener('pointerdown', onPointerDown)
-  // domElement.addEventListener('pointermove', onPointerMove)
-  // domElement.addEventListener('pointerleave', onPointerLeave)
-  // domElement.addEventListener('pointerup', onPointerUp)
-
-  onDestroy(() => {
-    // domElement.removeEventListener('pointerdown', onPointerDown)
-    // domElement.removeEventListener('pointermove', onPointerMove)
-    // domElement.removeEventListener('pointerleave', onPointerLeave)
-    // domElement.removeEventListener('pointerup', onPointerUp)
-  })
-
   // This is basically your update function
   useTask(delta => {
     // the object's position is bound to the prop
     if (!object) return
 
-    // camera is based on character so we rotation character first
-    // rotationQuat.setFromAxisAngle(axis, -rotateDelta.x * rotateSpeed * delta)
-    // object.quaternion.multiply(rotationQuat)
-
     // then we calculate our ideal's
     const offset = vectorFromObject(idealOffset)
     const lookAt = vectorFromObject(idealLookAt)
 
+    // camera is based on character so we rotation character first
+    // rotationQuat.setFromAxisAngle(axis, rotateSpeed * delta)
+    // object.quaternion.multiply(rotationQuat)
+
     // and how far we should move towards them
     const t = 1.0 - Math.pow(0.001, delta)
     currentPosition.lerp(offset, t)
     currentLookAt.lerp(lookAt, t)
 
-    // then finally set the camera, a bit behind the model
-    $camera!.position.copy(currentPosition)
-    const behindOffset = currentPosition
-      .clone()
+    // typescript HACKS! never do this! How does this work? who knows!
+    const robotPosition = vectorFromObject(
+      object as unknown as { x: number; y: number; z: number }
+    )
+
+    const horizontalOffsetDistance = 12 // Distance behind the leading vector
+    const direction = new Vector3(0, 0, 1) // Default forward direction in Three.js is negative z-axis, so behind is positive z-axis
+    const verticalOffset = new Vector3(0, -2.8, 0)
+
+    // Calculate the offset vector
+    const offsetVector = direction
       .normalize()
-      .multiplyScalar(8)
-      .setY(0.5)
+      .multiplyScalar(horizontalOffsetDistance)
+      .add(verticalOffset)
 
-    $camera!.position.copy(currentPosition).add(behindOffset)
+    // If the leading object is rotating, apply its rotation to the offset vector
+    const rotatedOffsetVector = offsetVector.applyQuaternion(object.quaternion)
 
-    $camera!.lookAt(currentLookAt)
-  })
+    // Calculate the trailing vector's position
+    const trailingVector = robotPosition.clone().sub(rotatedOffsetVector)
 
-  function onPointerMove(event: PointerEvent) {
-    const { x, y } = event
-    if (pointerDown && !isOrbiting) {
-      // calculate distance from init down
-      const distCheck =
-        Math.sqrt(
-          Math.pow(x - rotateStart.x, 2) + Math.pow(y - rotateStart.y, 2)
-        ) > 10
-      if (distCheck) {
-        isOrbiting = true
-      }
+    if (!shouldOrbit) {
+      // then finally set the camera, a bit behind the model
+      $camera!.position.copy(trailingVector)
+      // Rotate the offset around the Y-axis
+      $camera!.lookAt(currentLookAt)
     }
-    if (!isOrbiting) return
-
-    rotateEnd.set(x, y)
-    rotateDelta.subVectors(rotateEnd, rotateStart).multiplyScalar(rotateSpeed)
-    rotateStart.copy(rotateEnd)
-
-    invalidate()
-    dispatch('change')
-  }
-
-  function onPointerDown(event: PointerEvent) {
-    const { x, y } = event
-    rotateStart.set(x, y)
-    pointerDown = true
-  }
-
-  function onPointerUp() {
-    rotateDelta.set(0, 0)
-    pointerDown = false
-    isOrbiting = false
-  }
-
-  function onPointerLeave() {
-    rotateDelta.set(0, 0)
-    pointerDown = false
-    isOrbiting = false
-  }
+  })
 
   function vectorFromObject(vec: { x: number; y: number; z: number }) {
     const { x, y, z } = vec
@@ -147,32 +120,4 @@
     )
     return ideal
   }
-
-  function onKeyDown(event: KeyboardEvent) {
-    switch (event.key) {
-      case 'a':
-        rotateDelta.x = -2 * rotateSpeed
-        break
-      case 'd':
-        rotateDelta.x = 2 * rotateSpeed
-        break
-      default:
-        break
-    }
-  }
-
-  function onKeyUp(event: KeyboardEvent) {
-    switch (event.key) {
-      case 'a':
-        rotateDelta.x = 0
-        break
-      case 'd':
-        rotateDelta.x = 0
-        break
-      default:
-        break
-    }
-  }
 </script>
-
-<!-- <svelte:window on:keydown={onKeyDown} on:keyup={onKeyUp} /> -->
diff --git a/client/src/lib/Dashboard/Visualization/Scene.svelte b/client/src/lib/Dashboard/Visualization/Scene.svelte
index b530293..d276160 100644
--- a/client/src/lib/Dashboard/Visualization/Scene.svelte
+++ b/client/src/lib/Dashboard/Visualization/Scene.svelte
@@ -2,22 +2,37 @@
   import { T, useTask } from '@threlte/core'
   import { ContactShadows, Float, Grid, OrbitControls } from '@threlte/extras'
   import Hornet from './models/Hornet.svelte'
-  import { onMount } from 'svelte'
   import Controls from './Controls.svelte'
-  import type { Camera, Group, Object3D, Object3DEventMap } from 'three'
+  import {
+    Vector3,
+    type Camera,
+    type Group,
+    type Object3D,
+    type Object3DEventMap,
+  } from 'three'
   import {
     telemetryReadonlyStore,
     telemetryStore,
   } from '../../stores/telemetryStore'
   import { get } from 'svelte/store'
+  import { Vector2 } from 'three'
+  import { SmoothMotionController } from './smoothMotionController'
+  import { onMount } from 'svelte'
 
-  // const rotate90 = () => {
-  //   const originalRot = rot
-  // }
+  /* This is the root scene where the robot visualization is built.
+  It renders an infinite grid (it's not actually infinite, but we shouldn't run out
+  of space in realistic use), and a 3D model of the robot. The camera is locked
+  to the model, and the model is rotated to match the robot's orientation. 
+  A PID controller is used to smoothly rotate the model to match the robot's
+  orientation and dampen out jittering. How does it work? Who knows! 
+  75% percent of this was created while reading  
+  https://en.wikipedia.org/wiki/Quaternions_and_spatial_rotation,
+  and rest was generated by AI!
+  The rest of this codebase is remarkably jank-free, but this visualization module
+  is the most esoteric and jank code ever written.
+  */
 
-  // onMount(() => {
-  //   setTimeout(rotate90, 5000)
-  // })
+  let shouldOrbit = true
 
   // CONSTANTS
   const maxAngularVelocity = 2 // Max angular velocity, in radians per second
@@ -26,23 +41,8 @@
   // Proportional control factor
   const kP = 2 // Adjust this value based on responsiveness and stability needs
 
-  // simulate some turning for testing
-  // const simulateTurning = () => {
-  //   let delay = Math.random() * 4500 + 500
-  //   let randOffset = Math.random() * 170 * (Math.random() < 0.5 ? -1 : 1)
-  //   telemetryStore.update({
-  //     ...get(telemetryReadonlyStore),
-  //     orientation: get(telemetryReadonlyStore)['orientation'] + randOffset,
-  //   })
-  //   setTimeout(simulateTurning, delay)
-  // }
-  // simulateTurning()
-
   // Sync robot orientation with target rotation
   let targetRot = 0
-  telemetryReadonlyStore.subscribe(value => {
-    targetRot = (value['orientation'] * Math.PI) / 180 // convert deg to rad
-  })
 
   // Updates rotation to match target with PID controller (intended to be invoked in useTask)
   let rot = 0 // (initial) rotation in radians
@@ -80,38 +80,54 @@
     }
   }
 
-  // Assuming useTask is called every frame with the time delta
+  let robotPos: Vector3 = new Vector3(0, 0, 0)
+
+  const robotPosition = new Vector2(0, 0) // Initial position
+  const initialVelocity = { x: 0, y: 0 } // Initial velocity
+  // The smooth motion controller utilizes a cubic hermite spline to interpolate between
+  // the current simulation velocity and the robot's actual velocity
+  const controller = new SmoothMotionController(robotPosition, initialVelocity)
+
+  onMount(() => {
+    telemetryReadonlyStore.subscribe(value => {
+      targetRot = (value['orientation'] * Math.PI) / 180 // convert deg to rad
+      controller.setTargetVelocity({
+        x: value['chassis-x-speed'],
+        y: value['chassis-y-speed'],
+      })
+      shouldOrbit = value.gear === 'park' || value.gear === '-999'
+      if (shouldOrbit) {
+        robotPos = new Vector3(0, 0, 0)
+        controller.reset()
+      }
+    })
+  })
+
   useTask(delta => {
-    updateRotation(delta)
+    if (!shouldOrbit) {
+      updateRotation(delta)
+
+      controller.update(delta)
+      robotPos.x = controller.getPosition().x
+      robotPos.z = controller.getPosition().y
+    }
   })
 
   let capsule: Group<Object3DEventMap>
-  let capRef: Camera
+  let capRef: Group<Object3DEventMap>
   $: if (capsule) {
-    // typescript hacks because i dont know what im doing
-    capRef = capsule as unknown as Camera
+    capRef = capsule
   }
 </script>
 
-<!-- <T.PerspectiveCamera makeDefault position={[-10, 10, 10]} fov={15}>
+<T.PerspectiveCamera makeDefault position={[0, 8, -20]} fov={30} on:create>
   <OrbitControls
-    autoRotate
-    enableZoom={true}
-    enableDamping
-    autoRotateSpeed={0.5}
-    target.y={1.5}
-  />
-</T.PerspectiveCamera> -->
-
-<T.PerspectiveCamera makeDefault position={[0, 8, -20]} fov={75} on:create>
-  <OrbitControls
-    enableZoom={true}
-    enableDamping
-    autoRotateSpeed={5}
+    autoRotateSpeed={1.5}
     target.y={1.5}
     autoRotate
+    enableDamping
   />
-  <Controls bind:object={capRef} />
+  <Controls {shouldOrbit} bind:object={capRef} rotateSpeed={angularVelocity} />
 </T.PerspectiveCamera>
 
 <T.DirectionalLight intensity={0.8} position.x={5} position.y={10} />
@@ -122,20 +138,20 @@
   cellColor="#ffffff"
   sectionColor="#ffffff"
   sectionThickness={0}
-  fadeDistance={50}
+  fadeDistance={75}
   cellSize={2}
+  infiniteGrid
 />
 
 <ContactShadows scale={10} blur={2} far={2.5} opacity={0.5} />
 
 <Float floatIntensity={1} floatingRange={[0, 0.5]}>
-  <!-- <T.Mesh > -->
   <Hornet
-    position={[0, 1.7, 0]}
+    position.y={1.7}
+    position.z={robotPos.z}
+    position.x={robotPos.x}
     scale={[0.8, 0.8, 0.8]}
     bind:ref={capsule}
     rotation.y={rot}
   />
-  <!-- <T.MeshStandardMaterial color="#F8EBCE" /> -->
-  <!-- </T.Mesh> -->
 </Float>
diff --git a/client/src/lib/Dashboard/Visualization/smoothMotionController.ts b/client/src/lib/Dashboard/Visualization/smoothMotionController.ts
new file mode 100644
index 0000000..ee296c2
--- /dev/null
+++ b/client/src/lib/Dashboard/Visualization/smoothMotionController.ts
@@ -0,0 +1,54 @@
+import { Vector2 } from 'three'
+
+interface Velocity {
+  x: number
+  y: number
+}
+
+export class SmoothMotionController {
+  private currentPosition: Vector2
+  private currentVelocity: Vector2
+  private targetVelocity: Velocity
+  private dampingFactor: number
+
+  constructor(
+    initialPosition: Vector2,
+    initialVelocity: Velocity,
+    dampingFactor: number = 0.1
+  ) {
+    this.currentPosition = initialPosition
+    this.currentVelocity = new Vector2(initialVelocity.x, initialVelocity.y)
+    this.targetVelocity = { ...initialVelocity }
+    this.dampingFactor = dampingFactor
+  }
+
+  setTargetVelocity(velocity: Velocity) {
+    this.targetVelocity = velocity
+  }
+
+  update(delta: number) {
+    // Apply cubic interpolation to smoothly transition the current velocity towards the target velocity
+    this.currentVelocity.x +=
+      (this.targetVelocity.x - this.currentVelocity.x) *
+      this.dampingFactor *
+      delta
+    this.currentVelocity.y +=
+      (this.targetVelocity.y - this.currentVelocity.y) *
+      this.dampingFactor *
+      delta
+
+    // Update position based on the current velocity and the time delta
+    this.currentPosition.x += this.currentVelocity.x * delta * 3
+    this.currentPosition.y += this.currentVelocity.y * delta * 3
+  }
+
+  getPosition(): Vector2 {
+    return this.currentPosition
+  }
+
+  public reset() {
+    this.currentPosition = new Vector2(0, 0)
+    this.currentVelocity = new Vector2(0, 0)
+    this.targetVelocity = { x: 0, y: 0 }
+  }
+}