public override void Update(float dt)
{
//Now, generate a contact between the two shapes.
float distance;
Vector3 axis;
BoxContactDataCache manifold;
if (BoxBoxCollider.AreBoxesColliding(boxA.Shape, boxB.Shape, ref boxA.worldTransform, ref boxB.worldTransform, out distance, out axis, out manifold))
{
unsafe
{
BoxContactData* manifoldPointer = &manifold.D1;
Vector3.Negate(ref axis, out axis);
var toRemove = new TinyList<int>();
for (int i = 0; i < contacts.Count; i++)
{
bool found = false;
for (int j = manifold.Count - 1; j >= 0; j--)
{
if (contacts.Elements[i].Id == manifoldPointer[j].Id)
{
found = true;
//Update contact...
contacts.Elements[i].Position = manifoldPointer[j].Position;
contacts.Elements[i].PenetrationDepth = -manifoldPointer[j].Depth;
contacts.Elements[i].Normal = axis;
//Remove manifold entry
manifold.RemoveAt(j);
break;
}
}
if (!found)
{//No match found
toRemove.Add(i);
}
}
//toRemove is sorted by increasing index. Go backwards along it so that the indices are valid all the way through.
for (int i = toRemove.Count - 1; i >= 0; i--)
Remove(toRemove[i]);
//Add new contacts.
for (int i = 0; i < manifold.Count; i++)
{
var newContact = new ContactData
{
Position = manifoldPointer[i].Position,
PenetrationDepth = -manifoldPointer[i].Depth,
Normal = axis,
Id = manifoldPointer[i].Id
};
Add(ref newContact);
}
}
}
else
{
//Not colliding, so get rid of it.
for (int i = contacts.Count - 1; i >= 0; i--)
{
Remove(i);
}
}
}
#else
