/// <summary>
/// Reduces the number of contacts in the contact set to 1 contact.
/// </summary>
/// <param name="contactSet">The contact set.</param>
/// <remarks>
/// The contact with the biggest penetration depth is kept.
/// </remarks>
internal static void ReduceClosestPoints(ContactSet contactSet)
{
// Reduce to 1 contact.
int numberOfContacts = contactSet.Count;
if (numberOfContacts > 1)
{
// Keep the contact with the deepest penetration depth.
Contact bestContact = contactSet[0];
for (int i = 1; i < numberOfContacts; i++)
{
if (contactSet[i].PenetrationDepth > bestContact.PenetrationDepth)
{
bestContact = contactSet[i];
}
}
// Throw away other contacts.
foreach (var contact in contactSet)
{
if (contact != bestContact)
{
contact.Recycle();
}
}
contactSet.Clear();
contactSet.Add(bestContact);
}
Debug.Assert(contactSet.Count == 0 || contactSet.Count == 1);
// If we HaveContact but the contact shows a separation, delete contact.
// This can happen for TriangleMesh vs. TriangleMesh because the triangle mesh algorithm
// filters contacts if they have a bad normal. It can happen that all contacts are filtered
// and only a separated contact remains in the contact set.
if (contactSet.HaveContact && contactSet.Count > 0 && contactSet[0].PenetrationDepth < 0)
{
contactSet[0].Recycle();
contactSet.Clear();
}
}
public static void Merge(ContactSet target, ContactSet newContacts, CollisionQueryType type, float contactPositionTolerance)
{
Debug.Assert(target != null);
Debug.Assert(newContacts != null);
Debug.Assert(contactPositionTolerance >= 0, "The contact position tolerance must be greater than or equal to 0");
int numberOfNewContacts = newContacts.Count;
for (int i = 0; i < numberOfNewContacts; i++)
{
Merge(target, newContacts[i], type, contactPositionTolerance);
}
newContacts.Clear();
}
/// <summary>
/// Updates the contact information in the given contact set.
/// </summary>
/// <param name="contactSet">The contact set containing the last known contacts.</param>
/// <param name="deltaTime">
/// The time step size in seconds. (The elapsed simulation time since
/// <see cref="UpdateClosestPoints"/> or <see cref="UpdateContacts"/> was last called for this
/// contact set.)
/// </param>
/// <remarks>
/// If two objects move, the contact information will usually change and has to be updated.
/// Using the contact set containing the last known contacts, this method can compute the new
/// contacts faster than <see cref="GetContacts"/> if the poses of the objects haven't changed
/// drastically.
/// </remarks>
/// <exception cref="ArgumentNullException">
/// <paramref name="contactSet"/> is <see langword="null"/>.
/// </exception>
public void UpdateContacts(ContactSet contactSet, float deltaTime)
{
if (contactSet == null)
{
throw new ArgumentNullException("contactSet");
}
// Broad phase AABB check and collision filtering
if (HaveAabbContact(contactSet.ObjectA, contactSet.ObjectB))
{
// Narrow phase
AlgorithmMatrix[contactSet].UpdateContacts(contactSet, deltaTime);
}
else
{
foreach (var contact in contactSet)
{
contact.Recycle();
}
contactSet.Clear();
contactSet.HaveContact = false;
}
}
/// <summary>
/// Reduces the number of contacts in the contact set to 1 contact.
/// </summary>
/// <param name="contactSet">
/// The contact set. One shape in the contact set must be a <see cref="RayShape"/>!
/// </param>
/// <remarks>
/// The best ray hit is kept.
/// </remarks>
internal static void ReduceRayHits(ContactSet contactSet)
{
Debug.Assert(
contactSet.ObjectA.GeometricObject.Shape is RayShape ||
contactSet.ObjectB.GeometricObject.Shape is RayShape,
"ReduceRayHits was called for a contact set without a ray.");
// For separated contacts keep the contact with the smallest separation.
// If we have contact, keep the contact with the SMALLEST penetration (= shortest ray length)
// and remove all invalid contacts (with separation).
bool haveContact = contactSet.HaveContact;
float bestPenetrationDepth = haveContact ? float.PositiveInfinity : float.NegativeInfinity;
Contact bestContact = null;
int numberOfContacts = contactSet.Count;
for (int i = 0; i < numberOfContacts; i++)
{
Contact contact = contactSet[i];
float penetrationDepth = contact.PenetrationDepth;
if (haveContact)
{
// Search for positive and smallest penetration depth.
if (penetrationDepth >= 0 && penetrationDepth < bestPenetrationDepth)
{
bestContact = contact;
bestPenetrationDepth = penetrationDepth;
}
}
else
{
// Search for negative and largest penetration depth (Separation!)
Debug.Assert(penetrationDepth < 0, "HaveContact is false, but contact shows penetration.");
if (penetrationDepth > bestPenetrationDepth)
{
bestContact = contact;
bestPenetrationDepth = penetrationDepth;
}
}
}
// Keep best contact.
// Note: In some situations HaveContact is true, but the contact set does not contains any
// contacts with penetration. This happen, for example, when testing a ray inside a triangle
// mesh. The TriangleMeshAlgorithm automatically filters contacts with bad normals.
// When HaveContact is false, we should always have a contact (closest point).
// Throw away other contacts.
foreach (var contact in contactSet)
{
if (contact != bestContact)
{
contact.Recycle();
}
}
contactSet.Clear();
if (bestContact != null)
{
contactSet.Add(bestContact);
}
}
