StaticBodyWorld.cs

using ClientServer.Shared;
using Unity.Burst;
using Unity.Collections;
using Unity.Collections.LowLevel.Unsafe;
using Unity.Jobs;
using Unity.Mathematics;
using Unity.Physics;
using UnityEngine;

namespace CombatServer
{
    public class StaticBodyWorld
    {
        public long Id;
        public bool IsDynamic;
        public NativeList<StaticPhysicsBody> Bodies;


        private PhysicsWorld PhysicsWorld;
        private NativeQueue<StaticPhysicsBody> BodiesToAdd;
        private NativeHashMap<long, int> BodiesToRemove;
        private NativeQueue<StaticPhysicsBody> TmpBodiesQueue;
        private NativeHashSet<long> BodyIds;


        private bool Rebuild;
        private int CurrentId = 1;

        private JobHandle JobHandle;
        

        public StaticBodyWorld(long id)
        {
            Id = id;
            BodiesToAdd = new NativeQueue<StaticPhysicsBody>(Allocator.Persistent);
            BodiesToRemove = new NativeHashMap<long, int>(128, Allocator.Persistent);
            Bodies = new NativeList<StaticPhysicsBody>(Allocator.Persistent);
            TmpBodiesQueue = new NativeQueue<StaticPhysicsBody>(Allocator.Persistent);
            BodyIds = new NativeHashSet<long>(2, Allocator.Persistent);
            PhysicsWorld = new PhysicsWorld(0, 0, 0);


            // Broadphase rebuild fails with zero bodies
            float3 position = new float3(0f, 0f, 0f);
            var collider = PhysicsHelper.CreateBoxCollider(new float3(1f, 1f, 1f), quaternion.identity, CollisionFilter.Default);
            StaticPhysicsBody body = new StaticPhysicsBody { Position = position, Rotation = quaternion.identity, Collider = collider };
            body.Id = 0;
            BodiesToAdd.Enqueue(body);
        }

        public StaticWorldData WorldData
        {
            get
            {
                StaticWorldData world = new StaticWorldData();
                world.World = PhysicsWorld.CollisionWorld;
                world.Bodies = Bodies;
                return world;
            }
        }

        public void RequestRebuild()
        {
            Rebuild = true;
        }

        public void Complete()
        {
            JobHandle.Complete();
        }

        public JobHandle Combine(JobHandle other)
        {
            return JobHandle.CombineDependencies(JobHandle, other);
        }

        public void Set(JobHandle other)
        {
            JobHandle = other;
        }

        public void PrintStats()
        {
            Debug.LogFormat("WorldId:{0} Bodies:{1} BodiesToAdd:{2} BodiesToRemove:{3}", Id, Bodies.Length, BodiesToAdd.Count, BodiesToRemove.Count());
        }

        public void RemoveAllBodies(bool immedate = false)
        {
            Debug.LogFormat("RemoveAllBodies {0}", Bodies.Length);
            for (int i = 0; i < Bodies.Length; i++)
            {
                StaticPhysicsBody body = Bodies[i];
                if (body.Id > 0)
                {
                    RemoveBody(body.Id);
                }
                
            }

            if (immedate)
            {
                Update();
                Update();
            }
        }

        public bool RemoveBody(long id)
        {
            if (id == 0)
            {
                Debug.LogFormat("RemoveBody: invalid id {0}", id);
                return false;
            }

            if (!BodyIds.Contains(id))
            {
                return false;
            }
            BodyIds.Remove(id);

            BodiesToRemove.TryAdd(id, 1);
            return true;
        }

        public bool AddBody(StaticPhysicsBody body)
        {
            if (BodyIds.Contains(body.Id))
            {
                return false;
            }

            if (body.Id == 0)
            {
                Debug.LogFormat("AddBody: invalid id {0}", body.Id);
                return false;
            }

            body.Id = CurrentId;
            CurrentId++;
            BodiesToAdd.Enqueue(body);
            BodyIds.Add(body.Id);
            return true;
        }

