diff --git a/fission/src/mirabuf/MirabufSceneObject.ts b/fission/src/mirabuf/MirabufSceneObject.ts
index 4ce8675dd1..d1617f7645 100644
--- a/fission/src/mirabuf/MirabufSceneObject.ts
+++ b/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) {
diff --git a/fission/src/ui/panels/configuring/assembly-config/ConfigTypes.ts b/fission/src/ui/panels/configuring/assembly-config/ConfigTypes.ts
index 0383514a13..fc547560f2 100644
--- a/fission/src/ui/panels/configuring/assembly-config/ConfigTypes.ts
+++ b/fission/src/ui/panels/configuring/assembly-config/ConfigTypes.ts
@@ -36,4 +36,5 @@ export enum ConfigMode {
BRAIN,
DRIVETRAIN,
ALLIANCE,
+ CENTER_OF_GRAVITY,
}
diff --git a/fission/src/ui/panels/configuring/assembly-config/ConfigurePanel.tsx b/fission/src/ui/panels/configuring/assembly-config/ConfigurePanel.tsx
index 4ba18a9136..324ef70f11 100644
--- a/fission/src/ui/panels/configuring/assembly-config/ConfigurePanel.tsx
+++ b/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
case ConfigMode.DRIVETRAIN:
return
+ case ConfigMode.CENTER_OF_GRAVITY:
+ return
default:
throw new Error(`Config mode ${configMode} has no associated interface`)
}
@@ -235,10 +238,16 @@ const ConfigurePanel: React.FC>
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(
diff --git a/fission/src/ui/panels/configuring/assembly-config/interfaces/CenterOfGravityInterface.tsx b/fission/src/ui/panels/configuring/assembly-config/interfaces/CenterOfGravityInterface.tsx
new file mode 100644
index 0000000000..ecb1d1ec6b
--- /dev/null
+++ b/fission/src/ui/panels/configuring/assembly-config/interfaces/CenterOfGravityInterface.tsx
@@ -0,0 +1,216 @@
+import { Stack } from "@mui/material"
+import { useCallback, useEffect, useMemo, useRef, useState } from "react"
+import * as THREE from "three"
+import { ConfigurationSavedEvent } from "@/events/ConfigurationSavedEvent"
+import type MirabufSceneObject from "@/mirabuf/MirabufSceneObject"
+import { PAUSE_REF_ASSEMBLY_CONFIG } from "@/systems/physics/PhysicsTypes"
+import PreferencesSystem from "@/systems/preferences/PreferencesSystem"
+import type GizmoSceneObject from "@/systems/scene/GizmoSceneObject"
+import World from "@/systems/World"
+import Label from "@/ui/components/Label"
+import { Spacer } from "@/ui/components/StyledComponents"
+import TransformGizmoControl from "@/ui/components/TransformGizmoControl"
+import { convertJoltMat44ToThreeMatrix4 } from "@/util/TypeConversions"
+
+interface CenterOfGravityInterfaceProps {
+ selectedRobot: MirabufSceneObject
+}
+
+/**
+ * Saves the center of gravity configuration to the selected robot.
+ * The position is stored relative to the robot's root node so it moves with the robot.
+ *
+ * @param gizmo Reference to the transform gizmo object.
+ * @param selectedRobot Selected robot to save data to.
+ */
+function saveCenterOfGravity(gizmo: GizmoSceneObject, selectedRobot: MirabufSceneObject) {
+ if (!gizmo || !selectedRobot) {
+ return
+ }
+
+ const rootNodeId = selectedRobot.getRootNodeId()
+ if (!rootNodeId) {
+ return
+ }
+
+ const gizmoWorldPos = new THREE.Vector3()
+ gizmo.obj.getWorldPosition(gizmoWorldPos)
+
+ 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 relativePos = gizmoWorldPos.clone().sub(robotWorldPos)
+ relativePos.applyQuaternion(robotWorldQuat.clone().invert())
+
+ selectedRobot.modifiedCenterOfGravity = relativePos
+ selectedRobot.updateMeshTransforms()
+}
+
+const CenterOfGravityInterface: React.FC = ({ selectedRobot }) => {
+ const gizmoRef = useRef(undefined)
+ const [cogPosition, setCogPosition] = useState(new THREE.Vector3(0, 0, 0))
+
+ // Create the center of gravity sphere mesh
+ const cogSphereMesh = useMemo(() => {
+ const material = new THREE.MeshBasicMaterial({
+ color: 0xff00ff,
+ opacity: 0.8,
+ transparent: true,
+ depthTest: false,
+ depthWrite: false,
+ })
+ const sphere = new THREE.Mesh(new THREE.SphereGeometry(0.04), material) // larger than the visual sphere
+ return sphere
+ }, [])
+
+ const saveEvent = useCallback(() => {
+ if (gizmoRef.current && selectedRobot) {
+ saveCenterOfGravity(gizmoRef.current, selectedRobot)
+ }
+ }, [selectedRobot])
+
+ useEffect(() => {
+ World.physicsSystem.holdPause(PAUSE_REF_ASSEMBLY_CONFIG)
+
+ return () => {
+ selectedRobot.stabilizeAfterCenterOfGravityEdit()
+ World.physicsSystem.releasePause(PAUSE_REF_ASSEMBLY_CONFIG)
+ }
+ }, [])
+
+ useEffect(() => {
+ ConfigurationSavedEvent.listen(saveEvent)
+
+ return () => {
+ ConfigurationSavedEvent.removeListener(saveEvent)
+ }
+ }, [saveEvent])
+
+ useEffect(() => {
+ const updateInterval = setInterval(() => {
+ if (gizmoRef.current) {
+ const newPosition = new THREE.Vector3()
+ gizmoRef.current.obj.getWorldPosition(newPosition)
+ setCogPosition(newPosition)
+ }
+ }, 100)
+
+ return () => clearInterval(updateInterval)
+ }, [])
+
+ useEffect(() => {
+ const previousVisibility = PreferencesSystem.getGlobalPreference("ShowCenterOfMassIndicators")
+ PreferencesSystem.setGlobalPreference("ShowCenterOfMassIndicators", true)
+ selectedRobot.updateMeshTransforms()
+
+ return () => {
+ PreferencesSystem.setGlobalPreference("ShowCenterOfMassIndicators", previousVisibility)
+ selectedRobot.updateMeshTransforms()
+ }
+ }, [selectedRobot])
+
+ const postGizmoCreation = useCallback(
+ (gizmo: GizmoSceneObject) => {
+ const material = (gizmo.obj as THREE.Mesh).material as THREE.Material
+ material.depthTest = false
+
+ const rootNodeId = selectedRobot.getRootNodeId()
+ if (!rootNodeId) {
+ return
+ }
+
+ const robotTransform = convertJoltMat44ToThreeMatrix4(
+ World.physicsSystem.getBody(rootNodeId).GetWorldTransform()
+ )
+
+ if (selectedRobot.modifiedCenterOfGravity) {
+ const robotWorldPos = new THREE.Vector3()
+ const robotWorldQuat = new THREE.Quaternion()
+ const robotWorldScale = new THREE.Vector3()
+ robotTransform.decompose(robotWorldPos, robotWorldQuat, robotWorldScale)
+
+ const worldPos = selectedRobot.modifiedCenterOfGravity.clone()
+ worldPos.applyQuaternion(robotWorldQuat)
+ worldPos.add(robotWorldPos)
+
+ gizmo.obj.position.copy(worldPos)
+ } else {
+ gizmo.obj.position.copy(selectedRobot.currentCenterOfGravity)
+ }
+ },
+ [selectedRobot]
+ )
+
+ const handleReset = useCallback(() => {
+ selectedRobot.modifiedCenterOfGravity = undefined
+
+ if (gizmoRef.current) {
+ const actualCoG = selectedRobot.currentCenterOfGravity
+ gizmoRef.current.obj.position.copy(actualCoG)
+ setCogPosition(actualCoG)
+ }
+
+ selectedRobot.updateMeshTransforms()
+ }, [selectedRobot])
+
+ const gizmoComponent = useMemo(() => {
+ return (
+
+ )
+ }, [cogSphereMesh, postGizmoCreation])
+
+ return (
+
+
+
+ {Spacer(8)}
+
+ {gizmoComponent}
+
+ {Spacer(8)}
+
+
+
+
+
+
+ {Spacer(8)}
+
+ {/* Removed CoG effect strength control */}
+
+ {Spacer(8)}
+
+
+
+ )
+}
+
+export default CenterOfGravityInterface