diff --git a/src/CortexEngine.App/Program.cs b/src/CortexEngine.App/Program.cs index 9367a93..9edf2c8 100644 --- a/src/CortexEngine.App/Program.cs +++ b/src/CortexEngine.App/Program.cs @@ -243,16 +243,21 @@ class Program // Physics: create bodies, step, sync transforms if (!cameraTour) { + var toInit = new List<(Entity, RigidBody, Transform)>(); world.Each((Entity e, ref RigidBody rb, ref Transform t) => { if (!rb.IsInitialized) - { - physicsWorld.CreateBody(e, rb, t); - rb.IsInitialized = true; - e.Set(rb); - } + toInit.Add((e, rb, t)); }); + foreach (var (e, rbData, t) in toInit) + { + physicsWorld.CreateBody(e, rbData, t); + var rb = rbData; + rb.IsInitialized = true; + e.Set(rb); + } + physicsWorld.Update((float)timing.DeltaTime); physicsWorld.SyncTransforms(world); } diff --git a/src/Engine.Physics/PhysicsWorld.cs b/src/Engine.Physics/PhysicsWorld.cs index b381de4..f9edb2f 100644 --- a/src/Engine.Physics/PhysicsWorld.cs +++ b/src/Engine.Physics/PhysicsWorld.cs @@ -22,7 +22,10 @@ public sealed class PhysicsWorld : IDisposable private readonly PhysicsSystem _physicsSystem; private readonly BodyInterface _bodyInterface; - private readonly JobSystem _jobSystem; + private readonly JobSystemThreadPool _jobSystem; + private readonly BroadPhaseLayerInterfaceTable _broadPhaseLayerInterface; + private readonly ObjectLayerPairFilterTable _objectLayerPairFilter; + private readonly ObjectVsBroadPhaseLayerFilterTable _objectVsBroadPhaseLayerFilter; private readonly Dictionary _entityToBody = new(); private readonly Dictionary _bodyToEntity = new(); private readonly List _bodies = new(); @@ -38,23 +41,23 @@ public sealed class PhysicsWorld : IDisposable { Foundation.Init(false); - var broadPhaseLayerInterface = new BroadPhaseLayerInterfaceTable(2, 2); - broadPhaseLayerInterface.MapObjectToBroadPhaseLayer(Layers.NonMoving, new BroadPhaseLayer(0)); - broadPhaseLayerInterface.MapObjectToBroadPhaseLayer(Layers.Moving, new BroadPhaseLayer(1)); + _broadPhaseLayerInterface = new BroadPhaseLayerInterfaceTable(2, 2); + _broadPhaseLayerInterface.MapObjectToBroadPhaseLayer(Layers.NonMoving, new BroadPhaseLayer(0)); + _broadPhaseLayerInterface.MapObjectToBroadPhaseLayer(Layers.Moving, new BroadPhaseLayer(1)); - var objectLayerPairFilter = new ObjectLayerPairFilterTable(2); + _objectLayerPairFilter = new ObjectLayerPairFilterTable(2); - var objectVsBroadPhaseLayerFilter = new ObjectVsBroadPhaseLayerFilterTable( - broadPhaseLayerInterface, 2, objectLayerPairFilter, 2); + _objectVsBroadPhaseLayerFilter = new ObjectVsBroadPhaseLayerFilterTable( + _broadPhaseLayerInterface, 2, _objectLayerPairFilter, 2); var settings = new PhysicsSystemSettings { MaxBodies = MaxBodies, MaxBodyPairs = MaxBodyPairs, MaxContactConstraints = MaxContactConstraints, - BroadPhaseLayerInterface = broadPhaseLayerInterface, - ObjectLayerPairFilter = objectLayerPairFilter, - ObjectVsBroadPhaseLayerFilter = objectVsBroadPhaseLayerFilter, + BroadPhaseLayerInterface = _broadPhaseLayerInterface, + ObjectLayerPairFilter = _objectLayerPairFilter, + ObjectVsBroadPhaseLayerFilter = _objectVsBroadPhaseLayerFilter, }; _physicsSystem = new PhysicsSystem(settings); @@ -115,6 +118,8 @@ public sealed class PhysicsWorld : IDisposable public void SyncTransforms(World world) { + // Collect updates first to avoid calling entity.Set() during dictionary iteration + var updates = new List<(Entity entity, Vector3 pos, Quaternion rot)>(); foreach (var (entity, bodyId) in _entityToBody) { if ((ulong)entity.Id == 0) @@ -122,7 +127,12 @@ public sealed class PhysicsWorld : IDisposable var pos = _bodyInterface.GetPosition(bodyId); var rot = _bodyInterface.GetRotation(bodyId); + updates.Add((entity, pos, rot)); + } + // Apply updates outside the iteration + foreach (var (entity, pos, rot) in updates) + { if (entity.Has()) { var t = entity.Get(); @@ -172,6 +182,9 @@ public sealed class PhysicsWorld : IDisposable _jobSystem.Dispose(); _physicsSystem.Dispose(); + _objectVsBroadPhaseLayerFilter.Dispose(); + _objectLayerPairFilter.Dispose(); + _broadPhaseLayerInterface.Dispose(); Foundation.Shutdown(); } }