修改水

Assets/Obi/Scripts/Common/Backends/Burst/Collisions/BurstBox.cs

@@ -1,15 +1,14 @@
 #if (OBI_BURST && OBI_MATHEMATICS && OBI_COLLECTIONS)
-using Unity.Burst;
 using Unity.Collections;
-using Unity.Jobs;
 using Unity.Mathematics;
 
 namespace Obi
 {
-    public struct BurstBox : BurstLocalOptimization.IDistanceFunction
+    public struct BurstBox : BurstLocalOptimization.IDistanceFunction, IBurstCollider
     {
         public BurstColliderShape shape;
         public BurstAffineTransform colliderToSolver;
+        public float dt;
 
         public void Evaluate(float4 point, float4 radii, quaternion orientation, ref BurstLocalOptimization.SurfacePoint projectedPoint)
         {
@@ -19,7 +18,7 @@ namespace Obi
             // clamp the point to the surface of the box:
             point = colliderToSolver.InverseTransformPointUnscaled(point) - center;
 
-            if (shape.is2D)
+            if (shape.is2D != 0)
                 point[2] = 0;
 
             // get minimum distance for each axis:
@@ -56,100 +55,39 @@ namespace Obi
             projectedPoint.normal = colliderToSolver.TransformDirection(projectedPoint.normal);
         }
 
-        public static JobHandle GenerateContacts(ObiColliderWorld world,
-                                                BurstSolverImpl solver,
-                                                NativeList<Oni.ContactPair> contactPairs,
-                                                NativeQueue<BurstContact> contactQueue,
-                                                NativeArray<int> contactOffsetsPerType,
-                                                float deltaTime,
-                                                JobHandle inputDeps)
+        public void Contacts(int colliderIndex,
+                             int rigidbodyIndex,
+                             NativeArray<BurstRigidbody> rigidbodies,
+
+                              NativeArray<float4> positions,
+                              NativeArray<quaternion> orientations,
+                              NativeArray<float4> velocities,
+                              NativeArray<float4> radii,
+
+                              NativeArray<int> simplices,
+                              in BurstAabb simplexBounds,
+                              int simplexIndex,
+                              int simplexStart,
+                              int simplexSize,
+
+                              NativeQueue<BurstContact>.ParallelWriter contacts,
+                              int optimizationIterations,
+                              float optimizationTolerance)
         {
-            int pairCount = contactOffsetsPerType[(int)Oni.ShapeType.Box + 1] - contactOffsetsPerType[(int)Oni.ShapeType.Box];
-            if (pairCount == 0) return inputDeps;
-
-            var job = new GenerateBoxContactsJob
-            {
-                contactPairs = contactPairs,
-
-                positions = solver.positions,
-                orientations = solver.orientations,
-                velocities = solver.velocities,
-                invMasses = solver.invMasses,
-                radii = solver.principalRadii,
-
-                simplices = solver.simplices,
-                simplexCounts = solver.simplexCounts,
-
-                transforms = world.colliderTransforms.AsNativeArray<BurstAffineTransform>(),
-                shapes = world.colliderShapes.AsNativeArray<BurstColliderShape>(),
-
-                contactsQueue = contactQueue.AsParallelWriter(),
-
-                worldToSolver = solver.worldToSolver,
-                deltaTime = deltaTime,
-                parameters = solver.abstraction.parameters,
-                firstPair = contactOffsetsPerType[(int)Oni.ShapeType.Box]
-            };
-
-            inputDeps = job.Schedule(pairCount, 8, inputDeps);
-            return inputDeps;
-        }
-    }
-
-    [BurstCompile]
-    struct GenerateBoxContactsJob : IJobParallelFor
-    {
-        [ReadOnly] public NativeList<Oni.ContactPair> contactPairs;
-
-        // particle arrays:
-        [ReadOnly] public NativeArray<float4> velocities;
-        [ReadOnly] public NativeArray<float4> positions;
-        [ReadOnly] public NativeArray<quaternion> orientations;
-        [ReadOnly] public NativeArray<float> invMasses;
-        [ReadOnly] public NativeArray<float4> radii;
-
-        // simplex arrays:
-        [ReadOnly] public NativeArray<int> simplices;
-        [ReadOnly] public SimplexCounts simplexCounts;
-
-        // collider arrays:
-        [ReadOnly] public NativeArray<BurstAffineTransform> transforms;
-        [ReadOnly] public NativeArray<BurstColliderShape> shapes;
-
-        [WriteOnly]
-        [NativeDisableParallelForRestriction]
-        public NativeQueue<BurstContact>.ParallelWriter contactsQueue;
-
-        // auxiliar data:
-        [ReadOnly] public int firstPair;
-        [ReadOnly] public BurstAffineTransform worldToSolver;
-        [ReadOnly] public float deltaTime;
-        [ReadOnly] public Oni.SolverParameters parameters;
-
-        public void Execute(int i)
-        {
-            int simplexIndex = contactPairs[firstPair + i].bodyA;
-            int colliderIndex = contactPairs[firstPair + i].bodyB;
-
-            int simplexStart = simplexCounts.GetSimplexStartAndSize(simplexIndex, out int simplexSize);
-
-            BurstAffineTransform colliderToSolver = worldToSolver * transforms[colliderIndex];
-
-            BurstBox shape = new BurstBox { colliderToSolver = colliderToSolver, shape = shapes[colliderIndex] };
-
+            var co = new BurstContact() { bodyA = simplexIndex, bodyB = colliderIndex };
             float4 simplexBary = BurstMath.BarycenterForSimplexOfSize(simplexSize);
-            var colliderPoint = BurstLocalOptimization.Optimize(ref shape, positions, orientations, radii, simplices, simplexStart, simplexSize,
-                                                                ref simplexBary, out _, parameters.surfaceCollisionIterations, parameters.surfaceCollisionTolerance);
 
-            contactsQueue.Enqueue(new BurstContact
-            {
-                bodyA = simplexIndex,
-                bodyB = colliderIndex,
-                pointA = simplexBary,
-                pointB = colliderPoint.point,
-                normal = colliderPoint.normal * shape.shape.sign
-            });
+            var colliderPoint = BurstLocalOptimization.Optimize<BurstBox>(ref this, positions, orientations, radii, simplices, simplexStart, simplexSize,
+                                                        ref simplexBary, out float4 convexPoint, optimizationIterations, optimizationTolerance);
+
+            co.pointB = colliderPoint.point;
+            co.normal = colliderPoint.normal;
+            co.pointA = simplexBary;
+
+            contacts.Enqueue(co);
         }
+
     }
+
 }
 #endif

Assets/Obi/Scripts/Common/Backends/Burst/Collisions/BurstCapsule.cs

@@ -1,22 +1,21 @@
 #if (OBI_BURST && OBI_MATHEMATICS && OBI_COLLECTIONS)
-using Unity.Burst;
 using Unity.Collections;
-using Unity.Jobs;
 using Unity.Mathematics;
 
 namespace Obi
 {
-    public struct BurstCapsule : BurstLocalOptimization.IDistanceFunction
+    public struct BurstCapsule : BurstLocalOptimization.IDistanceFunction, IBurstCollider
     {
         public BurstColliderShape shape;
         public BurstAffineTransform colliderToSolver;
+        public float dt;
 
         public void Evaluate(float4 point, float4 radii, quaternion orientation, ref BurstLocalOptimization.SurfacePoint projectedPoint)
         {
             float4 center = shape.center * colliderToSolver.scale;
             point = colliderToSolver.InverseTransformPointUnscaled(point) - center;
 
-            if (shape.is2D)
+            if (shape.is2D != 0)
                 point[2] = 0;
 
             int direction = (int)shape.size.z;
@@ -37,100 +36,38 @@ namespace Obi
             projectedPoint.normal = colliderToSolver.TransformDirection(normal);
         }
 
-        public static JobHandle GenerateContacts(ObiColliderWorld world,
-                                                BurstSolverImpl solver,
-                                                NativeList<Oni.ContactPair> contactPairs,
-                                                NativeQueue<BurstContact> contactQueue,
-                                                NativeArray<int> contactOffsetsPerType,
-                                                float deltaTime,
-                                                JobHandle inputDeps)
+        public void Contacts(int colliderIndex,
+                             int rigidbodyIndex,
+                             NativeArray<BurstRigidbody> rigidbodies,
+
+                              NativeArray<float4> positions,
+                              NativeArray<quaternion> orientations,
+                              NativeArray<float4> velocities,
+                              NativeArray<float4> radii,
+
+                              NativeArray<int> simplices,
+                              in BurstAabb simplexBounds,
+                              int simplexIndex,
+                              int simplexStart,
+                              int simplexSize,
+
+                              NativeQueue<BurstContact>.ParallelWriter contacts,
+                              int optimizationIterations,
+                              float optimizationTolerance)
         {
-            int pairCount = contactOffsetsPerType[(int)Oni.ShapeType.Capsule + 1] - contactOffsetsPerType[(int)Oni.ShapeType.Capsule];
-            if (pairCount == 0) return inputDeps;
-
-            var job = new GenerateCapsuleContactsJob
-            {
-                contactPairs = contactPairs,
-
-                positions = solver.positions,
-                orientations = solver.orientations,
-                velocities = solver.velocities,
-                invMasses = solver.invMasses,
-                radii = solver.principalRadii,
-
-                simplices = solver.simplices,
-                simplexCounts = solver.simplexCounts,
-
-                transforms = world.colliderTransforms.AsNativeArray<BurstAffineTransform>(),
-                shapes = world.colliderShapes.AsNativeArray<BurstColliderShape>(),
-
-                contactsQueue = contactQueue.AsParallelWriter(),
-
-                worldToSolver = solver.worldToSolver,
-                deltaTime = deltaTime,
-                parameters = solver.abstraction.parameters,
-                firstPair = contactOffsetsPerType[(int)Oni.ShapeType.Capsule]
-            };
-
-            inputDeps = job.Schedule(pairCount, 8, inputDeps);
-            return inputDeps;
-        }
-    }
-
-    [BurstCompile]
-    struct GenerateCapsuleContactsJob : IJobParallelFor
-    {
-        [ReadOnly] public NativeList<Oni.ContactPair> contactPairs;
-
-        // particle arrays:
-        [ReadOnly] public NativeArray<float4> velocities;
-        [ReadOnly] public NativeArray<float4> positions;
-        [ReadOnly] public NativeArray<quaternion> orientations;
-        [ReadOnly] public NativeArray<float> invMasses;
-        [ReadOnly] public NativeArray<float4> radii;
-
-        // simplex arrays:
-        [ReadOnly] public NativeArray<int> simplices;
-        [ReadOnly] public SimplexCounts simplexCounts;
-
-        // collider arrays:
-        [ReadOnly] public NativeArray<BurstAffineTransform> transforms;
-        [ReadOnly] public NativeArray<BurstColliderShape> shapes;
-
-        [WriteOnly]
-        [NativeDisableParallelForRestriction]
-        public NativeQueue<BurstContact>.ParallelWriter contactsQueue;
-
-        // auxiliar data:
-        [ReadOnly] public int firstPair;
-        [ReadOnly] public BurstAffineTransform worldToSolver;
-        [ReadOnly] public float deltaTime;
-        [ReadOnly] public Oni.SolverParameters parameters;
-
-        public void Execute(int i)
-        {
-            int simplexIndex = contactPairs[firstPair + i].bodyA;
-            int colliderIndex = contactPairs[firstPair + i].bodyB;
-
-            int simplexStart = simplexCounts.GetSimplexStartAndSize(simplexIndex, out int simplexSize);
-
-            BurstAffineTransform colliderToSolver = worldToSolver * transforms[colliderIndex];
-
-            BurstCapsule shape = new BurstCapsule { colliderToSolver = colliderToSolver, shape = shapes[colliderIndex] };
-
+            var co = new BurstContact() { bodyA = simplexIndex, bodyB = colliderIndex };
             float4 simplexBary = BurstMath.BarycenterForSimplexOfSize(simplexSize);
-            var colliderPoint = BurstLocalOptimization.Optimize(ref shape, positions, orientations, radii, simplices, simplexStart, simplexSize,
-                                                                ref simplexBary, out _, parameters.surfaceCollisionIterations, parameters.surfaceCollisionTolerance);
 
-            contactsQueue.Enqueue(new BurstContact
-            {
-                bodyA = simplexIndex,
-                bodyB = colliderIndex,
-                pointA = simplexBary,
-                pointB = colliderPoint.point,
-                normal = colliderPoint.normal * shape.shape.sign
-            });
+            var colliderPoint = BurstLocalOptimization.Optimize<BurstCapsule>(ref this, positions, orientations, radii, simplices, simplexStart, simplexSize,
+                                                                              ref simplexBary, out float4 convexPoint, optimizationIterations, optimizationTolerance);
+
+            co.pointB = colliderPoint.point;
+            co.normal = colliderPoint.normal;
+            co.pointA = simplexBary;
+
+            contacts.Enqueue(co);
         }
     }
+
 }
 #endif

