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]); } } }