// This method performs ray casting on the CSO. The ray casting code is similar to the
// RayConvexAlgorithm code and also described in the paper:
// Gino van den Bergen “Ray Casting against General Convex Objects with
// Application to Continuous Collision Detection”, GDC 2005. www.dtecta.com
// When a new hit point on the ray is found, the object A and B are moved instead
// of selecting a new hit point on the ray. - So the CSO is always tested against the
// origin and the CSO is deforming. See this paper:
// http://www.continuousphysics.com/BulletContinuousCollisionDetection.pdf
// (TODO: Here we only consider linear movement. But the method in the paper should also work for rotating objects!)
/// <summary>
/// Gets the time of impact of the linear sweeps of both objects (ignoring rotational movement).
/// </summary>
/// <param name="objectA">The object A.</param>
/// <param name="targetPoseA">The target pose of A.</param>
/// <param name="objectB">The object B.</param>
/// <param name="targetPoseB">The target pose of B.</param>
/// <param name="allowedPenetration">The allowed penetration.</param>
/// <returns>The time of impact in the range [0, 1].</returns>
/// <remarks>
/// Both objects are moved from the current positions to their target positions. Angular
/// movement is ignored.
/// </remarks>
/// <exception cref="ArgumentNullException">
/// <paramref name="objectA"/> or <paramref name="objectB"/> is <see langword="null"/>.
/// </exception>
/// <exception cref="ArgumentException">
/// <paramref name="objectA"/> and <paramref name="objectB"/> are not convex shapes.
/// </exception>
internal static float GetTimeOfImpactLinearSweep(CollisionObject objectA, Pose targetPoseA, CollisionObject objectB, Pose targetPoseB, float allowedPenetration)
{
if (objectA == null)
{
throw new ArgumentNullException("objectA");
}
if (objectB == null)
{
throw new ArgumentNullException("objectB");
}
IGeometricObject geometricObjectA = objectA.GeometricObject;
IGeometricObject geometricObjectB = objectB.GeometricObject;
ConvexShape convexA = geometricObjectA.Shape as ConvexShape;
ConvexShape convexB = geometricObjectB.Shape as ConvexShape;
// Check if shapes are correct.
if (convexA == null || convexB == null)
{
throw new ArgumentException("objectA and objectB must have convex shapes.");
}
Pose startPoseA = geometricObjectA.Pose;
Pose startPoseB = geometricObjectB.Pose;
// Compute relative linear velocity.
Vector3 linearVelocityA = targetPoseA.Position - startPoseA.Position;
Vector3 linearVelocityB = targetPoseB.Position - startPoseB.Position;
Vector3 linearVelocityRelative = linearVelocityA - linearVelocityB;
// Abort if relative movement is zero.
float linearVelocityRelativeMagnitude = linearVelocityRelative.Length;
if (Numeric.IsZero(linearVelocityRelativeMagnitude))
{
return(1);
}
// Get a simplex solver from a resource pool.
var simplex = GjkSimplexSolver.Create();
try
{
Vector3 scaleA = geometricObjectA.Scale;
Vector3 scaleB = geometricObjectB.Scale;
Pose poseA = startPoseA;
Pose poseB = startPoseB;
Vector3 r = linearVelocityRelative; // ray
float λ = 0; // ray parameter
//Vector3 n = new Vector3(); // normal
// First point on the CSO.
Vector3 supportA = poseA.ToWorldPosition(convexA.GetSupportPoint(poseA.ToLocalDirection(-r), scaleA));
Vector3 supportB = poseB.ToWorldPosition(convexB.GetSupportPoint(poseB.ToLocalDirection(r), scaleB));
Vector3 v = supportA - supportB;
float distanceSquared = v.LengthSquared(); // ||v||²
int iterationCount = 0;
while (distanceSquared > Numeric.EpsilonF && // We could use a higher EpsilonF to abort earlier.
iterationCount < MaxNumberOfIterations)
{
iterationCount++;
// To get a contact at the TOI, the objects are shrunk by an amount proportional to the
// allowed penetration. Therefore we need this values:
Vector3 vNormalized = v;
vNormalized.TryNormalize();
Vector3 vA = poseA.ToLocalDirection(-vNormalized);
Vector3 vB = poseB.ToLocalDirection(vNormalized);
// Get support point on each object and subtract the half allowed penetration.
supportA = poseA.ToWorldPosition(convexA.GetSupportPoint(vA, scaleA) - 0.5f * vA * allowedPenetration);
supportB = poseB.ToWorldPosition(convexB.GetSupportPoint(vB, scaleB) - 0.5f * vB * allowedPenetration);
// The new CSO point.
Vector3 w = supportA - supportB;
float vDotW = Vector3.Dot(v, w); // v∙w
if (vDotW > 0)
{
float vDotR = Vector3.Dot(v, r); // v∙r
if (vDotR >= -Numeric.EpsilonF) // TODO: vDotR >= -Epsilon^2 ?
{
return(1); // No Hit.
}
λ = λ - vDotW / vDotR;
// Instead of moving the hit point on the ray, we move the objects, so that
// the hit point stays at the origin. - See Erwin Coumans' Bullet paper.
//x = λ * r;
//simplex.Clear(); // Configuration space obstacle (CSO) is translated whenever x is updated.
poseA.Position = startPoseA.Position + λ * (targetPoseA.Position - startPoseA.Position);
poseB.Position = startPoseB.Position + λ * (targetPoseB.Position - startPoseB.Position);
//n = v;
}
if (!simplex.Contains(w))
{
simplex.Add(w, supportA, supportB);
}
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.
if (0 < λ && λ <= 1)
{
return(λ);
// This would be a contact, but the local contact positions would not be optimal.
//result.Contact = ContactHelper.CreateContact(objectA, objectB, simplex.ClosestPoint, -n.Normalized, 0, false);
}
}
finally
{
// Recycle temporary heap objects.
simplex.Recycle();
}
return(1);
}
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.Contacts)
{
throw new GeometryException("GJK cannot handle contact queries. Use MPR instead.");
}
IGeometricObject objectA = contactSet.ObjectA.GeometricObject;
ConvexShape shapeA = objectA.Shape as ConvexShape;
Vector3 scaleA = objectA.Scale;
Pose poseA = objectA.Pose;
IGeometricObject objectB = contactSet.ObjectB.GeometricObject;
ConvexShape shapeB = objectB.Shape as ConvexShape;
Vector3 scaleB = objectB.Scale;
Pose poseB = objectB.Pose;
if (shapeA == null || shapeB == null)
{
throw new ArgumentException("The contact set must contain two convex shapes.", "contactSet");
}
// GJK builds a simplex of the CSO (A-B). This simplex is managed in a GjkSimplexSolver.
var simplex = GjkSimplexSolver.Create();
bool foundSeparatingAxis = false;
try
{
// v is the separating axis or the CSO point nearest to the origin.
// We start with last known separating axis or with an arbitrary CSO point.
Vector3 v;
if (contactSet.Count > 0)
{
// Use last separating axis.
// The contact normal points from A to B. This is the direction we want to sample first.
// If the frame-to-frame coherence is high we should get a point close to the origin.
// Note: To sample in the normal direction, we need to initialize the CSO point v with
// -normal.
v = -contactSet[0].Normal;
}
else
{
// Use difference of inner points.
Vector3 vA = poseA.ToWorldPosition(shapeA.InnerPoint * scaleA);
Vector3 vB = poseB.ToWorldPosition(shapeB.InnerPoint * scaleB);
v = vA - vB;
}
// If the inner points overlap, then we have already found a contact.
// We don't expect this case to happen often, so we simply choose an arbitrary separating
// axis to continue with the normal GJK code.
if (v.IsNumericallyZero)
{
v = Vector3.UnitZ;
}
// Cache inverted rotations.
var orientationAInverse = poseA.Orientation.Transposed;
var orientationBInverse = poseB.Orientation.Transposed;
int iterationCount = 0;
float distanceSquared = float.MaxValue;
float distanceEpsilon;
// Assume we have no contact.
contactSet.HaveContact = false;
do
{
// TODO: Translate A and B close to the origin to avoid numerical problems.
// This optimization is done in Bullet: The offset (a.Pose.Position + b.Pose.Position) / 2
// is subtracted from a.Pose and b.Pose. This offset is added when the Contact info is
// computed (also in EPA if the poses are still translated).
// Compute a new point w on the simplex. We seek for the point that is closest to the origin.
// Therefore, we get the support points on the current separating axis v.
Vector3 p = poseA.ToWorldPosition(shapeA.GetSupportPoint(orientationAInverse * -v, scaleA));
Vector3 q = poseB.ToWorldPosition(shapeB.GetSupportPoint(orientationBInverse * v, scaleB));
Vector3 w = p - q;
// w projected onto the separating axis.
float delta = Vector3.Dot(w, v);
// If v∙w > 0 then the objects do not overlap.
if (delta > 0)
{
// We have found a separating axis.
foundSeparatingAxis = true;
// Early exit for boolean and contact queries.
if (type == CollisionQueryType.Boolean || type == CollisionQueryType.Contacts)
{
// TODO: We could cache the separating axis n in ContactSet for future collision checks.
return;
}
// We continue for closest point queries because we don't know if there are other
// points closer than p and q.
}
// If the new w is already part of the simplex. We cannot improve any further.
if (simplex.Contains(w))
{
break;
}
// If the new w is not closer to the origin (within numerical tolerance), we stop.
if (distanceSquared - delta <= distanceSquared * Numeric.EpsilonF) // SOLID uses Epsilon = 10^-6
{
break;
}
// Add the new point to the simplex.
simplex.Add(w, p, q);
// Update the simplex. (Unneeded simplex points are removed).
simplex.Update();
// Get new point of simplex closest to the origin.
v = simplex.ClosestPoint;
float previousDistanceSquared = distanceSquared;
distanceSquared = v.LengthSquared();
if (previousDistanceSquared < distanceSquared)
{
// If the result got worse, we use the previous result. This happens for
// degenerate cases for example when the simplex is a tetrahedron with all
// 4 vertices in a plane.
distanceSquared = previousDistanceSquared;
simplex.Backup();
break;
}
// If the new simplex is invalid, we stop.
// Example: A simplex gets invalid if a fourth vertex is added to create a tetrahedron
// simplex but all vertices are in a plane. This can happen if a box corner nearly touches a
// face of another box.
if (!simplex.IsValid)
{
break;
}
// Compare the distance of v to the origin with the distance of the last iteration.
// We stop if the improvement is less than the numerical tolerance.
if (previousDistanceSquared - distanceSquared <= previousDistanceSquared * Numeric.EpsilonF)
{
break;
}
// If we reach the iteration limit, we stop.
iterationCount++;
if (iterationCount > MaxNumberOfIterations)
{
Debug.Assert(false, "GJK reached the iteration limit.");
break;
}
// Compute a scaled epsilon.
distanceEpsilon = Numeric.EpsilonFSquared * Math.Max(1, simplex.MaxVertexDistanceSquared);
// Loop until the simplex is full (i.e. it contains the origin) or we have come
// sufficiently close to the origin.
} while (!simplex.IsFull && distanceSquared > distanceEpsilon);
Debug.Assert(simplex.IsEmpty == false, "The GJK simplex must contain at least 1 point.");
// Compute contact normal and separation.
Vector3 normal = -simplex.ClosestPoint; // simplex.ClosestPoint = ClosestPointA-ClosestPointB
float distance;
distanceEpsilon = Numeric.EpsilonFSquared * Math.Max(1, simplex.MaxVertexDistanceSquared);
if (distanceSquared <= distanceEpsilon)
{
// Distance is approximately 0.
// --> Objects are in contact.
if (simplex.IsValid && normal.TryNormalize())
{
// Normal can be used but we have to invert it because for contact we
// have to compute normal as pointOnA - pointOnB.
normal = -normal;
}
else
{
// No useful normal. Use direction between inner points as a fallback.
Vector3 innerA = poseA.ToWorldPosition(shapeA.InnerPoint * scaleA);
normal = simplex.ClosestPointOnA - innerA;
if (!normal.TryNormalize())
{
Vector3 innerB = poseB.ToWorldPosition(shapeB.InnerPoint * scaleB);
normal = innerB - innerA;
if (!normal.TryNormalize())
{
normal = Vector3.UnitY;
// TODO: We could use better normal: e.g. normal of old contact or PreferredNormal?
}
}
}
distance = 0;
contactSet.HaveContact = true;
}
else
{
// Distance is greater than 0.
distance = (float)Math.Sqrt(distanceSquared);
normal /= distance;
// If the simplex is valid and full, then we have a contact.
if (simplex.IsFull && simplex.IsValid)
{
// Let's use the current result as an estimated contact info for
// shallow contacts.
// TODO: The following IF was added because this can occur for valid
// triangle vs. triangle separation. Check this.
if (!foundSeparatingAxis)
{
contactSet.HaveContact = true;
// Distance is a penetration depth
distance = -distance;
// Since the simplex tetrahedron can have any position in the Minkowsky difference,
// we do not know the real normal. Let's use the current normal and make
// sure that it points away from A. - This is only a heuristic...
Vector3 innerA = poseA.ToWorldPosition(shapeA.InnerPoint * scaleA);
if (Vector3.Dot(simplex.ClosestPointOnA - innerA, normal) < 0)
{
normal = -normal;
}
}
}
}
Debug.Assert(normal.IsNumericallyZero == false);
if (type != CollisionQueryType.Boolean)
{
Vector3 position = (simplex.ClosestPointOnA + simplex.ClosestPointOnB) / 2;
Contact contact = ContactHelper.CreateContact(contactSet, position, normal, -distance, false);
ContactHelper.Merge(contactSet, contact, type, CollisionDetection.ContactPositionTolerance);
}
}
finally
{
simplex.Recycle();
}
}
