public StatefulTriggerEvent(Entity entityA, Entity entityB, int bodyIndexA, int bodyIndexB,
ColliderKey colliderKeyA, ColliderKey colliderKeyB)
{
Entities = new EntityPair
{
EntityA = entityA,
EntityB = entityB
};
BodyIndices = new BodyIndexPair
{
BodyIndexA = bodyIndexA,
BodyIndexB = bodyIndexB
};
ColliderKeys = new ColliderKeyPair
{
ColliderKeyA = colliderKeyA,
ColliderKeyB = colliderKeyB
};
State = default;
}
public StatefulCollisionEvent(Entity entityA, Entity entityB, int bodyIndexA, int bodyIndexB,
ColliderKey colliderKeyA, ColliderKey colliderKeyB, float3 normal)
{
Entities = new EntityPair
{
EntityA = entityA,
EntityB = entityB
};
BodyIndices = new BodyIndexPair
{
BodyIndexA = bodyIndexA,
BodyIndexB = bodyIndexB
};
ColliderKeys = new ColliderKeyPair
{
ColliderKeyA = colliderKeyA,
ColliderKeyB = colliderKeyB
};
Normal = normal;
CollidingState = default;
CollisionDetails = default;
}
private static unsafe void ConvexTerrain(
Context context, ColliderKeyPair colliderKeys, Collider *convexColliderA, Collider *terrainColliderB, MTransform worldFromA, MTransform worldFromB,
float maxDistance, bool flipped)
{
ref var terrain = ref ((TerrainCollider *)terrainColliderB)->Terrain;
public void AddColliderKeys(ColliderKey *keys, int count)
{
var colliderKeys = new ColliderKeyPair {
ColliderKeyA = m_ConvexColliderKey, ColliderKeyB = m_ConvexColliderKey
};
CollisionFilter filter = m_ConvexColliderA->Filter;
// Collide the convex A with all overlapping leaves of B
switch (m_CompositeColliderB->Type)
{
// Special case meshes (since we know all polygons will be built on the fly)
case ColliderType.Mesh:
{
Mesh *mesh = &((MeshCollider *)m_CompositeColliderB)->Mesh;
uint numMeshKeyBits = mesh->NumColliderKeyBits;
var polygon = new PolygonCollider();
polygon.InitEmpty();
for (int i = 0; i < count; i++)
{
ColliderKey compositeKey = m_CompositeColliderKeyPath.GetLeafKey(keys[i]);
uint meshKey = compositeKey.Value >> (32 - (int)numMeshKeyBits);
if (mesh->GetPolygon(meshKey, filter, ref polygon))
{
if (m_Flipped)
{
colliderKeys.ColliderKeyA = compositeKey;
}
else
{
colliderKeys.ColliderKeyB = compositeKey;
}
switch (m_ConvexColliderA->CollisionType)
{
case CollisionType.Convex:
ConvexConvex(
m_Context, colliderKeys, m_ConvexColliderA, (Collider *)&polygon,
m_WorldFromA, m_WorldFromB, m_CollisionTolerance, m_Flipped);
break;
case CollisionType.Terrain:
TerrainConvex(
m_Context, colliderKeys, m_ConvexColliderA, (Collider *)&polygon,
m_WorldFromA, m_WorldFromB, m_CollisionTolerance, m_Flipped);
break;
default: // GetLeaf() may not return a composite collider
throw new NotImplementedException();
}
}
}
}
break;
// General case for all other composites (compounds, compounds of meshes, etc)
default:
{
for (int i = 0; i < count; i++)
{
ColliderKey compositeKey = m_CompositeColliderKeyPath.GetLeafKey(keys[i]);
m_CompositeColliderB->GetLeaf(compositeKey, out ChildCollider leaf);
if (CollisionFilter.IsCollisionEnabled(filter, leaf.Collider->Filter)) // TODO: shouldn't be needed if/when filtering is done fully by the BVH query
{
if (m_Flipped)
{
colliderKeys.ColliderKeyA = compositeKey;
}
else
{
colliderKeys.ColliderKeyB = compositeKey;
}
MTransform worldFromLeafB = Mul(m_WorldFromB, new MTransform(leaf.TransformFromChild));
switch (leaf.Collider->CollisionType)
{
case CollisionType.Convex:
ConvexConvex(
m_Context, colliderKeys, m_ConvexColliderA, leaf.Collider,
m_WorldFromA, worldFromLeafB, m_CollisionTolerance, m_Flipped);
break;
case CollisionType.Terrain:
ConvexTerrain(
m_Context, colliderKeys, m_ConvexColliderA, leaf.Collider,
m_WorldFromA, worldFromLeafB, m_CollisionTolerance, m_Flipped);
break;
default: // GetLeaf() may not return a composite collider
throw new NotImplementedException();
}
}
}
}
break;
}
}
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 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]);
}
}
}
