Implement adjustable center of gravity [AARD-1921]

fission/src/mirabuf/MirabufSceneObject.ts

@@ -99,7 +99,11 @@ class MirabufSceneObject extends SceneObject implements ContextSupplier {
 
     private _nameTag: SceneOverlayTag | undefined
     private _centerOfMassIndicator: THREE.Mesh | undefined
+
+    private _modifiedCenterOfGravity: THREE.Vector3 | undefined
     private _basePositionTransform: THREE.Vector3 | undefined
+    private _cogPhysicsCooldownFrames = 0
+
     private _intakeActive = false
     private _ejectorActive = false
 
@@ -194,7 +198,55 @@ class MirabufSceneObject extends SceneObject implements ContextSupplier {
         this._station = station
     }
 
-    public constructor(mirabufInstance: MirabufInstance, assemblyName: string, progressHandle?: ProgressHandle) {
+    public get modifiedCenterOfGravity(): THREE.Vector3 | undefined {
+        return this._modifiedCenterOfGravity
+    }
+
+    public set modifiedCenterOfGravity(position: THREE.Vector3 | undefined) {
+        this._modifiedCenterOfGravity = position
+    }
+
+    private computeModifiedCenterOfGravityInWorld(): THREE.Vector3 | undefined {
+        if (!this._modifiedCenterOfGravity) return undefined
+        const rootNodeId = this.getRootNodeId()
+        if (rootNodeId) {
+            const robotTransform = convertJoltMat44ToThreeMatrix4(
+                World.physicsSystem.getBody(rootNodeId).GetWorldTransform()
+            )
+            const robotWorldPos = new THREE.Vector3()
+            const robotWorldQuat = new THREE.Quaternion()
+            const robotWorldScale = new THREE.Vector3()
+            robotTransform.decompose(robotWorldPos, robotWorldQuat, robotWorldScale)
+
+            const worldCoG = this._modifiedCenterOfGravity.clone()
+            worldCoG.applyQuaternion(robotWorldQuat)
+            worldCoG.add(robotWorldPos)
+            return worldCoG
+        }
+        return this._modifiedCenterOfGravity.clone()
+    }
+
+    public get currentCenterOfGravity(): THREE.Vector3 {
+        if (this._modifiedCenterOfGravity) {
+            const worldCoG = this.computeModifiedCenterOfGravityInWorld()
+            if (worldCoG) return worldCoG
+        }
+        if (this._centerOfMassIndicator) {
+            return this._centerOfMassIndicator.position.clone()
+        }
+        return new THREE.Vector3(0, 0, 0)
+    }
+
+    public get cacheId() {
+        return this._cacheId
+    }
+
+    public constructor(
+        mirabufInstance: MirabufInstance,
+        assemblyName: string,
+        progressHandle?: ProgressHandle,
+        cacheId?: string
+    ) {
         super()
 
         this._mirabufInstance = mirabufInstance
@@ -379,6 +431,19 @@ class MirabufSceneObject extends SceneObject implements ContextSupplier {
             this.eject()
         }
 
+        if (this._cogPhysicsCooldownFrames > 0) {
+            this._cogPhysicsCooldownFrames -= 1
+        }
+
+        if (
+            this._modifiedCenterOfGravity &&
+            this.miraType === MiraType.ROBOT &&
+            !World.physicsSystem.isPaused &&
+            this._cogPhysicsCooldownFrames === 0
+        ) {
+            this.applyCenterOfGravityPhysics()
+        }
+
         this.updateMeshTransforms()
         this.updateBatches()
         this.updateNameTag()
@@ -495,8 +560,17 @@ class MirabufSceneObject extends SceneObject implements ContextSupplier {
             }
         })
         if (this._centerOfMassIndicator) {
-            const netCoM = totalMass > 0 ? weightedCOM.Div(totalMass) : weightedCOM
-            this._centerOfMassIndicator.position.set(netCoM.GetX(), netCoM.GetY(), netCoM.GetZ())
+            if (this._modifiedCenterOfGravity) {
+                const worldCoG = this.computeModifiedCenterOfGravityInWorld()
+                if (worldCoG) {
+                    this._centerOfMassIndicator.position.copy(worldCoG)
+                } else if (this._modifiedCenterOfGravity) {
+                    this._centerOfMassIndicator.position.copy(this._modifiedCenterOfGravity)
+                }
+            } else {
+                const netCoM = totalMass > 0 ? weightedCOM.Div(totalMass) : weightedCOM
+                this._centerOfMassIndicator.position.set(netCoM.GetX(), netCoM.GetY(), netCoM.GetZ())
+            }
             this._centerOfMassIndicator.visible = PreferencesSystem.getGlobalPreference("ShowCenterOfMassIndicators")
         }
     }