Assets/Obi/Scripts/Common/Backends/Burst/Collisions/BurstColliderShape.cs

@@ -14,28 +14,12 @@ namespace Obi
 
         public ColliderShape.ShapeType type;
         public float contactOffset;
-        public int dataIndex;       // index of the associated collider data in the collision world.
+        public int dataIndex;
         public int rigidbodyIndex;  // index of the associated rigidbody in the collision world.
         public int materialIndex;   // index of the associated material in the collision world.
-        public int forceZoneIndex;  // index of the associated force zone in the collision world.
         public int filter;
-        public int flags;           // first bit whether the collider is 2D (1) or 3D (0), second bit whether it's a trigger (1) or regular collider (0),
-                                    // third bit determines whether shape is inverted or not.
-
-        public bool is2D
-        {
-            get => (flags & 1) != 0;
-            set => flags |= value ? 1 : 0;
-        }
-        public bool isTrigger
-        {
-            get => (flags & 1 << 1) != 0 || forceZoneIndex >= 0;
-            set => flags |= value ? 1 << 1 : 0;
-        }
-        public float sign
-        {
-            get => (flags & 1 << 2) != 0 ? -1 : 1;
-        }
+        public int flags;           // for now, only used for trigger (1) or regular collider (0).
+        public int is2D;            // whether the collider is 2D (1) or 3D (0).
     }
 }
 #endif

Assets/Obi/Scripts/Common/Backends/Burst/Collisions/BurstColliderWorld.cs

@@ -4,8 +4,6 @@ using Unity.Collections;
 using Unity.Jobs;
 using Unity.Mathematics;
 using Unity.Burst;
-using System.Threading;
-using Unity.Collections.LowLevel.Unsafe;
 
 namespace Obi
 {
@@ -20,32 +18,24 @@ namespace Obi
             public int entity;
         }
 
-        public int referenceCount { get; private set; } = 0;
-        public int colliderCount { get; private set; } = 0;
+        private int refCount = 0;
+        private int colliderCount = 0;
 
         private NativeMultilevelGrid<int> grid;
         private NativeQueue<MovingCollider> movingColliders;
-
-        private NativeArray<int> colliderTypeCounts;
-        private NativeQueue<Oni.ContactPair> contactPairQueue;
-        public NativeList<Oni.ContactPair> contactPairs;
-        public NativeArray<int> contactOffsetsPerType;
-
         public NativeQueue<BurstContact> colliderContactQueue;
 
         public ObiNativeCellSpanList cellSpans;
 
+        public int referenceCount { get { return refCount; } }
+
         public void Awake()
         {
+
             this.grid = new NativeMultilevelGrid<int>(1000, Allocator.Persistent);
             this.movingColliders = new NativeQueue<MovingCollider>(Allocator.Persistent);
             this.colliderContactQueue = new NativeQueue<BurstContact>(Allocator.Persistent);
 
-            this.contactPairQueue = new NativeQueue<Oni.ContactPair>(Allocator.Persistent);
-            this.colliderTypeCounts = new NativeArray<int>(Oni.ColliderShapeTypeCount, Allocator.Persistent);
-            this.contactOffsetsPerType = new NativeArray<int>(Oni.ColliderShapeTypeCount + 1, Allocator.Persistent);
-            this.contactPairs = new NativeList<Oni.ContactPair>(Allocator.Persistent);
-
             this.cellSpans = new ObiNativeCellSpanList();
 
             ObiColliderWorld.GetInstance().RegisterImplementation(this);
@@ -57,30 +47,23 @@ namespace Obi
 
             grid.Dispose();
             movingColliders.Dispose();
-
-            colliderTypeCounts.Dispose();
-            contactPairQueue.Dispose();
-            contactPairs.Dispose();
-            contactOffsetsPerType.Dispose();
-
             colliderContactQueue.Dispose();
-
             cellSpans.Dispose();
         }
 
         public void IncreaseReferenceCount()
         {
-            referenceCount++;
+            refCount++;
         }
         public void DecreaseReferenceCount()
         {
-            if (--referenceCount <= 0 && gameObject != null)
+            if (--refCount <= 0 && gameObject != null)
                 DestroyImmediate(gameObject);  
         }
 
