/// <summary>
/// Removes an item at the given index.
/// </summary>
/// <param name="index">Index to remove.</param>
public unsafe void RemoveAt(int index)
{
BoxContactDataCache copy = this;
BoxContactData * pointer = ©.D1;
pointer[index] = pointer[Count - 1];
this = copy;
Count--;
}
///<summary>
/// Updates the manifold.
///</summary>
///<param name="dt">Timestep duration.</param>
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;
contacts.Elements[i].Validate();
//Update contact...
contacts.Elements[i].Position = manifoldPointer[j].Position;
contacts.Elements[i].PenetrationDepth = -manifoldPointer[j].Depth;
contacts.Elements[i].Normal = axis;
//Remove manifold entry
contacts.Elements[i].Validate();
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);
}
}
}
