Height Limit Penalty [AARD-1999] (#1230)

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

Author: BrandonPacewic

fission/src/mirabuf/MirabufSceneObject.ts

@@ -632,6 +632,101 @@ class MirabufSceneObject extends SceneObject implements ContextSupplier {
         return box
     }
 
+    /**
+     * Gets the maximum dimensions (length, width, height) of the mirabuf object.
+     *
+     * @returns An object containing the width (x), height (y), and depth (z) dimensions in meters.
+     */
+    public getDimensions(): { width: number; height: number; depth: number } {
+        const boundingBox = this.computeBoundingBox()
+        const size = new THREE.Vector3()
+        boundingBox.getSize(size)
+
+        return {
+            width: size.x,
+            height: size.y,
+            depth: size.z,
+        }
+    }
+
+    /**
+     * Calculates the robot's dimensions as if it had no rotation applied.
+     *
+     * @returns the object containing the width (x), height (y), and depth (z) dimensions in meters.
+     */
+    public getDimensionsWithoutRotation(): { width: number; height: number; depth: number } {
+        const rootNodeId = this.getRootNodeId()
+        if (!rootNodeId) {
+            console.warn("No root node found for robot, using regular dimensions")
+            return this.getDimensions()
+        }
+
+        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)
+
+        // Create inverse rotation matrix to "undo" the robot's rotation
+        const inverseRotation = new THREE.Matrix4().makeRotationFromQuaternion(rootRotation.clone().invert())
+
+        const unrotatedBox = new THREE.Box3()
+
+        this._mirabufInstance.parser.rigidNodes.forEach(rigidNode => {
+            const bodyId = this._mechanism.getBodyByNodeId(rigidNode.id)
+            if (!bodyId) return
+
+            const body = World.physicsSystem.getBody(bodyId)
+            const bodyTransform = convertJoltMat44ToThreeMatrix4(body.GetWorldTransform())
+
+            const shape = body.GetShape()
+            const scale = new JOLT.Vec3(1, 1, 1)
+            const triangleContext = new JOLT.ShapeGetTriangles(
+                shape,
+                JOLT.AABox.prototype.sBiggest(),
+                shape.GetCenterOfMass(),
+                JOLT.Quat.prototype.sIdentity(),
+                scale
+            )
+
+            try {
+                const vertices = new Float32Array(
+                    JOLT.HEAP32.buffer,
+                    triangleContext.GetVerticesData(),
+                    triangleContext.GetVerticesSize() / Float32Array.BYTES_PER_ELEMENT
+                )
+
+                for (let i = 0; i < vertices.length; i += 3) {
+                    const vertex = new THREE.Vector3(vertices[i], vertices[i + 1], vertices[i + 2])
+
+                    vertex.applyMatrix4(bodyTransform).applyMatrix4(inverseRotation)
+
+                    unrotatedBox.expandByPoint(vertex)
+                }
+            } finally {
+                JOLT.destroy(triangleContext)
+                JOLT.destroy(scale)
+            }
+        })
+
+        // Fallback if no vertices were processed
+        if (unrotatedBox.isEmpty()) {
+            console.warn("Could not process physics shapes, using regular dimensions")
+            return this.getDimensions()
+        }
+
+        const unrotatedSize = new THREE.Vector3()
+        unrotatedBox.getSize(unrotatedSize)
+
+        return {
+            width: unrotatedSize.x,
+            height: unrotatedSize.y,
+            depth: unrotatedSize.z,
+        }
+    }
+
     /**
      * Once a gizmo is created and attached to this mirabuf object, this will be executed to align the gizmo correctly.
      *

fission/src/systems/World.ts

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

fission/src/systems/input/InputSystem.ts

@@ -1,5 +1,5 @@
 import { KeyCode } from "@/systems/input/KeyboardTypes.ts"
-import MatchMode, { MatchModeType } from "@/systems/MatchMode"
+import MatchMode, { MatchModeType } from "@/systems/match_mode/MatchMode"
 import { DriveType } from "@/systems/simulation/behavior/Behavior.ts"
 import { TouchControlsAxes } from "@/ui/components/TouchControls"
 import Joystick from "../scene/Joystick"

fission/src/systems/DefaultMatchModeConfigs.ts renamed to fission/src/systems/match_mode/DefaultMatchModeConfigs.ts

@@ -1,4 +1,5 @@
 import { MatchModeConfig } from "@/ui/panels/configuring/MatchModeConfigPanel"
+import { convertFeetToMeters } from "@/util/UnitConversions"
 
 /** The purpose of this class is to store any defaults related to match mode configurations. */
 class DefaultMatchModeConfigs {
@@ -10,6 +11,9 @@ class DefaultMatchModeConfigs {
             autonomousTime: 15,
             teleopTime: 135,
             endgameTime: 20,
+            ignoreRotation: true,
+            maxHeight: Infinity,
+            heightPenalty: 0,
         }
     }
 
@@ -21,6 +25,9 @@ class DefaultMatchModeConfigs {
             autonomousTime: 15,
             teleopTime: 135,
             endgameTime: 20,
+            ignoreRotation: true,
+            maxHeight: convertFeetToMeters(4),
+            heightPenalty: 2,
         }
     }
 
@@ -32,6 +39,9 @@ class DefaultMatchModeConfigs {
             autonomousTime: 15,
             teleopTime: 135,
             endgameTime: 30,
+            ignoreRotation: true,
+            maxHeight: convertFeetToMeters(6.5),
+            heightPenalty: 5,
         }
     }
 
@@ -43,6 +53,9 @@ class DefaultMatchModeConfigs {
             autonomousTime: 5,
             teleopTime: 15,
             endgameTime: 5,
+            ignoreRotation: true,
+            maxHeight: Infinity,
+            heightPenalty: 0,
         }
     }
 

fission/src/systems/MatchMode.ts renamed to fission/src/systems/match_mode/MatchMode.ts

@@ -1,10 +1,11 @@
+import SimulationSystem from "../simulation/SimulationSystem"
+import { MatchModeConfig } from "@/ui/panels/configuring/MatchModeConfigPanel"
+import { SoundPlayer } from "../sound/SoundPlayer"
 import beep from "@/assets/sound-files/beep.wav"
 import MatchEnd from "@/assets/sound-files/MatchEnd.wav"
 import MatchResume from "@/assets/sound-files/MatchResume.wav"
+import RobotDimensionTracker from "./RobotDimensionTracker"
 import MatchStart from "@/assets/sound-files/MatchStart.wav"
-import { MatchModeConfig } from "@/ui/panels/configuring/MatchModeConfigPanel"
-import SimulationSystem from "./simulation/SimulationSystem"
-import { SoundPlayer } from "./sound/SoundPlayer"
 
 export enum MatchModeType {
     SANDBOX = 0,
@@ -17,6 +18,9 @@ export enum MatchModeType {
 export const DEFAULT_AUTONOMOUS_TIME = 15
 export const DEFAULT_TELEOP_TIME = 135
 export const DEFAULT_ENDGAME_TIME = 20
+export const DEFAULT_IGNORE_ROTATION = true
+export const DEFAULT_MAX_HEIGHT = Infinity
+export const DEFAULT_HEIGHT_PENALTY = 2
 
 class MatchMode {
     private static _instance: MatchMode
@@ -39,6 +43,9 @@ class MatchMode {
         autonomousTime: DEFAULT_AUTONOMOUS_TIME,
         teleopTime: DEFAULT_TELEOP_TIME,
         endgameTime: DEFAULT_ENDGAME_TIME,
+        ignoreRotation: DEFAULT_IGNORE_ROTATION,
+        maxHeight: DEFAULT_MAX_HEIGHT,
+        heightPenalty: DEFAULT_HEIGHT_PENALTY,
     }
 
     private constructor() {}
@@ -50,6 +57,7 @@ class MatchMode {
 
     setMatchModeConfig(config: MatchModeConfig) {
         this._matchModeConfig = config
+        RobotDimensionTracker.setConfigValues(config.ignoreRotation, config.maxHeight, config.heightPenalty)
     }
 
     startTimer(duration: number, functionCall: () => void, updateTimeLeft: boolean = true) {

fission/src/systems/match_mode/RobotDimensionTracker.ts

@@ -0,0 +1,43 @@
+import MirabufSceneObject from "@/mirabuf/MirabufSceneObject"
+import { MiraType } from "@/mirabuf/MirabufLoader"
+import SceneRenderer from "../scene/SceneRenderer"
+import MatchMode from "./MatchMode"
+import SimulationSystem from "@/systems/simulation/SimulationSystem"
+
+const BUFFER_HEIGHT = 0.1
+
+class RobotDimensionTracker {
+    private static _robotLastFramePenalty: Map<number, boolean> = new Map()
+    private static _ignoreRotation: boolean = true
+    private static _maxHeight: number = Infinity
+    private static _heightPenalty: number = 0
+
+    public static setConfigValues(ignoreRotation: boolean, maxHeight: number, heightPenalty: number) {
+        this._ignoreRotation = ignoreRotation
+        this._maxHeight = maxHeight
+        this._heightPenalty = heightPenalty
+    }
+
+    public static update(sceneRenderer: SceneRenderer): void {
+        if (!MatchMode.getInstance().isMatchEnabled()) return
+
+        const robots = [...sceneRenderer.sceneObjects.values()].filter(
+            (obj): obj is MirabufSceneObject => obj instanceof MirabufSceneObject && obj.miraType === MiraType.ROBOT
+        )
+
+        robots.forEach(robot => {
+            const dimensions = this._ignoreRotation ? robot.getDimensionsWithoutRotation() : robot.getDimensions()
+
+            if (dimensions.height > this._maxHeight + BUFFER_HEIGHT) {
+                if (!(this._robotLastFramePenalty.get(robot.id) ?? false)) {
+                    SimulationSystem.robotPenalty(robot, this._heightPenalty, "Height Expansion Limit")
+                }
+                this._robotLastFramePenalty.set(robot.id, true)
+            } else {
+                this._robotLastFramePenalty.set(robot.id, false)
+            }
+        })
+    }
+}
+
+export default RobotDimensionTracker