-        public void SetColliders(ObiNativeColliderShapeList shapes, ObiNativeAabbList bounds, ObiNativeAffineTransformList transforms)
+        public void SetColliders(ObiNativeColliderShapeList shapes, ObiNativeAabbList bounds, ObiNativeAffineTransformList transforms, int count)
         {
-            colliderCount = shapes.count;
+            colliderCount = count;
 
             // insert new empty cellspans at the end if needed:
             while (colliderCount > cellSpans.count)
@@ -91,10 +74,6 @@ namespace Obi
         {
         }
 
-        public void SetForceZones(ObiNativeForceZoneList rigidbody)
-        {
-        }
-
         public void SetCollisionMaterials(ObiNativeCollisionMaterialList materials)
         {
 
@@ -183,7 +162,7 @@ namespace Obi
                                                           new int4(GridHash.Quantize(velocityBounds.max.xyz, cellSize), level));
 
                 // if the collider is 2D, project it to the z = 0 cells.
-                if (shapes[i].is2D)
+                if (shapes[i].is2D != 0)
                 {
                     newSpan.min[2] = 0;
                     newSpan.max[2] = 0;
@@ -194,7 +173,7 @@ namespace Obi
                 if (i >= colliderCount || cellIndices[i] != newSpan)
                 {
                     // Add the collider to the list of moving colliders:
-                    movingColliders.Enqueue(new MovingCollider
+                    movingColliders.Enqueue(new MovingCollider()
                     {
                         oldSpan = cellIndices[i],
                         newSpan = newSpan,
@@ -251,7 +230,6 @@ namespace Obi
             [ReadOnly] public NativeArray<float> invMasses;
             [ReadOnly] public NativeArray<float4> radii;
             [ReadOnly] public NativeArray<int> filters;
-            [ReadOnly] public NativeArray<int> particleMaterialIndices;
 
             // simplex arrays:
             [ReadOnly] public NativeArray<int> simplices;
@@ -265,27 +243,47 @@ namespace Obi
             [ReadOnly] public NativeArray<BurstRigidbody> rigidbodies;
             [ReadOnly] public NativeArray<BurstAabb> bounds;
 
+            // distance field data:
+            [ReadOnly] public NativeArray<DistanceFieldHeader> distanceFieldHeaders;
+            [ReadOnly] public NativeArray<BurstDFNode> distanceFieldNodes;
+
+            // triangle mesh data:
+            [ReadOnly] public NativeArray<TriangleMeshHeader> triangleMeshHeaders;
+            [ReadOnly] public NativeArray<BIHNode> bihNodes;
+            [ReadOnly] public NativeArray<Triangle> triangles;
+            [ReadOnly] public NativeArray<float3> vertices;
+
+            // edge mesh data:
+            [ReadOnly] public NativeArray<EdgeMeshHeader> edgeMeshHeaders;
+            [ReadOnly] public NativeArray<BIHNode> edgeBihNodes;
+            [ReadOnly] public NativeArray<Edge> edges;
+            [ReadOnly] public NativeArray<float2> edgeVertices;
+
+            // height field data:
+            [ReadOnly] public NativeArray<HeightFieldHeader> heightFieldHeaders;
+            [ReadOnly] public NativeArray<float> heightFieldSamples;
+
+            // output contacts queue:
             [WriteOnly]
             [NativeDisableParallelForRestriction]
-            public NativeQueue<Oni.ContactPair>.ParallelWriter contactPairQueue;
-
-            [NativeDisableParallelForRestriction]
-            public NativeArray<int> colliderTypeCounts;
+            public NativeQueue<BurstContact>.ParallelWriter contactsQueue;
 
             // auxiliar data:
             [ReadOnly] public BurstAffineTransform solverToWorld;
+            [ReadOnly] public BurstAffineTransform worldToSolver;
             [ReadOnly] public float deltaTime;
             [ReadOnly] public Oni.SolverParameters parameters;
 
             public void Execute(int i)
             {
                 int simplexStart = simplexCounts.GetSimplexStartAndSize(i, out int simplexSize);
+                BurstAabb simplexBoundsSS = simplexBounds[i];
 
                 // get all colliders overlapped by the cell bounds, in all grid levels:
-                BurstAabb simplexBoundsWS = simplexBounds[i].Transformed(solverToWorld);
+                BurstAabb simplexBoundsWS = simplexBoundsSS.Transformed(solverToWorld);
                 NativeList<int> candidates = new NativeList<int>(16,Allocator.Temp);
 
-                // max size of the simplex bounds in cells:
+                // max size of the particle bounds in cells:
                 int3 maxSize = new int3(10);
                 bool is2D = parameters.mode == Oni.SolverParameters.Mode.Mode2D;
 
@@ -362,203 +360,139 @@ namespace Obi
 
                             if (shouldCollide && simplexBoundsWS.IntersectsAabb(in colliderBoundsWS, is2D))
                             {
-                                // increment the amount of contacts for this shape type:
-                                Interlocked.Increment(ref ((int*)colliderTypeCounts.GetUnsafePtr())[(int)shape.type]);
-
-                                // enqueue a new contact pair:
-                                contactPairQueue.Enqueue(new Oni.ContactPair{
-                                    bodyA = i,
-                                    bodyB = c
-                                });
+                                // generate contacts for the collider:
+                                BurstAffineTransform colliderToSolver = worldToSolver * transforms[c];
+                                GenerateContacts(in shape, in colliderToSolver, c, rb, i, simplexStart, simplexSize, simplexBoundsSS);
                             }
                         }
                     }
                 }
             }
-        }
 
-        [BurstCompile]
-        struct PrefixSumJob : IJob
-        {
-            [ReadOnly] public NativeArray<int> array;
-            public NativeArray<int> sum;
-
-            public void Execute()
+            private void GenerateContacts(in BurstColliderShape shape,
+                                          in BurstAffineTransform colliderToSolver,
+                                          int colliderIndex,
+                                          int rigidbodyIndex,
+                                          int simplexIndex,
+                                          int simplexStart,
+                                          int simplexSize,
+                                          in BurstAabb simplexBoundsSS)
             {
-                sum[0] = 0;
-                for (int i = 1; i < sum.Length; ++i)
-                    sum[i] = sum[i - 1] + array[i-1];
-            }
-        }
+                float4x4 solverToCollider;
+                BurstAabb simplexBoundsCS;
 
-        [BurstCompile]
-        struct SortContactPairsByShape : IJob
-        {
-            public NativeQueue<Oni.ContactPair> contactPairQueue;
-            [ReadOnly] public NativeArray<BurstColliderShape> shapes;
-            [ReadOnly] public NativeArray<int> start; // prefix sum
-            public NativeArray<int> count;
-
-            public NativeList<Oni.ContactPair> contactPairs;
-
-            public void Execute()
-            {
-                contactPairs.ResizeUninitialized(contactPairQueue.Count);
-
-                while (!contactPairQueue.IsEmpty())
+                switch (shape.type)
                 {
-                    var pair = contactPairQueue.Dequeue();
-                    int shapeType = (int)shapes[pair.bodyB].type;
+                    case ColliderShape.ShapeType.Sphere:
+                        BurstSphere sphereShape = new BurstSphere() { colliderToSolver = colliderToSolver, shape = shape, dt = deltaTime };
+                        sphereShape.Contacts(colliderIndex, rigidbodyIndex, rigidbodies, positions, orientations, velocities, radii, simplices, in simplexBoundsSS,
+                                             simplexIndex, simplexStart, simplexSize, contactsQueue, parameters.surfaceCollisionIterations, parameters.surfaceCollisionTolerance);
+                        break;
+                    case ColliderShape.ShapeType.Box:
+                        BurstBox boxShape = new BurstBox() { colliderToSolver = colliderToSolver, shape = shape, dt = deltaTime };
+                        boxShape.Contacts(colliderIndex, rigidbodyIndex, rigidbodies, positions, orientations, velocities, radii, simplices, in simplexBoundsSS,
+                                          simplexIndex, simplexStart, simplexSize, contactsQueue, parameters.surfaceCollisionIterations, parameters.surfaceCollisionTolerance);
+                        break;
+                    case ColliderShape.ShapeType.Capsule:
+                        BurstCapsule capsuleShape = new BurstCapsule(){colliderToSolver = colliderToSolver,shape = shape, dt = deltaTime };
+                        capsuleShape.Contacts(colliderIndex, rigidbodyIndex, rigidbodies, positions, orientations, velocities, radii, simplices, in simplexBoundsSS,
+                                              simplexIndex, simplexStart, simplexSize, contactsQueue, parameters.surfaceCollisionIterations, parameters.surfaceCollisionTolerance);
+                        break;
+                    case ColliderShape.ShapeType.SignedDistanceField:
 
-                    // write the pair directly at its position in the sorted array:
-                    contactPairs[start[shapeType] + (--count[shapeType])] = pair;
+                        if (shape.dataIndex < 0) return;
+
+                        BurstDistanceField distanceFieldShape = new BurstDistanceField()
+                        {
+                            colliderToSolver = colliderToSolver,
+                            solverToWorld = solverToWorld,
+                            shape = shape,
+                            distanceFieldHeaders = distanceFieldHeaders,
+                            dfNodes = distanceFieldNodes,
+                            dt = deltaTime,
+                            collisionMargin = parameters.collisionMargin
+                        };
+
+                        distanceFieldShape.Contacts(colliderIndex, rigidbodyIndex, rigidbodies, positions, orientations, velocities, radii, simplices, in simplexBoundsSS,
+                                                    simplexIndex, simplexStart, simplexSize, contactsQueue, parameters.surfaceCollisionIterations, parameters.surfaceCollisionTolerance);
+
+                        break;
+                    case ColliderShape.ShapeType.Heightmap:
+
+                        if (shape.dataIndex < 0) return;
+
+                        // invert a full matrix here to accurately represent collider bounds scale.
+                        solverToCollider = math.inverse(float4x4.TRS(colliderToSolver.translation.xyz, colliderToSolver.rotation, colliderToSolver.scale.xyz));
+                        simplexBoundsCS = simplexBoundsSS.Transformed(solverToCollider);
+
+                        BurstHeightField heightmapShape = new BurstHeightField()
+                        {
+                            colliderToSolver = colliderToSolver,
+                            solverToWorld = solverToWorld,
+                            shape = shape,
+                            header = heightFieldHeaders[shape.dataIndex],
+                            heightFieldSamples = heightFieldSamples,
+                            collisionMargin = parameters.collisionMargin,
+                            dt = deltaTime
+                        };
+
+                        heightmapShape.Contacts(colliderIndex, rigidbodyIndex, rigidbodies, positions, orientations, velocities, radii, simplices, in simplexBoundsCS,
+                                                    simplexIndex, simplexStart, simplexSize, contactsQueue, parameters.surfaceCollisionIterations, parameters.surfaceCollisionTolerance);
+
+                        break;
+                    case ColliderShape.ShapeType.TriangleMesh:
+
+                        if (shape.dataIndex < 0) return;
+
+                        // invert a full matrix here to accurately represent collider bounds scale.
+                        solverToCollider = math.inverse(float4x4.TRS(colliderToSolver.translation.xyz, colliderToSolver.rotation, colliderToSolver.scale.xyz));
+                        simplexBoundsCS = simplexBoundsSS.Transformed(solverToCollider);
+
+                        BurstTriangleMesh triangleMeshShape = new BurstTriangleMesh()
+                        {
+                            colliderToSolver = colliderToSolver,
+                            solverToWorld = solverToWorld,
+                            shape = shape,
+                            header = triangleMeshHeaders[shape.dataIndex],
+                            bihNodes = bihNodes,
+                            triangles = triangles,
+                            vertices = vertices,
+                            collisionMargin = parameters.collisionMargin,
+                            dt = deltaTime
+                        };
+
+                        triangleMeshShape.Contacts(colliderIndex, rigidbodyIndex, rigidbodies, positions, orientations, velocities, radii, simplices, in simplexBoundsCS,
+                                                    simplexIndex, simplexStart, simplexSize, contactsQueue, parameters.surfaceCollisionIterations, parameters.surfaceCollisionTolerance);
+
+                        break;
+                    case ColliderShape.ShapeType.EdgeMesh:
+
+                        if (shape.dataIndex < 0) return;
+
+                        // invert a full matrix here to accurately represent collider bounds scale.
+                        solverToCollider = math.inverse(float4x4.TRS(colliderToSolver.translation.xyz, colliderToSolver.rotation, colliderToSolver.scale.xyz));
+                        simplexBoundsCS = simplexBoundsSS.Transformed(solverToCollider);
+
+                        BurstEdgeMesh edgeMeshShape = new BurstEdgeMesh()
+                        {
+                            colliderToSolver = colliderToSolver,
+                            shape = shape,
+                            header = edgeMeshHeaders[shape.dataIndex],
+                            edgeBihNodes = edgeBihNodes,
+                            edges = edges,
+                            vertices = edgeVertices,
+                            dt = deltaTime
+                        };
+
+                        edgeMeshShape.Contacts(colliderIndex, rigidbodyIndex, rigidbodies, positions, orientations, velocities, radii, simplices, in simplexBoundsCS,
+                                                    simplexIndex, simplexStart, simplexSize, contactsQueue, parameters.surfaceCollisionIterations, parameters.surfaceCollisionTolerance);
+
+                        break;
                 }
             }
+
         }
 
-        [BurstCompile]
-        unsafe struct ApplyForceZonesJob : IJobParallelFor
-        {
-            // particle arrays:
-            [NativeDisableParallelForRestriction] public NativeArray<float4> externalForces;
-            [NativeDisableParallelForRestriction] public NativeArray<float4> wind;
-            [NativeDisableParallelForRestriction] public NativeArray<float4> velocities;
-            [NativeDisableParallelForRestriction] public NativeArray<float4> colors;
-            [NativeDisableParallelForRestriction] public NativeArray<float> life;
-            [ReadOnly] public NativeArray<float4> positions;
-            [ReadOnly] public NativeArray<float> invMasses;
-
-            // simplex arrays:
-            [ReadOnly] public NativeArray<int> simplices;
-            [ReadOnly] public SimplexCounts simplexCounts;
-
-            // collider arrays:
-            [ReadOnly] public NativeArray<BurstAffineTransform> transforms;
-            [ReadOnly] public NativeArray<BurstColliderShape> shapes;
-            [ReadOnly] public NativeArray<ForceZone> forceZones;
-
-            // contacts
-            [ReadOnly] public NativeArray<BurstContact> contacts;
-
-            // auxiliar data:
-            [ReadOnly] public BurstAffineTransform worldToSolver;
-            [ReadOnly] public float deltaTime;
-
-            public void Execute(int i)
-            {
-                var contact = contacts[i];
-                int forceZoneIndex = shapes[contact.bodyB].forceZoneIndex;
-
-                if (forceZoneIndex >= 0)
-                {
-                    int simplexStart = simplexCounts.GetSimplexStartAndSize(contact.bodyA, out int simplexSize);
-                    for (int j = 0; j < simplexSize; ++j)
-                    {
-                        int particleIndex = simplices[simplexStart + j];
-
-                        float distance = -math.dot(positions[particleIndex] - contact.pointB, contact.normal);
-                        if (distance < 0) continue;
-
-                        float4 axis = (worldToSolver * transforms[contact.bodyB]).TransformDirection(new float4(0, 0, 1, 0));
-
-                        // calculate falloff region based on min/max distances:
-                        float falloff = 1;
-                        float range = forceZones[forceZoneIndex].maxDistance - forceZones[forceZoneIndex].minDistance;
-                        if (math.abs(range) > BurstMath.epsilon)
-                            falloff = math.pow(math.saturate((distance - forceZones[forceZoneIndex].minDistance) / range), forceZones[forceZoneIndex].falloffPower);
-
-                        float forceIntensity = forceZones[forceZoneIndex].intensity * falloff;
-                        float dampIntensity  = forceZones[forceZoneIndex].damping * falloff;
-
-                        // tint particles:
-                        float mix = math.pow(1 - math.saturate(forceZones[forceZoneIndex].color.a * falloff), deltaTime);
-                        colors[particleIndex] = math.lerp((Vector4)forceZones[forceZoneIndex].color, colors[particleIndex], mix);
-
-                        // calculate force direction, depending on the type of the force field:
-                        float4 result = float4.zero;
-                        switch (forceZones[forceZoneIndex].type)
-                        {
-                            case ForceZone.ZoneType.Radial:
-                                    result = contact.normal * forceIntensity;
-                                break;
-                            case ForceZone.ZoneType.Vortex:
-                                    result = new float4(math.cross(axis.xyz * forceIntensity, contact.normal.xyz).xyz, 0);
-                                break;
-                            case ForceZone.ZoneType.Directional:
-                                    result = axis * forceIntensity;
-                                break;
-                            default:
-                                BurstMath.AtomicAdd(life, particleIndex, -forceIntensity * deltaTime);
-                                continue;
-                        }
-
-                        // apply damping:
-                        switch (forceZones[forceZoneIndex].dampingDir)
-                        {
-                            case ForceZone.DampingDirection.ForceDirection:
-                                {
-                                    float4 forceDir = math.normalizesafe(result);
-                                    result -= forceDir * math.dot(velocities[particleIndex], forceDir) * dampIntensity;
-                                }
-                                break;
-                            case ForceZone.DampingDirection.SurfaceDirection:
-                                result -= contact.normal * math.dot(velocities[particleIndex], contact.normal) * dampIntensity;
-                                break;
-                            default:
-                                result -= velocities[particleIndex] * dampIntensity;
-                                break;
-                        }
-
-                        if (invMasses[particleIndex] > 0)
-                        {
-                            switch (forceZones[forceZoneIndex].mode)
-                            {
-                                case ForceZone.ForceMode.Acceleration:
-                                    BurstMath.AtomicAdd(externalForces, particleIndex, result / simplexSize / invMasses[particleIndex]);
-                                    break;
-                                case ForceZone.ForceMode.Force:
-                                    BurstMath.AtomicAdd(externalForces, particleIndex, result / simplexSize);
-                                    break;
-                                case ForceZone.ForceMode.Wind:
-                                    BurstMath.AtomicAdd(wind, particleIndex, result / simplexSize);
-                                    break;
-                            }
-                        }
-                        
-                    }
-                }
-            }
-        }
-
-        public JobHandle ApplyForceZones(BurstSolverImpl solver, float deltaTime, JobHandle inputDeps)
-        {
-            var world = ObiColliderWorld.GetInstance();
-
-            var applyForceFieldsJob = new ApplyForceZonesJob
-            {
-                contacts = solver.abstraction.colliderContacts.AsNativeArray<BurstContact>(),
-
-                positions = solver.positions,
-                velocities = solver.velocities,
-                externalForces = solver.externalForces,
-                wind = solver.wind,
-                invMasses = solver.invMasses,
-                life = solver.life,
-                colors = solver.colors,
-
-                simplices = solver.simplices,
-                simplexCounts = solver.simplexCounts,
-
-                transforms = world.colliderTransforms.AsNativeArray<BurstAffineTransform>(),
-                shapes = world.colliderShapes.AsNativeArray<BurstColliderShape>(),
-                forceZones = world.forceZones.AsNativeArray<ForceZone>(),
-
-                worldToSolver = solver.worldToSolver,
-                deltaTime = deltaTime,
-            };
-
-            return applyForceFieldsJob.Schedule(solver.abstraction.colliderContacts.count, 64, inputDeps);
-        }
 
         public JobHandle GenerateContacts(BurstSolverImpl solver, float deltaTime, JobHandle inputDeps)
         {
@@ -575,7 +509,6 @@ namespace Obi
                 invMasses = solver.invMasses,
                 radii = solver.principalRadii,
                 filters = solver.filters,
-                particleMaterialIndices = solver.collisionMaterials,
 
                 simplices = solver.simplices,
                 simplexCounts = solver.simplexCounts,
@@ -587,46 +520,33 @@ namespace Obi
                 collisionMaterials = world.collisionMaterials.AsNativeArray<BurstCollisionMaterial>(),
                 bounds = world.colliderAabbs.AsNativeArray<BurstAabb>(),
 
-                contactPairQueue = contactPairQueue.AsParallelWriter(),
-                colliderTypeCounts = colliderTypeCounts,
+                distanceFieldHeaders = world.distanceFieldContainer.headers.AsNativeArray<DistanceFieldHeader>(),
+                distanceFieldNodes = world.distanceFieldContainer.dfNodes.AsNativeArray<BurstDFNode>(),
 
+                triangleMeshHeaders = world.triangleMeshContainer.headers.AsNativeArray<TriangleMeshHeader>(),
+                bihNodes = world.triangleMeshContainer.bihNodes.AsNativeArray<BIHNode>(),
+                triangles = world.triangleMeshContainer.triangles.AsNativeArray<Triangle>(),
+                vertices = world.triangleMeshContainer.vertices.AsNativeArray<float3>(),
+
+                edgeMeshHeaders = world.edgeMeshContainer.headers.AsNativeArray<EdgeMeshHeader>(),
+                edgeBihNodes = world.edgeMeshContainer.bihNodes.AsNativeArray<BIHNode>(),
+                edges = world.edgeMeshContainer.edges.AsNativeArray<Edge>(),
+                edgeVertices = world.edgeMeshContainer.vertices.AsNativeArray<float2>(),
+
+                heightFieldHeaders = world.heightFieldContainer.headers.AsNativeArray<HeightFieldHeader>(),
+                heightFieldSamples = world.heightFieldContainer.samples.AsNativeArray<float>(),
+
+                contactsQueue = colliderContactQueue.AsParallelWriter(),
                 solverToWorld = solver.solverToWorld,
+                worldToSolver = solver.worldToSolver,
                 deltaTime = deltaTime,
                 parameters = solver.abstraction.parameters
             };
 
-            inputDeps = generateColliderContactsJob.Schedule(solver.simplexCounts.simplexCount, 16, inputDeps);
+            return generateColliderContactsJob.Schedule(solver.simplexCounts.simplexCount, 16, inputDeps);
 
-            var prefixSumJob = new PrefixSumJob
-            {
-                array = colliderTypeCounts,
-                sum = contactOffsetsPerType
-            };
-            inputDeps = prefixSumJob.Schedule(inputDeps);
-
-            var sortPairsJob = new SortContactPairsByShape
-            {
-                contactPairQueue = contactPairQueue,
-                shapes = world.colliderShapes.AsNativeArray<BurstColliderShape>(),
-                start = contactOffsetsPerType,
-                count = colliderTypeCounts,
-                contactPairs = contactPairs
-            };
-            inputDeps = sortPairsJob.Schedule(inputDeps);
-
-            inputDeps.Complete();
-
-            inputDeps = BurstSphere.GenerateContacts(world,solver,contactPairs,colliderContactQueue,contactOffsetsPerType,deltaTime,inputDeps);
-            inputDeps = BurstBox.GenerateContacts(world,solver,contactPairs,colliderContactQueue, contactOffsetsPerType,deltaTime,inputDeps);
-            inputDeps = BurstCapsule.GenerateContacts(world, solver, contactPairs, colliderContactQueue, contactOffsetsPerType, deltaTime, inputDeps);
-            inputDeps = BurstDistanceField.GenerateContacts(world, solver, contactPairs, colliderContactQueue, contactOffsetsPerType, deltaTime, inputDeps);
-            inputDeps = BurstTriangleMesh.GenerateContacts(world, solver, contactPairs, colliderContactQueue, contactOffsetsPerType, deltaTime, inputDeps);
-            inputDeps = BurstHeightField.GenerateContacts(world, solver, contactPairs, colliderContactQueue, contactOffsetsPerType, deltaTime, inputDeps);
-            inputDeps = BurstEdgeMesh.GenerateContacts(world, solver, contactPairs, colliderContactQueue, contactOffsetsPerType, deltaTime, inputDeps);
-
-            return inputDeps;
         }
 
     }
 }
