private static unsafe void ConvexConvex(
Context context, ColliderKeyPair colliderKeys,
Collider *convexColliderA, Collider *convexColliderB, MTransform worldFromA, MTransform worldFromB,
float maxDistance, bool flipped)
{
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);
}
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]);
}
}
}
