public void Swapped()
{
CollisionObject a = new CollisionObject();
CollisionObject b = new CollisionObject();
ContactSet set = ContactSet.Create(a, b);
set.Add(ContactHelper.CreateContact(set, new Vector3(1, 2, 3), new Vector3(0, 0, 1), 10, false));
set.Add(ContactHelper.CreateContact(set, new Vector3(4, 5, 6), new Vector3(0, 0, 1), 10, false));
set.Add(ContactHelper.CreateContact(set, new Vector3(7, 8, 9), new Vector3(0, 0, 1), 10, false));
ContactSet swapped = set.Swapped;
Assert.AreEqual(set.ObjectA, swapped.ObjectB);
Assert.AreEqual(set.ObjectB, swapped.ObjectA);
Assert.AreEqual(set[0].PositionALocal, swapped[0].PositionBLocal);
Assert.AreEqual(set[1].Normal, -swapped[1].Normal);
}
public void TestContainment()
{
PlaneConvexAlgorithm algo = new PlaneConvexAlgorithm(new CollisionDetection());
CollisionObject a = new CollisionObject(new GeometricObject
{
Shape = new BoxShape(1, 2, 3),
Pose = new Pose(new Vector3(0, -1, 0)),
});
CollisionObject b = new CollisionObject(new GeometricObject
{
Shape = new PlaneShape(new Vector3(0, 1, 0), 0),
});
Assert.AreEqual(true, algo.HaveContact(a, b));
Assert.AreEqual(true, algo.HaveContact(b, a));
// Test contact set update.
ContactSet cs = ContactSet.Create(a, b);
cs.Add(Contact.Create());
algo.UpdateContacts(cs, 0);
Assert.AreEqual(1, cs.Count);
Assert.AreEqual(2, cs[0].PenetrationDepth);
}
public void Merge2()
{
ContactSet set1 = ContactSet.Create(new CollisionObject(), new CollisionObject());
ContactSet set2 = ContactSet.Create(set1.ObjectA, set1.ObjectB);
Contact contact = Contact.Create();
contact.Position = new Vector3(1, 2, 3);
set2.Add(contact);
ContactHelper.Merge(set2, set1, CollisionQueryType.Contacts, 0.01f);
Assert.AreEqual(1, set2.Count);
contact = Contact.Create();
contact.Position = new Vector3(1, 2, 3);
set1.Add(contact);
contact = Contact.Create();
contact.Position = new Vector3(2, 2, 3);
set1.Add(contact);
contact = Contact.Create();
contact.Position = new Vector3(3, 2, 3);
set1.Add(contact);
ContactHelper.Merge(set2, set1, CollisionQueryType.Contacts, 0.01f);
Assert.AreEqual(3, set2.Count);
}
public override void ComputeCollision(ContactSet contactSet, CollisionQueryType type)
{
if (type == CollisionQueryType.ClosestPoints)
{
// Closest point queries.
_closestPointsAlgorithm.ComputeCollision(contactSet, type);
if (contactSet.HaveContact)
{
// Penetration.
// Remember the closest point info in case we run into troubles.
// Get the last contact. We assume that this is the newest. In most cases there will
// only be one contact in the contact set.
Contact fallbackContact = (contactSet.Count > 0)
? contactSet[contactSet.Count - 1]
: null;
// Call the contact query algorithm.
_contactAlgorithm.ComputeCollision(contactSet, CollisionQueryType.Contacts);
if (!contactSet.HaveContact)
{
// Problem!
// The closest-point algorithm reported contact. The contact algorithm didn't find a contact.
// This can happen, for example, because of numerical inaccuracies in GJK vs. MPR.
// Keep the result of the closest-point computation, but decrease the penetration depth
// to indicate separation.
if (fallbackContact != null)
{
Debug.Assert(fallbackContact.PenetrationDepth == 0);
fallbackContact.PenetrationDepth = -Math.Min(10 * Numeric.EpsilonF, CollisionDetection.Epsilon);
foreach (var contact in contactSet)
{
if (contact != fallbackContact)
{
contact.Recycle();
}
}
contactSet.Clear();
contactSet.Add(fallbackContact);
}
contactSet.HaveContact = false;
}
}
}
else
{
// Boolean or contact queries.
_contactAlgorithm.ComputeCollision(contactSet, type);
}
}
public void TestMethods()
{
NoCollisionAlgorithm algo = new NoCollisionAlgorithm(new CollisionDetection());
CollisionObject a = new CollisionObject {
GeometricObject = new GeometricObject {
Shape = new SphereShape(1)
}
};
CollisionObject b = new CollisionObject {
GeometricObject = new GeometricObject {
Shape = new SphereShape(2)
}
};
Assert.AreEqual(a, algo.GetClosestPoints(a, b).ObjectA);
Assert.AreEqual(b, algo.GetClosestPoints(a, b).ObjectB);
Assert.AreEqual(0, algo.GetClosestPoints(a, b).Count);
Assert.AreEqual(a, algo.GetContacts(a, b).ObjectA);
Assert.AreEqual(b, algo.GetContacts(a, b).ObjectB);
Assert.AreEqual(0, algo.GetContacts(a, b).Count);
Assert.AreEqual(false, algo.HaveContact(a, b));
ContactSet cs = ContactSet.Create(a, b);
algo.UpdateClosestPoints(cs, 0);
Assert.AreEqual(a, cs.ObjectA);
Assert.AreEqual(b, cs.ObjectB);
Assert.AreEqual(0, cs.Count);
cs = ContactSet.Create(a, b);
cs.Add(Contact.Create());
algo.UpdateContacts(cs, 0);
Assert.AreEqual(a, cs.ObjectA);
Assert.AreEqual(b, cs.ObjectB);
Assert.AreEqual(0, cs.Count);
}
public void TestNoContact()
{
SphereSphereAlgorithm algo = new SphereSphereAlgorithm(new CollisionDetection());
CollisionObject objectA = new CollisionObject();
((GeometricObject)objectA.GeometricObject).Shape = new SphereShape(1);
CollisionObject objectB = new CollisionObject();
((GeometricObject)objectB.GeometricObject).Shape = new SphereShape(0.5f);
((GeometricObject)objectA.GeometricObject).Pose = new Pose(new Vector3F(0, 0, 0));
((GeometricObject)objectB.GeometricObject).Pose = new Pose(new Vector3F(1.6f, 0, 0));
ContactSet cs = ContactSet.Create(objectA, objectB);
cs.Add(ContactHelper.CreateContact(cs, Vector3F.Zero, Vector3F.UnitX, 0, false));
algo.UpdateContacts(cs, 0);
Assert.AreEqual(objectA, cs.ObjectA);
Assert.AreEqual(objectB, cs.ObjectB);
Assert.AreEqual(0, cs.Count);
}
public void UpdateClosestPoints()
{
ContactSet set = ContactSet.Create(_objectA, _objectB);
_collisionDetection.UpdateClosestPoints(set, 0);
Assert.AreEqual(1, set.Count);
Assert.AreEqual(_objectA, set.ObjectA);
Assert.AreEqual(_objectB, set.ObjectB);
set = ContactSet.Create(_objectA, _objectC);
set.Add(ContactHelper.CreateContact(set, new Vector3F(1, 0, 0), new Vector3F(1, 0, 0), -10, false));
_collisionDetection.UpdateClosestPoints(set, 0);
Assert.AreEqual(1, set.Count);
Assert.AreEqual(_objectA, set.ObjectA);
Assert.AreEqual(_objectC, set.ObjectB);
_collisionDetection.CollisionFilter = new CollisionFilter();
((CollisionFilter)_collisionDetection.CollisionFilter).Set(_objectA, _objectB, false);
set = ContactSet.Create(_objectA, _objectB);
_collisionDetection.UpdateClosestPoints(set, 0);
Assert.AreEqual(1, set.Count);
Assert.AreEqual(_objectA, set.ObjectA);
Assert.AreEqual(_objectB, set.ObjectB);
}
public override void ComputeCollision(ContactSet contactSet, CollisionQueryType type)
{
contactSet.Clear();
contactSet.Add(ContactHelper.CreateContact(contactSet, new Vector3F(1, 2, 3), Vector3F.UnitZ, 1, false));
if (type == CollisionQueryType.Contacts)
contactSet.Add(ContactHelper.CreateContact(contactSet, new Vector3F(2, 2, 3), Vector3F.UnitZ, 1.2f, false));
contactSet.HaveContact = true;
}
/// <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);
}
/// <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 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);
}
public override void ComputeCollision(ContactSet contactSet, CollisionQueryType type)
{
if (type == CollisionQueryType.ClosestPoints)
{
// Closest point queries.
_closestPointsAlgorithm.ComputeCollision(contactSet, type);
if (contactSet.HaveContact)
{
// Penetration.
// Remember the closest point info in case we run into troubles.
// Get the last contact. We assume that this is the newest. In most cases there will
// only be one contact in the contact set.
Contact fallbackContact = (contactSet.Count > 0)
? contactSet[contactSet.Count - 1]
: null;
// Call the contact query algorithm.
_contactAlgorithm.ComputeCollision(contactSet, CollisionQueryType.Contacts);
if (!contactSet.HaveContact)
{
// Problem!
// The closest-point algorithm reported contact. The contact algorithm didn't find a contact.
// This can happen, for example, because of numerical inaccuracies in GJK vs. MPR.
// Keep the result of the closest-point computation, but decrease the penetration depth
// to indicate separation.
if (fallbackContact != null)
{
Debug.Assert(fallbackContact.PenetrationDepth == 0);
fallbackContact.PenetrationDepth = -Math.Min(10 * Numeric.EpsilonF, CollisionDetection.Epsilon);
foreach (var contact in contactSet)
if (contact != fallbackContact)
contact.Recycle();
contactSet.Clear();
contactSet.Add(fallbackContact);
}
contactSet.HaveContact = false;
}
}
}
else
{
// Boolean or contact queries.
_contactAlgorithm.ComputeCollision(contactSet, type);
}
}
