Robot Falling of the Edge of the World [AARD-2034] (#1247)

Co-authored-by: Alexey Dmitriev <157652245+AlexD717@users.noreply.github.com>

Author: BrandonPacewic

fission/src/systems/World.ts

@@ -7,6 +7,7 @@ import DragModeSystem from "./scene/DragModeSystem"
 import SceneRenderer from "./scene/SceneRenderer"
 import SimulationSystem from "./simulation/SimulationSystem"
 import RobotDimensionTracker from "./match_mode/RobotDimensionTracker"
+import RobotPositionTracker from "./simulation/RobotPositionTracker"
 
 class World {
     private static _isAlive: boolean = false
@@ -119,6 +120,7 @@ class World {
         World._performanceMonitorSystem?.update(this._currentDeltaT)
 
         RobotDimensionTracker.update(World._sceneRenderer)
+        RobotPositionTracker.update(World._sceneRenderer)
     }
 
     public static get currentDeltaT(): number {

fission/src/systems/simulation/RobotPositionTracker.ts

@@ -0,0 +1,63 @@
+import MirabufSceneObject from "@/mirabuf/MirabufSceneObject"
+import { MiraType } from "@/mirabuf/MirabufLoader"
+import SceneRenderer from "../scene/SceneRenderer"
+import SimulationSystem from "@/systems/simulation/SimulationSystem"
+import World from "../World"
+import { convertJoltMat44ToThreeMatrix4 } from "@/util/TypeConversions"
+import JOLT from "@/util/loading/JoltSyncLoader"
+import * as THREE from "three"
+
+class RobotPositionTracker {
+    private static _mapBoundaryY: number = -4
+    private static _offMapPenalty: number = 0
+
+    public static update(sceneRenderer: SceneRenderer): void {
+        const robots = [...sceneRenderer.sceneObjects.values()].filter(
+            (obj): obj is MirabufSceneObject => obj instanceof MirabufSceneObject && obj.miraType === MiraType.ROBOT
+        )
+
+        robots.forEach(robot => {
+            const rootNodeId = robot.getRootNodeId()
+            if (!rootNodeId) {
+                return
+            }
+
+            const rootBody = World.physicsSystem.getBody(rootNodeId)
+            const rootTransform = convertJoltMat44ToThreeMatrix4(rootBody.GetWorldTransform())
+
+            const rootPosition = new THREE.Vector3()
+            const rootRotation = new THREE.Quaternion()
+            const rootScale = new THREE.Vector3()
+            rootTransform.decompose(rootPosition, rootRotation, rootScale)
+
+            if (rootPosition.y < this._mapBoundaryY) {
+                SimulationSystem.robotPenalty(robot, this._offMapPenalty, "Robot fell off the map")
+
+                // TODO: Once driver station is implemented, we should reset the robot to the driver station position
+                const resetPosition = new JOLT.RVec3(0, 0.2, 0)
+                const resetRotation = JOLT.Quat.prototype.sIdentity()
+                const zeroVelocity = new JOLT.Vec3(0, 0, 0)
+
+                robot.mirabufInstance.parser.rigidNodes.forEach(rigidNode => {
+                    const bodyId = robot.mechanism.getBodyByNodeId(rigidNode.id)
+                    if (bodyId) {
+                        World.physicsSystem.setBodyPositionRotationAndVelocity(
+                            bodyId,
+                            resetPosition,
+                            resetRotation,
+                            zeroVelocity,
+                            zeroVelocity,
+                            true
+                        )
+                    }
+                })
+
+                JOLT.destroy(resetPosition)
+                JOLT.destroy(resetRotation)
+                JOLT.destroy(zeroVelocity)
+            }
+        })
+    }
+}
+
+export default RobotPositionTracker