Intake and Ejector Configurator

fission/src/Synthesis.tsx

@@ -49,14 +49,15 @@ import ScoringZonesPanel from "@/panels/configuring/scoring/ScoringZonesPanel"
 import ZoneConfigPanel from "@/panels/configuring/scoring/ZoneConfigPanel"
 import ScoreboardPanel from "@/panels/information/ScoreboardPanel"
 import DriverStationPanel from "@/panels/simulation/DriverStationPanel"
+import PokerPanel from "@/panels/PokerPanel.tsx"
 import ManageAssembliesModal from "@/modals/spawning/ManageAssembliesModal.tsx"
 import World from "@/systems/World.ts"
 import { AddRobotsModal, AddFieldsModal, SpawningModal } from "@/modals/spawning/SpawningModals.tsx"
 import ImportLocalMirabufModal from "@/modals/mirabuf/ImportLocalMirabufModal.tsx"
 import APS from "./aps/APS.ts"
 import ImportMirabufPanel from "@/ui/panels/mirabuf/ImportMirabufPanel.tsx"
 import Skybox from "./ui/components/Skybox.tsx"
-import PokerPanel from "@/panels/PokerPanel.tsx"
+import ConfigureRobotModal from "./ui/modals/configuring/ConfigureRobotModal.tsx"
 
 const DEFAULT_MIRA_PATH = "/api/mira/Robots/Team 2471 (2018)_v7.mira"
 
@@ -129,7 +130,7 @@ function Synthesis() {
         }
         mainLoop()
 
