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