// 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(); } }