/// <summary>
/// Removes separated contacts.
/// </summary>
/// <param name="contactSet">The contact set.</param>
internal static void RemoveSeparatedContacts(ContactSet contactSet)
{
for (int i = contactSet.Count - 1; i >= 0; i--)
{
Contact contact = contactSet[i];
if (contact.PenetrationDepth < 0)
{
contactSet.RemoveAt(i);
contact.Recycle();
}
}
}
/// <summary>
/// Removes 1 contact.
/// </summary>
/// <param name="contactSet">The contact set.</param>
/// <remarks>
/// <c>contactSet[0]</c> is not changed. <c>contactSet[1]</c> to <c>contactSet[4]</c> are
/// tested. The contacts with the largest area are kept. The other one is removed.
/// </remarks>
private static void Reduce(ContactSet contactSet)
{
Debug.Assert(contactSet.Count > 4);
// Remember: The magnitude of the cross product of two vectors is equal to the area of the
// defined parallelogram. The parallelogram is twice as large as the triangle area defined
// between the three points.
Vector3F edge12 = contactSet[2].Position - contactSet[1].Position;
Vector3F edge13 = contactSet[3].Position - contactSet[1].Position;
Vector3F edge14 = contactSet[4].Position - contactSet[1].Position;
Vector3F edge23 = contactSet[3].Position - contactSet[2].Position;
Vector3F edge24 = contactSet[4].Position - contactSet[2].Position;
// Check 4 parallelograms.
float area = Vector3F.Cross(edge12, edge13).LengthSquared;
float maxArea = area;
int contactToDelete = 4;
area = Vector3F.Cross(edge12, edge14).LengthSquared;
if (area > maxArea)
{
maxArea = area;
contactToDelete = 3;
}
area = Vector3F.Cross(edge13, edge14).LengthSquared;
if (area > maxArea)
{
maxArea = area;
contactToDelete = 2;
}
area = Vector3F.Cross(edge23, edge24).LengthSquared;
if (area > maxArea)
{
// maxArea = area;
contactToDelete = 1;
}
contactSet[contactToDelete].Recycle();
// Remove 1 contact by moving the last contact in the contact set to its index.
// Unless the contact to delete is already the last one.
int maxIndex = contactSet.Count - 1;
if (contactToDelete != maxIndex)
{
contactSet[contactToDelete] = contactSet[maxIndex];
}
contactSet.RemoveAt(maxIndex);
}
public static void RemoveBadContacts(ContactSet contactSet, Vector3F normal, float minDotProduct)
{
for (int i = contactSet.Count - 1; i >= 0; i--)
{
Contact contact = contactSet[i];
Vector3F contactNormal = contact.Normal;
// float dot = Vector3F.Dot(contactNormal, normal);
// ----- Optimized version:
float dot = contactNormal.X * normal.X + contactNormal.Y * normal.Y + contactNormal.Z * normal.Z;
if (dot < minDotProduct)
{
contactSet.RemoveAt(i);
contact.Recycle();
}
}
}
/// <summary>
/// Updates the contact geometry of a contact set.
/// </summary>
/// <param name="contactSet">The contact set.</param>
/// <param name="deltaTime">
/// The time step in seconds. (The simulation time that has elapsed since the last time that an
/// Update-method was called.)
/// </param>
/// <param name="contactPositionTolerance">The contact position tolerance.</param>
/// <remarks>
/// <para>
/// The objects can still move relative to each other. This method updates the contact
/// information if the objects have moved. The <see cref="Contact.Lifetime"/> of persistent
/// contacts is increased. Invalid contacts are removed. Closest point pairs will be removed if
/// they start touching. Penetrating or touching contacts are removed if the objects have moved
/// more than <paramref name="contactPositionTolerance"/> or the contacts have separated.
/// </para>
/// <para>
/// Note: Only the info of the cached contacts is updated. New contacts are not discovered in
/// this method.
/// </para>
/// </remarks>
internal static void UpdateContacts(ContactSet contactSet, float deltaTime, float contactPositionTolerance)
{
Debug.Assert(contactPositionTolerance >= 0, "The contact position tolerance must be greater than or equal to 0");
int numberOfContacts = contactSet.Count;
if (numberOfContacts == 0)
{
return;
}
if (contactSet.ObjectA.Changed || contactSet.ObjectB.Changed)
{
// Objects have moved.
for (int i = numberOfContacts - 1; i >= 0; i--) // Reverse loop, because contacts might be removed.
{
Contact contact = contactSet[i];
// Increase Lifetime.
contact.Lifetime += deltaTime;
// Update all contacts and remove invalid contacts.
bool shouldRemove = UpdateContact(contactSet, contact, contactPositionTolerance);
if (shouldRemove)
{
contactSet.RemoveAt(i);
contact.Recycle();
}
}
}
else
{
// Nothing has moved.
for (int i = 0; i < numberOfContacts; i++)
{
// Increase Lifetime.
contactSet[i].Lifetime += deltaTime;
}
}
}