-#endif
+#endif

Assets/Obi/Scripts/Common/Backends/Burst/Collisions/BurstDistanceField.cs

@@ -1,15 +1,17 @@
 #if (OBI_BURST && OBI_MATHEMATICS && OBI_COLLECTIONS)
-using Unity.Burst;
 using Unity.Collections;
-using Unity.Jobs;
 using Unity.Mathematics;
 
 namespace Obi
 {
-    public struct BurstDistanceField : BurstLocalOptimization.IDistanceFunction
+    public struct BurstDistanceField : BurstLocalOptimization.IDistanceFunction, IBurstCollider
     {
         public BurstColliderShape shape;
         public BurstAffineTransform colliderToSolver;
+        public BurstAffineTransform solverToWorld;
+
+        public float dt;
+        public float collisionMargin;
 
         public NativeArray<DistanceFieldHeader> distanceFieldHeaders;
         public NativeArray<BurstDFNode> dfNodes;
@@ -18,143 +20,47 @@ namespace Obi
         {
             point = colliderToSolver.InverseTransformPoint(point);
 
-            if (shape.is2D)
+            if (shape.is2D != 0)
                 point[2] = 0;
 
             var header = distanceFieldHeaders[shape.dataIndex];
-            float4 sample = DFTraverse(point, in header);
+            float4 sample = DFTraverse(point, 0, in header, in dfNodes);
             float4 normal = new float4(math.normalize(sample.xyz), 0);
 
             projectedPoint.point = colliderToSolver.TransformPoint(point - normal * (sample[3] - shape.contactOffset));
             projectedPoint.normal = colliderToSolver.TransformDirection(normal);
         }
 
-        private float4 DFTraverse(float4 particlePosition, in DistanceFieldHeader header)
+        public void Contacts(int colliderIndex,
+                             int rigidbodyIndex,
+                             NativeArray<BurstRigidbody> rigidbodies,
+
+                              NativeArray<float4> positions,
+                              NativeArray<quaternion> orientations,
+                              NativeArray<float4> velocities,
+                              NativeArray<float4> radii,
+
+                              NativeArray<int> simplices,
+                              in BurstAabb simplexBounds,
+                              int simplexIndex,
+                              int simplexStart,
+                              int simplexSize,
+
+                              NativeQueue<BurstContact>.ParallelWriter contacts,
+                              int optimizationIterations,
+                              float optimizationTolerance)
         {
-            var stack = new NativeArray<int>(12, Allocator.Temp);
-            int stackTop = 0;
-
-            stack[stackTop++] = 0;
-
-            while (stackTop > 0)
-            {
-                int nodeIndex = stack[--stackTop];
-                var node = dfNodes[header.firstNode + nodeIndex];
-
-                // if the child node exists, recurse down the df octree:
-                if (node.firstChild >= 0)
-                    stack[stackTop++] = node.firstChild + node.GetOctant(particlePosition);
-                else
-                    return node.SampleWithGradient(particlePosition);
-            }
-            return float4.zero;
-        }
-
-        public static JobHandle GenerateContacts(ObiColliderWorld world,
-                                               BurstSolverImpl solver,
-                                               NativeList<Oni.ContactPair> contactPairs,
-                                               NativeQueue<BurstContact> contactQueue,
-                                               NativeArray<int> contactOffsetsPerType,
-                                               float deltaTime,
-                                               JobHandle inputDeps)
-        {
-            int pairCount = contactOffsetsPerType[(int)Oni.ShapeType.SignedDistanceField + 1] - contactOffsetsPerType[(int)Oni.ShapeType.SignedDistanceField];
-            if (pairCount == 0) return inputDeps;
-
-            var job = new GenerateDistanceFieldContactsJob
-            {
-                contactPairs = contactPairs,
-
-                positions = solver.positions,
-                orientations = solver.orientations,
-                velocities = solver.velocities,
-                invMasses = solver.invMasses,
-                radii = solver.principalRadii,
-
-                simplices = solver.simplices,
-                simplexCounts = solver.simplexCounts,
-
-                transforms = world.colliderTransforms.AsNativeArray<BurstAffineTransform>(),
-                shapes = world.colliderShapes.AsNativeArray<BurstColliderShape>(),
-                rigidbodies = world.rigidbodies.AsNativeArray<BurstRigidbody>(),
-
-                distanceFieldHeaders = world.distanceFieldContainer.headers.AsNativeArray<DistanceFieldHeader>(),
-                distanceFieldNodes = world.distanceFieldContainer.dfNodes.AsNativeArray<BurstDFNode>(),
-
-                contactsQueue = contactQueue.AsParallelWriter(),
-
-                solverToWorld = solver.inertialFrame,
-                worldToSolver = solver.worldToSolver,
-                deltaTime = deltaTime,
-                parameters = solver.abstraction.parameters,
-                firstPair = contactOffsetsPerType[(int)Oni.ShapeType.SignedDistanceField]
-            };
-
-            inputDeps = job.Schedule(pairCount, 1, inputDeps);
-            return inputDeps;
-        }
-    }
-
-    [BurstCompile]
-    struct GenerateDistanceFieldContactsJob : IJobParallelFor
-    {
-        [ReadOnly] public NativeList<Oni.ContactPair> contactPairs;
-
-        // particle arrays:
-        [ReadOnly] public NativeArray<float4> velocities;
-        [ReadOnly] public NativeArray<float4> positions;
-        [ReadOnly] public NativeArray<quaternion> orientations;
-        [ReadOnly] public NativeArray<float> invMasses;
-        [ReadOnly] public NativeArray<float4> radii;
-
-        // simplex arrays:
-        [ReadOnly] public NativeArray<int> simplices;
-        [ReadOnly] public SimplexCounts simplexCounts;
-
-        // collider arrays:
-        [ReadOnly] public NativeArray<BurstAffineTransform> transforms;
-        [ReadOnly] public NativeArray<BurstColliderShape> shapes;
-        [ReadOnly] public NativeArray<BurstRigidbody> rigidbodies;
-
-        // distance field data:
-        [ReadOnly] public NativeArray<DistanceFieldHeader> distanceFieldHeaders;
-        [ReadOnly] public NativeArray<BurstDFNode> distanceFieldNodes;
-
-        [WriteOnly]
-        [NativeDisableParallelForRestriction]
-        public NativeQueue<BurstContact>.ParallelWriter contactsQueue;
-
-        // auxiliar data:
-        [ReadOnly] public int firstPair;
-        [ReadOnly] public BurstInertialFrame solverToWorld;
-        [ReadOnly] public BurstAffineTransform worldToSolver;
-        [ReadOnly] public float deltaTime;
-        [ReadOnly] public Oni.SolverParameters parameters;
-
-        public void Execute(int i)
-        {
-
-            int simplexIndex = contactPairs[firstPair + i].bodyA;
-            int colliderIndex = contactPairs[firstPair + i].bodyB;
-            int rigidbodyIndex = shapes[colliderIndex].rigidbodyIndex;
-
-            if (shapes[colliderIndex].dataIndex < 0) return;
-
-            int simplexStart = simplexCounts.GetSimplexStartAndSize(simplexIndex, out int simplexSize);
-            BurstAffineTransform colliderToSolver = worldToSolver * transforms[colliderIndex];
-
-            BurstDistanceField dfShape = new BurstDistanceField()
-            {
-                colliderToSolver = colliderToSolver,
-                shape = shapes[colliderIndex],
-                distanceFieldHeaders = distanceFieldHeaders,
-                dfNodes = distanceFieldNodes
-            };
-
+            if (shape.dataIndex < 0) return;
 
+            var co = new BurstContact() { bodyA = simplexIndex, bodyB = colliderIndex };
             float4 simplexBary = BurstMath.BarycenterForSimplexOfSize(simplexSize);
-            var colliderPoint = BurstLocalOptimization.Optimize(ref dfShape, positions, orientations, radii, simplices, simplexStart, simplexSize,
-                                                                ref simplexBary, out float4 simplexPoint, parameters.surfaceCollisionIterations, parameters.surfaceCollisionTolerance);
+
+            var colliderPoint = BurstLocalOptimization.Optimize<BurstDistanceField>(ref this, positions, orientations, radii, simplices, simplexStart, simplexSize,
+                                                                ref simplexBary, out float4 simplexPoint, optimizationIterations, optimizationTolerance);
+
+            co.pointB = colliderPoint.point;
+            co.normal = colliderPoint.normal;
+            co.pointA = simplexBary;
 
             float4 velocity = float4.zero;
             float simplexRadius = 0;
@@ -170,18 +76,31 @@ namespace Obi
                 rbVelocity = BurstMath.GetRigidbodyVelocityAtPoint(rigidbodyIndex, colliderPoint.point, rigidbodies, solverToWorld);
 
             float dAB = math.dot(simplexPoint - colliderPoint.point, colliderPoint.normal);
-            float vel = math.dot(velocity - rbVelocity, colliderPoint.normal);
+            float vel = math.dot(velocity     - rbVelocity,          colliderPoint.normal);
 
-            //if (vel * deltaTime + dAB <= simplexRadius + shapes[colliderIndex].contactOffset + parameters.collisionMargin)
-                contactsQueue.Enqueue(new BurstContact
-                {
-                    bodyA = simplexIndex,
-                    bodyB = colliderIndex,
-                    pointA = simplexBary,
-                    pointB = colliderPoint.point,
-                    normal = colliderPoint.normal * dfShape.shape.sign
-                });
+            if (vel * dt + dAB <= simplexRadius + shape.contactOffset + collisionMargin)
+                contacts.Enqueue(co);
         }
+
+        private static float4 DFTraverse(float4 particlePosition,
+                                         int nodeIndex,
+                                         in DistanceFieldHeader header,
+                                         in NativeArray<BurstDFNode> dfNodes)
+        {
+            var node = dfNodes[header.firstNode + nodeIndex];
+
+            // if the child node exists, recurse down the df octree:
+            if (node.firstChild >= 0)
+            {
+                int octant = node.GetOctant(particlePosition);
+                return DFTraverse(particlePosition, node.firstChild + octant, in header, in dfNodes);
+            }
+            else
+            {
+                return node.SampleWithGradient(particlePosition);
+            }
+        }
+
     }
 
 }

Assets/Obi/Scripts/Common/Backends/Burst/Collisions/BurstEdgeMesh.cs

