private static unsafe void WriteManifold(ConvexConvexManifoldQueries.Manifold manifold, Context context, ColliderKeyPair colliderKeys,
Material materialA, Material materialB, bool flipped)
{
// 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);
}
}
context.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;
}
context.ContactWriter->Write(manifold[contactIndex]);
}
}
}
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]);
}
}
}