@@ -687,7 +761,11 @@ class MirabufSceneObject extends SceneObject implements ContextSupplier {
      *
      * @returns the object containing the width (x), height (y), and depth (z) dimensions in meters.
      */
-    public getDimensionsWithoutRotation(): { width: number; height: number; depth: number } {
+    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")
@@ -938,6 +1016,116 @@ class MirabufSceneObject extends SceneObject implements ContextSupplier {
             objectCollidedWith.robotLastInContactWith = this
         }
     }
+
+    /**
+     * Aries' CoG Simulation
+     * This method applies torque to the robot's root body to simulate the effect of a modified center of gravity.
+     */
+    private applyCenterOfGravityPhysics(): void {
+        if (!this._modifiedCenterOfGravity) return
+        if (World.physicsSystem.isPaused) return
+
+        const rootNodeId = this.getRootNodeId()
+        if (!rootNodeId) return
+
+        const rootBody = World.physicsSystem.getBody(rootNodeId)
+        if (!rootBody || rootBody.IsStatic()) return
+
+        const modifiedCoGWorld = this.computeModifiedCenterOfGravityInWorld()
+        if (!modifiedCoGWorld) return
+
+        let actualCoMWorld = new JOLT.RVec3(0, 0, 0)
+        let totalMass = 0
+
+        this._mirabufInstance.parser.rigidNodes.forEach(rn => {
+            const bodyId = this._mechanism.getBodyByNodeId(rn.id)
+            if (!bodyId) return
+
+            const body = World.physicsSystem.getBody(bodyId)
+            const inverseMass = body.GetMotionProperties().GetInverseMass()
+
+            if (inverseMass > 0) {
+                const mass = 1 / inverseMass
+                actualCoMWorld = actualCoMWorld.AddRVec3(body.GetCenterOfMassPosition().Mul(mass))
+                totalMass += mass
+            }
+        })
+
+        if (totalMass === 0) return
+
+        const actualCoM = actualCoMWorld.Div(totalMass)
+        const actualCoMVec3 = new THREE.Vector3(actualCoM.GetX(), actualCoM.GetY(), actualCoM.GetZ())
+
+        const offset = modifiedCoGWorld.clone().sub(actualCoMVec3)
+
+        // The torque needed is: τ = r × F
+        // where r is the offset and F is the gravitational force
+        const gravityForce = new THREE.Vector3(0, -9.81 * totalMass, 0)
+        const torque = new THREE.Vector3().crossVectors(offset, gravityForce)
+
+        const joltTorque = new JOLT.Vec3(torque.x, torque.y, torque.z)
+        rootBody.AddTorque(joltTorque)
+        JOLT.destroy(joltTorque)
+
+        const velocity = rootBody.GetLinearVelocity()
+        const speed = Math.sqrt(velocity.GetX() ** 2 + velocity.GetY() ** 2 + velocity.GetZ() ** 2)
+
+        if (speed > 0.1) {
+            const angularVel = rootBody.GetAngularVelocity()
+            const dampingFactor = 0.5
+            const dampingTorque = new JOLT.Vec3(
+                -angularVel.GetX() * dampingFactor * totalMass,
+                -angularVel.GetY() * dampingFactor * totalMass,
+                -angularVel.GetZ() * dampingFactor * totalMass
+            )
+            rootBody.AddTorque(dampingTorque)
+            JOLT.destroy(dampingTorque)
+        }
+
+        this._mirabufInstance.parser.rigidNodes.forEach(rn => {
+            if (rn.id === this._mechanism.rootBody) return
+
+            const bodyId = this._mechanism.getBodyByNodeId(rn.id)
+            if (!bodyId) return
+
+            const body = World.physicsSystem.getBody(bodyId)
+            if (body.IsStatic()) return
+
+            const inverseMass = body.GetMotionProperties().GetInverseMass()
+            if (inverseMass <= 0) return
+
+            const mass = 1 / inverseMass
+
+            const correctionFactor = mass / totalMass
+            const bodyTorque = new JOLT.Vec3(
+                torque.x * correctionFactor * 0.1,
+                torque.y * correctionFactor * 0.1,
+                torque.z * correctionFactor * 0.1
+            )
+            body.AddTorque(bodyTorque)
+            JOLT.destroy(bodyTorque)
+        })
+
+        JOLT.destroy(actualCoM)
+        JOLT.destroy(actualCoMWorld)
+    }
+
+    public stabilizeAfterCenterOfGravityEdit(): void {
+        this._mirabufInstance.parser.rigidNodes.forEach(rn => {
+            const bodyId = this._mechanism.getBodyByNodeId(rn.id)
+            if (!bodyId) return
+            if (!World.physicsSystem.isBodyAdded(bodyId)) return
+
+            const body = World.physicsSystem.getBody(bodyId)
+            if (!body || body.IsStatic()) return
+
+            const zero = new JOLT.Vec3(0, 0, 0)
+            body.SetLinearVelocity(zero)
+            body.SetAngularVelocity(zero)
+            JOLT.destroy(zero)
+        })
+        this._cogPhysicsCooldownFrames = 3
+    }
 }
 
 export async function createMirabuf(
@@ -996,8 +1184,8 @@ export class MirabufObjectChangeEvent extends Event {
                 cb(null)
             }
         }