@@ -1,17 +1,16 @@
 #if (OBI_BURST && OBI_MATHEMATICS && OBI_COLLECTIONS)
-using Unity.Burst;
 using Unity.Collections;
-using Unity.Jobs;
 using Unity.Mathematics;
 
 namespace Obi
 {
-    public struct BurstEdgeMesh : BurstLocalOptimization.IDistanceFunction
+    public struct BurstEdgeMesh : BurstLocalOptimization.IDistanceFunction, IBurstCollider
     {
 
         public BurstColliderShape shape;
         public BurstAffineTransform colliderToSolver;
         public int dataOffset;
+        public float dt;
 
         public EdgeMeshHeader header;
         public NativeArray<BIHNode> edgeBihNodes;
@@ -22,12 +21,12 @@ namespace Obi
         {
             point = colliderToSolver.InverseTransformPointUnscaled(point);
 
-            if (shape.is2D)
+            if (shape.is2D != 0)
                 point[2] = 0;
 
             Edge t = edges[header.firstEdge + dataOffset];
-            float4 v1 = (new float4(vertices[header.firstVertex + t.i1], 0, 0) + shape.center) * colliderToSolver.scale;
-            float4 v2 = (new float4(vertices[header.firstVertex + t.i2], 0, 0) + shape.center) * colliderToSolver.scale;
+            float4 v1 = (new float4(vertices[header.firstVertex + t.i1], 0) + shape.center) * colliderToSolver.scale;
+            float4 v2 = (new float4(vertices[header.firstVertex + t.i2], 0) + shape.center) * colliderToSolver.scale;
 
             float4 nearestPoint = BurstMath.NearestPointOnEdge(v1, v2, point, out float mu);
             float4 normal = math.normalizesafe(point - nearestPoint);
@@ -36,175 +35,90 @@ namespace Obi
             projectedPoint.point = colliderToSolver.TransformPointUnscaled(nearestPoint + normal * shape.contactOffset);
         }
 
-        public static JobHandle GenerateContacts(ObiColliderWorld world,
-                                               BurstSolverImpl solver,
-                                               NativeList<Oni.ContactPair> contactPairs,
-                                               NativeQueue<BurstContact> contactQueue,
-                                               NativeArray<int> contactOffsetsPerType,
-                                               float deltaTime,
-                                               JobHandle inputDeps)
+
+        public void Contacts(int colliderIndex,
+                             int rigidbodyIndex,
+                             NativeArray<BurstRigidbody> rigidbodies,
+
+                              NativeArray<float4> positions,
+                              NativeArray<quaternion> orientations,
+                              NativeArray<float4> velocities,
+                              NativeArray<float4> radii,
+
+                              NativeArray<int> simplices,
+                              in BurstAabb simplexBounds,
+                              int simplexIndex,
+                              int simplexStart,
+                              int simplexSize,
+
+                              NativeQueue<BurstContact>.ParallelWriter contacts,
+                              int optimizationIterations,
+                              float optimizationTolerance)
         {
-            int pairCount = contactOffsetsPerType[(int)Oni.ShapeType.EdgeMesh + 1] - contactOffsetsPerType[(int)Oni.ShapeType.EdgeMesh];
-            if (pairCount == 0) return inputDeps;
+            if (shape.dataIndex < 0) return;
 
-            var job = new GenerateEdgeMeshContactsJob
-            {
-                contactPairs = contactPairs,
-
-                positions = solver.positions,
-                orientations = solver.orientations,
-                velocities = solver.velocities,
-                invMasses = solver.invMasses,
-                radii = solver.principalRadii,
-
-                simplices = solver.simplices,
-                simplexCounts = solver.simplexCounts,
-                simplexBounds = solver.simplexBounds,
-
-                transforms = world.colliderTransforms.AsNativeArray<BurstAffineTransform>(),
-                shapes = world.colliderShapes.AsNativeArray<BurstColliderShape>(),
-                rigidbodies = world.rigidbodies.AsNativeArray<BurstRigidbody>(),
-
-                edgeMeshHeaders = world.edgeMeshContainer.headers.AsNativeArray<EdgeMeshHeader>(),
-                edgeBihNodes = world.edgeMeshContainer.bihNodes.AsNativeArray<BIHNode>(),
-                edges = world.edgeMeshContainer.edges.AsNativeArray<Edge>(),
-                edgeVertices = world.edgeMeshContainer.vertices.AsNativeArray<float2>(),
-
-                contactsQueue = contactQueue.AsParallelWriter(),
-
-                solverToWorld = solver.solverToWorld,
-                worldToSolver = solver.worldToSolver,
-                deltaTime = deltaTime,
-                parameters = solver.abstraction.parameters,
-                firstPair = contactOffsetsPerType[(int)Oni.ShapeType.TriangleMesh]
-            };
-
-            inputDeps = job.Schedule(pairCount, 1, inputDeps);
-            return inputDeps;
+            BIHTraverse(colliderIndex, simplexIndex, simplexStart, simplexSize,
+                        positions, orientations, radii, simplices, in simplexBounds, 0, contacts, optimizationIterations, optimizationTolerance);
         }
-    }
 
-    [BurstCompile]
-    struct GenerateEdgeMeshContactsJob : IJobParallelFor
-    {
-        [ReadOnly] public NativeList<Oni.ContactPair> contactPairs;
-
-        // particle arrays:
-        [ReadOnly] public NativeArray<float4> velocities;
-        [ReadOnly] public NativeArray<float4> positions;
-        [ReadOnly] public NativeArray<quaternion> orientations;
-        [ReadOnly] public NativeArray<float> invMasses;
-        [ReadOnly] public NativeArray<float4> radii;
-
-        // simplex arrays:
-        [ReadOnly] public NativeArray<int> simplices;
-        [ReadOnly] public SimplexCounts simplexCounts;
-        [ReadOnly] public NativeArray<BurstAabb> simplexBounds;
-
-        // collider arrays:
-        [ReadOnly] public NativeArray<BurstAffineTransform> transforms;
-        [ReadOnly] public NativeArray<BurstColliderShape> shapes;
-        [ReadOnly] public NativeArray<BurstRigidbody> rigidbodies;
-
-        // edge mesh data:
-        [ReadOnly] public NativeArray<EdgeMeshHeader> edgeMeshHeaders;
-        [ReadOnly] public NativeArray<BIHNode> edgeBihNodes;
-        [ReadOnly] public NativeArray<Edge> edges;
-        [ReadOnly] public NativeArray<float2> edgeVertices;
-
-        [WriteOnly]
-        [NativeDisableParallelForRestriction]
-        public NativeQueue<BurstContact>.ParallelWriter contactsQueue;
-
-        // auxiliar data:
-        [ReadOnly] public int firstPair;
-        [ReadOnly] public BurstAffineTransform solverToWorld;
-        [ReadOnly] public BurstAffineTransform worldToSolver;
-        [ReadOnly] public float deltaTime;
-        [ReadOnly] public Oni.SolverParameters parameters;
-
-        public void Execute(int i)
+        private void BIHTraverse(int colliderIndex,
+                                 int simplexIndex,
+                                 int simplexStart,
+                                 int simplexSize,
+                                 NativeArray<float4> positions,
+                                 NativeArray<quaternion> orientations,
+                                 NativeArray<float4> radii,
+                                 NativeArray<int> simplices,
+                                 in BurstAabb simplexBounds,
+                                 int nodeIndex,
+                                 NativeQueue<BurstContact>.ParallelWriter contacts,
+                                 int optimizationIterations,
+                                 float optimizationTolerance)
         {
-            int simplexIndex = contactPairs[firstPair + i].bodyA;
-            int colliderIndex = contactPairs[firstPair + i].bodyB;
-            var shape = shapes[colliderIndex];
+            var node = edgeBihNodes[header.firstNode + nodeIndex];
 
-            if (shape.dataIndex < 0)
-                return;
-
-            var header = edgeMeshHeaders[shape.dataIndex];
-
-            int simplexStart = simplexCounts.GetSimplexStartAndSize(simplexIndex, out int simplexSize);
-            var simplexBound = simplexBounds[simplexIndex];
-
-            BurstAffineTransform colliderToSolver = worldToSolver * transforms[colliderIndex];
-
-            // invert a full matrix here to accurately represent collider bounds scale.
-            var solverToCollider = math.inverse(float4x4.TRS(colliderToSolver.translation.xyz, colliderToSolver.rotation, colliderToSolver.scale.xyz));
-            var simplexBoundsCS = simplexBound.Transformed(solverToCollider);
-
-            float4 marginCS = new float4((shape.contactOffset + parameters.collisionMargin) / colliderToSolver.scale.xyz, 0);
-
-            BurstEdgeMesh edgeMeshShape = new BurstEdgeMesh()
+            if (node.firstChild >= 0)
             {
-                colliderToSolver = colliderToSolver,
-                shape = shape,
-                header = header,
-                edgeBihNodes = edgeBihNodes,
-                edges = edges,
-                vertices = edgeVertices
-            };
+                // visit min node:
+                if (simplexBounds.min[node.axis] <= node.min + shape.center[node.axis])
+                    BIHTraverse(colliderIndex, simplexIndex, simplexStart, simplexSize,
+                                positions, orientations, radii, simplices, in simplexBounds,
+                                node.firstChild, contacts, optimizationIterations, optimizationTolerance);
 
-            NativeQueue<int> queue = new NativeQueue<int>(Allocator.Temp);
-
-            queue.Enqueue(0);
-
-            while (!queue.IsEmpty())
+                // visit max node:
+                if (simplexBounds.max[node.axis] >= node.max + shape.center[node.axis])
+                    BIHTraverse(colliderIndex, simplexIndex, simplexStart, simplexSize,
+                                positions, orientations, radii, simplices, in simplexBounds,
+                                node.firstChild + 1, contacts, optimizationIterations, optimizationTolerance);
+            }
+            else
             {
-                int nodeIndex = queue.Dequeue();
-                var node = edgeBihNodes[header.firstNode + nodeIndex];
-
-                // leaf node:
-                if (node.firstChild < 0)
+                // check for contact against all triangles:
+                for (dataOffset = node.start; dataOffset < node.start + node.count; ++dataOffset)
                 {
-                    // check for contact against all triangles:
-                    for (int dataOffset = node.start; dataOffset < node.start + node.count; ++dataOffset)
+                    Edge t = edges[header.firstEdge + dataOffset];
+                    float4 v1 = new float4(vertices[header.firstVertex + t.i1], 0) + shape.center;
+                    float4 v2 = new float4(vertices[header.firstVertex + t.i2], 0) + shape.center;
+                    BurstAabb edgeBounds = new BurstAabb(v1, v2, shape.contactOffset + 0.01f);
+
+                    if (edgeBounds.IntersectsAabb(simplexBounds, shape.is2D != 0))
                     {
-                        Edge t = edges[header.firstEdge + dataOffset];
-                        float4 v1 = new float4(edgeVertices[header.firstVertex + t.i1], 0, 0) + shape.center;
-                        float4 v2 = new float4(edgeVertices[header.firstVertex + t.i2], 0, 0) + shape.center;
-                        BurstAabb edgeBounds = new BurstAabb(v1, v2, marginCS);
+                        var co = new BurstContact() { bodyA = simplexIndex, bodyB = colliderIndex };
+                        float4 simplexBary = BurstMath.BarycenterForSimplexOfSize(simplexSize);
 
-                        if (edgeBounds.IntersectsAabb(simplexBoundsCS, shape.is2D))
-                        {
-                            float4 simplexBary = BurstMath.BarycenterForSimplexOfSize(simplexSize);
+                        var colliderPoint = BurstLocalOptimization.Optimize<BurstEdgeMesh>(ref this, positions, orientations, radii, simplices, simplexStart, simplexSize,
+                                                                            ref simplexBary, out float4 convexPoint, optimizationIterations, optimizationTolerance);
 
-                            edgeMeshShape.dataOffset = dataOffset;
-                            var colliderPoint = BurstLocalOptimization.Optimize(ref edgeMeshShape, positions, orientations, radii, simplices, simplexStart, simplexSize,
-                                                                                ref simplexBary, out float4 convexPoint, parameters.surfaceCollisionIterations, parameters.surfaceCollisionTolerance);
+                        co.pointB = colliderPoint.point;
+                        co.normal = colliderPoint.normal;
+                        co.pointA = simplexBary;
 
-                            contactsQueue.Enqueue(new BurstContact(){
-                                bodyA = simplexIndex,
-                                bodyB = colliderIndex,
-                                pointA = simplexBary,
-                                pointB = colliderPoint.point,
-                                normal = colliderPoint.normal * edgeMeshShape.shape.sign
-                            });
-                        }
+                        contacts.Enqueue(co);
                     }
                 }
-                else // check min and/or max children:
-                {
-                    // visit min node:
-                    if (simplexBoundsCS.min[node.axis] <= node.min)
-                        queue.Enqueue(node.firstChild);
-
-                    // visit max node:
-                    if (simplexBoundsCS.max[node.axis] >= node.max)
-                        queue.Enqueue(node.firstChild + 1);
-                }
             }
         }
+
     }
 
 }

