private void UpdateContacts()
{
CollisionDomain.Update(_fixedTimeStep);
IslandManager.ContactSetLinks.Clear();
// Go through contacts and add contact constraints.
foreach (ContactSet contactSet in CollisionDomain.ContactSets)
{
RigidBody bodyA = contactSet.ObjectA.GeometricObject as RigidBody;
RigidBody bodyB = contactSet.ObjectB.GeometricObject as RigidBody;
if (bodyA != null && bodyB != null)
{
// Check if a dynamic body is involved and if collision response is enabled.
bool responseEnabled = (bodyA.MotionType == MotionType.Dynamic || bodyB.MotionType == MotionType.Dynamic)
&& bodyA.CollisionResponseEnabled
&& bodyB.CollisionResponseEnabled
&& (ResponseFilter == null || ResponseFilter.Filter(new Pair<RigidBody>(bodyA, bodyB)));
if (responseEnabled)
IslandManager.ContactSetLinks.Add(new Pair<RigidBody>(bodyA, bodyB));
int numberOfContacts = contactSet.Count;
for (int i = 0; i < numberOfContacts; i++)
{
var contact = contactSet[i];
ContactConstraint constraint = contact.UserData as ContactConstraint;
if (constraint != null)
{
if (responseEnabled)
{
// Contact constraint is still in use.
// --> Mark contact constraint as active.
constraint.Used = true;
}
else
{
// The response was disabled.
// --> Remove an old constact constraint.
contact.UserData = null;
}
}
else if (responseEnabled)
{
// Create a new contact constraint.
constraint = ContactConstraint.Create(bodyA, bodyB, contact);
contact.UserData = constraint;
ContactConstraintsInternal.Add(constraint);
constraint.Used = true;
}
}
}
}
// ----- Recycle old contact constraints.
int numberOfConstraints = ContactConstraintsInternal.Count;
int numberOfUsedConstraints = numberOfConstraints;
for (int i = numberOfConstraints - 1; i >= 0; i--)
{
var constraint = ContactConstraintsInternal[i];
if (constraint.Used)
{
// The contact constraint is still in use.
// Keep constraint and reset flag.
constraint.Used = false;
}
else
{
numberOfUsedConstraints--;
// Recycle contact constraint.
constraint.Recycle();
// The contact constraint is no longer in use.
// Swap a used constraint to this index.
ContactConstraintsInternal[i] = ContactConstraintsInternal[numberOfUsedConstraints];
// Not needed because we call List.RemoveRange for the end of the list.
//ContactConstraintsInternal[numberOfUsedConstraints] = constraint;
}
}
// Remove recycled contacts at end of list.
int numberOfUnusedConstraints = numberOfConstraints - numberOfUsedConstraints;
if (numberOfUnusedConstraints > 0)
{
ContactConstraintsInternal.RemoveRange(numberOfUsedConstraints, numberOfUnusedConstraints);
}
}
