/// <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();
}
}
/// <summary>
/// Performs more collision tests while slightly rotating one collision object.
/// </summary>
/// <param name="collisionDetection">The collision detection.</param>
/// <param name="contactSet">
/// The contact set; must contain at least 1 <see cref="Contact"/>.
/// </param>
/// <param name="perturbB">
/// if set to <see langword="true"/> collision object B will be rotated; otherwise collision
/// object A will be rotated.
/// </param>
/// <param name="testMethod">The test method that is called to compute contacts.</param>
/// <remarks>
/// This method rotates one object 3 times and calls contact computation for the new
/// orientations. It is recommended to call this method only when the contact set has 1 new
/// contact.
/// </remarks>
internal static void TestWithPerturbations(CollisionDetection collisionDetection, ContactSet contactSet, bool perturbB, Action <ContactSet> testMethod)
{
Debug.Assert(contactSet != null);
Debug.Assert(contactSet.Count > 0 && contactSet.HaveContact || !contactSet.IsPerturbationTestAllowed);
Debug.Assert(testMethod != null);
// Make this test only if there is 1 contact.
// If there are 0 contacts, we assume that the contact pair is separated.
// If there are more than 3 contacts, then we already have a lot of contacts to work with, no
// need to search for more.
if (!contactSet.HaveContact || contactSet.Count == 0 || contactSet.Count >= 4 || !contactSet.IsPerturbationTestAllowed)
{
return;
}
// Get data of object that will be rotated.
var collisionObject = (perturbB) ? contactSet.ObjectB : contactSet.ObjectA;
var geometricObject = collisionObject.GeometricObject;
var pose = geometricObject.Pose;
// Get normal, pointing to the test object.
var normal = contactSet[0].Normal;
if (!perturbB)
{
normal = -normal;
}
var contactPosition = contactSet[0].Position;
var centerToContact = contactPosition - pose.Position;
// Compute a perturbation angle proportional to the dimension of the object.
var radius = geometricObject.Aabb.Extent.Length;
var angle = collisionDetection.ContactPositionTolerance / radius;
// axis1 is in the contact tangent plane, orthogonal to normal.
var axis1 = Vector3F.Cross(normal, centerToContact);
// If axis1 is zero then normal and centerToContact are collinear. This happens
// for example for spheres or cone tips against flat faces. In these cases we assume
// that there will be max. 1 contact.
if (axis1.IsNumericallyZero)
{
return;
}
var axis1Local = pose.ToLocalDirection(axis1);
var rotation = Matrix33F.CreateRotation(axis1Local, -angle);
// Use temporary test objects.
var testGeometricObject = TestGeometricObject.Create();
testGeometricObject.Shape = geometricObject.Shape;
testGeometricObject.Scale = geometricObject.Scale;
testGeometricObject.Pose = new Pose(pose.Position, pose.Orientation * rotation);
var testCollisionObject = ResourcePools.TestCollisionObjects.Obtain();
testCollisionObject.SetInternal(collisionObject, testGeometricObject);
var testContactSet = perturbB ? ContactSet.Create(contactSet.ObjectA, testCollisionObject)
: ContactSet.Create(testCollisionObject, contactSet.ObjectB);
testContactSet.IsPerturbationTestAllowed = false; // Avoid recursive perturbation tests!
testContactSet.PreferredNormal = contactSet.PreferredNormal;
// Compute next contacts.
testMethod(testContactSet);
if (testContactSet.Count > 0)
{
// axis2 is in the contact tangent plane, orthogonal to axis1.
var axis2 = Vector3F.Cross(axis1, normal);
var axis2Local = pose.ToLocalDirection(axis2);
var rotation2 = Matrix33F.CreateRotation(axis2Local, -angle);
testGeometricObject.Pose = new Pose(pose.Position, pose.Orientation * rotation2);
// Compute next contacts.
testMethod(testContactSet);
// Invert rotation2.
rotation2.Transpose();
testGeometricObject.Pose = new Pose(pose.Position, pose.Orientation * rotation2);
// Compute next contacts.
testMethod(testContactSet);
}
// Set HaveContact. It is reset when a perturbation separates the objects.
testContactSet.HaveContact = true;
// TODO: Test if we need this:
// The contact world positions are not really correct because one object was rotated.
// UpdateContacts recomputes the world positions from the local positions.
UpdateContacts(testContactSet, 0, collisionDetection.ContactPositionTolerance);
// Merge contacts of testContactSet into contact set, but do not change existing contacts.
foreach (var contact in testContactSet)
{
// We call TryMerge() to see if the contact is similar to an existing contact.
bool exists = TryMergeWithNearestContact(
contactSet,
contact,
collisionDetection.ContactPositionTolerance,
false); // The existing contact must no be changed!
if (exists)
{
// We can throw away the new contact because a similar is already in the contact set.
contact.Recycle();
}
else
{
// Add new contact.
contactSet.Add(contact);
}
}
// Recycle temporary objects.
testContactSet.Recycle();
ResourcePools.TestCollisionObjects.Recycle(testCollisionObject);
testGeometricObject.Recycle();
}
/// <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);
}
}