Assets/Obi/Scripts/Common/Backends/Burst/Collisions/BurstHeightField.cs

@@ -1,15 +1,17 @@
 #if (OBI_BURST && OBI_MATHEMATICS && OBI_COLLECTIONS)
-using Unity.Burst;
 using Unity.Collections;
-using Unity.Jobs;
 using Unity.Mathematics;
 
 namespace Obi
 {
-    public struct BurstHeightField : BurstLocalOptimization.IDistanceFunction
+    public struct BurstHeightField : BurstLocalOptimization.IDistanceFunction, IBurstCollider
     {
+
         public BurstColliderShape shape;
         public BurstAffineTransform colliderToSolver;
+        public BurstAffineTransform solverToWorld;
+        public float dt;
+        public float collisionMargin;
 
         public BurstMath.CachedTri tri;
         public float4 triNormal;
@@ -21,7 +23,7 @@ namespace Obi
         {
             point = colliderToSolver.InverseTransformPoint(point);
 
-            float4 nearestPoint = BurstMath.NearestPointOnTri(tri, point, out _);
+            float4 nearestPoint = BurstMath.NearestPointOnTri(tri, point, out float4 bary);
             float4 normal = math.normalizesafe(point - nearestPoint);
 
             // flip the contact normal if it points below ground: (doesn't work with holes)
@@ -31,123 +33,30 @@ namespace Obi
             projectedPoint.normal = colliderToSolver.TransformDirection(normal);
         }
 
-        public static JobHandle GenerateContacts(ObiColliderWorld world,
-                                               BurstSolverImpl solver,
-                                               NativeList<Oni.ContactPair> contactPairs,
-                                               NativeQueue<BurstContact> contactQueue,
-                                               NativeArray<int> contactOffsetsPerType,
-                                               float deltaTime,
-                                               JobHandle inputDeps)
+        public void Contacts(int colliderIndex,
+                             int rigidbodyIndex,
+                              NativeArray<BurstRigidbody> rigidbodies,
+
+                              NativeArray<float4> positions,
+                              NativeArray<quaternion> orientations,
+                              NativeArray<float4> velocities,
+                              NativeArray<float4> radii,
+
+                              NativeArray<int> simplices,
+                              in BurstAabb simplexBounds,
+                              int simplexIndex,
+                              int simplexStart,
+                              int simplexSize,
+
+                              NativeQueue<BurstContact>.ParallelWriter contacts,
+                              int optimizationIterations,
+                              float optimizationTolerance)
         {
-            int pairCount = contactOffsetsPerType[(int)Oni.ShapeType.Heightmap + 1] - contactOffsetsPerType[(int)Oni.ShapeType.Heightmap];
-            if (pairCount == 0) return inputDeps;
+            if (shape.dataIndex < 0) return;
 
-            var job = new GenerateHeightFieldContactsJob
-            {
-                contactPairs = contactPairs,
+            triNormal = float4.zero;
 
-                positions = solver.positions,
-                orientations = solver.orientations,
-                velocities = solver.velocities,
-                invMasses = solver.invMasses,
-                radii = solver.principalRadii,
-
-                simplices = solver.simplices,
-                simplexCounts = solver.simplexCounts,
-                simplexBounds = solver.simplexBounds,
-
-                transforms = world.colliderTransforms.AsNativeArray<BurstAffineTransform>(),
-                shapes = world.colliderShapes.AsNativeArray<BurstColliderShape>(),
-                rigidbodies = world.rigidbodies.AsNativeArray<BurstRigidbody>(),
-
-                heightFieldHeaders = world.heightFieldContainer.headers.AsNativeArray<HeightFieldHeader>(),
-                heightFieldSamples = world.heightFieldContainer.samples.AsNativeArray<float>(),
-
-                contactsQueue = contactQueue.AsParallelWriter(),
-
-                solverToWorld = solver.inertialFrame,
-                worldToSolver = solver.worldToSolver,
-                deltaTime = deltaTime,
-                parameters = solver.abstraction.parameters,
-                firstPair = contactOffsetsPerType[(int)Oni.ShapeType.Heightmap]
-            };
-
-            inputDeps = job.Schedule(pairCount, 1, inputDeps);
-            return inputDeps;
-        }
-
-    }
-
-    [BurstCompile]
-    struct GenerateHeightFieldContactsJob : IJobParallelFor
-    {
-        [ReadOnly] public NativeList<Oni.ContactPair> contactPairs;
-
-        // particle arrays:
-        [ReadOnly] public NativeArray<float4> velocities;
-        [ReadOnly] public NativeArray<float4> positions;
-        [ReadOnly] public NativeArray<quaternion> orientations;
-        [ReadOnly] public NativeArray<float> invMasses;
-        [ReadOnly] public NativeArray<float4> radii;
-
-        // simplex arrays:
-        [ReadOnly] public NativeArray<int> simplices;
-        [ReadOnly] public SimplexCounts simplexCounts;
-        [ReadOnly] public NativeArray<BurstAabb> simplexBounds;
-
-        // collider arrays:
-        [ReadOnly] public NativeArray<BurstAffineTransform> transforms;
-        [ReadOnly] public NativeArray<BurstColliderShape> shapes;
-        [ReadOnly] public NativeArray<BurstRigidbody> rigidbodies;
-
-        // height field data:
-        [ReadOnly] public NativeArray<HeightFieldHeader> heightFieldHeaders;
-        [ReadOnly] public NativeArray<float> heightFieldSamples;
-
-        [WriteOnly]
-        [NativeDisableParallelForRestriction]
-        public NativeQueue<BurstContact>.ParallelWriter contactsQueue;
-
-        // auxiliar data:
-        [ReadOnly] public int firstPair;
-        [ReadOnly] public BurstInertialFrame solverToWorld;
-        [ReadOnly] public BurstAffineTransform worldToSolver;
-        [ReadOnly] public float deltaTime;
-        [ReadOnly] public Oni.SolverParameters parameters;
-
-        public void Execute(int i)
-        {
-
-            int simplexIndex = contactPairs[firstPair + i].bodyA;
-            int colliderIndex = contactPairs[firstPair + i].bodyB;
-            var shape = shapes[colliderIndex];
-
-            if (shape.dataIndex < 0)
-                return;
-
-            var header = heightFieldHeaders[shape.dataIndex];
-            int rigidbodyIndex = shapes[colliderIndex].rigidbodyIndex;
-
-            int simplexStart = simplexCounts.GetSimplexStartAndSize(simplexIndex, out int simplexSize);
-            var simplexBound = simplexBounds[simplexIndex];
-
-            BurstAffineTransform colliderToSolver = worldToSolver * transforms[colliderIndex];
-
-            // invert a full matrix here to accurately represent collider bounds scale.
-            var solverToCollider = math.inverse(float4x4.TRS(colliderToSolver.translation.xyz, colliderToSolver.rotation, colliderToSolver.scale.xyz));
-            var simplexBoundsCS = simplexBound.Transformed(solverToCollider);
-
-            BurstHeightField triangleMeshShape = new BurstHeightField()
-            {
-                colliderToSolver = colliderToSolver,
-                shape = shapes[colliderIndex],
-                header = heightFieldHeaders[shapes[colliderIndex].dataIndex],
-                heightFieldSamples = heightFieldSamples
-            };
-
-            float4 triNormal = float4.zero;
-
-            var co = new BurstContact { bodyA = simplexIndex, bodyB = colliderIndex };
+            var co = new BurstContact() { bodyA = simplexIndex, bodyB = colliderIndex };
 
             int resolutionU = (int)shape.center.x;
             int resolutionV = (int)shape.center.y;
@@ -157,8 +66,8 @@ namespace Obi
             float cellHeight = shape.size.z / (resolutionV - 1);
 
             // calculate particle bounds min/max cells:
-            int2 min = new int2((int)math.floor(simplexBoundsCS.min[0] / cellWidth), (int)math.floor(simplexBoundsCS.min[2] / cellHeight));
-            int2 max = new int2((int)math.floor(simplexBoundsCS.max[0] / cellWidth), (int)math.floor(simplexBoundsCS.max[2] / cellHeight));
+            int2 min = new int2((int)math.floor(simplexBounds.min[0] / cellWidth), (int)math.floor(simplexBounds.min[2] / cellHeight));
+            int2 max = new int2((int)math.floor(simplexBounds.max[0] / cellWidth), (int)math.floor(simplexBounds.max[2] / cellHeight));
 
             for (int su = min[0]; su <= max[0]; ++su)
             {
@@ -197,11 +106,11 @@ namespace Obi
                             float4 v2 = new float4(max_x, h4, max_z, 0);
                             float4 v3 = new float4(min_x, h1, min_z, 0);
 
-                            triangleMeshShape.tri.Cache(v1, v2, v3);
+                            tri.Cache(v1, v2, v3);
                             triNormal.xyz = math.normalizesafe(math.cross((v2 - v1).xyz, (v3 - v1).xyz));
 
-                            var colliderPoint = BurstLocalOptimization.Optimize(ref triangleMeshShape, positions, orientations, radii, simplices, simplexStart, simplexSize,
-                                                                                ref simplexBary, out convexPoint, parameters.surfaceCollisionIterations, parameters.surfaceCollisionTolerance);
+                            var colliderPoint = BurstLocalOptimization.Optimize<BurstHeightField>(ref this, positions, orientations, radii, simplices, simplexStart, simplexSize,
+                                                                                ref simplexBary, out convexPoint, optimizationIterations, optimizationTolerance);
 
                             float4 velocity = float4.zero;
                             float simplexRadius = 0;
@@ -219,12 +128,12 @@ namespace Obi
                             float dAB = math.dot(convexPoint - colliderPoint.point, colliderPoint.normal);
                             float vel = math.dot(velocity - rbVelocity, colliderPoint.normal);
 
-                            if (vel * deltaTime + dAB <= simplexRadius + shape.contactOffset + parameters.collisionMargin)
+                            if (vel * dt + dAB <= simplexRadius + shape.contactOffset + collisionMargin)
                             {
                                 co.pointB = colliderPoint.point;
-                                co.normal = colliderPoint.normal * triangleMeshShape.shape.sign;
+                                co.normal = colliderPoint.normal;
                                 co.pointA = simplexBary;
-                                contactsQueue.Enqueue(co);
+                                contacts.Enqueue(co);
                             }
 
                             // ------contact against the second triangle------:
@@ -232,11 +141,11 @@ namespace Obi
                             v2 = new float4(max_x, h4, max_z, 0);
                             v3 = new float4(max_x, h2, min_z, 0);
 
-                            triangleMeshShape.tri.Cache(v1, v2, v3);
+                            tri.Cache(v1, v2, v3);
                             triNormal.xyz = math.normalizesafe(math.cross((v2 - v1).xyz, (v3 - v1).xyz));
 
-                            colliderPoint = BurstLocalOptimization.Optimize(ref triangleMeshShape, positions, orientations, radii, simplices, simplexStart, simplexSize,
-                                                                            ref simplexBary, out convexPoint, parameters.surfaceCollisionIterations, parameters.surfaceCollisionTolerance);
+                            colliderPoint = BurstLocalOptimization.Optimize<BurstHeightField>(ref this, positions, orientations, radii, simplices, simplexStart, simplexSize,
+                                                                                ref simplexBary, out convexPoint, optimizationIterations, optimizationTolerance);
 
                             velocity = float4.zero;
                             simplexRadius = 0;
@@ -254,19 +163,21 @@ namespace Obi
                             dAB = math.dot(convexPoint - colliderPoint.point, colliderPoint.normal);
                             vel = math.dot(velocity - rbVelocity, colliderPoint.normal);
 
-                            if (vel * deltaTime + dAB <= simplexRadius + shape.contactOffset + parameters.collisionMargin)
+                            if (vel * dt + dAB <= simplexRadius + shape.contactOffset + collisionMargin)
                             {
                                 co.pointB = colliderPoint.point;
-                                co.normal = colliderPoint.normal * triangleMeshShape.shape.sign;
+                                co.normal = colliderPoint.normal;
                                 co.pointA = simplexBary;
 
-                                contactsQueue.Enqueue(co);
+                                contacts.Enqueue(co);
                             }
                         }
                     }
                 }
             }
+            
         }
+
     }
 
 }

Assets/Obi/Scripts/Common/Backends/Burst/Collisions/BurstLocalOptimization.cs

@@ -50,7 +50,6 @@ namespace Obi
                 convexRadii += radii[particle] * convexBary[j];
                 convexOrientation.value += orientations[particle].value * convexBary[j];
             }
