/// <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>
/// 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>
/// Updates the contact geometry for a single contact.
/// </summary>
/// <param name="contactSet">The contact set.</param>
/// <param name="contact">The contact to be updated.</param>
/// <param name="contactPositionTolerance">The contact position tolerance.</param>
/// <returns>
/// <see langword="true"/> if the contact is invalid and should be removed.
/// </returns>
private static bool UpdateContact(ContactSet contactSet, Contact contact, float contactPositionTolerance)
{
Pose poseA = contactSet.ObjectA.GeometricObject.Pose;
Pose poseB = contactSet.ObjectB.GeometricObject.Pose;
// Get local positions in world space.
//Vector3F positionA = poseA.ToWorldPosition(contact.PositionALocal);
//Vector3F positionB = poseB.ToWorldPosition(contact.PositionBLocal);
// ----- Optimized version:
Vector3F positionALocal = contact.PositionALocal;
Vector3F positionA;
positionA.X = poseA.Orientation.M00 * positionALocal.X + poseA.Orientation.M01 * positionALocal.Y + poseA.Orientation.M02 * positionALocal.Z + poseA.Position.X;
positionA.Y = poseA.Orientation.M10 * positionALocal.X + poseA.Orientation.M11 * positionALocal.Y + poseA.Orientation.M12 * positionALocal.Z + poseA.Position.Y;
positionA.Z = poseA.Orientation.M20 * positionALocal.X + poseA.Orientation.M21 * positionALocal.Y + poseA.Orientation.M22 * positionALocal.Z + poseA.Position.Z;
Vector3F positionBLocal = contact.PositionBLocal;
Vector3F positionB;
positionB.X = poseB.Orientation.M00 * positionBLocal.X + poseB.Orientation.M01 * positionBLocal.Y + poseB.Orientation.M02 * positionBLocal.Z + poseB.Position.X;
positionB.Y = poseB.Orientation.M10 * positionBLocal.X + poseB.Orientation.M11 * positionBLocal.Y + poseB.Orientation.M12 * positionBLocal.Z + poseB.Position.Y;
positionB.Z = poseB.Orientation.M20 * positionBLocal.X + poseB.Orientation.M21 * positionBLocal.Y + poseB.Orientation.M22 * positionBLocal.Z + poseB.Position.Z;
// Update Position.
contact.Position = (positionA + positionB) / 2;
// Update contacts and closest points differently:
if (contact.PenetrationDepth >= 0)
{
// ----- Contact.
Vector3F bToA = positionA - positionB; // Vector from contact on A to contact on B
if (!contact.IsRayHit)
{
// ----- Normal contact.
// Update penetration depth: Difference of world position projected onto normal.
//contact.PenetrationDepth = Vector3F.Dot(bToA, contact.Normal);
// ----- Optimized version:
Vector3F contactNormal = contact.Normal;
contact.PenetrationDepth = bToA.X * contactNormal.X + bToA.Y * contactNormal.Y + bToA.Z * contactNormal.Z;
}
else
{
// ----- Ray hit.
// Update penetration depth: Contact position to ray origin projected onto ray direction.
// Get ray. Only one shape is a ray because ray vs. ray do normally not collide.
RayShape ray = contactSet.ObjectA.GeometricObject.Shape as RayShape;
float rayScale = contactSet.ObjectA.GeometricObject.Scale.X; // Non-uniformly scaled rays are not support, so we only need Scale.X!
Vector3F hitPositionLocal; // Hit position in local space of ray.
if (ray != null)
{
hitPositionLocal = poseA.ToLocalPosition(contact.Position);
}
else
{
// The other object must be the ray.
ray = contactSet.ObjectB.GeometricObject.Shape as RayShape;
rayScale = contactSet.ObjectB.GeometricObject.Scale.X; // Non-uniformly scaled rays are not support, so we only need Scale.X!
hitPositionLocal = poseB.ToLocalPosition(contact.Position);
}
// Now, we have found the ray, unless there is a composite shape with a child ray - which
// is not supported.
if (ray != null)
{
contact.PenetrationDepth = Vector3F.Dot(hitPositionLocal - ray.Origin * rayScale, ray.Direction);
// If the new penetration depth is negative or greater than the ray length,
// the objects have separated along the ray direction.
if (contact.PenetrationDepth < 0 || contact.PenetrationDepth > ray.Length * rayScale)
{
return(true);
}
}
}
// Remove points with negative penetration depth.
if (contact.PenetrationDepth < 0)
{
return(true);
}
// Check drift.
float driftSquared;
if (contact.IsRayHit)
{
// For ray casts: Remove contact if movement in any direction is too large.
driftSquared = bToA.LengthSquared;
}
else
{
// For contacts: Remove contacts if horizontal movement (perpendicular to contact normal)
// is too large.
driftSquared = (bToA - contact.Normal * contact.PenetrationDepth).LengthSquared;
}
// Remove contact if drift is too large.
return(driftSquared > contactPositionTolerance * contactPositionTolerance);
}
else
{
// ----- Closest point pair.
// Update distance. Since we do not check the geometric objects, the new distance
// could be a separation or a penetration. We assume it is a separation and
// use a "-" sign.
// We have no problem if we are wrong and this is actually a penetration because this
// contact is automatically updated or removed when new contacts are computed in
// the narrow phase.
Vector3F aToB = positionB - positionA; // Vector from contact on A to contact on B
contact.PenetrationDepth = -aToB.Length;
// If points moved into contact, remove this pair, because we don't have a valid
// contact normal.
if (Numeric.IsZero(contact.PenetrationDepth))
{
return(true);
}
// Update normal.
contact.Normal = aToB.Normalized;
return(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);
}
}