-        World.SceneRenderer.updateSkyboxColors(defaultTheme)
+        World.SceneRenderer.UpdateSkyboxColors(defaultTheme)
 
         // Cleanup
         return () => {
@@ -213,15 +214,26 @@ const initialModals = [
     <ConfigMotorModal key="config-motor" modalId="config-motor" />,
     <ManageAssembliesModal key="manage-assemblies" modalId="manage-assemblies" />,
     <ImportLocalMirabufModal key="import-local-mirabuf" modalId="import-local-mirabuf" />,
+    <ConfigureRobotModal key="config-robot" modalId="config-robot" />,
 ]
 
 const initialPanels: ReactElement[] = [
     <RobotSwitchPanel key="multibot" panelId="multibot" openLocation="right" sidePadding={8} />,
     <DriverStationPanel key="driver-station" panelId="driver-station" />,
     <SpawnLocationsPanel key="spawn-locations" panelId="spawn-locations" />,
     <ScoreboardPanel key="scoreboard" panelId="scoreboard" />,
-    <ConfigureGamepiecePickupPanel key="config-gamepiece-pickup" panelId="config-gamepiece-pickup" />,
-    <ConfigureShotTrajectoryPanel key="config-shot-trajectory" panelId="config-shot-trajectory" />,
+    <ConfigureGamepiecePickupPanel
+        key="config-gamepiece-pickup"
+        panelId="config-gamepiece-pickup"
+        openLocation="right"
+        sidePadding={8}
+    />,
+    <ConfigureShotTrajectoryPanel
+        key="config-shot-trajectory"
+        panelId="config-shot-trajectory"
+        openLocation="right"
+        sidePadding={8}
+    />,
     <ScoringZonesPanel key="scoring-zones" panelId="scoring-zones" />,
     <ZoneConfigPanel key="zone-config" panelId="zone-config" />,
     <ImportMirabufPanel key="import-mirabuf" panelId="import-mirabuf" />,

fission/src/mirabuf/EjectableSceneObject.ts

@@ -0,0 +1,112 @@
+import SceneObject from "@/systems/scene/SceneObject"
+import MirabufSceneObject from "./MirabufSceneObject"
+import Jolt from "@barclah/jolt-physics"
+import World from "@/systems/World"
+import {
+    Array_ThreeMatrix4,
+    JoltMat44_ThreeMatrix4,
+    JoltQuat_ThreeQuaternion,
+    ThreeQuaternion_JoltQuat,
+    ThreeVector3_JoltVec3,
+} from "@/util/TypeConversions"
+import * as THREE from "three"
+
+class EjectableSceneObject extends SceneObject {
+    private _parentAssembly: MirabufSceneObject
+    private _gamePieceBodyId?: Jolt.BodyID
+
+    private _parentBodyId?: Jolt.BodyID
+    private _deltaTransformation?: THREE.Matrix4
+    private _ejectVelocity?: number
+
+    public get gamePieceBodyId() {
+        return this._gamePieceBodyId
+    }
+
+    public constructor(parentAssembly: MirabufSceneObject, gamePieceBody: Jolt.BodyID) {
+        super()
+
+        console.debug("Trying to create ejectable...")
+
+        this._parentAssembly = parentAssembly
+        this._gamePieceBodyId = gamePieceBody
+    }
+
+    public Setup(): void {
+        if (this._parentAssembly.ejectorPreferences && this._gamePieceBodyId) {
+            this._parentBodyId = this._parentAssembly.mechanism.nodeToBody.get(
+                this._parentAssembly.ejectorPreferences.parentNode ?? this._parentAssembly.rootNodeId
+            )
+
+            this._deltaTransformation = Array_ThreeMatrix4(this._parentAssembly.ejectorPreferences.deltaTransformation)
+            this._ejectVelocity = this._parentAssembly.ejectorPreferences.ejectorVelocity
+
+            World.PhysicsSystem.DisablePhysicsForBody(this._gamePieceBodyId)
+
+            console.debug("Ejectable created successfully!")
+        }
+    }
+
+    public Update(): void {
+        if (this._parentBodyId && this._deltaTransformation && this._gamePieceBodyId) {
+            if (!World.PhysicsSystem.IsBodyAdded(this._gamePieceBodyId)) {
+                this._gamePieceBodyId = undefined
+                return
+            }
+
+            // I had a think and free wrote this matrix math on a whim. It worked first try and I honestly can't quite remember how it works... -Hunter
+            const gpBody = World.PhysicsSystem.GetBody(this._gamePieceBodyId)
+            const posToCOM = JoltMat44_ThreeMatrix4(gpBody.GetCenterOfMassTransform()).premultiply(
+                JoltMat44_ThreeMatrix4(gpBody.GetWorldTransform()).invert()
+            )
+
+            const body = World.PhysicsSystem.GetBody(this._parentBodyId)
+            const bodyTransform = posToCOM
+                .invert()
+                .premultiply(
+                    this._deltaTransformation.clone().premultiply(JoltMat44_ThreeMatrix4(body.GetWorldTransform()))
+                )
+            const position = new THREE.Vector3(0, 0, 0)
+            const rotation = new THREE.Quaternion(0, 0, 0, 1)
+            bodyTransform.decompose(position, rotation, new THREE.Vector3(1, 1, 1))
+
+            World.PhysicsSystem.SetBodyPosition(this._gamePieceBodyId, ThreeVector3_JoltVec3(position), false)
+            World.PhysicsSystem.SetBodyRotation(this._gamePieceBodyId, ThreeQuaternion_JoltQuat(rotation), false)
+        }
+    }
+
+    public Eject() {
+        if (!this._parentBodyId || !this._ejectVelocity || !this._gamePieceBodyId) {
+            return
+        }
+
+        if (!World.PhysicsSystem.IsBodyAdded(this._gamePieceBodyId)) {
+            this._gamePieceBodyId = undefined
+            return
+        }
+
+        const parentBody = World.PhysicsSystem.GetBody(this._parentBodyId)
+        const gpBody = World.PhysicsSystem.GetBody(this._gamePieceBodyId)
+        const ejectDir = new THREE.Vector3(0, 0, 1)
+            .applyQuaternion(JoltQuat_ThreeQuaternion(gpBody.GetRotation()))
+            .normalize()
+
+        World.PhysicsSystem.EnablePhysicsForBody(this._gamePieceBodyId)
+        gpBody.SetLinearVelocity(
+            parentBody.GetLinearVelocity().Add(ThreeVector3_JoltVec3(ejectDir.multiplyScalar(this._ejectVelocity)))
+        )
+        gpBody.SetAngularVelocity(parentBody.GetAngularVelocity())
+
+        this._parentBodyId = undefined
+    }
+
+    public Dispose(): void {
+        console.debug("Destroying ejectable")
+
+        if (this._gamePieceBodyId) {
+            World.PhysicsSystem.EnablePhysicsForBody(this._gamePieceBodyId)
+        }
+    }
+}
+
+export default EjectableSceneObject

fission/src/mirabuf/IntakeSensorSceneObject.ts

@@ -0,0 +1,104 @@
+import SceneObject from "@/systems/scene/SceneObject"
+import MirabufSceneObject, { RigidNodeAssociate } from "./MirabufSceneObject"
+import Jolt from "@barclah/jolt-physics"
+import * as THREE from "three"
+import World from "@/systems/World"
+import JOLT from "@/util/loading/JoltSyncLoader"
+import {
+    Array_ThreeMatrix4,
+    JoltMat44_ThreeMatrix4,
+    ThreeQuaternion_JoltQuat,
+    ThreeVector3_JoltVec3,
+} from "@/util/TypeConversions"
+
+class IntakeSensorSceneObject extends SceneObject {
+    private _parentAssembly: MirabufSceneObject
+    private _parentBodyId?: Jolt.BodyID
+    private _deltaTransformation?: THREE.Matrix4
+
+    private _joltBodyId?: Jolt.BodyID
+    private _mesh?: THREE.Mesh
+
+    public constructor(parentAssembly: MirabufSceneObject) {
+        super()
+
+        console.debug("Trying to create intake sensor...")
+
+        this._parentAssembly = parentAssembly
+    }
+
+    public Setup(): void {
+        if (this._parentAssembly.intakePreferences) {
+            this._parentBodyId = this._parentAssembly.mechanism.nodeToBody.get(
+                this._parentAssembly.intakePreferences.parentNode ?? this._parentAssembly.rootNodeId
+            )
+
+            this._deltaTransformation = Array_ThreeMatrix4(this._parentAssembly.intakePreferences.deltaTransformation)
+
+            this._joltBodyId = World.PhysicsSystem.CreateSensor(
+                new JOLT.SphereShapeSettings(this._parentAssembly.intakePreferences.zoneDiameter / 2.0)
+            )
+            if (!this._joltBodyId) {
+                console.error("Failed to create intake. No Jolt Body")
+                return
+            }
+
+            this._mesh = World.SceneRenderer.CreateSphere(
+                this._parentAssembly.intakePreferences.zoneDiameter / 2.0,
+                World.SceneRenderer.CreateToonMaterial(0x5eeb67)
+            )
+            World.SceneRenderer.scene.add(this._mesh)
+
+            console.debug("Intake sensor created successfully!")
+        }
+    }
+
+    public Update(): void {
+        if (this._joltBodyId && this._parentBodyId && this._deltaTransformation) {
+            const parentBody = World.PhysicsSystem.GetBody(this._parentBodyId)
+            const bodyTransform = this._deltaTransformation
+                .clone()
+                .premultiply(JoltMat44_ThreeMatrix4(parentBody.GetWorldTransform()))
+            const position = new THREE.Vector3(0, 0, 0)
+            const rotation = new THREE.Quaternion(0, 0, 0, 1)
+            bodyTransform.decompose(position, rotation, new THREE.Vector3(1, 1, 1))
+
+            World.PhysicsSystem.SetBodyPosition(this._joltBodyId, ThreeVector3_JoltVec3(position))
+            World.PhysicsSystem.SetBodyRotation(this._joltBodyId, ThreeQuaternion_JoltQuat(rotation))
+
+            if (this._mesh) {
+                this._mesh.position.setFromMatrixPosition(bodyTransform)
+                this._mesh.rotation.setFromRotationMatrix(bodyTransform)
+            }
+
+            if (!World.PhysicsSystem.isPaused) {
+                // TEMPORARY GAME PIECE DETECTION
+                const hitRes = World.PhysicsSystem.RayCast(ThreeVector3_JoltVec3(position), new JOLT.Vec3(0, 0, 3))
+                if (hitRes) {
+                    const gpAssoc = <RigidNodeAssociate>World.PhysicsSystem.GetBodyAssociation(hitRes.data.mBodyID)
+                    // This works, however the check for game piece is doing two checks.
+                    if (gpAssoc && gpAssoc.isGamePiece) {
+                        console.debug("Found game piece!")
+                        this._parentAssembly.SetEjectable(hitRes.data.mBodyID, false)
+                    }
+                }
+            }
+        }
+    }
+
+    public Dispose(): void {
+        console.debug("Destroying intake sensor")
+
+        if (this._joltBodyId) {
+            World.PhysicsSystem.DestroyBodyIds(this._joltBodyId)
+
+            if (this._mesh) {
+                this._mesh.geometry.dispose()
+                ;(this._mesh.material as THREE.Material).dispose()
+                World.SceneRenderer.scene.remove(this._mesh)
+            }
+        }
+    }
+}
+
+export default IntakeSensorSceneObject

fission/src/mirabuf/MirabufParser.ts

@@ -2,6 +2,8 @@ import * as THREE from "three"
 import { mirabuf } from "@/proto/mirabuf"
 import { MirabufTransform_ThreeMatrix4 } from "@/util/TypeConversions"
 
+export type RigidNodeId = string
+
 export enum ParseErrorSeverity {
     Unimportable = 10,
     LikelyIssues = 6,
@@ -60,8 +62,8 @@ class MirabufParser {
     public get groundedNode() {
         return this._groundedNode ? new RigidNodeReadOnly(this._groundedNode) : undefined
     }
-    public get rigidNodes(): Array<RigidNodeReadOnly> {
-        return this._rigidNodes.map(x => new RigidNodeReadOnly(x))
+    public get rigidNodes(): Map<RigidNodeId, RigidNodeReadOnly> {
+        return new Map(this._rigidNodes.map(x => [x.id, new RigidNodeReadOnly(x)]))
     }
     public get directedGraph() {
         return this._directedGraph
@@ -121,6 +123,7 @@ class MirabufParser {
                     const instNode = this.BinarySearchDesignTree(inst.info!.GUID!)
                     if (instNode) {
                         const gpRn = this.NewRigidNode(GAMEPIECE_SUFFIX)
+                        gpRn.isGamePiece = true
                         this.MovePartToRigidNode(instNode!.value!, gpRn)
                         instNode.children &&
                             traverseTree(instNode.children, x => this.MovePartToRigidNode(x.value!, gpRn))
@@ -400,21 +403,23 @@ class MirabufParser {
  */
 class RigidNode {
     public isRoot: boolean
-    public id: string
+    public id: RigidNodeId
     public parts: Set<string> = new Set()
     public isDynamic: boolean
+    public isGamePiece: boolean
 
-    public constructor(id: string, isDynamic?: boolean) {
+    public constructor(id: RigidNodeId, isDynamic?: boolean, isGamePiece?: boolean) {
         this.id = id
         this.isDynamic = isDynamic ?? true
         this.isRoot = false
+        this.isGamePiece = isGamePiece ?? false
     }
 }
 
 export class RigidNodeReadOnly {
     private _original: RigidNode
 
-    public get id(): string {
+    public get id(): RigidNodeId {
         return this._original.id
     }
 
@@ -430,6 +435,10 @@ export class RigidNodeReadOnly {
         return this._original.isRoot
     }
 
+    public get isGamePiece(): boolean {
+        return this._original.isGamePiece
+    }
+
     public constructor(original: RigidNode) {
         this._original = original
     }