-            convexPoint.w = 0;
         }
 
         public static SurfacePoint Optimize<T>(ref T function,
@@ -113,7 +112,6 @@ namespace Obi
                 {
                     int particle = simplices[simplexStart + j];
                     float4 candidate = positions[particle] - convexPoint;
-                    candidate.w = 0;
 
                     // here, we adjust the candidate by projecting it to the engrosed simplex's surface:
                     candidate -= pointInFunction.normal * (radii[particle].x - convexThickness.x);
@@ -178,8 +176,6 @@ namespace Obi
 
                 float4 candidateC = positions[simplices[simplexStart]] - pointInFunction.point;
                 float4 candidateD = positions[simplices[simplexStart + 1]] - pointInFunctionD.point;
-                candidateC.w = 0;
-                candidateD.w = 0;
 
                 candidateC -= pointInFunction.normal * (radii[simplices[simplexStart]].x - convexThickness.x);
                 candidateD -= pointInFunctionD.normal * (radii[simplices[simplexStart + 1]].x - convexThicknessD.x);

Assets/Obi/Scripts/Common/Backends/Burst/Collisions/BurstSimplex.cs

@@ -24,9 +24,9 @@ namespace Obi
         {
             if (simplexSize == 3)
             {
-                tri.Cache(new float4(positions[simplices[simplexStart]].xyz,0),
-                          new float4(positions[simplices[simplexStart + 1]].xyz,0),
-                          new float4(positions[simplices[simplexStart + 2]].xyz,0));
+                tri.Cache(positions[simplices[simplexStart]],
+                          positions[simplices[simplexStart + 1]],
+                          positions[simplices[simplexStart + 2]]);
             }
         }
 
@@ -35,17 +35,16 @@ namespace Obi
             switch (simplexSize)
             {
                 case 1:
-                default:
                     {
-                        float4 p1 = positions[simplices[simplexStart]]; p1.w = 0;
+                        float4 p1 = positions[simplices[simplexStart]];
                         projectedPoint.bary = new float4(1, 0, 0, 0);
                         projectedPoint.point = p1;
                     }
                     break;
                 case 2:
                     {
-                        float4 p1 = positions[simplices[simplexStart]]; p1.w = 0;
-                        float4 p2 = positions[simplices[simplexStart + 1]]; p2.w = 0;
+                        float4 p1 = positions[simplices[simplexStart]];
+                        float4 p2 = positions[simplices[simplexStart + 1]];
                         BurstMath.NearestPointOnEdge(p1, p2, point, out float mu);
                         projectedPoint.bary = new float4(1 - mu, mu, 0, 0);
                         projectedPoint.point = p1 * projectedPoint.bary[0] + p2 * projectedPoint.bary[1];

Assets/Obi/Scripts/Common/Backends/Burst/Collisions/BurstSphere.cs

@@ -1,22 +1,21 @@
 #if (OBI_BURST && OBI_MATHEMATICS && OBI_COLLECTIONS)
-using Unity.Burst;
 using Unity.Collections;
-using Unity.Jobs;
 using Unity.Mathematics;
 
 namespace Obi
 {
-    public struct BurstSphere : BurstLocalOptimization.IDistanceFunction
+    public struct BurstSphere : BurstLocalOptimization.IDistanceFunction, IBurstCollider
     {
         public BurstColliderShape shape;
         public BurstAffineTransform colliderToSolver;
+        public float dt;
 
         public void Evaluate(float4 point, float4 radii, quaternion orientation, ref BurstLocalOptimization.SurfacePoint projectedPoint)
         {
             float4 center = shape.center * colliderToSolver.scale;
             point = colliderToSolver.InverseTransformPointUnscaled(point) - center;
 
-            if (shape.is2D)
+            if (shape.is2D != 0)
                 point[2] = 0;
 
             float radius = shape.size.x * math.cmax(colliderToSolver.scale.xyz);
@@ -28,98 +27,36 @@ namespace Obi
             projectedPoint.normal = colliderToSolver.TransformDirection(normal);
         }
 
-        public static JobHandle GenerateContacts(ObiColliderWorld world,
-                                                BurstSolverImpl solver,
-                                                NativeList<Oni.ContactPair> contactPairs,
-                                                NativeQueue<BurstContact> contactQueue,
-                                                NativeArray<int> contactOffsetsPerType,
-                                                float deltaTime,
-                                                JobHandle inputDeps)
+        public void Contacts(int colliderIndex,
+                             int rigidbodyIndex,
+                             NativeArray<BurstRigidbody> rigidbodies,
+
+                             NativeArray<float4> positions,
+                             NativeArray<quaternion> orientations,
+                             NativeArray<float4> velocities,
+                             NativeArray<float4> radii,
+
+                             NativeArray<int> simplices,
+                             in BurstAabb simplexBounds,
+                             int simplexIndex,
+                             int simplexStart,
+                             int simplexSize,
+
+                             NativeQueue<BurstContact>.ParallelWriter contacts,
+                             int optimizationIterations,
+                             float optimizationTolerance)
         {
-            int pairCount = contactOffsetsPerType[(int)Oni.ShapeType.Sphere + 1] - contactOffsetsPerType[(int)Oni.ShapeType.Sphere];
-            if (pairCount == 0) return inputDeps;
-
-            var job = new GenerateSphereContactsJob
-            {
-                contactPairs = contactPairs,
-
-                positions = solver.positions,
-                orientations = solver.orientations,
-                velocities = solver.velocities,
-                invMasses = solver.invMasses,
-                radii = solver.principalRadii,
-
-                simplices = solver.simplices,
-                simplexCounts = solver.simplexCounts,
-
-                transforms = world.colliderTransforms.AsNativeArray<BurstAffineTransform>(),
-                shapes = world.colliderShapes.AsNativeArray<BurstColliderShape>(),
-
-                contactsQueue = contactQueue.AsParallelWriter(),
-
-                worldToSolver = solver.worldToSolver,
-                deltaTime = deltaTime,
-                parameters = solver.abstraction.parameters,
-                firstPair = contactOffsetsPerType[(int)Oni.ShapeType.Sphere]
-            };
-
-            inputDeps = job.Schedule(pairCount, 8, inputDeps);
-            return inputDeps;
-        }
-    }
-
-    [BurstCompile]
-    struct GenerateSphereContactsJob : IJobParallelFor
-    {
-        [ReadOnly] public NativeList<Oni.ContactPair> contactPairs;
-
-        // particle arrays:
-        [ReadOnly] public NativeArray<float4> velocities;
-        [ReadOnly] public NativeArray<float4> positions;
-        [ReadOnly] public NativeArray<quaternion> orientations;
-        [ReadOnly] public NativeArray<float> invMasses;
-        [ReadOnly] public NativeArray<float4> radii;
-
-        // simplex arrays:
-        [ReadOnly] public NativeArray<int> simplices;
-        [ReadOnly] public SimplexCounts simplexCounts;
-
-        // collider arrays:
-        [ReadOnly] public NativeArray<BurstAffineTransform> transforms;
-        [ReadOnly] public NativeArray<BurstColliderShape> shapes;
-
-        [WriteOnly]
-        [NativeDisableParallelForRestriction]
-        public NativeQueue<BurstContact>.ParallelWriter contactsQueue;
-
-        // auxiliar data:
-        [ReadOnly] public int firstPair;
-        [ReadOnly] public BurstAffineTransform worldToSolver;
-        [ReadOnly] public float deltaTime;
-        [ReadOnly] public Oni.SolverParameters parameters;
-
-        public void Execute(int i)
-        {
-            int simplexIndex = contactPairs[firstPair + i].bodyA;
-            int colliderIndex = contactPairs[firstPair + i].bodyB;
-
-            int simplexStart = simplexCounts.GetSimplexStartAndSize(simplexIndex, out int simplexSize);
-
-            BurstAffineTransform colliderToSolver = worldToSolver * transforms[colliderIndex];
-
-            BurstSphere shape = new BurstSphere { colliderToSolver = colliderToSolver, shape = shapes[colliderIndex] };
-            
+            var co = new BurstContact() { bodyA = simplexIndex, bodyB = colliderIndex };
             float4 simplexBary = BurstMath.BarycenterForSimplexOfSize(simplexSize);
-            var colliderPoint = BurstLocalOptimization.Optimize(ref shape, positions, orientations, radii, simplices, simplexStart, simplexSize,
-                                                                ref simplexBary, out _, parameters.surfaceCollisionIterations, parameters.surfaceCollisionTolerance);
 
-            contactsQueue.Enqueue(new BurstContact {
-                bodyA = simplexIndex,
-                bodyB = colliderIndex,
-                pointA = simplexBary,
-                pointB = colliderPoint.point,
-                normal = colliderPoint.normal * shape.shape.sign
-            });
+            var colliderPoint = BurstLocalOptimization.Optimize<BurstSphere>(ref this, positions, orientations, radii, simplices, simplexStart, simplexSize,
+                                                                             ref simplexBary, out float4 convexPoint, optimizationIterations, optimizationTolerance);
+
+            co.pointB = colliderPoint.point;
+            co.normal = colliderPoint.normal;
+            co.pointA = simplexBary;
+
+            contacts.Enqueue(co);
         }
     }
 

Assets/Obi/Scripts/Common/Backends/Burst/Collisions/BurstTriangleMesh.cs

@@ -1,23 +1,30 @@
 #if (OBI_BURST && OBI_MATHEMATICS && OBI_COLLECTIONS)
-using Unity.Burst;
 using Unity.Collections;
-using Unity.Jobs;
 using Unity.Mathematics;
 
 namespace Obi
 {
-    public struct BurstTriangleMesh : BurstLocalOptimization.IDistanceFunction
+    public struct BurstTriangleMesh : BurstLocalOptimization.IDistanceFunction, IBurstCollider
     {
         public BurstColliderShape shape;
         public BurstAffineTransform colliderToSolver;
+        public BurstAffineTransform solverToWorld;
 
-        public BurstMath.CachedTri tri;
+        public TriangleMeshHeader header;
+        public NativeArray<BIHNode> bihNodes;
+        public NativeArray<Triangle> triangles;
+        public NativeArray<float3> vertices;
+
+        public float dt;
+        public float collisionMargin;
+
+        private BurstMath.CachedTri tri;
 
         public void Evaluate(float4 point, float4 radii, quaternion orientation, ref BurstLocalOptimization.SurfacePoint projectedPoint)
         {
             point = colliderToSolver.InverseTransformPointUnscaled(point);
 
-            if (shape.is2D)
+            if (shape.is2D != 0)
                 point[2] = 0;
 
             float4 nearestPoint = BurstMath.NearestPointOnTri(tri, point, out float4 bary);
@@ -27,194 +34,116 @@ namespace Obi
             projectedPoint.normal = colliderToSolver.TransformDirection(normal);
         }
 
-        public static JobHandle GenerateContacts(ObiColliderWorld world,
-                                               BurstSolverImpl solver,
-                                               NativeList<Oni.ContactPair> contactPairs,
-                                               NativeQueue<BurstContact> contactQueue,
-                                               NativeArray<int> contactOffsetsPerType,
-                                               float deltaTime,
-                                               JobHandle inputDeps)
+        public void Contacts(int colliderIndex,
+                             int rigidbodyIndex,
+                             NativeArray<BurstRigidbody> rigidbodies,
+
+                              NativeArray<float4> positions,
+                              NativeArray<quaternion> orientations,
+                              NativeArray<float4> velocities,
+                              NativeArray<float4> radii,
+
+                              NativeArray<int> simplices,
+                              in BurstAabb simplexBounds,
+                              int simplexIndex,
+                              int simplexStart,
+                              int simplexSize,
+
+                              NativeQueue<BurstContact>.ParallelWriter contacts,
+                              int optimizationIterations,
+                              float optimizationTolerance)
         {
-            int pairCount = contactOffsetsPerType[(int)Oni.ShapeType.TriangleMesh + 1] - contactOffsetsPerType[(int)Oni.ShapeType.TriangleMesh];
-            if (pairCount == 0) return inputDeps;
 
-            var job = new GenerateTriangleMeshContactsJob
-            {
-                contactPairs = contactPairs,
-
-                positions = solver.positions,
-                orientations = solver.orientations,
-                velocities = solver.velocities,
-                invMasses = solver.invMasses,
-                radii = solver.principalRadii,
-
-                simplices = solver.simplices,
-                simplexCounts = solver.simplexCounts,
-                simplexBounds = solver.simplexBounds,
-
-                transforms = world.colliderTransforms.AsNativeArray<BurstAffineTransform>(),
-                shapes = world.colliderShapes.AsNativeArray<BurstColliderShape>(),
-                rigidbodies = world.rigidbodies.AsNativeArray<BurstRigidbody>(),
-
-                triangleMeshHeaders = world.triangleMeshContainer.headers.AsNativeArray<TriangleMeshHeader>(),
-                bihNodes = world.triangleMeshContainer.bihNodes.AsNativeArray<BIHNode>(),
-                triangles = world.triangleMeshContainer.triangles.AsNativeArray<Triangle>(),
-                vertices = world.triangleMeshContainer.vertices.AsNativeArray<float3>(),
-
-                contactsQueue = contactQueue.AsParallelWriter(),
-
-                solverToWorld = solver.inertialFrame,
-                worldToSolver = solver.worldToSolver,
-                deltaTime = deltaTime,
-                parameters = solver.abstraction.parameters,
-                firstPair = contactOffsetsPerType[(int)Oni.ShapeType.TriangleMesh]
-            };
-
-            inputDeps = job.Schedule(pairCount, 1, inputDeps);
-            return inputDeps;
+            BIHTraverse(colliderIndex, rigidbodyIndex, simplexIndex, simplexStart, simplexSize,
+                        rigidbodies, positions, orientations, velocities, radii, simplices, in simplexBounds, 0, contacts, optimizationIterations, optimizationTolerance);
+            
         }
-    }
 
-    [BurstCompile]
-    struct GenerateTriangleMeshContactsJob : IJobParallelFor
-    {
-        [ReadOnly] public NativeList<Oni.ContactPair> contactPairs;
-
-        // particle arrays:
-        [ReadOnly] public NativeArray<float4> velocities;
-        [ReadOnly] public NativeArray<float4> positions;
-        [ReadOnly] public NativeArray<quaternion> orientations;
-        [ReadOnly] public NativeArray<float> invMasses;
-        [ReadOnly] public NativeArray<float4> radii;
-
-        // simplex arrays:
-        [ReadOnly] public NativeArray<int> simplices;
-        [ReadOnly] public SimplexCounts simplexCounts;
-        [ReadOnly] public NativeArray<BurstAabb> simplexBounds;
-
-        // collider arrays:
-        [ReadOnly] public NativeArray<BurstAffineTransform> transforms;
-        [ReadOnly] public NativeArray<BurstColliderShape> shapes;
-        [ReadOnly] public NativeArray<BurstRigidbody> rigidbodies;
-
-        // triangle mesh data:
-        [ReadOnly] public NativeArray<TriangleMeshHeader> triangleMeshHeaders;
-        [ReadOnly] public NativeArray<BIHNode> bihNodes;
-        [ReadOnly] public NativeArray<Triangle> triangles;
-        [ReadOnly] public NativeArray<float3> vertices;
-
-        [WriteOnly]
-        [NativeDisableParallelForRestriction]
-        public NativeQueue<BurstContact>.ParallelWriter contactsQueue;
-
-        // auxiliar data:
-        [ReadOnly] public int firstPair;
-        [ReadOnly] public BurstInertialFrame solverToWorld;
-        [ReadOnly] public BurstAffineTransform worldToSolver;
-        [ReadOnly] public float deltaTime;
-        [ReadOnly] public Oni.SolverParameters parameters;
-
-        public void Execute(int i)
+        private void BIHTraverse(int colliderIndex,
+                                 int rigidbodyIndex,
+                                 int simplexIndex,
+                                 int simplexStart,
+                                 int simplexSize,
+                                 NativeArray<BurstRigidbody> rigidbodies,
+                                 NativeArray<float4> positions,
+                                 NativeArray<quaternion> orientations,
+                                 NativeArray<float4> velocities,
+                                 NativeArray<float4> radii,
+                                 NativeArray<int> simplices,
+                                 in BurstAabb simplexBounds,
+                                 int nodeIndex,
+                                 NativeQueue<BurstContact>.ParallelWriter contacts,
+                                 int optimizationIterations,
+                                 float optimizationTolerance)
         {
-            int simplexIndex = contactPairs[firstPair + i].bodyA;
-            int colliderIndex = contactPairs[firstPair + i].bodyB;
-            var shape = shapes[colliderIndex];
+            var node = bihNodes[header.firstNode + nodeIndex];
 
-            if (shape.dataIndex < 0)
-                return;
+            if (node.firstChild >= 0)
+            { 
+                // visit min node:
+                if (simplexBounds.min[node.axis] <= node.min)
+                    BIHTraverse(colliderIndex, rigidbodyIndex, simplexIndex, simplexStart, simplexSize,
+                                rigidbodies, positions, orientations, velocities, radii, simplices, in simplexBounds,
+                                node.firstChild, contacts, optimizationIterations, optimizationTolerance);
 
-            int rigidbodyIndex = shape.rigidbodyIndex;
-            var header = triangleMeshHeaders[shape.dataIndex];
-
-            int simplexStart = simplexCounts.GetSimplexStartAndSize(simplexIndex, out int simplexSize);
-            var simplexBound = simplexBounds[simplexIndex];
-
-            BurstAffineTransform colliderToSolver = worldToSolver * transforms[colliderIndex];
-
-            // invert a full matrix here to accurately represent collider bounds scale.
-            var solverToCollider = math.inverse(float4x4.TRS(colliderToSolver.translation.xyz, colliderToSolver.rotation, colliderToSolver.scale.xyz));
-            var simplexBoundsCS = simplexBound.Transformed(solverToCollider);
-
-            float4 marginCS = new float4((shape.contactOffset + parameters.collisionMargin) / colliderToSolver.scale.xyz, 0);
-
-            BurstTriangleMesh triangleMeshShape = new BurstTriangleMesh()
+                // visit max node:
+                if (simplexBounds.max[node.axis] >= node.max)
+                    BIHTraverse(colliderIndex, rigidbodyIndex, simplexIndex, simplexStart, simplexSize,
+                                rigidbodies, positions, orientations, velocities, radii, simplices, in simplexBounds,
+                                node.firstChild + 1, contacts, optimizationIterations, optimizationTolerance);
+            }
+            else
             {
-                colliderToSolver = colliderToSolver,
-                shape = shape
-            };
-
-            NativeQueue<int> queue = new NativeQueue<int>(Allocator.Temp);
-
-            queue.Enqueue(0);
-
-            while (!queue.IsEmpty())
-            {
-                int nodeIndex = queue.Dequeue();
-                var node = bihNodes[header.firstNode + nodeIndex];
-
-                // leaf node:
-                if (node.firstChild < 0)
+                // check for contact against all triangles:
+                for (int dataOffset = node.start; dataOffset < node.start + node.count; ++dataOffset)
                 {
-                    // check for contact against all triangles:
-                    for (int dataOffset = node.start; dataOffset < node.start + node.count; ++dataOffset)
+                    Triangle t = triangles[header.firstTriangle + dataOffset];
+                    float4 v1 = new float4(vertices[header.firstVertex + t.i1], 0);
+                    float4 v2 = new float4(vertices[header.firstVertex + t.i2], 0);
+                    float4 v3 = new float4(vertices[header.firstVertex + t.i3], 0);
+                    BurstAabb triangleBounds = new BurstAabb(v1, v2, v3, shape.contactOffset + collisionMargin);
+
+                    if (triangleBounds.IntersectsAabb(simplexBounds, shape.is2D != 0))
                     {
-                        Triangle t = triangles[header.firstTriangle + dataOffset];
-                        float4 v1 = new float4(vertices[header.firstVertex + t.i1], 0);
-                        float4 v2 = new float4(vertices[header.firstVertex + t.i2], 0);
-                        float4 v3 = new float4(vertices[header.firstVertex + t.i3], 0);
-                        BurstAabb triangleBounds = new BurstAabb(v1, v2, v3, marginCS);
+                        float4 simplexBary = BurstMath.BarycenterForSimplexOfSize(simplexSize);
+                        tri.Cache(v1 * colliderToSolver.scale, v2 * colliderToSolver.scale, v3 * colliderToSolver.scale);
 
-                        if (triangleBounds.IntersectsAabb(simplexBoundsCS, shape.is2D))
+                        var colliderPoint = BurstLocalOptimization.Optimize<BurstTriangleMesh>(ref this, positions, orientations, radii, simplices, simplexStart, simplexSize,
+                                                                            ref simplexBary, out float4 simplexPoint, optimizationIterations, optimizationTolerance);
+
+                        float4 velocity = float4.zero;
+                        float simplexRadius = 0;
+                        for (int j = 0; j < simplexSize; ++j)
                         {
-                            float4 simplexBary = BurstMath.BarycenterForSimplexOfSize(simplexSize);
+                            int particleIndex = simplices[simplexStart + j];
+                            simplexRadius += radii[particleIndex].x * simplexBary[j];
+                            velocity += velocities[particleIndex] * simplexBary[j];
+                        }
 
-                            triangleMeshShape.tri.Cache(v1 * colliderToSolver.scale, v2 * colliderToSolver.scale, v3 * colliderToSolver.scale);
+                        float4 rbVelocity = float4.zero;
+                        if (rigidbodyIndex >= 0)
+                            rbVelocity = BurstMath.GetRigidbodyVelocityAtPoint(rigidbodyIndex, colliderPoint.point, rigidbodies, solverToWorld);
 
-                            var colliderPoint = BurstLocalOptimization.Optimize(ref triangleMeshShape, positions, orientations, radii, simplices, simplexStart, simplexSize,
-                                                                                ref simplexBary, out float4 simplexPoint, parameters.surfaceCollisionIterations, parameters.surfaceCollisionTolerance);
+                        float dAB = math.dot(simplexPoint - colliderPoint.point, colliderPoint.normal);
+                        float vel = math.dot(velocity     - rbVelocity, colliderPoint.normal); 
 
-                            float4 velocity = float4.zero;
-                            float simplexRadius = 0;
-                            for (int j = 0; j < simplexSize; ++j)
+                        if (vel * dt + dAB <= simplexRadius + shape.contactOffset + collisionMargin)
+                        {
+                            contacts.Enqueue(new BurstContact()
                             {
-                                int particleIndex = simplices[simplexStart + j];
-                                simplexRadius += radii[particleIndex].x * simplexBary[j];
-                                velocity += velocities[particleIndex] * simplexBary[j];
-                            }
-
-                            float4 rbVelocity = float4.zero;
-                            if (rigidbodyIndex >= 0)
-                                rbVelocity = BurstMath.GetRigidbodyVelocityAtPoint(rigidbodyIndex, colliderPoint.point, rigidbodies, solverToWorld);
-
-                            float dAB = math.dot(simplexPoint - colliderPoint.point, colliderPoint.normal);
-                            float vel = math.dot(velocity - rbVelocity, colliderPoint.normal);
-
-                            if (vel * deltaTime + dAB <= simplexRadius + shape.contactOffset + parameters.collisionMargin)
-                            {
-                                contactsQueue.Enqueue(new BurstContact()
-                                {
-                                    bodyA = simplexIndex,
-                                    bodyB = colliderIndex,
-                                    pointA = simplexBary,
-                                    pointB = colliderPoint.point,
-                                    normal = colliderPoint.normal * triangleMeshShape.shape.sign
-                                });
-                            }
+                                bodyA = simplexIndex,
+                                bodyB = colliderIndex,
+                                pointA = simplexBary,
+                                pointB = colliderPoint.point,
+                                normal = colliderPoint.normal,
+                            });
                         }
                     }
                 }
-                else // check min and/or max children:
-                {
-                    // visit min node:
-                    if (simplexBoundsCS.min[node.axis] <= node.min)
-                        queue.Enqueue(node.firstChild);
-
-                    // visit max node:
-                    if (simplexBoundsCS.max[node.axis] >= node.max)
-                        queue.Enqueue(node.firstChild + 1);
-                }
             }
         }
+
     }
+
 }
 #endif

Assets/Obi/Scripts/Common/Backends/Burst/Collisions/IBurstCollider.cs

@@ -0,0 +1,29 @@
+#if (OBI_BURST && OBI_MATHEMATICS && OBI_COLLECTIONS)
+using Unity.Collections;
+using Unity.Mathematics;
+
+namespace Obi
+{
+    interface IBurstCollider
+    {
+        void Contacts(int colliderIndex,
+                      int rigidbodyIndex,
+                      NativeArray<BurstRigidbody> rigidbodies,
+
+                      NativeArray<float4> positions,
+                      NativeArray<quaternion> orientations,
+                      NativeArray<float4> velocities,
+                      NativeArray<float4> radii,
+
+                      NativeArray<int> simplices,
+                      in BurstAabb simplexBounds,
+                      int simplexIndex,
+                      int simplexStart,
+                      int simplexSize,
+
+                      NativeQueue<BurstContact>.ParallelWriter contacts,
+                      int optimizationIterations,
+                      float optimizationTolerance);
+    }
+}
+#endif

Assets/Obi/Scripts/Common/Backends/Burst/Collisions/IBurstCollider.cs.meta

@@ -0,0 +1,11 @@
+fileFormatVersion: 2
+guid: 424c135125b644408aeda26a57bd6e40
+MonoImporter:
+  externalObjects: {}
+  serializedVersion: 2
+  defaultReferences: []
+  executionOrder: 0
+  icon: {instanceID: 0}
+  userData: 
+  assetBundleName: 
+  assetBundleVariant: