public static void Merge(ContactSet contactSet, Contact newContact, CollisionQueryType type, float contactPositionTolerance)
{
Debug.Assert(contactSet != null);
Debug.Assert(newContact != null);
Debug.Assert(contactPositionTolerance >= 0, "The contact position tolerance must be greater than or equal to 0");
Debug.Assert(type == CollisionQueryType.ClosestPoints || type == CollisionQueryType.Contacts);
if (type == CollisionQueryType.Contacts && newContact.PenetrationDepth < 0)
{
return; // Do not merge separated contacts.
}
// ----- Simplest case: Contact set is empty.
if (contactSet.Count == 0)
{
// Simply add the new contact.
contactSet.Add(newContact);
return;
}
// ----- Try to merge with nearest old contact.
bool merged = TryMergeWithNearestContact(contactSet, newContact, contactPositionTolerance, true);
if (merged)
{
newContact.Recycle();
return;
}
// ----- Default: Add the new contact.
contactSet.Add(newContact);
}
/// <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();
}
}
}
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;
}
}
}
public static void Merge(ContactSet contactSet, Contact newContact, CollisionQueryType type, float contactPositionTolerance)
{
Debug.Assert(contactSet != null);
Debug.Assert(newContact != null);
Debug.Assert(contactPositionTolerance >= 0, "The contact position tolerance must be greater than or equal to 0");
Debug.Assert(type == CollisionQueryType.ClosestPoints || type == CollisionQueryType.Contacts);
if (type == CollisionQueryType.Contacts && newContact.PenetrationDepth < 0)
return; // Do not merge separated contacts.
// ----- Simplest case: Contact set is empty.
if (contactSet.Count == 0)
{
// Simply add the new contact.
contactSet.Add(newContact);
return;
}
// ----- Try to merge with nearest old contact.
bool merged = TryMergeWithNearestContact(contactSet, newContact, contactPositionTolerance, true);
if (merged)
{
newContact.Recycle();
return;
}
// ----- Default: Add the new contact.
contactSet.Add(newContact);
}
/// <summary>
/// Called when the shape was changed.
/// </summary>
/// <param name="sender">The sender.</param>
/// <param name="eventArgs">
/// The <see cref="ShapeChangedEventArgs"/> instance containing the event data.
/// </param>
private void OnShapeChanged(object sender, ShapeChangedEventArgs eventArgs)
{
Changed = true;
var shape = (_geometricObject != null) ? _geometricObject.Shape : null;
// Check if shape type was changed (which invalidates the cached collision algos).
// Instead of comparing the types, we simply compare the instances.
ShapeTypeChanged = ShapeTypeChanged || (_shape != shape);
// Remember current shape.
_shape = shape;
RayShape ray = shape as RayShape;
if (ray != null)
{
_shapeType = ray.StopsAtFirstHit ? ShapeType.RayThatStopsAtFirstHit : ShapeType.Ray;
}
else
{
_shapeType = ShapeType.Default;
}
if (_domain == null)
{
return;
}
Debug.Assert(_domain != null);
// Clear existing contact info only.
int feature = eventArgs.Feature;
if (feature == -1)
{
// Invalidate all contacts that contain this collision object.
foreach (var contactSet in _domain.ContactSets.GetContacts(this))
{
foreach (var contact in contactSet)
{
contact.Recycle();
}
contactSet.Clear();
contactSet.IsValid = false;
}
return;
}
Debug.Assert(feature >= 0);
// Remove only the contacts of the given feature.
foreach (var contactSet in _domain.ContactSets.GetContacts(this))
{
if (contactSet.ObjectA == this)
{
for (int i = contactSet.Count - 1; i >= 0; i--)
{
Contact contact = contactSet[i];
if (contact.FeatureA == -1 || contact.FeatureA == feature)
{
contactSet.RemoveAt(i);
contact.Recycle();
}
}
}
else
{
Debug.Assert(contactSet.ObjectB == this);
for (int i = contactSet.Count - 1; i >= 0; i--)
{
Contact contact = contactSet[i];
if (contact.FeatureB == -1 || contact.FeatureB == feature)
{
contactSet.RemoveAt(i);
contact.Recycle();
}
}
}
contactSet.IsValid = 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);
}
}
