feat: add robot movement animation
This commit is contained in:
parent
3c7584cecf
commit
5f19b74d2f
3 changed files with 156 additions and 141 deletions
|
|
@ -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,89 +64,53 @@
|
|||
)
|
||||
}
|
||||
|
||||
// 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)
|
||||
|
||||
// Calculate the trailing vector's position
|
||||
const trailingVector = robotPosition.clone().sub(rotatedOffsetVector)
|
||||
|
||||
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)
|
||||
}
|
||||
})
|
||||
|
||||
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 (!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
|
||||
const ideal = new Vector3(x, y, z)
|
||||
|
|
@ -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} /> -->
|
||||
|
|
|
|||
|
|
@ -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 => {
|
||||
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>
|
||||
|
|
|
|||
|
|
@ -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 }
|
||||
}
|
||||
}
|
||||
Loading…
Reference in a new issue