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
}
}
