// Remove all contacts where the angle between the contact normal and the triangle normal
// is less than the given angle. The limit angle is given as cos(angle) (= dot product).
private static void RemoveBadContacts(bool swapped, ContactSet testContactSet, Vector3 triangleNormal, float cosMinAngle)
{
// Note: We assume that we have only one contact per set.
if (testContactSet.Count > 0)
{
if (!swapped)
{
if (Vector3.Dot(testContactSet[0].Normal, triangleNormal) < cosMinAngle)
{
foreach (var contact in testContactSet)
{
contact.Recycle();
}
testContactSet.Clear();
}
}
else
{
if (Vector3.Dot(-testContactSet[0].Normal, triangleNormal) < cosMinAngle)
{
foreach (var contact in testContactSet)
{
contact.Recycle();
}
testContactSet.Clear();
}
}
}
}
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 override void ComputeCollision(ContactSet contactSet, CollisionQueryType type)
{
contactSet.Clear();
contactSet.Add(ContactHelper.CreateContact(contactSet, new Vector3(1, 2, 3), Vector3.UnitZ, 1, false));
if (type == CollisionQueryType.Contacts)
{
contactSet.Add(ContactHelper.CreateContact(contactSet, new Vector3(2, 2, 3), Vector3.UnitZ, 1.2f, false));
}
contactSet.HaveContact = true;
}
/// <summary>
/// Performs a collision query to update the contact information in the contact set.
/// </summary>
/// <param name="contactSet">The contact set.</param>
/// <param name="deltaTime">
/// The time step size in seconds. (The elapsed simulation time since the contact set was
/// updated the last time.)
/// </param>
/// <remarks>
/// <para>
/// This method updates contact information stored in the given contact set. This method is
/// usually faster than <see cref="GetContacts"/> because the information in
/// <paramref name="contactSet"/> is reused and updated.
/// </para>
/// <para>
/// The life time counter of persistent contacts is increased.
/// </para>
/// </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");
}
// Remove separated contacts - the contact set could contain separated
// closest points of a GetClosestPoints query.
ContactHelper.RemoveSeparatedContacts(contactSet);
// Update cached information.
ContactHelper.UpdateContacts(contactSet, deltaTime, CollisionDetection.ContactPositionTolerance);
// We do not compute the detailed contact information for triggers.
bool ignoreContactInfo = (contactSet.ObjectA.Type == CollisionObjectType.Trigger ||
contactSet.ObjectB.Type == CollisionObjectType.Trigger);
// We have to make the full contact query if one object is a ray with StopsAtFirstHit because
// we need the PenetrationDepth for sorting the hits.
if (contactSet.ObjectA.IsRayThatStopsAtFirstHit || contactSet.ObjectB.IsRayThatStopsAtFirstHit)
{
ignoreContactInfo = false;
}
if (ignoreContactInfo)
{
// Check cached flag. If necessary, make only boolean check.
if (contactSet.HaveContact == false || contactSet.Count == 0)
{
ComputeCollision(contactSet, CollisionQueryType.Boolean);
}
}
else
{
// Compute new contact info.
ComputeCollision(contactSet, CollisionQueryType.Contacts);
}
if (contactSet.HaveContact == false)
{
// No contact: Remove old contacts if objects do not touch.
foreach (var contact in contactSet)
{
contact.Recycle();
}
contactSet.Clear();
}
else
{
// Reduce ray cast results to 1 contact.
if (contactSet.ObjectA.IsRay || contactSet.ObjectB.IsRay)
{
ContactHelper.ReduceRayHits(contactSet);
}
// We have contact: Call contact filter.
if (CollisionDetection.ContactFilter != null)
{
CollisionDetection.ContactFilter.Filter(contactSet);
}
}
CheckResult(contactSet, false);
contactSet.IsValid = true;
}
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>
/// 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 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();
}
public override void ComputeCollision(ContactSet contactSet, CollisionQueryType type)
{
// Ray vs. convex has at max 1 contact.
Debug.Assert(contactSet.Count <= 1);
// Object A should be the ray.
// Object B should be the convex.
IGeometricObject rayObject = contactSet.ObjectA.GeometricObject;
IGeometricObject convexObject = contactSet.ObjectB.GeometricObject;
// Swap object if necessary.
bool swapped = (convexObject.Shape is RayShape);
if (swapped)
MathHelper.Swap(ref rayObject, ref convexObject);
RayShape rayShape = rayObject.Shape as RayShape;
ConvexShape convexShape = convexObject.Shape as ConvexShape;
// Check if shapes are correct.
if (rayShape == null || convexShape == null)
throw new ArgumentException("The contact set must contain a ray and a convex shape.", "contactSet");
// Call line segment vs. convex for closest points queries.
if (type == CollisionQueryType.ClosestPoints)
{
// Find point on ray closest to the convex shape.
// Call GJK.
_gjk.ComputeCollision(contactSet, type);
if (contactSet.HaveContact == false)
return;
// Otherwise compute 1 contact ...
// GJK result is invalid for penetration.
foreach (var contact in contactSet)
contact.Recycle();
contactSet.Clear();
}
// Assume no contact.
contactSet.HaveContact = false;
// Get transformations.
Vector3F rayScale = rayObject.Scale;
Vector3F convexScale = convexObject.Scale;
Pose convexPose = convexObject.Pose;
Pose rayPose = rayObject.Pose;
// See Raycasting paper of van den Bergen or Bullet.
// Note: Compute in local space of convex (object B).
// Scale ray and transform ray to local space of convex.
Ray rayWorld = new Ray(rayShape);
rayWorld.Scale(ref rayScale); // Scale ray.
rayWorld.ToWorld(ref rayPose); // Transform ray to world space.
Ray ray = rayWorld;
ray.ToLocal(ref convexPose); // Transform ray to local space of convex.
var simplex = GjkSimplexSolver.Create();
try
{
Vector3F s = ray.Origin; // source
Vector3F r = ray.Direction * ray.Length; // ray
float λ = 0; // ray parameter
Vector3F x = s; // hit spot (on ray)
Vector3F n = new Vector3F(); // normal
Vector3F v = x - convexShape.GetSupportPoint(ray.Direction, convexScale);
// v = x - arbitrary point. Vector used for support mapping.
float distanceSquared = v.LengthSquared; // ||v||²
int iterationCount = 0;
while (distanceSquared > Numeric.EpsilonF && iterationCount < MaxNumberOfIterations)
{
iterationCount++;
Vector3F p = convexShape.GetSupportPoint(v, convexScale); // point on convex
Vector3F w = x - p; // simplex/Minkowski difference point
float vDotW = Vector3F.Dot(v, w); // v∙w
if (vDotW > 0)
{
float vDotR = Vector3F.Dot(v, r); // v∙r
if (vDotR >= 0) // TODO: vDotR >= - Epsilon^2 ?
return; // No Hit.
λ = λ - vDotW / vDotR;
x = s + λ * r;
simplex.Clear(); // Configuration space obstacle (CSO) is translated whenever x is updated.
w = x - p;
n = v;
}
simplex.Add(w, x, p);
simplex.Update();
v = simplex.ClosestPoint;
distanceSquared = (simplex.IsValid && !simplex.IsFull) ? v.LengthSquared : 0;
}
// We have a contact if the hit is inside the ray length.
contactSet.HaveContact = (0 <= λ && λ <= 1);
if (type == CollisionQueryType.Boolean || (type == CollisionQueryType.Contacts && !contactSet.HaveContact))
{
// HaveContact queries can exit here.
// GetContacts queries can exit here if we don't have a contact.
return;
}
float penetrationDepth = λ * ray.Length;
Debug.Assert(contactSet.HaveContact, "Separation was not detected by GJK above.");
// Convert back to world space.
Vector3F position = rayWorld.Origin + rayWorld.Direction * penetrationDepth;
n = convexPose.ToWorldDirection(n);
if (!n.TryNormalize())
n = Vector3F.UnitY;
if (swapped)
n = -n;
// Update contact set.
Contact contact = ContactHelper.CreateContact(contactSet, position, -n, penetrationDepth, true);
ContactHelper.Merge(contactSet, contact, type, CollisionDetection.ContactPositionTolerance);
}
finally
{
simplex.Recycle();
}
}
/// <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;
}
}
// Remove all contacts where the angle between the contact normal and the triangle normal
// is less than the given angle. The limit angle is given as cos(angle) (= dot product).
private static void RemoveBadContacts(bool swapped, ContactSet testContactSet, Vector3F triangleNormal, float cosMinAngle)
{
// Note: We assume that we have only one contact per set.
if (testContactSet.Count > 0)
{
if (!swapped)
{
if (Vector3F.Dot(testContactSet[0].Normal, triangleNormal) < cosMinAngle)
{
foreach (var contact in testContactSet)
contact.Recycle();
testContactSet.Clear();
}
}
else
{
if (Vector3F.Dot(-testContactSet[0].Normal, triangleNormal) < cosMinAngle)
{
foreach (var contact in testContactSet)
contact.Recycle();
testContactSet.Clear();
}
}
}
}
public override void ComputeCollision(ContactSet contactSet, CollisionQueryType type)
{
// Ray vs. box has at max 1 contact.
Debug.Assert(contactSet.Count <= 1);
// Object A should be the ray.
// Object B should be the box.
IGeometricObject rayObject = contactSet.ObjectA.GeometricObject;
IGeometricObject boxObject = contactSet.ObjectB.GeometricObject;
// Swap objects if necessary.
bool swapped = (boxObject.Shape is RayShape);
if (swapped)
{
MathHelper.Swap(ref rayObject, ref boxObject);
}
RayShape rayShape = rayObject.Shape as RayShape;
BoxShape boxShape = boxObject.Shape as BoxShape;
// Check if shapes are correct.
if (rayShape == null || boxShape == null)
{
throw new ArgumentException("The contact set must contain a ray and a box.", "contactSet");
}
// Call line segment vs. box for closest points queries.
if (type == CollisionQueryType.ClosestPoints)
{
// Find point on ray closest to the box.
// Call GJK.
_gjk.ComputeCollision(contactSet, type);
if (contactSet.HaveContact == false)
{
return;
}
// Otherwise compute 1 contact ...
// GJK result is invalid for penetration.
foreach (var contact in contactSet)
{
contact.Recycle();
}
contactSet.Clear();
}
// Assume no contact.
contactSet.HaveContact = false;
// Get transformations.
Vector3F rayScale = rayObject.Scale;
Vector3F boxScale = Vector3F.Absolute(boxObject.Scale);
Pose rayPose = rayObject.Pose;
Pose boxPose = boxObject.Pose;
// Apply scale to box.
Vector3F boxExtent = boxShape.Extent * boxScale;
// See SOLID and Bergen: "Collision Detection in Interactive 3D Environments", p. 75.
// Note: Compute in box local space.
// Apply scale to ray and transform to local space of box.
Ray rayWorld = new Ray(rayShape);
rayWorld.Scale(ref rayScale); // Apply scale to ray.
rayWorld.ToWorld(ref rayPose); // Transform ray to world space.
Ray ray = rayWorld;
ray.ToLocal(ref boxPose); // Transform ray from world to local space.
uint startOutcode = GeometryHelper.GetOutcode(boxExtent, ray.Origin);
uint endOutcode = GeometryHelper.GetOutcode(boxExtent, ray.Origin + ray.Direction * ray.Length);
if ((startOutcode & endOutcode) != 0)
{
// A face of the box is a separating plane.
return;
}
// Assertion: The ray can intersect with the box but may not...
float λEnter = 0; // ray parameter where ray enters box
float λExit = 1; // ray parameter where ray exits box
uint bit = 1;
Vector3F r = ray.Direction * ray.Length; // ray vector
Vector3F halfExtent = 0.5f * boxExtent; // Box half-extent vector.
Vector3F normal = Vector3F.Zero; // normal vector
for (int i = 0; i < 3; i++)
{
if ((startOutcode & bit) != 0)
{
// Intersection is an entering point.
float λ = (-ray.Origin[i] - halfExtent[i]) / r[i];
if (λEnter < λ)
{
λEnter = λ;
normal = new Vector3F();
normal[i] = 1;
}
}
else if ((endOutcode & bit) != 0)
{
// Intersection is an exciting point.
float λ = (-ray.Origin[i] - halfExtent[i]) / r[i];
if (λExit > λ)
{
λExit = λ;
}
}
bit <<= 1;
if ((startOutcode & bit) != 0)
{
// Intersection is an entering point.
float λ = (-ray.Origin[i] + halfExtent[i]) / r[i];
if (λEnter < λ)
{
λEnter = λ;
normal = new Vector3F();
normal[i] = -1;
}
}
else if ((endOutcode & bit) != 0)
{
// Intersection is an exciting point.
float λ = (-ray.Origin[i] + halfExtent[i]) / r[i];
if (λExit > λ)
{
λExit = λ;
}
}
bit <<= 1;
}
if (λEnter <= λExit)
{
// The ray intersects the box.
contactSet.HaveContact = true;
if (type == CollisionQueryType.Boolean)
{
return;
}
float penetrationDepth = λEnter * ray.Length;
if (normal == Vector3F.Zero)
{
normal = Vector3F.UnitX;
}
// Create contact info.
Vector3F position = rayWorld.Origin + rayWorld.Direction * penetrationDepth;
normal = boxPose.ToWorldDirection(normal);
if (swapped)
{
normal = -normal;
}
// Update contact set.
Contact contact = ContactHelper.CreateContact(contactSet, position, normal, penetrationDepth, true);
ContactHelper.Merge(contactSet, contact, type, CollisionDetection.ContactPositionTolerance);
}
}
public override void ComputeCollision(ContactSet contactSet, CollisionQueryType type)
{
// Ray vs. box has at max 1 contact.
Debug.Assert(contactSet.Count <= 1);
// Object A should be the ray.
// Object B should be the box.
IGeometricObject rayObject = contactSet.ObjectA.GeometricObject;
IGeometricObject boxObject = contactSet.ObjectB.GeometricObject;
// Swap objects if necessary.
bool swapped = (boxObject.Shape is RayShape);
if (swapped)
MathHelper.Swap(ref rayObject, ref boxObject);
RayShape rayShape = rayObject.Shape as RayShape;
BoxShape boxShape = boxObject.Shape as BoxShape;
// Check if shapes are correct.
if (rayShape == null || boxShape == null)
throw new ArgumentException("The contact set must contain a ray and a box.", "contactSet");
// Call line segment vs. box for closest points queries.
if (type == CollisionQueryType.ClosestPoints)
{
// Find point on ray closest to the box.
// Call GJK.
_gjk.ComputeCollision(contactSet, type);
if (contactSet.HaveContact == false)
return;
// Otherwise compute 1 contact ...
// GJK result is invalid for penetration.
foreach (var contact in contactSet)
contact.Recycle();
contactSet.Clear();
}
// Assume no contact.
contactSet.HaveContact = false;
// Get transformations.
Vector3F rayScale = rayObject.Scale;
Vector3F boxScale = Vector3F.Absolute(boxObject.Scale);
Pose rayPose = rayObject.Pose;
Pose boxPose = boxObject.Pose;
// Apply scale to box.
Vector3F boxExtent = boxShape.Extent * boxScale;
// See SOLID and Bergen: "Collision Detection in Interactive 3D Environments", p. 75.
// Note: Compute in box local space.
// Apply scale to ray and transform to local space of box.
Ray rayWorld = new Ray(rayShape);
rayWorld.Scale(ref rayScale); // Apply scale to ray.
rayWorld.ToWorld(ref rayPose); // Transform ray to world space.
Ray ray = rayWorld;
ray.ToLocal(ref boxPose); // Transform ray from world to local space.
uint startOutcode = GeometryHelper.GetOutcode(boxExtent, ray.Origin);
uint endOutcode = GeometryHelper.GetOutcode(boxExtent, ray.Origin + ray.Direction * ray.Length);
if ((startOutcode & endOutcode) != 0)
{
// A face of the box is a separating plane.
return;
}
// Assertion: The ray can intersect with the box but may not...
float λEnter = 0; // ray parameter where ray enters box
float λExit = 1; // ray parameter where ray exits box
uint bit = 1;
Vector3F r = ray.Direction * ray.Length; // ray vector
Vector3F halfExtent = 0.5f * boxExtent; // Box half-extent vector.
Vector3F normal = Vector3F.Zero; // normal vector
for (int i = 0; i < 3; i++)
{
if ((startOutcode & bit) != 0)
{
// Intersection is an entering point.
float λ = (-ray.Origin[i] - halfExtent[i]) / r[i];
if (λEnter < λ)
{
λEnter = λ;
normal = new Vector3F();
normal[i] = 1;
}
}
else if ((endOutcode & bit) != 0)
{
// Intersection is an exciting point.
float λ = (-ray.Origin[i] - halfExtent[i]) / r[i];
if (λExit > λ)
λExit = λ;
}
bit <<= 1;
if ((startOutcode & bit) != 0)
{
// Intersection is an entering point.
float λ = (-ray.Origin[i] + halfExtent[i]) / r[i];
if (λEnter < λ)
{
λEnter = λ;
normal = new Vector3F();
normal[i] = -1;
}
}
else if ((endOutcode & bit) != 0)
{
// Intersection is an exciting point.
float λ = (-ray.Origin[i] + halfExtent[i]) / r[i];
if (λExit > λ)
λExit = λ;
}
bit <<= 1;
}
if (λEnter <= λExit)
{
// The ray intersects the box.
contactSet.HaveContact = true;
if (type == CollisionQueryType.Boolean)
return;
float penetrationDepth = λEnter * ray.Length;
if (normal == Vector3F.Zero)
normal = Vector3F.UnitX;
// Create contact info.
Vector3F position = rayWorld.Origin + rayWorld.Direction * penetrationDepth;
normal = boxPose.ToWorldDirection(normal);
if (swapped)
normal = -normal;
// Update contact set.
Contact contact = ContactHelper.CreateContact(contactSet, position, normal, penetrationDepth, true);
ContactHelper.Merge(contactSet, contact, type, CollisionDetection.ContactPositionTolerance);
}
}
/// <summary>
/// Performs a collision query to update the contact information in the contact set.
/// </summary>
/// <param name="contactSet">The contact set.</param>
/// <param name="deltaTime">
/// The time step size in seconds. (The elapsed simulation time since the contact set was
/// updated the last time.)
/// </param>
/// <remarks>
/// <para>
/// This method updates contact information stored in the given contact set. This method is
/// usually faster than <see cref="GetContacts"/> because the information in
/// <paramref name="contactSet"/> is reused and updated.
/// </para>
/// <para>
/// The life time counter of persistent contacts is increased.
/// </para>
/// </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");
// Remove separated contacts - the contact set could contain separated
// closest points of a GetClosestPoints query.
ContactHelper.RemoveSeparatedContacts(contactSet);
// Update cached information.
ContactHelper.UpdateContacts(contactSet, deltaTime, CollisionDetection.ContactPositionTolerance);
// We do not compute the detailed contact information for triggers.
bool ignoreContactInfo = (contactSet.ObjectA.Type == CollisionObjectType.Trigger
|| contactSet.ObjectB.Type == CollisionObjectType.Trigger);
// We have to make the full contact query if one object is a ray with StopsAtFirstHit because
// we need the PenetrationDepth for sorting the hits.
if (contactSet.ObjectA.IsRayThatStopsAtFirstHit || contactSet.ObjectB.IsRayThatStopsAtFirstHit)
ignoreContactInfo = false;
if (ignoreContactInfo)
{
// Check cached flag. If necessary, make only boolean check.
if (contactSet.HaveContact == false || contactSet.Count == 0)
ComputeCollision(contactSet, CollisionQueryType.Boolean);
}
else
{
// Compute new contact info.
ComputeCollision(contactSet, CollisionQueryType.Contacts);
}
if (contactSet.HaveContact == false)
{
// No contact: Remove old contacts if objects do not touch.
foreach (var contact in contactSet)
contact.Recycle();
contactSet.Clear();
}
else
{
// Reduce ray cast results to 1 contact.
if (contactSet.ObjectA.IsRay || contactSet.ObjectB.IsRay)
ContactHelper.ReduceRayHits(contactSet);
// We have contact: Call contact filter.
if (CollisionDetection.ContactFilter != null)
CollisionDetection.ContactFilter.Filter(contactSet);
}
CheckResult(contactSet, false);
contactSet.IsValid = true;
}
private void AddTriangleContacts(ContactSet contactSet,
bool swapped,
int triangleIndex,
CollisionQueryType type,
ContactSet testContactSet,
CollisionObject testCollisionObject,
TestGeometricObject testGeometricObject,
TriangleShape testTriangle)
{
// Object A should be the triangle mesh.
CollisionObject collisionObjectA = (swapped) ? contactSet.ObjectB : contactSet.ObjectA;
CollisionObject collisionObjectB = (swapped) ? contactSet.ObjectA : contactSet.ObjectB;
IGeometricObject geometricObjectA = collisionObjectA.GeometricObject;
var triangleMeshShape = ((TriangleMeshShape)geometricObjectA.Shape);
Triangle triangle = triangleMeshShape.Mesh.GetTriangle(triangleIndex);
Pose poseA = geometricObjectA.Pose;
Vector3F scaleA = geometricObjectA.Scale;
// Find collision algorithm.
CollisionAlgorithm collisionAlgorithm = CollisionDetection.AlgorithmMatrix[typeof(TriangleShape), collisionObjectB.GeometricObject.Shape.GetType()];
// Apply scaling.
testTriangle.Vertex0 = triangle.Vertex0 * scaleA;
testTriangle.Vertex1 = triangle.Vertex1 * scaleA;
testTriangle.Vertex2 = triangle.Vertex2 * scaleA;
// Set the shape temporarily to the current triangles.
testGeometricObject.Shape = testTriangle;
testGeometricObject.Scale = Vector3F.One;
testGeometricObject.Pose = poseA;
testCollisionObject.SetInternal(collisionObjectA, testGeometricObject);
// Make a temporary contact set.
// (Object A and object B should have the same order as in contactSet; otherwise we couldn't
// simply merge them.)
Debug.Assert(testContactSet.Count == 0, "testContactSet needs to be cleared.");
if (swapped)
{
testContactSet.Reset(collisionObjectB, testCollisionObject);
}
else
{
testContactSet.Reset(testCollisionObject, collisionObjectB);
}
if (type == CollisionQueryType.Boolean)
{
collisionAlgorithm.ComputeCollision(testContactSet, CollisionQueryType.Boolean);
contactSet.HaveContact = contactSet.HaveContact || testContactSet.HaveContact;
}
else
{
// No perturbation test. Most triangle mesh shapes are either complex and automatically
// have more contacts. Or they are complex and will not be used for stacking
// where full contact sets would be needed.
testContactSet.IsPerturbationTestAllowed = false;
// TODO: Copy separating axis info and similar things into triangleContactSet.
// But currently this info is not used in the queries.
// For closest points: If we know that we have a contact, then we can make a
// faster contact query instead of a closest-point query.
CollisionQueryType queryType = (contactSet.HaveContact) ? CollisionQueryType.Contacts : type;
collisionAlgorithm.ComputeCollision(testContactSet, queryType);
contactSet.HaveContact = contactSet.HaveContact || testContactSet.HaveContact;
if (testContactSet.HaveContact && testContactSet.Count > 0 && !triangleMeshShape.IsTwoSided)
{
// To compute the triangle normal in world space we take the normal of the unscaled
// triangle and transform the normal with: (M^-1)^T = 1 / scale
Vector3F triangleNormalLocal = Vector3F.Cross(triangle.Vertex1 - triangle.Vertex0, triangle.Vertex2 - triangle.Vertex0) / scaleA;
Vector3F triangleNormal = poseA.ToWorldDirection(triangleNormalLocal);
if (triangleNormal.TryNormalize())
{
var preferredNormal = swapped ? -triangleNormal : triangleNormal;
// ----- Remove bad normal.
// Triangles are double sided, but meshes are single sided.
// --> Remove contacts where the contact normal points into the wrong direction.
ContactHelper.RemoveBadContacts(testContactSet, preferredNormal, -Numeric.EpsilonF);
if (testContactSet.Count > 0 && triangleMeshShape.EnableContactWelding)
{
var contactDotTriangle = Vector3F.Dot(testContactSet[0].Normal, preferredNormal);
if (contactDotTriangle < WeldingLimit)
{
// Bad normal. Perform welding.
Vector3F contactPositionOnTriangle = swapped
? testContactSet[0].PositionBLocal / scaleA
: testContactSet[0].PositionALocal / scaleA;
Vector3F neighborNormal;
float triangleDotNeighbor;
GetNeighborNormal(triangleIndex, triangle, contactPositionOnTriangle, triangleNormal, triangleMeshShape, poseA, scaleA, out neighborNormal, out triangleDotNeighbor);
if (triangleDotNeighbor < float.MaxValue && Numeric.IsLess(contactDotTriangle, triangleDotNeighbor))
{
// Normal is not in allowed range.
// Test again in triangle normal direction.
Contact c0 = testContactSet[0];
testContactSet.RemoveAt(0);
testContactSet.Clear();
testContactSet.PreferredNormal = preferredNormal;
collisionAlgorithm.ComputeCollision(testContactSet, queryType);
testContactSet.PreferredNormal = Vector3F.Zero;
if (testContactSet.Count > 0)
{
Contact c1 = testContactSet[0];
float contact1DotTriangle = Vector3F.Dot(c1.Normal, preferredNormal);
// We use c1 instead of c0 if it has lower penetration depth (then it is simply
// better). Or we use c1 if the penetration depth increase is in an allowed range
// and c1 has a normal in the allowed range.
if (c1.PenetrationDepth < c0.PenetrationDepth ||
Numeric.IsGreaterOrEqual(contact1DotTriangle, triangleDotNeighbor) &&
c1.PenetrationDepth < c0.PenetrationDepth + CollisionDetection.ContactPositionTolerance)
{
c0.Recycle();
c0 = c1;
testContactSet.RemoveAt(0);
contactDotTriangle = contact1DotTriangle;
}
}
if (Numeric.IsLess(contactDotTriangle, triangleDotNeighbor))
{
// Clamp contact to allowed normal:
// We keep the contact position on the mesh and the penetration depth. We set
// a new normal and compute the other related values for this normal.
if (!swapped)
{
var positionAWorld = c0.PositionAWorld;
c0.Normal = neighborNormal;
var positionBWorld = positionAWorld - c0.Normal * c0.PenetrationDepth;
c0.Position = (positionAWorld + positionBWorld) / 2;
c0.PositionBLocal = testContactSet.ObjectB.GeometricObject.Pose.ToLocalPosition(positionBWorld);
}
else
{
var positionBWorld = c0.PositionBWorld;
c0.Normal = -neighborNormal;
var positionAWorld = positionBWorld + c0.Normal * c0.PenetrationDepth;
c0.Position = (positionAWorld + positionBWorld) / 2;
c0.PositionALocal = testContactSet.ObjectA.GeometricObject.Pose.ToLocalPosition(positionAWorld);
}
}
c0.Recycle();
}
}
}
}
}
#region ----- Merge testContactSet into contactSet -----
if (testContactSet.Count > 0)
{
// Set the shape feature of the new contacts.
int numberOfContacts = testContactSet.Count;
for (int i = 0; i < numberOfContacts; i++)
{
Contact contact = testContactSet[i];
//if (contact.Lifetime.Ticks == 0) // Currently, this check is not necessary because triangleSet does not contain old contacts.
//{
if (swapped)
{
contact.FeatureB = triangleIndex;
}
else
{
contact.FeatureA = triangleIndex;
}
//}
}
// Merge the contact info.
ContactHelper.Merge(contactSet, testContactSet, type, CollisionDetection.ContactPositionTolerance);
}
#endregion
}
}
public override void ComputeCollision(ContactSet contactSet, CollisionQueryType type)
{
// Ray vs. convex has at max 1 contact.
Debug.Assert(contactSet.Count <= 1);
// Object A should be the ray.
// Object B should be the convex.
IGeometricObject rayObject = contactSet.ObjectA.GeometricObject;
IGeometricObject convexObject = contactSet.ObjectB.GeometricObject;
// Swap object if necessary.
bool swapped = (convexObject.Shape is RayShape);
if (swapped)
{
MathHelper.Swap(ref rayObject, ref convexObject);
}
RayShape rayShape = rayObject.Shape as RayShape;
ConvexShape convexShape = convexObject.Shape as ConvexShape;
// Check if shapes are correct.
if (rayShape == null || convexShape == null)
{
throw new ArgumentException("The contact set must contain a ray and a convex shape.", "contactSet");
}
// Call line segment vs. convex for closest points queries.
if (type == CollisionQueryType.ClosestPoints)
{
// Find point on ray closest to the convex shape.
// Call GJK.
_gjk.ComputeCollision(contactSet, type);
if (contactSet.HaveContact == false)
{
return;
}
// Otherwise compute 1 contact ...
// GJK result is invalid for penetration.
foreach (var contact in contactSet)
{
contact.Recycle();
}
contactSet.Clear();
}
// Assume no contact.
contactSet.HaveContact = false;
// Get transformations.
Vector3 rayScale = rayObject.Scale;
Vector3 convexScale = convexObject.Scale;
Pose convexPose = convexObject.Pose;
Pose rayPose = rayObject.Pose;
// See Raycasting paper of van den Bergen or Bullet.
// Note: Compute in local space of convex (object B).
// Scale ray and transform ray to local space of convex.
Ray rayWorld = new Ray(rayShape);
rayWorld.Scale(ref rayScale); // Scale ray.
rayWorld.ToWorld(ref rayPose); // Transform ray to world space.
Ray ray = rayWorld;
ray.ToLocal(ref convexPose); // Transform ray to local space of convex.
var simplex = GjkSimplexSolver.Create();
try
{
Vector3 s = ray.Origin; // source
Vector3 r = ray.Direction * ray.Length; // ray
float λ = 0; // ray parameter
Vector3 x = s; // hit spot (on ray)
Vector3 n = new Vector3(); // normal
Vector3 v = x - convexShape.GetSupportPoint(ray.Direction, convexScale);
// v = x - arbitrary point. Vector used for support mapping.
float distanceSquared = v.LengthSquared(); // ||v||²
int iterationCount = 0;
while (distanceSquared > Numeric.EpsilonF && iterationCount < MaxNumberOfIterations)
{
iterationCount++;
Vector3 p = convexShape.GetSupportPoint(v, convexScale); // point on convex
Vector3 w = x - p; // simplex/Minkowski difference point
float vDotW = Vector3.Dot(v, w); // v∙w
if (vDotW > 0)
{
float vDotR = Vector3.Dot(v, r); // v∙r
if (vDotR >= 0) // TODO: vDotR >= - Epsilon^2 ?
{
return; // No Hit.
}
λ = λ - vDotW / vDotR;
x = s + λ * r;
simplex.Clear(); // Configuration space obstacle (CSO) is translated whenever x is updated.
w = x - p;
n = v;
}
simplex.Add(w, x, p);
simplex.Update();
v = simplex.ClosestPoint;
distanceSquared = (simplex.IsValid && !simplex.IsFull) ? v.LengthSquared() : 0;
}
// We have a contact if the hit is inside the ray length.
contactSet.HaveContact = (0 <= λ && λ <= 1);
if (type == CollisionQueryType.Boolean || (type == CollisionQueryType.Contacts && !contactSet.HaveContact))
{
// HaveContact queries can exit here.
// GetContacts queries can exit here if we don't have a contact.
return;
}
float penetrationDepth = λ * ray.Length;
Debug.Assert(contactSet.HaveContact, "Separation was not detected by GJK above.");
// Convert back to world space.
Vector3 position = rayWorld.Origin + rayWorld.Direction * penetrationDepth;
n = convexPose.ToWorldDirection(n);
if (!n.TryNormalize())
{
n = Vector3.UnitY;
}
if (swapped)
{
n = -n;
}
// Update contact set.
Contact contact = ContactHelper.CreateContact(contactSet, position, -n, penetrationDepth, true);
ContactHelper.Merge(contactSet, contact, type, CollisionDetection.ContactPositionTolerance);
}
finally
{
simplex.Recycle();
}
}
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);
}
}
private void AddTriangleContacts(ContactSet contactSet,
bool swapped,
int triangleIndex,
CollisionQueryType type,
ContactSet testContactSet,
CollisionObject testCollisionObject,
TestGeometricObject testGeometricObject,
TriangleShape testTriangle)
{
// Object A should be the triangle mesh.
CollisionObject collisionObjectA = (swapped) ? contactSet.ObjectB : contactSet.ObjectA;
CollisionObject collisionObjectB = (swapped) ? contactSet.ObjectA : contactSet.ObjectB;
IGeometricObject geometricObjectA = collisionObjectA.GeometricObject;
var triangleMeshShape = ((TriangleMeshShape)geometricObjectA.Shape);
Triangle triangle = triangleMeshShape.Mesh.GetTriangle(triangleIndex);
Pose poseA = geometricObjectA.Pose;
Vector3F scaleA = geometricObjectA.Scale;
// Find collision algorithm.
CollisionAlgorithm collisionAlgorithm = CollisionDetection.AlgorithmMatrix[typeof(TriangleShape), collisionObjectB.GeometricObject.Shape.GetType()];
// Apply scaling.
testTriangle.Vertex0 = triangle.Vertex0 * scaleA;
testTriangle.Vertex1 = triangle.Vertex1 * scaleA;
testTriangle.Vertex2 = triangle.Vertex2 * scaleA;
// Set the shape temporarily to the current triangles.
testGeometricObject.Shape = testTriangle;
testGeometricObject.Scale = Vector3F.One;
testGeometricObject.Pose = poseA;
testCollisionObject.SetInternal(collisionObjectA, testGeometricObject);
// Make a temporary contact set.
// (Object A and object B should have the same order as in contactSet; otherwise we couldn't
// simply merge them.)
Debug.Assert(testContactSet.Count == 0, "testContactSet needs to be cleared.");
if (swapped)
testContactSet.Reset(collisionObjectB, testCollisionObject);
else
testContactSet.Reset(testCollisionObject, collisionObjectB);
if (type == CollisionQueryType.Boolean)
{
collisionAlgorithm.ComputeCollision(testContactSet, CollisionQueryType.Boolean);
contactSet.HaveContact = contactSet.HaveContact || testContactSet.HaveContact;
}
else
{
// No perturbation test. Most triangle mesh shapes are either complex and automatically
// have more contacts. Or they are complex and will not be used for stacking
// where full contact sets would be needed.
testContactSet.IsPerturbationTestAllowed = false;
// TODO: Copy separating axis info and similar things into triangleContactSet.
// But currently this info is not used in the queries.
// For closest points: If we know that we have a contact, then we can make a
// faster contact query instead of a closest-point query.
CollisionQueryType queryType = (contactSet.HaveContact) ? CollisionQueryType.Contacts : type;
collisionAlgorithm.ComputeCollision(testContactSet, queryType);
contactSet.HaveContact = contactSet.HaveContact || testContactSet.HaveContact;
if (testContactSet.HaveContact && testContactSet.Count > 0 && !triangleMeshShape.IsTwoSided)
{
// To compute the triangle normal in world space we take the normal of the unscaled
// triangle and transform the normal with: (M^-1)^T = 1 / scale
Vector3F triangleNormalLocal = Vector3F.Cross(triangle.Vertex1 - triangle.Vertex0, triangle.Vertex2 - triangle.Vertex0) / scaleA;
Vector3F triangleNormal = poseA.ToWorldDirection(triangleNormalLocal);
if (triangleNormal.TryNormalize())
{
var preferredNormal = swapped ? -triangleNormal : triangleNormal;
// ----- Remove bad normal.
// Triangles are double sided, but meshes are single sided.
// --> Remove contacts where the contact normal points into the wrong direction.
ContactHelper.RemoveBadContacts(testContactSet, preferredNormal, -Numeric.EpsilonF);
if (testContactSet.Count > 0 && triangleMeshShape.EnableContactWelding)
{
var contactDotTriangle = Vector3F.Dot(testContactSet[0].Normal, preferredNormal);
if (contactDotTriangle < WeldingLimit)
{
// Bad normal. Perform welding.
Vector3F contactPositionOnTriangle = swapped
? testContactSet[0].PositionBLocal / scaleA
: testContactSet[0].PositionALocal / scaleA;
Vector3F neighborNormal;
float triangleDotNeighbor;
GetNeighborNormal(triangleIndex, triangle, contactPositionOnTriangle, triangleNormal, triangleMeshShape, poseA, scaleA, out neighborNormal, out triangleDotNeighbor);
if (triangleDotNeighbor < float.MaxValue && Numeric.IsLess(contactDotTriangle, triangleDotNeighbor))
{
// Normal is not in allowed range.
// Test again in triangle normal direction.
Contact c0 = testContactSet[0];
testContactSet.RemoveAt(0);
testContactSet.Clear();
testContactSet.PreferredNormal = preferredNormal;
collisionAlgorithm.ComputeCollision(testContactSet, queryType);
testContactSet.PreferredNormal = Vector3F.Zero;
if (testContactSet.Count > 0)
{
Contact c1 = testContactSet[0];
float contact1DotTriangle = Vector3F.Dot(c1.Normal, preferredNormal);
// We use c1 instead of c0 if it has lower penetration depth (then it is simply
// better). Or we use c1 if the penetration depth increase is in an allowed range
// and c1 has a normal in the allowed range.
if (c1.PenetrationDepth < c0.PenetrationDepth
|| Numeric.IsGreaterOrEqual(contact1DotTriangle, triangleDotNeighbor)
&& c1.PenetrationDepth < c0.PenetrationDepth + CollisionDetection.ContactPositionTolerance)
{
c0.Recycle();
c0 = c1;
testContactSet.RemoveAt(0);
contactDotTriangle = contact1DotTriangle;
}
}
if (Numeric.IsLess(contactDotTriangle, triangleDotNeighbor))
{
// Clamp contact to allowed normal:
// We keep the contact position on the mesh and the penetration depth. We set
// a new normal and compute the other related values for this normal.
if (!swapped)
{
var positionAWorld = c0.PositionAWorld;
c0.Normal = neighborNormal;
var positionBWorld = positionAWorld - c0.Normal * c0.PenetrationDepth;
c0.Position = (positionAWorld + positionBWorld) / 2;
c0.PositionBLocal = testContactSet.ObjectB.GeometricObject.Pose.ToLocalPosition(positionBWorld);
}
else
{
var positionBWorld = c0.PositionBWorld;
c0.Normal = -neighborNormal;
var positionAWorld = positionBWorld + c0.Normal * c0.PenetrationDepth;
c0.Position = (positionAWorld + positionBWorld) / 2;
c0.PositionALocal = testContactSet.ObjectA.GeometricObject.Pose.ToLocalPosition(positionAWorld);
}
}
c0.Recycle();
}
}
}
}
}
#region ----- Merge testContactSet into contactSet -----
if (testContactSet.Count > 0)
{
// Set the shape feature of the new contacts.
int numberOfContacts = testContactSet.Count;
for (int i = 0; i < numberOfContacts; i++)
{
Contact contact = testContactSet[i];
//if (contact.Lifetime.Ticks == 0) // Currently, this check is not necessary because triangleSet does not contain old contacts.
//{
if (swapped)
contact.FeatureB = triangleIndex;
else
contact.FeatureA = triangleIndex;
//}
}
// Merge the contact info.
ContactHelper.Merge(contactSet, testContactSet, type, CollisionDetection.ContactPositionTolerance);
}
#endregion
}
}