private static unsafe void ConvexConvex( Context context, ColliderKeyPair colliderKeys, Collider *convexColliderA, Collider *convexColliderB, MTransform worldFromA, MTransform worldFromB, float maxDistance, bool flipped, ref BlockStream.Writer contactWriter) { MTransform aFromB = Mul(Inverse(worldFromA), worldFromB); ConvexConvexManifoldQueries.Manifold contactManifold; switch (convexColliderA->Type) { case ColliderType.Sphere: switch (convexColliderB->Type) { case ColliderType.Sphere: ConvexConvexManifoldQueries.SphereSphere( (SphereCollider *)convexColliderA, (SphereCollider *)convexColliderB, worldFromA, aFromB, maxDistance, out contactManifold); break; case ColliderType.Capsule: ConvexConvexManifoldQueries.CapsuleSphere( (CapsuleCollider *)convexColliderB, (SphereCollider *)convexColliderA, worldFromB, Inverse(aFromB), maxDistance, out contactManifold); flipped = !flipped; break; case ColliderType.Triangle: ConvexConvexManifoldQueries.TriangleSphere( (PolygonCollider *)convexColliderB, (SphereCollider *)convexColliderA, worldFromB, Inverse(aFromB), maxDistance, out contactManifold); flipped = !flipped; break; case ColliderType.Box: ConvexConvexManifoldQueries.BoxSphere( (BoxCollider *)convexColliderB, (SphereCollider *)convexColliderA, worldFromB, Inverse(aFromB), maxDistance, out contactManifold); flipped = !flipped; break; case ColliderType.Quad: case ColliderType.Cylinder: case ColliderType.Convex: ConvexConvexManifoldQueries.ConvexConvex( ref ((SphereCollider *)convexColliderA)->ConvexHull, ref ((ConvexCollider *)convexColliderB)->ConvexHull, worldFromA, aFromB, maxDistance, out contactManifold); break; default: throw new NotImplementedException(); } break; case ColliderType.Box: switch (convexColliderB->Type) { case ColliderType.Sphere: ConvexConvexManifoldQueries.BoxSphere( (BoxCollider *)convexColliderA, (SphereCollider *)convexColliderB, worldFromA, aFromB, maxDistance, out contactManifold); break; case ColliderType.Triangle: ConvexConvexManifoldQueries.BoxTriangle( (BoxCollider *)convexColliderA, (PolygonCollider *)convexColliderB, worldFromA, aFromB, maxDistance, out contactManifold); break; case ColliderType.Box: ConvexConvexManifoldQueries.BoxBox( (BoxCollider *)convexColliderA, (BoxCollider *)convexColliderB, worldFromA, aFromB, maxDistance, out contactManifold); break; case ColliderType.Capsule: case ColliderType.Quad: case ColliderType.Cylinder: case ColliderType.Convex: ConvexConvexManifoldQueries.ConvexConvex( ref ((BoxCollider *)convexColliderA)->ConvexHull, ref ((ConvexCollider *)convexColliderB)->ConvexHull, worldFromA, aFromB, maxDistance, out contactManifold); break; default: throw new NotImplementedException(); } break; case ColliderType.Capsule: switch (convexColliderB->Type) { case ColliderType.Sphere: ConvexConvexManifoldQueries.CapsuleSphere( (CapsuleCollider *)convexColliderA, (SphereCollider *)convexColliderB, worldFromA, aFromB, maxDistance, out contactManifold); break; case ColliderType.Capsule: ConvexConvexManifoldQueries.CapsuleCapsule( (CapsuleCollider *)convexColliderA, (CapsuleCollider *)convexColliderB, worldFromA, aFromB, maxDistance, out contactManifold); break; case ColliderType.Triangle: ConvexConvexManifoldQueries.CapsuleTriangle( (CapsuleCollider *)convexColliderA, (PolygonCollider *)convexColliderB, worldFromA, aFromB, maxDistance, out contactManifold); break; case ColliderType.Quad: case ColliderType.Box: case ColliderType.Cylinder: case ColliderType.Convex: ConvexConvexManifoldQueries.ConvexConvex( ref ((CapsuleCollider *)convexColliderA)->ConvexHull, ref ((ConvexCollider *)convexColliderB)->ConvexHull, worldFromA, aFromB, maxDistance, out contactManifold); break; default: throw new NotImplementedException(); } break; case ColliderType.Triangle: switch (convexColliderB->Type) { case ColliderType.Sphere: ConvexConvexManifoldQueries.TriangleSphere( (PolygonCollider *)convexColliderA, (SphereCollider *)convexColliderB, worldFromA, aFromB, maxDistance, out contactManifold); break; case ColliderType.Capsule: ConvexConvexManifoldQueries.CapsuleTriangle( (CapsuleCollider *)convexColliderB, (PolygonCollider *)convexColliderA, worldFromB, Inverse(aFromB), maxDistance, out contactManifold); flipped = !flipped; break; case ColliderType.Box: ConvexConvexManifoldQueries.BoxTriangle( (BoxCollider *)convexColliderB, (PolygonCollider *)convexColliderA, worldFromB, Inverse(aFromB), maxDistance, out contactManifold); flipped = !flipped; break; case ColliderType.Triangle: case ColliderType.Quad: case ColliderType.Cylinder: case ColliderType.Convex: ConvexConvexManifoldQueries.ConvexConvex( ref ((PolygonCollider *)convexColliderA)->ConvexHull, ref ((ConvexCollider *)convexColliderB)->ConvexHull, worldFromA, aFromB, maxDistance, out contactManifold); break; default: throw new NotImplementedException(); } break; case ColliderType.Quad: case ColliderType.Cylinder: case ColliderType.Convex: ConvexConvexManifoldQueries.ConvexConvex( ref ((ConvexCollider *)convexColliderA)->ConvexHull, ref ((ConvexCollider *)convexColliderB)->ConvexHull, worldFromA, aFromB, maxDistance, out contactManifold); break; default: throw new NotImplementedException(); } // Write results to stream if (contactManifold.NumContacts > 0) { if (flipped) { contactManifold.Flip(); } var header = new ContactHeader { BodyPair = context.BodyIndices, BodyCustomDatas = context.BodyCustomDatas, NumContacts = contactManifold.NumContacts, Normal = contactManifold.Normal, ColliderKeys = colliderKeys }; // Apply materials { Material materialA = ((ConvexColliderHeader *)convexColliderA)->Material; Material materialB = ((ConvexColliderHeader *)convexColliderB)->Material; Material.MaterialFlags combinedFlags = materialA.Flags | materialB.Flags; if ((combinedFlags & Material.MaterialFlags.IsTrigger) != 0) { header.JacobianFlags |= JacobianFlags.IsTrigger; } else { if ((combinedFlags & Material.MaterialFlags.EnableCollisionEvents) != 0) { header.JacobianFlags |= JacobianFlags.EnableCollisionEvents; } if ((combinedFlags & Material.MaterialFlags.EnableMassFactors) != 0) { header.JacobianFlags |= JacobianFlags.EnableMassFactors; } if ((combinedFlags & Material.MaterialFlags.EnableSurfaceVelocity) != 0) { header.JacobianFlags |= JacobianFlags.EnableSurfaceVelocity; } if ((combinedFlags & Material.MaterialFlags.EnableMaxImpulse) != 0) { header.JacobianFlags |= JacobianFlags.EnableMaxImpulse; } header.CoefficientOfFriction = Material.GetCombinedFriction(materialA, materialB); header.CoefficientOfRestitution = Material.GetCombinedRestitution(materialA, materialB); } } contactWriter.Write(header); for (int contactIndex = 0; contactIndex < header.NumContacts; contactIndex++) { contactWriter.Write(contactManifold[contactIndex]); } } }
// Write a set of contact manifolds for a pair of bodies to the given stream. public static unsafe void BodyBody(ref PhysicsWorld world, BodyIndexPair pair, float timeStep, ref BlockStream.Writer contactWriter) { RigidBody rigidBodyA = world.Bodies[pair.BodyAIndex]; RigidBody rigidBodyB = world.Bodies[pair.BodyBIndex]; Collider *colliderA = rigidBodyA.Collider; Collider *colliderB = rigidBodyB.Collider; if (colliderA == null || colliderB == null || !CollisionFilter.IsCollisionEnabled(colliderA->Filter, colliderB->Filter)) { return; } // Build combined motion expansion MotionExpansion expansion; { MotionExpansion GetBodyExpansion(int bodyIndex, NativeSlice <MotionVelocity> mvs) { return(bodyIndex < mvs.Length ? mvs[bodyIndex].CalculateExpansion(timeStep) : MotionExpansion.Zero); } MotionExpansion expansionA = GetBodyExpansion(pair.BodyAIndex, world.MotionVelocities); MotionExpansion expansionB = GetBodyExpansion(pair.BodyBIndex, world.MotionVelocities); expansion = new MotionExpansion { Linear = expansionA.Linear - expansionB.Linear, Uniform = expansionA.Uniform + expansionB.Uniform + world.CollisionTolerance }; } var context = new Context { BodyIndices = pair, BodyCustomDatas = new CustomDataPair { CustomDataA = rigidBodyA.CustomData, CustomDataB = rigidBodyB.CustomData } }; var worldFromA = new MTransform(rigidBodyA.WorldFromBody); var worldFromB = new MTransform(rigidBodyB.WorldFromBody); // Dispatch to appropriate manifold generator switch (colliderA->CollisionType) { case CollisionType.Convex: switch (colliderB->CollisionType) { case CollisionType.Convex: ConvexConvex(context, ColliderKeyPair.Empty, colliderA, colliderB, worldFromA, worldFromB, expansion.MaxDistance, false, ref contactWriter); break; case CollisionType.Composite: ConvexComposite(context, ColliderKey.Empty, colliderA, colliderB, worldFromA, worldFromB, expansion, false, ref contactWriter); break; } break; case CollisionType.Composite: switch (colliderB->CollisionType) { case CollisionType.Convex: CompositeConvex(context, colliderA, colliderB, worldFromA, worldFromB, expansion, false, ref contactWriter); break; case CollisionType.Composite: CompositeComposite(context, colliderA, colliderB, worldFromA, worldFromB, expansion, false, ref contactWriter); break; } break; } }
private static unsafe void ConvexTerrain( Context context, ColliderKeyPair colliderKeys, Collider *convexColliderA, Collider *terrainColliderB, MTransform worldFromA, MTransform worldFromB, float maxDistance, bool flipped, ref BlockStream.Writer contactWriter) { ref var terrain = ref ((TerrainCollider *)terrainColliderB)->Terrain;
// Write a set of contact manifolds for a pair of bodies to the given stream. public static unsafe void BodyBody(RigidBody rigidBodyA, RigidBody rigidBodyB, MotionVelocity motionVelocityA, MotionVelocity motionVelocityB, float collisionTolerance, float timeStep, BodyIndexPair pair, ref BlockStream.Writer contactWriter) { Collider *colliderA = rigidBodyA.Collider; Collider *colliderB = rigidBodyB.Collider; if (colliderA == null || colliderB == null || !CollisionFilter.IsCollisionEnabled(colliderA->Filter, colliderB->Filter)) { return; } // Build combined motion expansion MotionExpansion expansion; { MotionExpansion expansionA = motionVelocityA.CalculateExpansion(timeStep); MotionExpansion expansionB = motionVelocityB.CalculateExpansion(timeStep); expansion = new MotionExpansion { Linear = expansionA.Linear - expansionB.Linear, Uniform = expansionA.Uniform + expansionB.Uniform + collisionTolerance }; } var context = new Context { BodyIndices = pair, BodyCustomTags = new CustomTagsPair { CustomTagsA = rigidBodyA.CustomTags, CustomTagsB = rigidBodyB.CustomTags }, BodiesHaveInfiniteMass = !math.any(motionVelocityA.InverseInertiaAndMass) && !math.any(motionVelocityB.InverseInertiaAndMass) }; var worldFromA = new MTransform(rigidBodyA.WorldFromBody); var worldFromB = new MTransform(rigidBodyB.WorldFromBody); // Dispatch to appropriate manifold generator switch (colliderA->CollisionType) { case CollisionType.Convex: switch (colliderB->CollisionType) { case CollisionType.Convex: ConvexConvex(context, ColliderKeyPair.Empty, colliderA, colliderB, worldFromA, worldFromB, expansion.MaxDistance, false, ref contactWriter); break; case CollisionType.Composite: ConvexComposite(context, ColliderKey.Empty, colliderA, colliderB, worldFromA, worldFromB, expansion, false, ref contactWriter); break; case CollisionType.Terrain: ConvexTerrain(context, ColliderKeyPair.Empty, colliderA, colliderB, worldFromA, worldFromB, expansion.MaxDistance, false, ref contactWriter); break; } break; case CollisionType.Composite: switch (colliderB->CollisionType) { case CollisionType.Convex: CompositeConvex(context, colliderA, colliderB, worldFromA, worldFromB, expansion, false, ref contactWriter); break; case CollisionType.Composite: CompositeComposite(context, colliderA, colliderB, worldFromA, worldFromB, expansion, false, ref contactWriter); break; case CollisionType.Terrain: CompositeTerrain(context, colliderA, colliderB, worldFromA, worldFromB, expansion.MaxDistance, false, ref contactWriter); break; } break; case CollisionType.Terrain: switch (colliderB->CollisionType) { case CollisionType.Convex: TerrainConvex(context, ColliderKeyPair.Empty, colliderA, colliderB, worldFromA, worldFromB, expansion.MaxDistance, false, ref contactWriter); break; case CollisionType.Composite: TerrainComposite(context, colliderA, colliderB, worldFromA, worldFromB, expansion.MaxDistance, false, ref contactWriter); break; case CollisionType.Terrain: UnityEngine.Assertions.Assert.IsTrue(false); break; } break; } }
private static unsafe void ConvexConvex( Context context, ColliderKeyPair colliderKeys, Collider *convexColliderA, Collider *convexColliderB, MTransform worldFromA, MTransform worldFromB, float maxDistance, bool flipped, ref BlockStream.Writer contactWriter) { Material materialA = ((ConvexColliderHeader *)convexColliderA)->Material; Material materialB = ((ConvexColliderHeader *)convexColliderB)->Material; // Skip if the bodies have infinite mass and the materials don't want to raise any solver events, // since the resulting contacts can't have any effect during solving if (context.BodiesHaveInfiniteMass) { if (((materialA.Flags | materialB.Flags) & (Material.MaterialFlags.IsTrigger | Material.MaterialFlags.EnableCollisionEvents)) == 0) { return; } } MTransform aFromB = Mul(Inverse(worldFromA), worldFromB); ConvexConvexManifoldQueries.Manifold contactManifold; switch (convexColliderA->Type) { case ColliderType.Sphere: switch (convexColliderB->Type) { case ColliderType.Sphere: ConvexConvexManifoldQueries.SphereSphere( (SphereCollider *)convexColliderA, (SphereCollider *)convexColliderB, worldFromA, aFromB, maxDistance, out contactManifold); break; case ColliderType.Capsule: ConvexConvexManifoldQueries.CapsuleSphere( (CapsuleCollider *)convexColliderB, (SphereCollider *)convexColliderA, worldFromB, Inverse(aFromB), maxDistance, out contactManifold); flipped = !flipped; break; case ColliderType.Triangle: ConvexConvexManifoldQueries.TriangleSphere( (PolygonCollider *)convexColliderB, (SphereCollider *)convexColliderA, worldFromB, Inverse(aFromB), maxDistance, out contactManifold); flipped = !flipped; break; case ColliderType.Box: ConvexConvexManifoldQueries.BoxSphere( (BoxCollider *)convexColliderB, (SphereCollider *)convexColliderA, worldFromB, Inverse(aFromB), maxDistance, out contactManifold); flipped = !flipped; break; case ColliderType.Quad: case ColliderType.Cylinder: case ColliderType.Convex: ConvexConvexManifoldQueries.ConvexConvex( ref ((SphereCollider *)convexColliderA)->ConvexHull, ref ((ConvexCollider *)convexColliderB)->ConvexHull, worldFromA, aFromB, maxDistance, out contactManifold); break; default: throw new NotImplementedException(); } break; case ColliderType.Box: switch (convexColliderB->Type) { case ColliderType.Sphere: ConvexConvexManifoldQueries.BoxSphere( (BoxCollider *)convexColliderA, (SphereCollider *)convexColliderB, worldFromA, aFromB, maxDistance, out contactManifold); break; case ColliderType.Triangle: ConvexConvexManifoldQueries.BoxTriangle( (BoxCollider *)convexColliderA, (PolygonCollider *)convexColliderB, worldFromA, aFromB, maxDistance, out contactManifold); break; case ColliderType.Box: ConvexConvexManifoldQueries.BoxBox( (BoxCollider *)convexColliderA, (BoxCollider *)convexColliderB, worldFromA, aFromB, maxDistance, out contactManifold); break; case ColliderType.Capsule: case ColliderType.Quad: case ColliderType.Cylinder: case ColliderType.Convex: ConvexConvexManifoldQueries.ConvexConvex( ref ((BoxCollider *)convexColliderA)->ConvexHull, ref ((ConvexCollider *)convexColliderB)->ConvexHull, worldFromA, aFromB, maxDistance, out contactManifold); break; default: throw new NotImplementedException(); } break; case ColliderType.Capsule: switch (convexColliderB->Type) { case ColliderType.Sphere: ConvexConvexManifoldQueries.CapsuleSphere( (CapsuleCollider *)convexColliderA, (SphereCollider *)convexColliderB, worldFromA, aFromB, maxDistance, out contactManifold); break; case ColliderType.Capsule: ConvexConvexManifoldQueries.CapsuleCapsule( (CapsuleCollider *)convexColliderA, (CapsuleCollider *)convexColliderB, worldFromA, aFromB, maxDistance, out contactManifold); break; case ColliderType.Triangle: ConvexConvexManifoldQueries.CapsuleTriangle( (CapsuleCollider *)convexColliderA, (PolygonCollider *)convexColliderB, worldFromA, aFromB, maxDistance, out contactManifold); break; case ColliderType.Quad: case ColliderType.Box: case ColliderType.Cylinder: case ColliderType.Convex: ConvexConvexManifoldQueries.ConvexConvex( ref ((CapsuleCollider *)convexColliderA)->ConvexHull, ref ((ConvexCollider *)convexColliderB)->ConvexHull, worldFromA, aFromB, maxDistance, out contactManifold); break; default: throw new NotImplementedException(); } break; case ColliderType.Triangle: switch (convexColliderB->Type) { case ColliderType.Sphere: ConvexConvexManifoldQueries.TriangleSphere( (PolygonCollider *)convexColliderA, (SphereCollider *)convexColliderB, worldFromA, aFromB, maxDistance, out contactManifold); break; case ColliderType.Capsule: ConvexConvexManifoldQueries.CapsuleTriangle( (CapsuleCollider *)convexColliderB, (PolygonCollider *)convexColliderA, worldFromB, Inverse(aFromB), maxDistance, out contactManifold); flipped = !flipped; break; case ColliderType.Box: ConvexConvexManifoldQueries.BoxTriangle( (BoxCollider *)convexColliderB, (PolygonCollider *)convexColliderA, worldFromB, Inverse(aFromB), maxDistance, out contactManifold); flipped = !flipped; break; case ColliderType.Triangle: case ColliderType.Quad: case ColliderType.Cylinder: case ColliderType.Convex: ConvexConvexManifoldQueries.ConvexConvex( ref ((PolygonCollider *)convexColliderA)->ConvexHull, ref ((ConvexCollider *)convexColliderB)->ConvexHull, worldFromA, aFromB, maxDistance, out contactManifold); break; default: throw new NotImplementedException(); } break; case ColliderType.Quad: case ColliderType.Cylinder: case ColliderType.Convex: ConvexConvexManifoldQueries.ConvexConvex( ref ((ConvexCollider *)convexColliderA)->ConvexHull, ref ((ConvexCollider *)convexColliderB)->ConvexHull, worldFromA, aFromB, maxDistance, out contactManifold); break; default: throw new NotImplementedException(); } WriteManifold(contactManifold, context, colliderKeys, materialA, materialB, flipped, ref contactWriter); }
private static void WriteManifold(ConvexConvexManifoldQueries.Manifold manifold, Context context, ColliderKeyPair colliderKeys, Material materialA, Material materialB, bool flipped, ref BlockStream.Writer contactWriter) { // Write results to stream if (manifold.NumContacts > 0) { if (flipped) { manifold.Flip(); } var header = new ContactHeader { BodyPair = context.BodyIndices, BodyCustomTags = context.BodyCustomTags, NumContacts = manifold.NumContacts, Normal = manifold.Normal, ColliderKeys = colliderKeys }; // Apply materials { Material.MaterialFlags combinedFlags = materialA.Flags | materialB.Flags; if ((combinedFlags & Material.MaterialFlags.IsTrigger) != 0) { header.JacobianFlags |= JacobianFlags.IsTrigger; } else { if ((combinedFlags & Material.MaterialFlags.EnableCollisionEvents) != 0) { header.JacobianFlags |= JacobianFlags.EnableCollisionEvents; } if ((combinedFlags & Material.MaterialFlags.EnableMassFactors) != 0) { header.JacobianFlags |= JacobianFlags.EnableMassFactors; } if ((combinedFlags & Material.MaterialFlags.EnableSurfaceVelocity) != 0) { header.JacobianFlags |= JacobianFlags.EnableSurfaceVelocity; } header.CoefficientOfFriction = Material.GetCombinedFriction(materialA, materialB); header.CoefficientOfRestitution = Material.GetCombinedRestitution(materialA, materialB); } } contactWriter.Write(header); // Group the contact points in 2s (when 4-6 contact points) and 3s (6 or more contact points) // to avoid the order forcing the magnitude of the impulse on one side of the face. // When less than 4 contact points access them in order. int startIndex = 0; int increment = header.NumContacts < 6 ? math.max(header.NumContacts / 2, 1) : (header.NumContacts / 3 + ((header.NumContacts % 3 > 0) ? 1 : 0)); for (int contactIndex = 0; ; contactIndex += increment) { if (contactIndex >= header.NumContacts) { startIndex++; if (startIndex == increment) { break; } contactIndex = startIndex; } contactWriter.Write(manifold[contactIndex]); } } }
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 // TODO - Box-box and box-triangle manifolds have special manifold generation code that trades some accuracy for performance, see comments in // ConvexConvexManifoldQueries.BoxBox() and BoxTriangle(). It may change later, until then they get an exception from the closest point distance test. ColliderType typeA = leafA.Collider->Type; ColliderType typeB = leafB.Collider->Type; bool skipClosestPointTest = (typeA == ColliderType.Box && (typeB == ColliderType.Box || typeB == ColliderType.Triangle)) || (typeB == ColliderType.Box && typeA == ColliderType.Triangle); if (!skipClosestPointTest) { 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 } }