private void ContactRemoval(ContactPoint contact, PhysicsComponent component0, PhysicsComponent component1) { var existingPair = component0.Collisions.FirstOrDefault(x => x.InternalEquals(component0, component1)); if (existingPair == null) { #if DEBUG //should not happen? throw new Exception("Pair not present."); #else return; #endif } if (existingPair.Contacts.Contains(contact)) { existingPair.Contacts.Remove(contact); removedContactsCache.Add(contact); contactToCollision.Remove(contact); if (existingPair.Contacts.Count == 0) { component0.Collisions.Remove(existingPair); component1.Collisions.Remove(existingPair); removedCollisionsCache.Add(existingPair); } } else { #if DEBUG //should not happen? throw new Exception("Contact not in pair."); #endif } }
internal void RemoveContact(ContactPoint point) { point.Valid = false; var collision = point.Collision; if (collision == null || collision.Contacts.Count == 0) return; collision.Contacts.Remove(point); removedContactsCache.Add(point); if (collision.Contacts.Count > 0) return; collision.ColliderA.Collisions.Remove(collision); collision.ColliderB.Collisions.Remove(collision); removedCollisionsCache.Add(collision); }
internal void ProcessContacts() { var contactsProfiler = Profiler.Begin(PhysicsProfilingKeys.ContactsProfilingKey); 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); } } } contactsProfiler.End("Contacts: {0}", alivePairsFastCache.Count); }
unsafe internal void ContactTest(PhysicsComponent component) { foreach (var collision in component.Collisions) { skippedCollisions.Add(collision); } IntPtr buffer; int bufferSize; collisionWorld.GetCollisions(component.NativeCollisionObject, out buffer, out bufferSize); var contacts = (ContactData *)buffer; for (var i = 0; i < bufferSize; i++) { var contact = contacts[i]; var obj0 = BulletSharp.CollisionObject.GetManaged((IntPtr)contact.ColliderA); var obj1 = BulletSharp.CollisionObject.GetManaged((IntPtr)contact.ColliderB); var component0 = (PhysicsComponent)obj0.UserObject; var component1 = (PhysicsComponent)obj1.UserObject; if (((int)component0.CanCollideWith & (int)component1.CollisionGroup) != 0 || ((int)component1.CanCollideWith & (int)component0.CollisionGroup) != 0) { var skip = false; foreach (var collision in component0.Collisions) { if ((collision.ColliderA == component0 && collision.ColliderB == component1) || (collision.ColliderA == component1 && collision.ColliderB == component0)) { var oldContact = collision.Contacts[0]; oldContact.Distance = contact.Distance; oldContact.Normal = new Vector3(contact.NormalX, contact.NormalY, contact.NormalZ); oldContact.PositionOnA = new Vector3(contact.PositionOnAx, contact.PositionOnAy, contact.PositionOnAz); oldContact.PositionOnB = new Vector3(contact.PositionOnBx, contact.PositionOnBy, contact.PositionOnBz); updatedContactsCache.Add(oldContact); skippedCollisions.Remove(collision); skip = true; break; } } if (skip) { continue; } var newCollision = new Collision { Contacts = new TrackingCollection <ContactPoint>(), ColliderA = component0, ColliderB = component1 }; //todo this has to change var newContact = new ContactPoint { Collision = newCollision, Distance = contact.Distance, LifeTime = 0, Normal = new Vector3(contact.NormalX, contact.NormalY, contact.NormalZ), PositionOnA = new Vector3(contact.PositionOnAx, contact.PositionOnAy, contact.PositionOnAz), PositionOnB = new Vector3(contact.PositionOnBx, contact.PositionOnBy, contact.PositionOnBz) }; newCollision.Contacts.Add(newContact); component0.Collisions.Add(newCollision); component1.Collisions.Add(newCollision); newCollisionsCache.Add(newCollision); newContactsFastCache.Add(newContact); newCollisions.Add(newCollision); } } }
internal void RemoveContact(ContactPoint point) { var collision = point?.Collision; if (collision == null || collision.Contacts.Count == 0) return; collision.Contacts.Remove(point); removedContactsCache.Add(point); //if (collision.Contacts.Count > 0) return; //collision.ColliderA.Collisions.Remove(collision); //collision.ColliderB.Collisions.Remove(collision); //removedCollisionsCache.Add(collision); }
unsafe internal void ContactTest(PhysicsComponent component) { foreach (var collision in component.Collisions) { skippedCollisions.Add(collision); } IntPtr buffer; int bufferSize; collisionWorld.GetCollisions(component.NativeCollisionObject, out buffer, out bufferSize); var contacts = (ContactData*) buffer; for (var i = 0; i < bufferSize; i++) { var contact = contacts[i]; var obj0 = BulletSharp.CollisionObject.GetManaged((IntPtr)contact.ColliderA); var obj1 = BulletSharp.CollisionObject.GetManaged((IntPtr)contact.ColliderB); var component0 = (PhysicsComponent)obj0.UserObject; var component1 = (PhysicsComponent)obj1.UserObject; if (((int)component0.CanCollideWith & (int)component1.CollisionGroup) != 0 || ((int)component1.CanCollideWith & (int)component0.CollisionGroup) != 0) { var skip = false; foreach (var collision in component0.Collisions) { if ((collision.ColliderA == component0 && collision.ColliderB == component1) || (collision.ColliderA == component1 && collision.ColliderB == component0)) { var oldContact = collision.Contacts[0]; oldContact.Distance = contact.Distance; oldContact.Normal = new Vector3(contact.NormalX, contact.NormalY, contact.NormalZ); oldContact.PositionOnA = new Vector3(contact.PositionOnAx, contact.PositionOnAy, contact.PositionOnAz); oldContact.PositionOnB = new Vector3(contact.PositionOnBx, contact.PositionOnBy, contact.PositionOnBz); updatedContactsCache.Add(oldContact); skippedCollisions.Remove(collision); skip = true; break; } } if (skip) { continue; } var newCollision = new Collision { Contacts = new TrackingCollection<ContactPoint>(), ColliderA = component0, ColliderB = component1 }; //todo this has to change var newContact = new ContactPoint { Collision = newCollision, Distance = contact.Distance, LifeTime = 0, Normal = new Vector3(contact.NormalX, contact.NormalY, contact.NormalZ), PositionOnA = new Vector3(contact.PositionOnAx, contact.PositionOnAy, contact.PositionOnAz), PositionOnB = new Vector3(contact.PositionOnBx, contact.PositionOnBy, contact.PositionOnBz) }; newCollision.Contacts.Add(newContact); component0.Collisions.Add(newCollision); component1.Collisions.Add(newCollision); newCollisionsCache.Add(newCollision); newContactsFastCache.Add(newContact); newCollisions.Add(newCollision); } } }
private void ContactRemoval(ContactPoint contact, PhysicsComponent component0, PhysicsComponent component1) { Collision existingPair = null; foreach (var x in component0.Collisions) { if (x.InternalEquals(component0, component1)) { existingPair = x; break; } } if (existingPair == null) { #if DEBUG //should not happen? throw new Exception("Pair not present."); #else return; #endif } if (existingPair.Contacts.Contains(contact)) { existingPair.Contacts.Remove(contact); removedContactsCache.Add(contact); contactToCollision.Remove(contact); if (existingPair.Contacts.Count == 0) { component0.Collisions.Remove(existingPair); component1.Collisions.Remove(existingPair); removedCollisionsCache.Add(existingPair); } } else { #if DEBUG //should not happen? throw new Exception("Contact not in pair."); #endif } }