        public void Dispose()
        {
            JobHandle.Complete();
            BodyIds.Dispose();
            if (BodiesToAdd.IsCreated) BodiesToAdd.Dispose();
            if (BodiesToRemove.IsCreated) BodiesToRemove.Dispose();
            if (Bodies.IsCreated) Bodies.Dispose();
            if (TmpBodiesQueue.IsCreated) TmpBodiesQueue.Dispose();
            PhysicsWorld.Dispose();
        }

        public void Update()
        {
            if (Rebuild)
            {
                JobHandle.Complete();
            }

            if (BodiesToRemove.Count() > 0)
            {
                JobHandle.Complete();
                RemoveBodiesJob removeJob = new RemoveBodiesJob
                {
                    BodiesToRemove = BodiesToRemove,
                    Bodies = Bodies,
                    TmpBodiesQueue = TmpBodiesQueue
                };
                JobHandle = removeJob.Schedule(JobHandle);
                Rebuild = true;
                return;
            }

            int bodiesToAddCount = BodiesToAdd.Count;

            if (bodiesToAddCount == 0 && !Rebuild)
            {
                //JobHandle = default;
                return;
            }

            JobHandle.Complete();
            Rebuild = false;

            if (bodiesToAddCount > 0)
            {
                while (BodiesToAdd.TryDequeue(out StaticPhysicsBody body))
                {
                    Bodies.Add(body);
                }
            }

            int numBodies = Bodies.Length;

            PhysicsWorld.Reset(numBodies, 0, 0);

            Debug.Assert(PhysicsWorld.StaticBodies.Length == numBodies);

            UpdateBodiesJob addBodiesJob = new UpdateBodiesJob
            {
                WorldBodies = PhysicsWorld.StaticBodies,
                Bodies = Bodies
            };
            JobHandle = addBodiesJob.Schedule(PhysicsWorld.StaticBodies.Length, 64, JobHandle);


            var haveStaticBodiesChanged = new NativeArray<int>(1, Allocator.TempJob);
            haveStaticBodiesChanged[0] = 1;
            PhysicsStep stepComponent = PhysicsStep.Default;
            float timeStep = 1f / 60f;
            JobHandle = PhysicsWorld.CollisionWorld.ScheduleBuildBroadphaseJobs(ref PhysicsWorld, timeStep, stepComponent.Gravity,
                    haveStaticBodiesChanged, JobHandle, true);

            haveStaticBodiesChanged.Dispose(JobHandle);

            return;
        }


        [BurstCompile]
        struct RemoveBodiesJob : IJob
        {
            public NativeHashMap<long, int> BodiesToRemove;
            public NativeList<StaticPhysicsBody> Bodies;
            public NativeQueue<StaticPhysicsBody> TmpBodiesQueue;

            public void Execute()
            {
                for (int i = 0; i < Bodies.Length; i++)
                {
                    StaticPhysicsBody body = Bodies[i];
                    if (!BodiesToRemove.TryGetValue(body.Id, out int discard))
                    {
                        TmpBodiesQueue.Enqueue(body);
                    }
                }
                BodiesToRemove.Clear();
                Bodies.Clear();

                while (TmpBodiesQueue.TryDequeue(out StaticPhysicsBody body))
                {
                    Bodies.Add(body);
                }

            }
        }

        [BurstCompile]
        struct UpdateBodiesJob : IJobParallelFor
        {
            [NativeDisableContainerSafetyRestriction]
            public NativeSlice<RigidBody> WorldBodies;

            [NativeDisableContainerSafetyRestriction]
            public NativeList<StaticPhysicsBody> Bodies;

            public unsafe void Execute(int index)
            {
                StaticPhysicsBody body = Bodies[index];
                WorldBodies[index] = body.RigidBody;
            }
        }
    }
}