// Tests that the points of a manifold are all coplanar static unsafe void CheckManifoldFlat(ref ConvexConvexManifoldQueries.Manifold manifold, float3 normal, string failureMessage) { float3 point0 = manifold[0].Position + normal * manifold[0].Distance; float3 point1 = manifold[1].Position + normal * manifold[1].Distance; for (int i = 2; i < manifold.NumContacts; i++) { // Try to calculate a plane from points 0, 1, iNormal float3 point = manifold[i].Position + normal * manifold[i].Distance; float3 cross = math.cross(point - point0, point - point1); if (math.lengthsq(cross) > 1e-6f) { // Test that each point in the manifold is on the plane float3 faceNormal = math.normalize(cross); float dot = math.dot(point0, faceNormal); for (int j = 2; j < manifold.NumContacts; j++) { float3 testPoint = manifold[j].Position + normal * manifold[j].Distance; Assert.AreEqual(dot, math.dot(faceNormal, testPoint), tolerance, failureMessage + " contact " + j); } break; } } }
public unsafe void ManifoldQueryTest() { const uint seed = 0x98765432; Random rnd = new Random(seed); int numWorlds = 1000; uint dbgWorld = 0; if (dbgWorld > 0) { numWorlds = 1; } for (int iWorld = 0; iWorld < numWorlds; iWorld++) { // Save state to repro this query without doing everything that came before it if (dbgWorld > 0) { rnd.state = dbgWorld; } uint worldState = rnd.state; Physics.PhysicsWorld world = TestUtils.GenerateRandomWorld(ref rnd, rnd.NextInt(1, 20), 3.0f); // Manifold test // TODO would be nice if we could change the world collision tolerance for (int iBodyA = 0; iBodyA < world.NumBodies; iBodyA++) { for (int iBodyB = iBodyA + 1; iBodyB < world.NumBodies; iBodyB++) { Physics.RigidBody bodyA = world.Bodies[iBodyA]; Physics.RigidBody bodyB = world.Bodies[iBodyB]; if (bodyA.Collider->Type == ColliderType.Mesh && bodyB.Collider->Type == ColliderType.Mesh) { continue; // TODO - no mesh-mesh manifold support yet } // Build manifolds BlockStream contacts = new BlockStream(1, 0, Allocator.Temp); BlockStream.Writer contactWriter = contacts; contactWriter.BeginForEachIndex(0); ManifoldQueries.BodyBody(ref world, new BodyIndexPair { BodyAIndex = iBodyA, BodyBIndex = iBodyB }, 1.0f, ref contactWriter); contactWriter.EndForEachIndex(); // Read each manifold BlockStream.Reader contactReader = contacts; contactReader.BeginForEachIndex(0); int manifoldIndex = 0; while (contactReader.RemainingItemCount > 0) { string failureMessage = iWorld + " (" + worldState + ") " + iBodyA + " vs " + iBodyB + " #" + manifoldIndex; manifoldIndex++; // Read the manifold header ContactHeader header = contactReader.Read <ContactHeader>(); ConvexConvexManifoldQueries.Manifold manifold = new ConvexConvexManifoldQueries.Manifold(); manifold.NumContacts = header.NumContacts; manifold.Normal = header.Normal; // Get the leaf shapes ChildCollider leafA, leafB; { Collider.GetLeafCollider(bodyA.Collider, bodyA.WorldFromBody, header.ColliderKeys.ColliderKeyA, out leafA); Collider.GetLeafCollider(bodyB.Collider, bodyB.WorldFromBody, header.ColliderKeys.ColliderKeyB, out leafB); } // Read each contact point int minIndex = 0; for (int iContact = 0; iContact < header.NumContacts; iContact++) { // Read the contact and find the closest ContactPoint contact = contactReader.Read <ContactPoint>(); manifold[iContact] = contact; if (contact.Distance < manifold[minIndex].Distance) { minIndex = iContact; } // Check that the contact point is on or inside the shape CheckPointOnSurface(ref leafA, contact.Position + manifold.Normal * contact.Distance, failureMessage + " contact " + iContact + " leaf A"); CheckPointOnSurface(ref leafB, contact.Position, failureMessage + " contact " + iContact + " leaf B"); } // Check the closest point { ContactPoint closestPoint = manifold[minIndex]; RigidTransform aFromWorld = math.inverse(leafA.TransformFromChild); DistanceQueries.Result result = new DistanceQueries.Result { PositionOnAinA = math.transform(aFromWorld, closestPoint.Position + manifold.Normal * closestPoint.Distance), NormalInA = math.mul(aFromWorld.rot, manifold.Normal), Distance = closestPoint.Distance }; MTransform aFromB = new MTransform(math.mul(aFromWorld, leafB.TransformFromChild)); float referenceDistance = DistanceQueries.ConvexConvex(leafA.Collider, leafB.Collider, aFromB).Distance; ValidateDistanceResult(result, ref ((ConvexCollider *)leafA.Collider)->ConvexHull, ref ((ConvexCollider *)leafB.Collider)->ConvexHull, aFromB, referenceDistance, failureMessage + " closest point"); } // Check that the manifold is flat CheckManifoldFlat(ref manifold, manifold.Normal, failureMessage + ": non-flat A"); CheckManifoldFlat(ref manifold, float3.zero, failureMessage + ": non-flat B"); } contacts.Dispose(); } } world.Dispose(); // TODO leaking memory if the test fails } }