From 261c82d1f5e5e531afae48f8db7267d5b669a5d3 Mon Sep 17 00:00:00 2001 From: KyroVibe Date: Wed, 20 Mar 2024 21:03:38 -0600 Subject: [PATCH] Jointing done. No motor control tested --- fission/src/Synthesis.tsx | 4 +- fission/src/mirabuf/MirabufParser.ts | 5 +- fission/src/mirabuf/MirabufSceneObject.ts | 10 +- fission/src/systems/physics/PhysicsSystem.ts | 154 +++++++++++++++---- fission/src/test/PhysicsSystem.test.ts | 8 +- 5 files changed, 139 insertions(+), 42 deletions(-) diff --git a/fission/src/Synthesis.tsx b/fission/src/Synthesis.tsx index 46b332d168..4209eeb1f6 100644 --- a/fission/src/Synthesis.tsx +++ b/fission/src/Synthesis.tsx @@ -55,11 +55,11 @@ import DriverStationPanel from "./panels/simulation/DriverStationPanel" import ManageAssembliesModal from './modals/spawning/ManageAssembliesModal.tsx'; import World from './systems/World.ts'; -// const DEFAULT_MIRA_PATH = 'test_mira/Team_2471_(2018)_v7.mira'; +const DEFAULT_MIRA_PATH = 'test_mira/Team_2471_(2018)_v7.mira'; // const DEFAULT_MIRA_PATH = 'test_mira/Dozer_v2.mira'; // const DEFAULT_MIRA_PATH = 'test_mira/PhysicsSpikeTest_v1.mira'; // const DEFAULT_MIRA_PATH = 'test_mira/SliderTestFission_v2.mira'; -const DEFAULT_MIRA_PATH = 'test_mira/HingeTestFission_v1.mira'; +// const DEFAULT_MIRA_PATH = 'test_mira/HingeTestFission_v1.mira'; function Synthesis() { const { openModal, closeModal, getActiveModalElement } = diff --git a/fission/src/mirabuf/MirabufParser.ts b/fission/src/mirabuf/MirabufParser.ts index f47f9d5630..9ae2dde91d 100644 --- a/fission/src/mirabuf/MirabufParser.ts +++ b/fission/src/mirabuf/MirabufParser.ts @@ -10,6 +10,7 @@ export enum ParseErrorSeverity { } export const GROUNDED_JOINT_ID = 'grounded'; +export const GAMEPIECE_SUFFIX = '_gp'; export type ParseError = [severity: ParseErrorSeverity, message: string]; @@ -95,7 +96,7 @@ class MirabufParser { if (gamepieceDefinitions.has(inst.partDefinitionReference!)) { const instNode = this.BinarySearchDesignTree(inst.info!.GUID!); if (instNode) { - const gpRn = this.NewRigidNode('gp'); + const gpRn = this.NewRigidNode(GAMEPIECE_SUFFIX); this.MovePartToRigidNode(instNode!.value!, gpRn); instNode.children && traverseTree(instNode.children, x => this.MovePartToRigidNode(x.value!, gpRn)); } else { @@ -155,7 +156,7 @@ class MirabufParser { } private NewRigidNode(suffix?: string): RigidNode { - const node = new RigidNode((this._nodeNameCounter++).toString() + (suffix ? `_${suffix}` : '')); + const node = new RigidNode(`${this._nodeNameCounter++}${suffix ? suffix : ''}`); this._rigidNodes.push(node); return node; } diff --git a/fission/src/mirabuf/MirabufSceneObject.ts b/fission/src/mirabuf/MirabufSceneObject.ts index 5b32d4647e..71b8d41e18 100644 --- a/fission/src/mirabuf/MirabufSceneObject.ts +++ b/fission/src/mirabuf/MirabufSceneObject.ts @@ -8,8 +8,9 @@ import Jolt from '@barclah/jolt-physics'; import { JoltMat44_ThreeMatrix4 } from "@/util/TypeConversions"; import * as THREE from 'three'; import JOLT from "@/util/loading/JoltSyncLoader"; +import { LayerReserve } from "@/systems/physics/PhysicsSystem"; -const DEBUG_BODIES = false; +const DEBUG_BODIES = true; interface RnDebugMeshes { colliderMesh: THREE.Mesh; @@ -21,13 +22,17 @@ class MirabufSceneObject extends SceneObject { private _mirabufInstance: MirabufInstance; private _bodies: Map; private _debugBodies: Map | null; + private _physicsLayerReserve: LayerReserve | undefined = undefined; public constructor(mirabufInstance: MirabufInstance) { super(); this._mirabufInstance = mirabufInstance; - this._bodies = World.PhysicsSystem.CreateBodiesFromParser(mirabufInstance.parser); + if (this._mirabufInstance.parser.assembly.dynamic) { + this._physicsLayerReserve = new LayerReserve(); + } + this._bodies = World.PhysicsSystem.CreateBodiesFromParser(mirabufInstance.parser, this._physicsLayerReserve); World.PhysicsSystem.CreateJointsFromParser(mirabufInstance.parser, this._bodies); this._debugBodies = null; @@ -94,6 +99,7 @@ class MirabufSceneObject extends SceneObject { (x.comMesh.material as THREE.Material).dispose(); }); this._debugBodies?.clear(); + this._physicsLayerReserve?.Release(); } private CreateMeshForShape(shape: Jolt.Shape): THREE.Mesh { diff --git a/fission/src/systems/physics/PhysicsSystem.ts b/fission/src/systems/physics/PhysicsSystem.ts index d8c1992d40..bd152e5124 100644 --- a/fission/src/systems/physics/PhysicsSystem.ts +++ b/fission/src/systems/physics/PhysicsSystem.ts @@ -3,12 +3,20 @@ import JOLT from "../../util/loading/JoltSyncLoader"; import Jolt from "@barclah/jolt-physics"; import * as THREE from 'three'; import { mirabuf } from '../../proto/mirabuf'; -import MirabufParser, { GROUNDED_JOINT_ID, RigidNodeReadOnly } from "../../mirabuf/MirabufParser"; +import MirabufParser, { GAMEPIECE_SUFFIX, GROUNDED_JOINT_ID, RigidNodeReadOnly } from "../../mirabuf/MirabufParser"; import WorldSystem from "../WorldSystem"; -const LAYER_NOT_MOVING = 0; -const LAYER_MOVING = 1; -const COUNT_OBJECT_LAYERS = 2; +/** + * Layers used for determining enabled/disabled collisions. + */ +const LAYER_FIELD = 0; // Used for grounded rigid node of a field as well as any rigid nodes jointed to it. +const LAYER_GENERAL_DYNAMIC = 1; // Used for game pieces or any general dynamic objects that can collide with anything and everything. +const RobotLayers: number[] = [ // Reserved layers for robots. Robot layers have no collision with themselves but have collision with everything else. + 2, 3, 4, 5, 6, 7, 8, 9 +]; + +// Please update this accordingly. +const COUNT_OBJECT_LAYERS = 10; const STANDARD_TIME_STEP = 1.0 / 120.0; const STANDARD_SUB_STEPS = 3; @@ -74,7 +82,7 @@ class PhysicsSystem extends WorldSystem { pos, rot, mass ? JOLT.EMotionType_Dynamic : JOLT.EMotionType_Static, - mass ? LAYER_MOVING : LAYER_NOT_MOVING + mass ? LAYER_GENERAL_DYNAMIC : LAYER_FIELD ); if (mass) { creationSettings.mMassPropertiesOverride.mMass = mass; @@ -109,7 +117,7 @@ class PhysicsSystem extends WorldSystem { pos, rot, mass ? JOLT.EMotionType_Dynamic : JOLT.EMotionType_Static, - LAYER_NOT_MOVING + mass ? LAYER_GENERAL_DYNAMIC : LAYER_FIELD ); if (mass) { creationSettings.mMassPropertiesOverride.mMass = mass; @@ -144,6 +152,12 @@ class PhysicsSystem extends WorldSystem { return settings.Create(); } + /** + * Creates all the joints for a mirabuf assembly given an already compiled mapping of rigid nodes to bodies. + * + * @param parser Mirabuf parser with complete set of rigid nodes and assembly data. + * @param rnMapping Mapping of the name of rigid groups to Jolt bodies. Retrieved from CreateBodiesFromParser. + */ public CreateJointsFromParser(parser: MirabufParser, rnMapping: Map) { const jointData = parser.assembly.data!.joints!; for (const [jGuid, jInst] of (Object.entries(jointData.jointInstances!) as [string, mirabuf.joint.JointInstance][])) { @@ -173,15 +187,10 @@ class PhysicsSystem extends WorldSystem { switch (jDef.jointMotionType!) { case mirabuf.joint.JointMotion.REVOLUTE: - this.CreateRevoluteJoint(jInst, jDef, bodyA, bodyB, parser.assembly.info!.version!); + this.CreateHingeConstraint(jInst, jDef, bodyA, bodyB, parser.assembly.info!.version!); break; case mirabuf.joint.JointMotion.SLIDER: - console.debug(`SLIDER: ${ - parser.assembly.data!.parts!.partInstances![jInst.parentPart]!.info!.name! - } <-> ${ - parser.assembly.data!.parts!.partInstances![jInst.childPart]!.info!.name! - }`); - this.CreateSliderJoint(jInst, jDef, bodyA, bodyB); + this.CreateSliderConstraint(jInst, jDef, bodyA, bodyB); break; default: console.debug('Unsupported joint detected. Skipping...'); @@ -190,9 +199,19 @@ class PhysicsSystem extends WorldSystem { } } - private CreateRevoluteJoint( + /** + * Creates a Hinge constraint. + * + * @param jointInstance Joint instance. + * @param jointDefinition Joint definition. + * @param bodyA Parent body to connect. + * @param bodyB Child body to connect. + * @param versionNum Version number of the export. Used for compatability purposes. + * @returns Resulting Jolt Hinge Constraint. + */ + private CreateHingeConstraint( jointInstance: mirabuf.joint.JointInstance, jointDefinition: mirabuf.joint.Joint, - bodyA: Jolt.Body, bodyB: Jolt.Body, versionNum: number) { + bodyA: Jolt.Body, bodyB: Jolt.Body, versionNum: number): Jolt.HingeConstraint { // HINGE CONSTRAINT const hingeConstraintSettings = new JOLT.HingeConstraintSettings(); @@ -231,18 +250,29 @@ class PhysicsSystem extends WorldSystem { const upper = piSafetyCheck(rotationalFreedom.limits.upper ?? 0) - currentPos; const lower = piSafetyCheck(rotationalFreedom.limits.lower ?? 0) - currentPos; - console.debug(`Lower: ${lower}\nUpper: ${upper}\nCurrent: ${currentPos}`); - hingeConstraintSettings.mLimitsMin = -upper; hingeConstraintSettings.mLimitsMax = -lower; } - this._joltPhysSystem.AddConstraint(hingeConstraintSettings.Create(bodyA, bodyB)); + const constraint = hingeConstraintSettings.Create(bodyA, bodyB); + this._joltPhysSystem.AddConstraint(constraint); + + return JOLT.castObject(constraint, JOLT.HingeConstraint); } - private CreateSliderJoint( + /** + * Creates a new slider constraint. + * + * @param jointInstance Joint instance. + * @param jointDefinition Joint definition. + * @param bodyA Parent body to connect. + * @param bodyB Child body to connect. + * + * @returns Resulting Jolt constraint. + */ + private CreateSliderConstraint( jointInstance: mirabuf.joint.JointInstance, jointDefinition: mirabuf.joint.Joint, - bodyA: Jolt.Body, bodyB: Jolt.Body) { + bodyA: Jolt.Body, bodyB: Jolt.Body): Jolt.SliderConstraint { const sliderConstraintSettings = new JOLT.SliderConstraintSettings(); @@ -292,6 +322,8 @@ class PhysicsSystem extends WorldSystem { this._constraints.push(constraint); this._joltPhysSystem.AddConstraint(constraint); + + return JOLT.castObject(constraint, JOLT.SliderConstraint); } /** @@ -300,8 +332,14 @@ class PhysicsSystem extends WorldSystem { * @param parser MirabufParser containing properly parsed RigidNodes * @returns Mapping of Jolt BodyIDs */ - public CreateBodiesFromParser(parser: MirabufParser): Map { + public CreateBodiesFromParser(parser: MirabufParser, layerReserve?: LayerReserve): Map { const rnToBodies = new Map(); + + if ((parser.assembly.dynamic && !layerReserve) || layerReserve?.isReleased) { + throw new Error('No layer reserve for dynamic assembly'); + } + + const reservedLayer: number | undefined = layerReserve?.layer; filterNonPhysicsNodes(parser.rigidNodes, parser.assembly).forEach(rn => { @@ -314,6 +352,10 @@ class PhysicsSystem extends WorldSystem { const minBounds = new JOLT.Vec3(1000000.0, 1000000.0, 1000000.0); const maxBounds = new JOLT.Vec3(-1000000.0, -1000000.0, -1000000.0); + const rnLayer: number = reservedLayer + ? reservedLayer + : (rn.name.endsWith(GAMEPIECE_SUFFIX) ? LAYER_GENERAL_DYNAMIC : LAYER_FIELD); + rn.parts.forEach(partId => { const partInstance = parser.assembly.data!.parts!.partInstances![partId]!; if (partInstance.skipCollider == null || partInstance == undefined || partInstance.skipCollider == false) { @@ -364,15 +406,16 @@ class PhysicsSystem extends WorldSystem { const shape = shapeResult.Get(); - if (rn.isDynamic) + if (rn.isDynamic) { shape.GetMassProperties().mMass = totalMass == 0.0 ? 1 : totalMass; + } const bodySettings = new JOLT.BodyCreationSettings( shape, new JOLT.Vec3(0.0, 0.0, 0.0), new JOLT.Quat(0, 0, 0, 1), rn.isDynamic ? JOLT.EMotionType_Dynamic : JOLT.EMotionType_Static, - rn.isDynamic ? LAYER_MOVING : LAYER_NOT_MOVING + rnLayer ); const body = this._joltBodyInterface.CreateBody(bodySettings); this._joltBodyInterface.AddBody(body.GetID(), JOLT.EActivation_Activate); @@ -380,7 +423,7 @@ class PhysicsSystem extends WorldSystem { // Little testing components body.SetRestitution(0.4); - const angVelocity = new JOLT.Vec3(0, 0, 2); + const angVelocity = new JOLT.Vec3(0, 3, 0); body.SetAngularVelocity(angVelocity); JOLT.destroy(angVelocity); } @@ -493,19 +536,66 @@ class PhysicsSystem extends WorldSystem { } } +export class LayerReserve { + private _layer: number; + private _isReleased: boolean; + + public get layer() { return this._layer; } + public get isReleased() { return this._isReleased; } + + public constructor() { + this._layer = RobotLayers.pop()!; + this._isReleased = false; + } + + public Release() { + if (!this._isReleased) { + RobotLayers.push(this._layer); + this._isReleased = true; + } + } +} + +/** + * Initialize collision groups and filtering for Jolt. + * + * @param settings Jolt object used for applying filters. + */ function SetupCollisionFiltering(settings: Jolt.JoltSettings) { const objectFilter = new JOLT.ObjectLayerPairFilterTable(COUNT_OBJECT_LAYERS); - objectFilter.EnableCollision(LAYER_NOT_MOVING, LAYER_MOVING); - // TODO: Collision between dynamic objects temporarily disabled. - // objectFilter.EnableCollision(LAYER_MOVING, LAYER_MOVING); + + // Enable Field layer collisions + objectFilter.EnableCollision(LAYER_GENERAL_DYNAMIC, LAYER_GENERAL_DYNAMIC); + objectFilter.EnableCollision(LAYER_FIELD, LAYER_GENERAL_DYNAMIC); + for (let i = 0; i < RobotLayers.length; i++) { + objectFilter.EnableCollision(LAYER_FIELD, RobotLayers[i]); + objectFilter.EnableCollision(LAYER_GENERAL_DYNAMIC, RobotLayers[i]); + } + + // Enable Collisions between other robots + for (let i = 0; i < RobotLayers.length - 1; i++) { + for (let j = i + 1; j < RobotLayers.length; j++) { + objectFilter.EnableCollision(RobotLayers[i], RobotLayers[j]); + } + } - const BP_LAYER_NOT_MOVING = new JOLT.BroadPhaseLayer(LAYER_NOT_MOVING); - const BP_LAYER_MOVING = new JOLT.BroadPhaseLayer(LAYER_MOVING); - const COUNT_BROAD_PHASE_LAYERS = 2; + const BP_LAYER_FIELD = new JOLT.BroadPhaseLayer(LAYER_FIELD); + const BP_LAYER_GENERAL_DYNAMIC = new JOLT.BroadPhaseLayer(LAYER_GENERAL_DYNAMIC); + + const bpRobotLayers = new Array(RobotLayers.length); + for (let i = 0; i < bpRobotLayers.length; i++) { + bpRobotLayers[i] = new JOLT.BroadPhaseLayer(RobotLayers[i]); + } + + const COUNT_BROAD_PHASE_LAYERS = 2 + RobotLayers.length; const bpInterface = new JOLT.BroadPhaseLayerInterfaceTable(COUNT_OBJECT_LAYERS, COUNT_BROAD_PHASE_LAYERS); - bpInterface.MapObjectToBroadPhaseLayer(LAYER_NOT_MOVING, BP_LAYER_NOT_MOVING); - bpInterface.MapObjectToBroadPhaseLayer(LAYER_MOVING, BP_LAYER_MOVING); + + bpInterface.MapObjectToBroadPhaseLayer(LAYER_FIELD, BP_LAYER_FIELD); + bpInterface.MapObjectToBroadPhaseLayer(LAYER_GENERAL_DYNAMIC, BP_LAYER_GENERAL_DYNAMIC); + for (let i = 0; i < bpRobotLayers.length; i++) { + bpInterface.MapObjectToBroadPhaseLayer(RobotLayers[i], bpRobotLayers[i]); + } settings.mObjectLayerPairFilter = objectFilter; settings.mBroadPhaseLayerInterface = bpInterface; diff --git a/fission/src/test/PhysicsSystem.test.ts b/fission/src/test/PhysicsSystem.test.ts index 2665ae56a5..0efeb05f34 100644 --- a/fission/src/test/PhysicsSystem.test.ts +++ b/fission/src/test/PhysicsSystem.test.ts @@ -1,5 +1,5 @@ import { test, expect, describe, assert } from 'vitest'; -import PhysicsSystem from '../systems/physics/PhysicsSystem'; +import PhysicsSystem, { LayerReserve } from '../systems/physics/PhysicsSystem'; import { LoadMirabufLocal } from '@/mirabuf/MirabufLoader'; import MirabufParser from '@/mirabuf/MirabufParser'; @@ -64,12 +64,12 @@ describe('Physics Sansity Checks', () => { }); }); -describe('Mirabuf Body Loading', () => { +describe('Mirabuf Physics Loading', () => { test('Body Loading (Dozer)', () => { const assembly = LoadMirabufLocal('./public/test_mira/Dozer_v2.mira'); const parser = new MirabufParser(assembly); const physSystem = new PhysicsSystem(); - const mapping = physSystem.CreateBodiesFromParser(parser); + const mapping = physSystem.CreateBodiesFromParser(parser, new LayerReserve()); expect(mapping.size).toBe(7); }); @@ -78,7 +78,7 @@ describe('Mirabuf Body Loading', () => { const assembly = LoadMirabufLocal('./public/test_mira/Team_2471_(2018)_v7.mira'); const parser = new MirabufParser(assembly); const physSystem = new PhysicsSystem(); - const mapping = physSystem.CreateBodiesFromParser(parser); + const mapping = physSystem.CreateBodiesFromParser(parser, new LayerReserve()); expect(mapping.size).toBe(10); });