private void PersistentManifoldContactProcessed(BulletSharp.ManifoldPoint cp, BulletSharp.CollisionObject body0, BulletSharp.CollisionObject body1) { if (body0 == null || body1 == null) { return; } //this can fail and will fail in the case of multiple scenes and bodies not of the current simulation ( working as intended ) Collider colA, colB; if (!aliveColliders.TryGetValue(body0, out colA)) { return; } if (!aliveColliders.TryGetValue(body1, out colB)) { return; } if (colA == null || colB == null || !colA.ContactsAlwaysValid && !colB.ContactsAlwaysValid) { return; } //Pairs management Collision pair = null; var newPair = true; foreach (var pair1 in colA.Collisions) { if ((pair1.ColliderA != colA || pair1.ColliderB != colB) && (pair1.ColliderA != colB || pair1.ColliderB != colA)) { continue; } pair = pair1; newPair = false; break; } if (pair == null) { pair = new Collision { ColliderA = colA, ColliderB = colB, Contacts = new List <ContactPoint>() }; colA.Collisions.Add(pair); colB.Collisions.Add(pair); } //Contacts management ContactPoint contact = null; var newContact = true; foreach (var contact1 in pair.Contacts) { if (contact1.Handle.IsAllocated && cp.UserPersistentPtr != IntPtr.Zero && contact1.Handle.Target != GCHandle.FromIntPtr(cp.UserPersistentPtr).Target) { continue; } contact = contact1; newContact = false; break; } if (contact == null) { contact = new ContactPoint { Distance = cp.Distance, PositionOnA = new Vector3(cp.PositionWorldOnA.X, cp.PositionWorldOnA.Y, cp.PositionWorldOnA.Z), PositionOnB = new Vector3(cp.PositionWorldOnB.X, cp.PositionWorldOnB.Y, cp.PositionWorldOnB.Z), Normal = new Vector3(cp.NormalWorldOnB.X, cp.NormalWorldOnB.Y, cp.NormalWorldOnB.Z), Pair = pair }; pair.Contacts.Add(contact); contact.Handle = GCHandle.Alloc(contact); cp.UserPersistentPtr = GCHandle.ToIntPtr(contact.Handle); contact.Manifold = cp; } if (newPair) { //are we the first pair we detect? if (colA.Collisions.Count == 1) { firstPairsCache.Add(new KeyValuePair <Collider, Collision>(colA, pair)); } //are we the first pair we detect? if (colB.Collisions.Count == 1) { firstPairsCache.Add(new KeyValuePair <Collider, Collision>(colB, pair)); } newPairsCache.Add(new KeyValuePair <Collider, Collision>(colA, pair)); newPairsCache.Add(new KeyValuePair <Collider, Collision>(colB, pair)); } if (newContact) { newContactsCache.Add(new KeyValuePair <Collision, ContactPoint>(pair, contact)); } else { updatedContactsCache.Add(new KeyValuePair <Collision, ContactPoint>(pair, contact)); } }
private void PersistentManifoldContactProcessed(BulletSharp.ManifoldPoint cp, BulletSharp.CollisionObject body0, BulletSharp.CollisionObject body1) { var colA = body0 != null ? aliveColliders[body0] : null; var colB = body1 != null ? aliveColliders[body1] : null; if (colA == null || colB == null || !colA.ContactsAlwaysValid && !colB.ContactsAlwaysValid) return; //Pairs management Collision pair = null; var newPair = true; foreach (var pair1 in colA.Collisions) { if ((pair1.ColliderA != colA || pair1.ColliderB != colB) && (pair1.ColliderA != colB || pair1.ColliderB != colA)) continue; pair = pair1; newPair = false; break; } if (pair == null) { pair = new Collision { ColliderA = colA, ColliderB = colB, Contacts = new List<ContactPoint>() }; colA.Collisions.Add(pair); colB.Collisions.Add(pair); } //Contacts management ContactPoint contact = null; var newContact = true; foreach (var contact1 in pair.Contacts) { if (contact1 != cp.UserPersistentData) continue; contact = contact1; newContact = false; break; } if (contact == null) { contact = new ContactPoint { Distance = cp.Distance, PositionOnA = new Vector3(cp.PositionWorldOnA.X, cp.PositionWorldOnA.Y, cp.PositionWorldOnA.Z), PositionOnB = new Vector3(cp.PositionWorldOnB.X, cp.PositionWorldOnB.Y, cp.PositionWorldOnB.Z), Normal = new Vector3(cp.NormalWorldOnB.X, cp.NormalWorldOnB.Y, cp.NormalWorldOnB.Z), Pair = pair }; pair.Contacts.Add(contact); cp.UserPersistentData = contact; contact.Manifold = cp; } if (newPair) { //are we the first pair we detect? if (colA.Collisions.Count == 1) { firstPairsCache.Add(new KeyValuePair<Collider, Collision>(colA, pair)); } //are we the first pair we detect? if (colB.Collisions.Count == 1) { firstPairsCache.Add(new KeyValuePair<Collider, Collision>(colB, pair)); } newPairsCache.Add(new KeyValuePair<Collider, Collision>(colA, pair)); newPairsCache.Add(new KeyValuePair<Collider, Collision>(colB, pair)); } if (newContact) { newContactsCache.Add(new KeyValuePair<Collision, ContactPoint>(pair, contact)); } else { updatedContactsCache.Add(new KeyValuePair<Collision, ContactPoint>(pair, contact)); } }
private void PersistentManifoldContactProcessed(BulletSharp.ManifoldPoint cp, BulletSharp.CollisionObject body0, BulletSharp.CollisionObject body1) { if (body0 == null || body1 == null) return; //this can fail and will fail in the case of multiple scenes and bodies not of the current simulation ( working as intended ) Collider colA, colB; if (!aliveColliders.TryGetValue(body0, out colA)) return; if (!aliveColliders.TryGetValue(body1, out colB)) return; if (colA == null || colB == null || !colA.ContactsAlwaysValid && !colB.ContactsAlwaysValid) return; //Pairs management Collision pair = null; var newPair = true; foreach (var pair1 in colA.Collisions) { if ((pair1.ColliderA != colA || pair1.ColliderB != colB) && (pair1.ColliderA != colB || pair1.ColliderB != colA)) continue; pair = pair1; newPair = false; break; } if (pair == null) { pair = new Collision { ColliderA = colA, ColliderB = colB, Contacts = new List<ContactPoint>() }; colA.Collisions.Add(pair); colB.Collisions.Add(pair); } //Contacts management ContactPoint contact = null; var newContact = true; foreach (var contact1 in pair.Contacts) { if (contact1.Handle.IsAllocated && cp.UserPersistentPtr != IntPtr.Zero && contact1.Handle.Target != GCHandle.FromIntPtr(cp.UserPersistentPtr).Target) continue; contact = contact1; newContact = false; break; } if (contact == null) { contact = new ContactPoint { Distance = cp.Distance, PositionOnA = new Vector3(cp.PositionWorldOnA.X, cp.PositionWorldOnA.Y, cp.PositionWorldOnA.Z), PositionOnB = new Vector3(cp.PositionWorldOnB.X, cp.PositionWorldOnB.Y, cp.PositionWorldOnB.Z), Normal = new Vector3(cp.NormalWorldOnB.X, cp.NormalWorldOnB.Y, cp.NormalWorldOnB.Z), Pair = pair }; pair.Contacts.Add(contact); contact.Handle = GCHandle.Alloc(contact); cp.UserPersistentPtr = GCHandle.ToIntPtr(contact.Handle); contact.Manifold = cp; } if (newPair) { //are we the first pair we detect? if (colA.Collisions.Count == 1) { firstPairsCache.Add(new KeyValuePair<Collider, Collision>(colA, pair)); } //are we the first pair we detect? if (colB.Collisions.Count == 1) { firstPairsCache.Add(new KeyValuePair<Collider, Collision>(colB, pair)); } newPairsCache.Add(new KeyValuePair<Collider, Collision>(colA, pair)); newPairsCache.Add(new KeyValuePair<Collider, Collision>(colB, pair)); } if (newContact) { newContactsCache.Add(new KeyValuePair<Collision, ContactPoint>(pair, contact)); } else { updatedContactsCache.Add(new KeyValuePair<Collision, ContactPoint>(pair, contact)); } }
internal void ProcessContacts() { contactsProfilingState.Begin(); processedPairsFastCache.Clear(); var numManifolds = collisionWorld.Dispatcher.NumManifolds; for (var i = 0; i < numManifolds; i++) { var manifold = collisionWorld.Dispatcher.GetManifoldByIndexInternal(i); var bodyA = manifold.Body0; var bodyB = manifold.Body1; var colA = (Collider)bodyA?.UserObject; var colB = (Collider)bodyB?.UserObject; if (colA == null || colB == null) { continue; } if (!colA.ContactsAlwaysValid && !colB.ContactsAlwaysValid) { continue; } //Pairs management Collision pair = null; var newPair = true; foreach (var pair1 in colA.Collisions) { if ((pair1.ColliderA != colA || pair1.ColliderB != colB) && (pair1.ColliderA != colB || pair1.ColliderB != colA)) { continue; } pair = pair1; newPair = false; break; } var numContacts = manifold.NumContacts; if (numContacts == 0 && newPair) { continue; } newContactsFastCache.Clear(); updatedContactsFastCache.Clear(); deletedContactsFastCache.Clear(); if (newPair) { if (collisionsQueue.Count > 0) { pair = collisionsQueue.Dequeue(); } else { pair = new Collision { Contacts = new List <ContactPoint>() }; } pair.ColliderA = colA; pair.ColliderB = colB; pair.Contacts.Clear(); colA.Collisions.Add(pair); colB.Collisions.Add(pair); alivePairsFastCache.Add(pair); } else { foreach (var contact in pair.Contacts) { deletedContactsFastCache.Add(contact); } } processedPairsFastCache.Add(pair); for (var y = 0; y < numContacts; y++) { var cp = manifold.GetContactPoint(y); ContactPoint contact = null; var newContact = true; foreach (var contact1 in pair.Contacts) { if (contact1.Handle.IsAllocated && cp.UserPersistentPtr != IntPtr.Zero && contact1.Handle.Target != GCHandle.FromIntPtr(cp.UserPersistentPtr).Target) { continue; } contact = contact1; newContact = false; break; } contactsProfilingState.Mark(); if (newContact) { contact = contactsQueue.Count > 0 ? contactsQueue.Dequeue() : new ContactPoint(); contact.Distance = cp.Distance; contact.PositionOnA = new Vector3(cp.PositionWorldOnA.X, cp.PositionWorldOnA.Y, cp.PositionWorldOnA.Z); contact.PositionOnB = new Vector3(cp.PositionWorldOnB.X, cp.PositionWorldOnB.Y, cp.PositionWorldOnB.Z); contact.Normal = new Vector3(cp.NormalWorldOnB.X, cp.NormalWorldOnB.Y, cp.NormalWorldOnB.Z); contact.Pair = pair; contact.Handle = GCHandle.Alloc(contact); cp.UserPersistentPtr = GCHandle.ToIntPtr(contact.Handle); pair.Contacts.Add(contact); } else { deletedContactsFastCache.Remove(contact); } if (newContact) { newContactsFastCache.Add(contact); } else { updatedContactsFastCache.Add(contact); } } //deliver async events if (newPair) { //are we the first pair we detect? if (colA.Collisions.Count == 1) { while (colA.FirstCollisionChannel.Balance < 0) { colA.FirstCollisionChannel.Send(pair); } } //are we the first pair we detect? if (colB.Collisions.Count == 1) { while (colB.FirstCollisionChannel.Balance < 0) { colB.FirstCollisionChannel.Send(pair); } } while (colA.NewPairChannel.Balance < 0) { colA.NewPairChannel.Send(pair); } while (colB.NewPairChannel.Balance < 0) { colB.NewPairChannel.Send(pair); } } foreach (var contact in newContactsFastCache) { while (contact.Pair.NewContactChannel.Balance < 0) { contact.Pair.NewContactChannel.Send(contact); } } foreach (var contact in updatedContactsFastCache) { while (contact.Pair.ContactUpdateChannel.Balance < 0) { contact.Pair.ContactUpdateChannel.Send(contact); } } foreach (var contact in deletedContactsFastCache) { while (contact.Pair.ContactEndedChannel.Balance < 0) { contact.Pair.ContactEndedChannel.Send(contact); } pair.Contacts.Remove(contact); contact.Handle.Free(); contactsQueue.Enqueue(contact); } if (pair.Contacts.Count == 0) { colA.Collisions.Remove(pair); colB.Collisions.Remove(pair); alivePairsFastCache.Remove(pair); collisionsQueue.Enqueue(pair); while (colA.PairEndedChannel.Balance < 0) { colA.PairEndedChannel.Send(pair); } while (colB.PairEndedChannel.Balance < 0) { colB.PairEndedChannel.Send(pair); } if (colA.Collisions.Count == 0) { while (colA.AllPairsEndedChannel.Balance < 0) { colA.AllPairsEndedChannel.Send(pair); } } if (colB.Collisions.Count == 0) { while (colB.AllPairsEndedChannel.Balance < 0) { colB.AllPairsEndedChannel.Send(pair); } } } } //Sometimes narrowphase is skipped it seems and we might get some stuck pair! foreach (var pair in alivePairsFastCache) { if (!processedPairsFastCache.Contains(pair)) { removedPairsFastCache.Enqueue(pair); } } while (removedPairsFastCache.Count > 0) { var pair = removedPairsFastCache.Dequeue(); alivePairsFastCache.Remove(pair); //this pair got removed! foreach (var contactPoint in pair.Contacts) { while (contactPoint.Pair.ContactEndedChannel.Balance < 0) { contactPoint.Pair.ContactEndedChannel.Send(contactPoint); } contactPoint.Handle.Free(); contactsQueue.Enqueue(contactPoint); } var colA = pair.ColliderA; var colB = pair.ColliderB; colA.Collisions.Remove(pair); colB.Collisions.Remove(pair); collisionsQueue.Enqueue(pair); while (colA.PairEndedChannel.Balance < 0) { colA.PairEndedChannel.Send(pair); } while (colB.PairEndedChannel.Balance < 0) { colB.PairEndedChannel.Send(pair); } if (colA.Collisions.Count == 0) { while (colA.AllPairsEndedChannel.Balance < 0) { colA.AllPairsEndedChannel.Send(pair); } } if (colB.Collisions.Count == 0) { while (colB.AllPairsEndedChannel.Balance < 0) { colB.AllPairsEndedChannel.Send(pair); } } } contactsProfilingState.End(); }
private void PersistentManifoldContactProcessed(BulletSharp.ManifoldPoint cp, BulletSharp.CollisionObject body0, BulletSharp.CollisionObject body1) { var colA = aliveColliders[body0]; var colB = aliveColliders[body1]; if (colA == null || colB == null || !colA.ContactsAlwaysValid && !colB.ContactsAlwaysValid) { return; } //Pairs management Collision pair = null; var newPair = true; foreach (var pair1 in colA.Pairs) { if ((pair1.ColliderA != colA || pair1.ColliderB != colB) && (pair1.ColliderA != colB || pair1.ColliderB != colA)) { continue; } pair = pair1; newPair = false; break; } if (pair == null) { pair = new Collision { ColliderA = colA, ColliderB = colB, Contacts = new List <ContactPoint>() }; colA.Pairs.Add(pair); colB.Pairs.Add(pair); } //Contacts management ContactPoint contact = null; var newContact = true; foreach (var contact1 in pair.Contacts) { if (contact1 != cp.UserPersistentData) { continue; } contact = contact1; newContact = false; break; } if (contact == null) { contact = new ContactPoint { Distance = cp.Distance, PositionOnA = new Vector3(cp.PositionWorldOnA.X, cp.PositionWorldOnA.Y, cp.PositionWorldOnA.Z), PositionOnB = new Vector3(cp.PositionWorldOnB.X, cp.PositionWorldOnB.Y, cp.PositionWorldOnB.Z), Normal = new Vector3(cp.NormalWorldOnB.X, cp.NormalWorldOnB.Y, cp.NormalWorldOnB.Z), Pair = pair }; pair.Contacts.Add(contact); cp.UserPersistentData = contact; } if (newPair) { //are we the first pair we detect? if (colA.Pairs.Count == 1) { firstPairsCache.Add(new KeyValuePair <Collider, Collision>(colA, pair)); } //are we the first pair we detect? if (colB.Pairs.Count == 1) { firstPairsCache.Add(new KeyValuePair <Collider, Collision>(colB, pair)); } newPairsCache.Add(new KeyValuePair <Collider, Collision>(colA, pair)); newPairsCache.Add(new KeyValuePair <Collider, Collision>(colB, pair)); } if (newContact) { newContactsCache.Add(new KeyValuePair <Collision, ContactPoint>(pair, contact)); } else { updatedContactsCache.Add(new KeyValuePair <Collision, ContactPoint>(pair, contact)); } }
internal void ProcessContacts() { var numManifolds = collisionWorld.Dispatcher.NumManifolds; for (var i = 0; i < numManifolds; i++) { var manifold = collisionWorld.Dispatcher.GetManifoldByIndexInternal(i); var bodyA = manifold.Body0; var bodyB = manifold.Body1; Collider colA, colB; if (!aliveColliders.TryGetValue(bodyA, out colA)) continue; if (!aliveColliders.TryGetValue(bodyB, out colB)) continue; if (colA == null || colB == null || !colA.ContactsAlwaysValid && !colB.ContactsAlwaysValid) continue; newContactsFastCache.Clear(); updatedContactsFastCache.Clear(); deletedContactsFastCache.Clear(); //Pairs management Collision pair = null; var newPair = true; foreach (var pair1 in colA.Collisions) { if ((pair1.ColliderA != colA || pair1.ColliderB != colB) && (pair1.ColliderA != colB || pair1.ColliderB != colA)) continue; pair = pair1; newPair = false; break; } var numContacts = manifold.NumContacts; if (numContacts == 0 && pair == null) { continue; } if (pair == null) { pair = new Collision { ColliderA = colA, ColliderB = colB, Contacts = new List<ContactPoint>() }; colA.Collisions.Add(pair); colB.Collisions.Add(pair); } else { foreach (var contact in pair.Contacts) { deletedContactsFastCache.Add(contact); } } for (var y = 0; y < numContacts; y++) { var cp = manifold.GetContactPoint(y); ContactPoint contact = null; var newContact = true; foreach (var contact1 in pair.Contacts) { if (cp != contact1.Manifold) continue; contact = contact1; newContact = false; break; } if (contact == null) { contact = new ContactPoint { Distance = cp.Distance, PositionOnA = new Vector3(cp.PositionWorldOnA.X, cp.PositionWorldOnA.Y, cp.PositionWorldOnA.Z), PositionOnB = new Vector3(cp.PositionWorldOnB.X, cp.PositionWorldOnB.Y, cp.PositionWorldOnB.Z), Normal = new Vector3(cp.NormalWorldOnB.X, cp.NormalWorldOnB.Y, cp.NormalWorldOnB.Z), Pair = pair, Manifold = cp }; pair.Contacts.Add(contact); } else { deletedContactsFastCache.Remove(contact); } if (newContact) { newContactsFastCache.Add(contact); } else { updatedContactsFastCache.Add(contact); } } //deliver async events if (newPair) { //are we the first pair we detect? if (colA.Collisions.Count == 1) { while (colA.FirstCollisionChannel.Balance < 0) { colA.FirstCollisionChannel.Send(pair); } } //are we the first pair we detect? if (colB.Collisions.Count == 1) { while (colB.FirstCollisionChannel.Balance < 0) { colB.FirstCollisionChannel.Send(pair); } } while (colA.NewPairChannel.Balance < 0) { colA.NewPairChannel.Send(pair); } while (colB.NewPairChannel.Balance < 0) { colB.NewPairChannel.Send(pair); } } foreach (var contact in newContactsFastCache) { while (contact.Pair.NewContactChannel.Balance < 0) { contact.Pair.NewContactChannel.Send(contact); } } foreach (var contact in updatedContactsFastCache) { while (contact.Pair.ContactUpdateChannel.Balance < 0) { contact.Pair.ContactUpdateChannel.Send(contact); } } foreach (var contact in deletedContactsFastCache) { while (contact.Pair.ContactEndedChannel.Balance < 0) { contact.Pair.ContactEndedChannel.Send(contact); } pair.Contacts.Remove(contact); } if (pair.Contacts.Count == 0) { colA.Collisions.Remove(pair); colB.Collisions.Remove(pair); while (colA.PairEndedChannel.Balance < 0) { colA.PairEndedChannel.Send(pair); } while (colB.PairEndedChannel.Balance < 0) { colB.PairEndedChannel.Send(pair); } if (colA.Collisions.Count == 0) { while (colA.AllPairsEndedChannel.Balance < 0) { colA.AllPairsEndedChannel.Send(pair); } } if (colB.Collisions.Count == 0) { while (colB.AllPairsEndedChannel.Balance < 0) { colB.AllPairsEndedChannel.Send(pair); } } } } }