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
}
}
