升级obi

2026-01-22 22:08:21 +08:00
parent 120b8cda26
commit 20f14322bc
1067 changed files with 149894 additions and 29583 deletions
--- a/Assets/Obi/Scripts/Common/Backends/Burst/Collisions/BurstEdgeMesh.cs
+++ b/Assets/Obi/Scripts/Common/Backends/Burst/Collisions/BurstEdgeMesh.cs
@@ -1,16 +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 BurstEdgeMesh : BurstLocalOptimization.IDistanceFunction, IBurstCollider
+    public struct BurstEdgeMesh : BurstLocalOptimization.IDistanceFunction
    {

        public BurstColliderShape shape;
        public BurstAffineTransform colliderToSolver;
        public int dataOffset;
-        public float dt;

        public EdgeMeshHeader header;
        public NativeArray<BIHNode> edgeBihNodes;
@@ -21,12 +22,12 @@ namespace Obi
        {
            point = colliderToSolver.InverseTransformPointUnscaled(point);

-            if (shape.is2D != 0)
+            if (shape.is2D)
                point[2] = 0;

            Edge t = edges[header.firstEdge + dataOffset];
-            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 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 nearestPoint = BurstMath.NearestPointOnEdge(v1, v2, point, out float mu);
            float4 normal = math.normalizesafe(point - nearestPoint);
@@ -35,90 +36,175 @@ namespace Obi
            projectedPoint.point = colliderToSolver.TransformPointUnscaled(nearestPoint + normal * shape.contactOffset);
        }

-
-        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)
+        public static JobHandle GenerateContacts(ObiColliderWorld world,
+                                               BurstSolverImpl solver,
+                                               NativeList<Oni.ContactPair> contactPairs,
+                                               NativeQueue<BurstContact> contactQueue,
+                                               NativeArray<int> contactOffsetsPerType,
+                                               float deltaTime,
+                                               JobHandle inputDeps)
        {
-            if (shape.dataIndex < 0) return;
+            int pairCount = contactOffsetsPerType[(int)Oni.ShapeType.EdgeMesh + 1] - contactOffsetsPerType[(int)Oni.ShapeType.EdgeMesh];
+            if (pairCount == 0) return inputDeps;

-            BIHTraverse(colliderIndex, simplexIndex, simplexStart, simplexSize,
-                        positions, orientations, radii, simplices, in simplexBounds, 0, contacts, optimizationIterations, optimizationTolerance);
+            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;
        }
+    }

-        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)
+    [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)
        {
-            var node = edgeBihNodes[header.firstNode + nodeIndex];
+            int simplexIndex = contactPairs[firstPair + i].bodyA;
+            int colliderIndex = contactPairs[firstPair + i].bodyB;
+            var shape = shapes[colliderIndex];

-            if (node.firstChild >= 0)
-            {
-                // 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);
+            if (shape.dataIndex < 0)
+                return;

-                // 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
+            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()
            {
-                // check for contact against all triangles:
-                for (dataOffset = node.start; dataOffset < node.start + node.count; ++dataOffset)
+                colliderToSolver = colliderToSolver,
+                shape = shape,
+                header = header,
+                edgeBihNodes = edgeBihNodes,
+                edges = edges,
+                vertices = edgeVertices
+            };
+
+            NativeQueue<int> queue = new NativeQueue<int>(Allocator.Temp);
+
+            queue.Enqueue(0);
+
+            while (!queue.IsEmpty())
+            {
+                int nodeIndex = queue.Dequeue();
+                var node = edgeBihNodes[header.firstNode + nodeIndex];
+
+                // leaf node:
+                if (node.firstChild < 0)
                {
-                    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))
+                    // check for contact against all triangles:
+                    for (int dataOffset = node.start; dataOffset < node.start + node.count; ++dataOffset)
                    {
-                        var co = new BurstContact() { bodyA = simplexIndex, bodyB = colliderIndex };
-                        float4 simplexBary = BurstMath.BarycenterForSimplexOfSize(simplexSize);
+                        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 colliderPoint = BurstLocalOptimization.Optimize<BurstEdgeMesh>(ref this, positions, orientations, radii, simplices, simplexStart, simplexSize,
-                                                                            ref simplexBary, out float4 convexPoint, optimizationIterations, optimizationTolerance);
+                        if (edgeBounds.IntersectsAabb(simplexBoundsCS, shape.is2D))
+                        {
+                            float4 simplexBary = BurstMath.BarycenterForSimplexOfSize(simplexSize);

-                        co.pointB = colliderPoint.point;
-                        co.normal = colliderPoint.normal;
-                        co.pointA = simplexBary;
+                            edgeMeshShape.dataOffset = dataOffset;
+                            var colliderPoint = BurstLocalOptimization.Optimize(ref edgeMeshShape, positions, orientations, radii, simplices, simplexStart, simplexSize,
+                                                                                ref simplexBary, out float4 convexPoint, parameters.surfaceCollisionIterations, parameters.surfaceCollisionTolerance);

-                        contacts.Enqueue(co);
+                            contactsQueue.Enqueue(new BurstContact(){
+                                bodyA = simplexIndex,
+                                bodyB = colliderIndex,
+                                pointA = simplexBary,
+                                pointB = colliderPoint.point,
+                                normal = colliderPoint.normal * edgeMeshShape.shape.sign
+                            });
+                        }
                    }
                }
+                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);
+                }
            }
        }
-
    }

 }