-        window.addEventListener(this._eventKey, listener)
-        return () => window.removeEventListener(this._eventKey, listener)
+        window.addEventListener(MirabufObjectChangeEvent._eventKey, listener)
+        return () => window.removeEventListener(MirabufObjectChangeEvent._eventKey, listener)
     }
 
     public static dispatch(obj: MirabufSceneObject | null) {

fission/src/ui/panels/configuring/assembly-config/ConfigTypes.ts

@@ -36,4 +36,5 @@ export enum ConfigMode {
     BRAIN,
     DRIVETRAIN,
     ALLIANCE,
+    CENTER_OF_GRAVITY,
 }

fission/src/ui/panels/configuring/assembly-config/ConfigurePanel.tsx

@@ -21,6 +21,7 @@ import AssemblySelection, { type AssemblySelectionOption } from "./configure/Ass
 import ConfigModeSelection, { ConfigModeSelectionOption } from "./configure/ConfigModeSelection"
 import AllianceSelectionInterface from "./interfaces/AllianceSelectionInterface"
 import BrainSelectionInterface from "./interfaces/BrainSelectionInterface"
+import CenterOfGravityInterface from "./interfaces/CenterOfGravityInterface"
 import ConfigureGamepiecePickupInterface from "./interfaces/ConfigureGamepiecePickupInterface"
 import ConfigureShotTrajectoryInterface from "./interfaces/ConfigureShotTrajectoryInterface"
 import ConfigureSubsystemsInterface from "./interfaces/ConfigureSubsystemsInterface"
@@ -109,6 +110,8 @@ const ConfigInterface: React.FC<ConfigInterfaceProps<void, ConfigurePanelCustomP
             return <AllianceSelectionInterface selectedAssembly={assembly} />
         case ConfigMode.DRIVETRAIN:
             return <DrivetrainSelectionInterface selectedAssembly={assembly} />
+        case ConfigMode.CENTER_OF_GRAVITY:
+            return <CenterOfGravityInterface selectedRobot={assembly} />
         default:
             throw new Error(`Config mode ${configMode} has no associated interface`)
     }
@@ -235,10 +238,16 @@ const ConfigurePanel: React.FC<PanelImplProps<void, ConfigurePanelCustomProps>>
 
                     new ConfigModeSelectionOption("Drivetrain", ConfigMode.DRIVETRAIN, "Sets the drivetrain type."),
 
+                    new ConfigModeSelectionOption(
+                        "Center of Gravity",
+                        ConfigMode.CENTER_OF_GRAVITY,
+                        "Adjust the robot's center of gravity to modify its balance and physics behavior."
+                    ),
+
                     new ConfigModeSelectionOption(
                         "Intake",
                         ConfigMode.INTAKE,
-                        "Configure the robot’s intake position and parent node for picking up game pieces."
+                        "Configure the robot's intake position and parent node for picking up game pieces."
                     ),
 
                     new ConfigModeSelectionOption(