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