Spawn Intake/Ejector in the middle of the robot [SYNTH-204]

fission/src/mirabuf/MirabufSceneObject.ts

@@ -46,6 +46,7 @@ import {
     convertJoltMat44ToThreeMatrix4,
     convertJoltRVec3ToJoltVec3,
     convertJoltVec3ToThreeVector3,
+    convertThreeMatrix4ToArray,
     convertThreeVector3ToJoltVec3,
 } from "@/util/TypeConversions"
 import { createMeshForShape } from "@/util/threejs/MeshCreation.ts"
@@ -292,6 +293,12 @@ class MirabufSceneObject extends SceneObject implements ContextSupplier {
             simLayer.setBrain(this._brain)
         }
 
+        this.updateBatches()
+
+        if (this.miraType === MiraType.ROBOT) {
+            this.centerDefaultZoneTransformations()
+        }
+
         // Intake
         this.updateIntakeSensor()
         this.updateScoringZones()
@@ -301,8 +308,6 @@ class MirabufSceneObject extends SceneObject implements ContextSupplier {
             setSpotlightAssembly(this)
         }
 
-        this.updateBatches()
-
         this._basePositionTransform = this.getPositionTransform()
 
         this.moveToSpawnLocation()
@@ -326,6 +331,51 @@ class MirabufSceneObject extends SceneObject implements ContextSupplier {
         return transform
     }
 
+    public getCenter(vec: THREE.Vector3 = new THREE.Vector3()): THREE.Vector3 {
+        return this.computeBoundingBox().getCenter(vec)
+    }
+
+    public ensureDefaultZoneTransformations() {
+        let changed = false
+
+        if (this.intakePreferences.deltaTransformation.length === 0) {
+            this.intakePreferences.deltaTransformation = this.computeCenteredDeltaTransformation(
+                this.intakePreferences.parentNode
+            )
+            changed = true
+        }
+
+        if (this.ejectorPreferences.deltaTransformation.length === 0) {
+            this.ejectorPreferences.deltaTransformation = this.computeCenteredDeltaTransformation(
+                this.ejectorPreferences.parentNode
+            )
+            changed = true
+        }
+
+        if (changed) {
+            PreferencesSystem.setRobotPreferences(this.assemblyName, this.robotPreferences)
+            PreferencesSystem.savePreferences()
+        }
+    }
+
+    private centerDefaultZoneTransformations() {
+        this.ensureDefaultZoneTransformations()
+    }
+    private computeCenteredDeltaTransformation(parentNode: string | undefined): number[] {
+        const nodeBodyId =
+            this.mechanism.nodeToBody.get(parentNode ?? this.rootNodeId) ??
+            this.mechanism.nodeToBody.get(this.rootNodeId)!
+
+        const robotTransformation = convertJoltMat44ToThreeMatrix4(
+            World.physicsSystem.getBody(nodeBodyId)!.GetWorldTransform()
+        )
+
+        const centeredTransformation = robotTransformation.clone().setPosition(this.getCenter())
+        const deltaTransformation = centeredTransformation.premultiply(robotTransformation.invert())
+
+        return convertThreeMatrix4ToArray(deltaTransformation)
+    }
+
     public moveToSpawnLocation() {
         const referencePos = new THREE.Vector3()
         const pos =
@@ -870,6 +920,10 @@ class MirabufSceneObject extends SceneObject implements ContextSupplier {
         // Ensure backwards compatibility for showZoneAlways field
         this._robotPreferences.intake.showZoneAlways ??= false
 
+        if (this.miraType === MiraType.ROBOT) {
+            this.centerDefaultZoneTransformations()
+        }
+
         setTimeout(() => this.sendPreferences())
 
         // For fields, sync dev-tool data with field preferences

fission/src/systems/preferences/PreferenceTypes.ts

@@ -225,15 +225,15 @@ export function defaultRobotPreferences(): RobotPreferences {
         inputsSchemes: [],
         motors: [],
         intake: {
-            deltaTransformation: [1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1],
+            deltaTransformation: [],
             zoneDiameter: 0.5,
             parentNode: undefined,
             showZoneAlways: false,
             maxPieces: 1,
             animationDuration: 0.5,
         },
         ejector: {
-            deltaTransformation: [1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1],
+            deltaTransformation: [],
             ejectorVelocity: 1,
             parentNode: undefined,
             ejectOrder: "FIFO",

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

@@ -258,7 +258,9 @@ const ConfigurePanel: React.FC<PanelImplProps<void, ConfigurePanelCustomProps>>
         if (selectedAssembly) {
             const name = selectedAssembly.assemblyName
 
-            const robotPrefs = PreferencesSystem.getRobotPreferences(name)
+            selectedAssembly.ensureDefaultZoneTransformations()
+
+            const robotPrefs = selectedAssembly.robotPreferences
             const fieldPrefs = PreferencesSystem.getFieldPreferences(name)
             const motorPrefs = PreferencesSystem.getMotorPreferences(name)
 

fission/src/ui/panels/configuring/assembly-config/interfaces/ConfigureGamepiecePickupInterface.tsx

@@ -295,7 +295,7 @@ const ConfigureGamepiecePickupInterface: React.FC<ConfigPickupProps> = ({ select
                         const robotTransformation = convertJoltMat44ToThreeMatrix4(
                             World.physicsSystem.getBody(selectedRobot.getRootNodeId()!)!.GetWorldTransform()
                         )
-                        gizmoRef.current.obj.position.setFromMatrixPosition(robotTransformation)
+                        gizmoRef.current.obj.position.copy(selectedRobot.getCenter())
                         gizmoRef.current.obj.rotation.setFromRotationMatrix(robotTransformation)
                     }
                     setZoneSize(0.5)

fission/src/ui/panels/configuring/assembly-config/interfaces/ConfigureShotTrajectoryInterface.tsx

@@ -250,7 +250,7 @@ const ConfigureShotTrajectoryInterface: React.FC<ConfigEjectorProps> = ({ select
                         const robotTransformation = convertJoltMat44ToThreeMatrix4(
                             World.physicsSystem.getBody(selectedRobot.getRootNodeId()!)!.GetWorldTransform()
                         )
-                        gizmoRef.current.obj.position.setFromMatrixPosition(robotTransformation)
+                        gizmoRef.current.obj.position.copy(selectedRobot.getCenter())
                         gizmoRef.current.obj.rotation.setFromRotationMatrix(robotTransformation)
                     }
                     setEjectorVelocity(1)