Merge branch 'dev' into zachr/1945/input-scheme-mode-conflict

Author: PepperLola

fission/src/mirabuf/EjectableSceneObject.ts

@@ -21,6 +21,21 @@ class EjectableSceneObject extends SceneObject {
     private _deltaTransformation?: THREE.Matrix4
     private _ejectVelocity?: number
 
+    // Animation state
+    private _animationStartTime = 0
+    private _animationDuration = EjectableSceneObject._defaultAnimationDuration
+    private _startTranslation?: THREE.Vector3
+    private _startRotation?: THREE.Quaternion
+
+    private static _defaultAnimationDuration = 0.5
+
+    public static setAnimationDuration(duration: number) {
+        EjectableSceneObject._defaultAnimationDuration = duration
+    }
+    public static getAnimationDuration() {
+        return EjectableSceneObject._defaultAnimationDuration
+    }
+
     public get gamePieceBodyId() {
         return this._gamePieceBodyId
     }
@@ -49,15 +64,24 @@ class EjectableSceneObject extends SceneObject {
             )
             this._ejectVelocity = this._parentAssembly.ejectorPreferences.ejectorVelocity
 
+            // Record start transform at the game piece center of mass
+            const gpBody = World.physicsSystem.getBody(this._gamePieceBodyId)
+            this._startTranslation = new THREE.Vector3(0, 0, 0)
+            this._startRotation = new THREE.Quaternion(0, 0, 0, 1)
+            convertJoltMat44ToThreeMatrix4(gpBody.GetCenterOfMassTransform()).decompose(
+                this._startTranslation,
+                this._startRotation,
+                new THREE.Vector3(1, 1, 1)
+            )
+
+            this._animationDuration = EjectableSceneObject._defaultAnimationDuration
+            this._animationStartTime = performance.now()
+
             World.physicsSystem.disablePhysicsForBody(this._gamePieceBodyId)
 
-            // Checks if the gamepiece comes from a zone for persistent point score updates
-            // because gamepieces removed by intake are not detected in the collision listener
+            // Remove from any scoring zones
             const zones = [...World.sceneRenderer.sceneObjects.entries()]
-                .filter(x => {
-                    const y = x[1] instanceof ScoringZoneSceneObject
-                    return y
-                })
+                .filter(x => x[1] instanceof ScoringZoneSceneObject)
                 .map(x => x[1]) as ScoringZoneSceneObject[]
 
             zones.forEach(x => {
@@ -69,26 +93,51 @@ class EjectableSceneObject extends SceneObject {
     }
 
     public update(): void {
+        const now = performance.now()
+        const elapsed = (now - this._animationStartTime) / 1000
+        const tRaw = elapsed / this._animationDuration
+        const t = Math.min(tRaw, 1)
+
+        // ease-in curve for gradual acceleration
+        const easedT = t * t
+
         if (this._parentBodyId && this._deltaTransformation && this._gamePieceBodyId) {
             if (!World.physicsSystem.isBodyAdded(this._gamePieceBodyId)) {
                 this._gamePieceBodyId = undefined
                 return
             }
 
-            // I had a think and free wrote this matrix math on a whim. It worked first try and I honestly can't quite remember how it works... -Hunter
             const gpBody = World.physicsSystem.getBody(this._gamePieceBodyId)
             const posToCOM = convertJoltMat44ToThreeMatrix4(gpBody.GetCenterOfMassTransform()).premultiply(
                 convertJoltMat44ToThreeMatrix4(gpBody.GetWorldTransform()).invert()
             )
 
             const body = World.physicsSystem.getBody(this._parentBodyId)
-            const bodyTransform = posToCOM
-                .invert()
-                .premultiply(
-                    this._deltaTransformation
-                        .clone()
-                        .premultiply(convertJoltMat44ToThreeMatrix4(body.GetWorldTransform()))
-                )
+            let desiredPosition = new THREE.Vector3(0, 0, 0)
+            let desiredRotation = new THREE.Quaternion(0, 0, 0, 1)
+
+            // Compute target world transform
+            const desiredTransform = this._deltaTransformation
+                .clone()
+                .premultiply(convertJoltMat44ToThreeMatrix4(body.GetWorldTransform()))
+
+            desiredTransform.decompose(desiredPosition, desiredRotation, new THREE.Vector3(1, 1, 1))
+
+            if (t < 1 && this._startTranslation && this._startRotation) {
+                // gradual acceleration via easedT
+                desiredPosition = new THREE.Vector3().lerpVectors(this._startTranslation, desiredPosition, easedT)
+                desiredRotation = new THREE.Quaternion().copy(this._startRotation).slerp(desiredRotation, easedT)
+            }
+            // } else if (t >= 1) {
+            //     // snap instantly and re-enable physics
+            //     World.physicsSystem.enablePhysicsForBody(this._gamePieceBodyId)
+            // }
+
+            // apply the transform
+            desiredTransform.identity().compose(desiredPosition, desiredRotation, new THREE.Vector3(1, 1, 1))
+
+            const bodyTransform = posToCOM.clone().invert().premultiply(desiredTransform)
+
             const position = new THREE.Vector3(0, 0, 0)
             const rotation = new THREE.Quaternion(0, 0, 0, 1)
             bodyTransform.decompose(position, rotation, new THREE.Vector3(1, 1, 1))

fission/src/mirabuf/MirabufSceneObject.ts

@@ -45,6 +45,8 @@ import WPILibBrain from "@/systems/simulation/wpilib_brain/WPILibBrain"
 import { OnContactAddedEvent } from "@/systems/physics/ContactEvents"
 import FieldMiraEditor from "./FieldMiraEditor"
 
+import { DriveType } from "@/systems/simulation/behavior/Behavior.ts"
+
 const DEBUG_BODIES = false
 
 interface RnDebugMeshes {

fission/src/systems/MatchMode.ts

@@ -20,10 +20,13 @@ export const DEFAULT_ENDGAME_TIME = 20
 
 class MatchMode {
     private static _instance: MatchMode
-    private _matchEnabled: boolean = false
     private _endgame: boolean = false
     private _matchModeType: MatchModeType = MatchModeType.SANDBOX
 
+    private setMatchModeType(val: MatchModeType) {
+        this._matchModeType = val
+        new MatchStateChangeEvent(val).dispatch()
+    }
     private _initialTime: number = 0
     private _timeLeft: number = 0
     private _intervalId: number | null = null
@@ -77,7 +80,7 @@ class MatchMode {
 
     autonomousModeStart(openModal: (modalName: string) => void) {
         SoundPlayer.play(MatchStart)
-        this._matchModeType = MatchModeType.AUTONOMOUS
+        this.setMatchModeType(MatchModeType.AUTONOMOUS)
         this.startTimer(this._matchModeConfig.autonomousTime, () => this.autonomousModeEnd(openModal))
     }
 
@@ -88,7 +91,7 @@ class MatchMode {
 
     teleopModeStart(openModal: (modalName: string) => void) {
         SoundPlayer.play(MatchResume)
-        this._matchModeType = MatchModeType.TELEOP
+        this.setMatchModeType(MatchModeType.TELEOP)
         this.startTimer(this._matchModeConfig.teleopTime, () => this.matchEnded(openModal))
     }
 
@@ -98,22 +101,19 @@ class MatchMode {
     }
 
     start(openModal: (modalName: string) => void) {
-        this._matchEnabled = true
         this.autonomousModeStart(openModal)
         SimulationSystem.resetScores()
     }
 
     matchEnded(openModal: (modalName: string) => void) {
         SoundPlayer.play(MatchEnd)
         clearInterval(this._intervalId as number)
-        this._matchEnabled = false
-        this._matchModeType = MatchModeType.MATCH_ENDED
+        this.setMatchModeType(MatchModeType.MATCH_ENDED)
         if (openModal) openModal("match-results")
     }
 
     sandboxModeStart() {
-        this._matchEnabled = false
-        this._matchModeType = MatchModeType.SANDBOX
+        this.setMatchModeType(MatchModeType.SANDBOX)
         clearInterval(this._intervalId as number)
         this._initialTime = 0
         this._timeLeft = 0
@@ -122,7 +122,7 @@ class MatchMode {
     }
 
     isMatchEnabled(): boolean {
-        return this._matchEnabled
+        return !(this._matchModeType == MatchModeType.SANDBOX || this._matchModeType == MatchModeType.MATCH_ENDED)
     }
 
     isEndgame(): boolean {
@@ -158,3 +158,25 @@ export class UpdateTimeLeft extends Event {
         window.removeEventListener(UpdateTimeLeft.EVENT_KEY, func as (e: Event) => void)
     }
 }
+
+export class MatchStateChangeEvent extends Event {
+    public static readonly EVENT_KEY = "MatchEnd"
+
+    public readonly matchModeType: MatchModeType
+    constructor(matchModeType: MatchModeType) {
+        super(MatchStateChangeEvent.EVENT_KEY)
+        this.matchModeType = matchModeType
+    }
+
+    public dispatch(): void {
+        window.dispatchEvent(this)
+    }
+
+    public static addListener(func: (e: MatchStateChangeEvent) => void) {
+        window.addEventListener(MatchStateChangeEvent.EVENT_KEY, func as (e: Event) => void)
+    }
+
+    public static removeListener(func: (e: MatchStateChangeEvent) => void) {
+        window.removeEventListener(MatchStateChangeEvent.EVENT_KEY, func as (e: Event) => void)
+    }
+}