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