private Vector3 DoMpr(CollisionQueryType type, ContactSet contactSet, Vector3 v0)
{
int iterationCount = 0;
const int iterationLimit = 100;
CollisionObject collisionObjectA = contactSet.ObjectA;
IGeometricObject geometricObjectA = collisionObjectA.GeometricObject;
ConvexShape shapeA = (ConvexShape)geometricObjectA.Shape;
Vector3 scaleA = geometricObjectA.Scale;
Pose poseA = geometricObjectA.Pose;
CollisionObject collisionObjectB = contactSet.ObjectB;
IGeometricObject geometricObjectB = collisionObjectB.GeometricObject;
ConvexShape shapeB = (ConvexShape)geometricObjectB.Shape;
Vector3 scaleB = geometricObjectB.Scale;
Pose poseB = geometricObjectB.Pose;
// Cache inverted rotations.
var orientationAInverse = poseA.Orientation.Transposed;
var orientationBInverse = poseB.Orientation.Transposed;
Vector3 n = -v0; // Shoot from v0 to the origin.
Vector3 v1A = poseA.ToWorldPosition(shapeA.GetSupportPoint(orientationAInverse * -n, scaleA));
Vector3 v1B = poseB.ToWorldPosition(shapeB.GetSupportPoint(orientationBInverse * n, scaleB));
Vector3 v1 = v1B - v1A;
// Separating axis test:
if (Vector3.Dot(v1, n) < 0)
{
// TODO: We could cache the separating axis n in ContactSet for future collision checks.
// Also in the separating axis tests below.
return(Vector3.Zero);
}
// Second support direction = perpendicular to plane of origin, v0 and v1.
n = Vector3.Cross(v1, v0);
// If n is a zero vector, then origin, v0 and v1 are on a line with the origin inside the support plane.
if (n.IsNumericallyZero)
{
// Contact found.
contactSet.HaveContact = true;
if (type == CollisionQueryType.Boolean)
{
return(Vector3.Zero);
}
// Compute contact information.
// (v0 is an inner point. v1 is a support point on the CSO. => The contact normal is -v1.
// However, v1 could be close to the origin. To avoid numerical
// problems we use v0 - v1, which is the same direction.)
Vector3 normal = v0 - v1;
if (!normal.TryNormalize())
{
// This happens for Point vs. flat object when they are on the same position.
// Maybe we could even find a better normal.
normal = Vector3.UnitY;
}
Vector3 position = (v1A + v1B) / 2;
float penetrationDepth = v1.Length;
Contact contact = ContactHelper.CreateContact(contactSet, position, normal, penetrationDepth, false);
ContactHelper.Merge(contactSet, contact, type, CollisionDetection.ContactPositionTolerance);
return(Vector3.Zero);
}
Vector3 v2A = poseA.ToWorldPosition(shapeA.GetSupportPoint(orientationAInverse * -n, scaleA));
Vector3 v2B = poseB.ToWorldPosition(shapeB.GetSupportPoint(orientationBInverse * n, scaleB));
Vector3 v2 = v2B - v2A;
// Separating axis test:
if (Vector3.Dot(v2, n) < 0)
{
return(Vector3.Zero);
}
// Third support direction = perpendicular to plane of v0, v1 and v2.
n = Vector3.Cross(v1 - v0, v2 - v0);
// If the origin is on the negative side of the plane, then reverse the plane direction.
// n must point into the origin direction and not away...
if (Vector3.Dot(n, v0) > 0)
{
MathHelper.Swap(ref v1, ref v2);
MathHelper.Swap(ref v1A, ref v2A);
MathHelper.Swap(ref v1B, ref v2B);
n = -n;
}
if (n.IsNumericallyZero)
{
// Degenerate case:
// Interpretation (HelmutG): v2 is on the line with v0 and v1. I think this can only happen
// if the CSO is flat and in the plane of (origin, v0, v1).
// This happens for example in Point vs. Line Segment, or triangle vs. triangle when both
// triangles are in the same plane.
// Simply ignore this case (Infinite small/flat objects do not touch).
return(Vector3.Zero);
}
// Search for a valid portal.
Vector3 v3, v3A, v3B;
while (true)
{
iterationCount++;
// Abort if we cannot find a valid portal.
if (iterationCount > iterationLimit)
{
return(Vector3.Zero);
}
// Get next support point.
//v3A = poseA.ToWorldPosition(shapeA.GetSupportPoint(orientationAInverse * -n, scaleA));
//v3B = poseB.ToWorldPosition(shapeB.GetSupportPoint(orientationBInverse * n, scaleB));
//v3 = v3B - v3A;
// ----- Optimized version:
Vector3 supportDirectionA;
supportDirectionA.X = -(orientationAInverse.M00 * n.X + orientationAInverse.M01 * n.Y + orientationAInverse.M02 * n.Z);
supportDirectionA.Y = -(orientationAInverse.M10 * n.X + orientationAInverse.M11 * n.Y + orientationAInverse.M12 * n.Z);
supportDirectionA.Z = -(orientationAInverse.M20 * n.X + orientationAInverse.M21 * n.Y + orientationAInverse.M22 * n.Z);
Vector3 supportPointA = shapeA.GetSupportPoint(supportDirectionA, scaleA);
v3A.X = poseA.Orientation.M00 * supportPointA.X + poseA.Orientation.M01 * supportPointA.Y + poseA.Orientation.M02 * supportPointA.Z + poseA.Position.X;
v3A.Y = poseA.Orientation.M10 * supportPointA.X + poseA.Orientation.M11 * supportPointA.Y + poseA.Orientation.M12 * supportPointA.Z + poseA.Position.Y;
v3A.Z = poseA.Orientation.M20 * supportPointA.X + poseA.Orientation.M21 * supportPointA.Y + poseA.Orientation.M22 * supportPointA.Z + poseA.Position.Z;
Vector3 supportDirectionB;
supportDirectionB.X = orientationBInverse.M00 * n.X + orientationBInverse.M01 * n.Y + orientationBInverse.M02 * n.Z;
supportDirectionB.Y = orientationBInverse.M10 * n.X + orientationBInverse.M11 * n.Y + orientationBInverse.M12 * n.Z;
supportDirectionB.Z = orientationBInverse.M20 * n.X + orientationBInverse.M21 * n.Y + orientationBInverse.M22 * n.Z;
Vector3 supportPointB = shapeB.GetSupportPoint(supportDirectionB, scaleB);
v3B.X = poseB.Orientation.M00 * supportPointB.X + poseB.Orientation.M01 * supportPointB.Y + poseB.Orientation.M02 * supportPointB.Z + poseB.Position.X;
v3B.Y = poseB.Orientation.M10 * supportPointB.X + poseB.Orientation.M11 * supportPointB.Y + poseB.Orientation.M12 * supportPointB.Z + poseB.Position.Y;
v3B.Z = poseB.Orientation.M20 * supportPointB.X + poseB.Orientation.M21 * supportPointB.Y + poseB.Orientation.M22 * supportPointB.Z + poseB.Position.Z;
v3 = v3B - v3A;
// Separating axis test:
//if (Vector3.Dot(v3, n) < 0)
if (v3.X * n.X + v3.Y * n.Y + v3.Z * n.Z < 0)
{
return(Vector3.Zero);
}
// v0, v1, v2, v3 form a tetrahedron.
// v0 is an inner point of the CSO and v1, v2, v3 are support points.
// v1, v2, v3 should form a valid portal.
// If origin is outside the plane of v0, v1, v3 then the portal is invalid and we choose a new n.
//if (Vector3.Dot(Vector3.Cross(v1, v3), v0) < 0) // ORIENT3D test, see Ericson: "Real-Time Collision Detection"
if ((v1.Y * v3.Z - v1.Z * v3.Y) * v0.X
+ (v1.Z * v3.X - v1.X * v3.Z) * v0.Y
+ (v1.X * v3.Y - v1.Y * v3.X) * v0.Z < 0)
{
v2 = v3; // Get rid of v2. A new v3 will be chosen in the next iteration.
v2A = v3A;
v2B = v3B;
//n = Vector3.Cross(v1 - v0, v3 - v0);
// ----- Optimized version:
Vector3 v1MinusV0;
v1MinusV0.X = v1.X - v0.X;
v1MinusV0.Y = v1.Y - v0.Y;
v1MinusV0.Z = v1.Z - v0.Z;
Vector3 v3MinusV0;
v3MinusV0.X = v3.X - v0.X;
v3MinusV0.Y = v3.Y - v0.Y;
v3MinusV0.Z = v3.Z - v0.Z;
n.X = v1MinusV0.Y * v3MinusV0.Z - v1MinusV0.Z * v3MinusV0.Y;
n.Y = v1MinusV0.Z * v3MinusV0.X - v1MinusV0.X * v3MinusV0.Z;
n.Z = v1MinusV0.X * v3MinusV0.Y - v1MinusV0.Y * v3MinusV0.X;
continue;
}
// If origin is outside the plane of v0, v2, v3 then the portal is invalid and we choose a new n.
//if (Vector3.Dot(Vector3.Cross(v3, v2), v0) < 0)
if ((v3.Y * v2.Z - v3.Z * v2.Y) * v0.X
+ (v3.Z * v2.X - v3.X * v2.Z) * v0.Y
+ (v3.X * v2.Y - v3.Y * v2.X) * v0.Z < 0)
{
v1 = v3; // Get rid of v1. A new v3 will be chosen in the next iteration.
v1A = v3A;
v1B = v3B;
//n = Vector3.Cross(v3 - v0, v2 - v0);
// ----- Optimized version:
Vector3 v3MinusV0;
v3MinusV0.X = v3.X - v0.X;
v3MinusV0.Y = v3.Y - v0.Y;
v3MinusV0.Z = v3.Z - v0.Z;
Vector3 v2MinusV0;
v2MinusV0.X = v2.X - v0.X;
v2MinusV0.Y = v2.Y - v0.Y;
v2MinusV0.Z = v2.Z - v0.Z;
n.X = v3MinusV0.Y * v2MinusV0.Z - v3MinusV0.Z * v2MinusV0.Y;
n.Y = v3MinusV0.Z * v2MinusV0.X - v3MinusV0.X * v2MinusV0.Z;
n.Z = v3MinusV0.X * v2MinusV0.Y - v3MinusV0.Y * v2MinusV0.X;
continue;
}
// If come to here, then we have found a valid portal to begin with.
// (We have a tetrahedron that contains the ray (v0 to origin)).
break;
}
// Refine the portal
while (true)
{
iterationCount++;
// Store old n. Numerical inaccuracy can lead to endless loops where n is constant.
Vector3 oldN = n;
// Compute outward pointing normal of the portal
//n = Vector3.Cross(v2 - v1, v3 - v1);
Vector3 v2MinusV1;
v2MinusV1.X = v2.X - v1.X;
v2MinusV1.Y = v2.Y - v1.Y;
v2MinusV1.Z = v2.Z - v1.Z;
Vector3 v3MinusV1;
v3MinusV1.X = v3.X - v1.X;
v3MinusV1.Y = v3.Y - v1.Y;
v3MinusV1.Z = v3.Z - v1.Z;
n.X = v2MinusV1.Y * v3MinusV1.Z - v2MinusV1.Z * v3MinusV1.Y;
n.Y = v2MinusV1.Z * v3MinusV1.X - v2MinusV1.X * v3MinusV1.Z;
n.Z = v2MinusV1.X * v3MinusV1.Y - v2MinusV1.Y * v3MinusV1.X;
//if (!n.TryNormalize())
// ----- Optimized version:
float nLengthSquared = n.LengthSquared();
if (nLengthSquared < Numeric.EpsilonFSquared)
{
// The portal is degenerate (some vertices of v1, v2, v3 are identical).
// This can happen for coplanar shapes, e.g. long thin triangles in the
// same plane. The portal (v1, v2, v3) is a line segment.
// This might be a contact or not. We use the GJK as a fallback to check this case.
if (_gjk == null)
{
_gjk = new Gjk(CollisionDetection);
}
_gjk.ComputeCollision(contactSet, CollisionQueryType.Boolean);
if (contactSet.HaveContact == false)
{
return(Vector3.Zero);
}
// GJK reports a contact - but it cannot compute contact positions.
// We use the best point on the current portal as the contact point.
// Find the point closest to the origin.
float u, v, w;
GeometryHelper.GetClosestPoint(new Triangle(v1, v2, v3), Vector3.Zero, out u, out v, out w);
Vector3 vClosest = u * v1 + v * v2 + w * v3;
// We have not found a separating axis so far. --> Contact.
contactSet.HaveContact = true;
if (type == CollisionQueryType.Boolean)
{
return(Vector3.Zero);
}
// The points on the objects have the same barycentric coordinates.
Vector3 pointOnA = u * v1A + v * v2A + w * v3A;
Vector3 pointOnB = u * v1B + v * v2B + w * v3B;
Vector3 normal = pointOnA - pointOnB;
if (!normal.TryNormalize())
{
if (contactSet.IsPreferredNormalAvailable)
{
normal = contactSet.PreferredNormal;
}
else
{
normal = Vector3.UnitY;
}
}
Vector3 position = (pointOnA + pointOnB) / 2;
float penetrationDepth = vClosest.Length;
Contact contact = ContactHelper.CreateContact(contactSet, position, normal, penetrationDepth, false);
ContactHelper.Merge(contactSet, contact, type, CollisionDetection.ContactPositionTolerance);
return(Vector3.Zero);
}
// ----- Optimized version: Rest of n.TryNormalize():
float nLength = (float)Math.Sqrt(nLengthSquared);
float scale = 1.0f / nLength;
n.X *= scale;
n.Y *= scale;
n.Z *= scale;
// Separating axis test:
// Testing > instead of >= is important otherwise coplanar triangles may report false contacts
// because the portal is in the same plane as the origin.
if (!contactSet.HaveContact &&
v1.X * n.X + v1.Y * n.Y + v1.Z * n.Z > 0) // Optimized version of && Vector3.Dot(v1, n) > 0)
{
// Portal points aways from origin --> Origin is in the tetrahedron.
contactSet.HaveContact = true;
if (type == CollisionQueryType.Boolean)
{
return(Vector3.Zero);
}
}
// Find new support point.
//Vector3 v4A = poseA.ToWorldPosition(shapeA.GetSupportPoint(orientationAInverse * -n, scaleA));
//Vector3 v4B = poseB.ToWorldPosition(shapeB.GetSupportPoint(orientationBInverse * n, scaleB));
//Vector3 v4 = v4B - v4A;
// ----- Optimized version:
Vector3 supportDirectionA;
supportDirectionA.X = -(orientationAInverse.M00 * n.X + orientationAInverse.M01 * n.Y + orientationAInverse.M02 * n.Z);
supportDirectionA.Y = -(orientationAInverse.M10 * n.X + orientationAInverse.M11 * n.Y + orientationAInverse.M12 * n.Z);
supportDirectionA.Z = -(orientationAInverse.M20 * n.X + orientationAInverse.M21 * n.Y + orientationAInverse.M22 * n.Z);
Vector3 supportPointA = shapeA.GetSupportPoint(supportDirectionA, scaleA);
Vector3 v4A;
v4A.X = poseA.Orientation.M00 * supportPointA.X + poseA.Orientation.M01 * supportPointA.Y + poseA.Orientation.M02 * supportPointA.Z + poseA.Position.X;
v4A.Y = poseA.Orientation.M10 * supportPointA.X + poseA.Orientation.M11 * supportPointA.Y + poseA.Orientation.M12 * supportPointA.Z + poseA.Position.Y;
v4A.Z = poseA.Orientation.M20 * supportPointA.X + poseA.Orientation.M21 * supportPointA.Y + poseA.Orientation.M22 * supportPointA.Z + poseA.Position.Z;
Vector3 supportDirectionB;
supportDirectionB.X = orientationBInverse.M00 * n.X + orientationBInverse.M01 * n.Y + orientationBInverse.M02 * n.Z;
supportDirectionB.Y = orientationBInverse.M10 * n.X + orientationBInverse.M11 * n.Y + orientationBInverse.M12 * n.Z;
supportDirectionB.Z = orientationBInverse.M20 * n.X + orientationBInverse.M21 * n.Y + orientationBInverse.M22 * n.Z;
Vector3 supportPointB = shapeB.GetSupportPoint(supportDirectionB, scaleB);
Vector3 v4B;
v4B.X = poseB.Orientation.M00 * supportPointB.X + poseB.Orientation.M01 * supportPointB.Y + poseB.Orientation.M02 * supportPointB.Z + poseB.Position.X;
v4B.Y = poseB.Orientation.M10 * supportPointB.X + poseB.Orientation.M11 * supportPointB.Y + poseB.Orientation.M12 * supportPointB.Z + poseB.Position.Y;
v4B.Z = poseB.Orientation.M20 * supportPointB.X + poseB.Orientation.M21 * supportPointB.Y + poseB.Orientation.M22 * supportPointB.Z + poseB.Position.Z;
Vector3 v4 = v4B - v4A;
// Separating axis test:
if (!contactSet.HaveContact && // <--- New (see below).
v4.X * n.X + v4.Y * n.Y + v4.Z * n.Z < 0) // Optimized version of && Vector3.Dot(v4, n) < 0)
{
// Following assert can fail. For example if the above dot product returns -0.000000001
// for nearly perfectly touching objects. Therefore I have added the condition
// hit == false to the condition.
return(Vector3.Zero);
}
// Test if we have refined more than the collision epsilon.
// Condition 1: Project the point difference v4-v3 onto normal n and check whether we have
// improved in this direction.
// Condition 2: If n has not changed, then we couldn't improve anymore. This is caused
// by numerical problems, e.g. when a large object (>10000) is checked.
//if (Vector3.Dot(v4 - v3, n) <= CollisionDetection.Epsilon
// ----- Optimized version:
if ((v4.X - v3.X) * n.X + (v4.Y - v3.Y) * n.Y + (v4.Z - v3.Z) * n.Z <= CollisionDetection.Epsilon ||
Vector3.AreNumericallyEqual(n, oldN) ||
iterationCount >= iterationLimit)
{
// We have the final portal.
if (!contactSet.HaveContact)
{
return(Vector3.Zero);
}
if (type == CollisionQueryType.Boolean)
{
return(Vector3.Zero);
}
// Find the point closest to the origin.
float u, v, w;
GeometryHelper.GetClosestPoint(new Triangle(v1, v2, v3), Vector3.Zero, out u, out v, out w);
// Note: If u, v or w is 0 or 1, then the point was probably outside portal triangle.
// We can use the returned data, but re-running MPR will give us a better contact.
Vector3 closest = u * v1 + v * v2 + w * v3;
// The points on the objects have the same barycentric coordinates.
Vector3 pointOnA = u * v1A + v * v2A + w * v3A;
Vector3 pointOnB = u * v1B + v * v2B + w * v3B;
// Use difference between points as normal direction, only if it can be normalized.
Vector3 normal = pointOnA - pointOnB;
if (!normal.TryNormalize())
{
normal = -n; // Else use the inverted normal of the portal.
}
Vector3 position = (pointOnA + pointOnB) / 2;
float penetrationDepth = closest.Length;
Contact contact = ContactHelper.CreateContact(contactSet, position, normal, penetrationDepth, false);
ContactHelper.Merge(contactSet, contact, type, CollisionDetection.ContactPositionTolerance);
// If real closest point is outside the portal triangle, then one of u, v, w will
// be exactly 0 or 1. In this case we should run a new MPR with the portal ray n.
if (u == 0 || v == 0 || w == 0 || u == 1 || v == 1 || w == 1)
{
return(n);
}
return(Vector3.Zero);
}
// Now we have a new point v4 and have to make a new portal by eliminating v1, v2 or v3.
// The possible new tetrahedron faces are: (v0, v1, v4), (v0, v4, v2), (v0, v4, v3)
// We don't know the orientation yet.
// Test with the ORIENT3D test.
//Vector3 cross = Vector3.Cross(v4, v0);
// ----- Optimized version:
Vector3 cross;
cross.X = v4.Y * v0.Z - v4.Z * v0.Y;
cross.Y = v4.Z * v0.X - v4.X * v0.Z;
cross.Z = v4.X * v0.Y - v4.Y * v0.X;
//if (Vector3.Dot(v1, cross) > 0)
if (v1.X * cross.X + v1.Y * cross.Y + v1.Z * cross.Z > 0)
{
// Eliminate v3 or v1.
//if (Vector3.Dot(v2, cross) > 0)
if (v2.X * cross.X + v2.Y * cross.Y + v2.Z * cross.Z > 0)
{
v1 = v4;
v1A = v4A;
v1B = v4B;
}
else
{
v3 = v4;
v3A = v4A;
v3B = v4B;
}
}
else
{
// Eliminate v1 or v2.
//if (Vector3.Dot(v3, cross) > 0)
if (v3.X * cross.X + v3.Y * cross.Y + v3.Z * cross.Z > 0)
{
v2 = v4;
v2A = v4A;
v2B = v4B;
}
else
{
v1 = v4;
v1A = v4A;
v1B = v4B;
}
}
}
}
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)
{
// Object A should be the plane.
// Object B should be the other object.
IGeometricObject planeObject = contactSet.ObjectA.GeometricObject;
IGeometricObject convexObject = contactSet.ObjectB.GeometricObject;
// Swap objects if necessary.
bool swapped = (convexObject.Shape is PlaneShape);
if (swapped)
{
MathHelper.Swap(ref planeObject, ref convexObject);
}
PlaneShape planeShape = planeObject.Shape as PlaneShape;
ConvexShape convexShape = convexObject.Shape as ConvexShape;
// Check if shapes are correct.
if (planeShape == null || convexShape == null)
{
throw new ArgumentException("The contact set must contain a plane and a convex shape.", "contactSet");
}
// Get transformations.
Vector3F scalePlane = planeObject.Scale;
Vector3F scaleB = convexObject.Scale;
Pose planePose = planeObject.Pose;
Pose poseB = convexObject.Pose;
// Apply scale to plane and transform plane into world space.
Plane planeWorld = new Plane(planeShape);
planeWorld.Scale(ref scalePlane); // Scale plane.
planeWorld.ToWorld(ref planePose); // Transform plane to world space.
// Transform plane normal to local space of convex.
Vector3F planeNormalLocalB = poseB.ToLocalDirection(planeWorld.Normal);
// Get support vertex nearest to the plane.
Vector3F supportVertexBLocal = convexShape.GetSupportPoint(-planeNormalLocalB, scaleB);
// Transform support vertex into world space.
Vector3F supportVertexBWorld = poseB.ToWorldPosition(supportVertexBLocal);
// Project vertex onto separating axis (given by plane normal).
float distance = Vector3F.Dot(supportVertexBWorld, planeWorld.Normal);
// Check for collision.
float penetrationDepth = planeWorld.DistanceFromOrigin - distance;
contactSet.HaveContact = (penetrationDepth >= 0);
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;
}
// Position is between support vertex and plane.
Vector3F position = supportVertexBWorld + planeWorld.Normal * (penetrationDepth / 2);
Vector3F normal = (swapped) ? -planeWorld.Normal : planeWorld.Normal;
// Update contact set.
Contact contact = ContactHelper.CreateContact(contactSet, position, normal, penetrationDepth, false);
ContactHelper.Merge(contactSet, contact, type, CollisionDetection.ContactPositionTolerance);
if (CollisionDetection.FullContactSetPerFrame &&
type == CollisionQueryType.Contacts &&
contactSet.Count > 0 &&
contactSet.Count < 4)
{
// Special treatment for tetrahedra: Test all vertices against plane.
IList <Vector3F> vertices = null;
if (convexShape is ConvexHullOfPoints)
{
var convexHullOfPoints = (ConvexHullOfPoints)convexShape;
vertices = convexHullOfPoints.Points;
}
else if (convexShape is ConvexPolyhedron)
{
var convexPolyhedron = (ConvexPolyhedron)convexShape;
vertices = convexPolyhedron.Vertices;
}
if (vertices != null && vertices.Count <= 8)
{
// Convex has 8 or less vertices. Explicitly test all vertices against the plane.
int numberOfVertices = vertices.Count;
for (int i = 0; i < numberOfVertices; i++)
{
// Test is the same as above.
var vertex = vertices[i];
Vector3F scaledVertex = vertex * scaleB;
if (scaledVertex != supportVertexBLocal) // supportVertexBLocal has already been added.
{
Vector3F vertexWorld = poseB.ToWorldPosition(scaledVertex);
distance = Vector3F.Dot(vertexWorld, planeWorld.Normal);
penetrationDepth = planeWorld.DistanceFromOrigin - distance;
if (penetrationDepth >= 0)
{
position = vertexWorld + planeWorld.Normal * (penetrationDepth / 2);
normal = (swapped) ? -planeWorld.Normal : planeWorld.Normal;
contact = ContactHelper.CreateContact(contactSet, position, normal, penetrationDepth, false);
ContactHelper.Merge(contactSet, contact, type, CollisionDetection.ContactPositionTolerance);
}
}
}
}
else
{
// Convex is a complex shape with more than 4 vertices.
ContactHelper.TestWithPerturbations(
CollisionDetection,
contactSet,
!swapped, // Perturb the convex object, not the plane.
_computeContactsMethod);
}
}
}
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;
Vector3F scaleA = objectA.Scale;
Pose poseA = objectA.Pose;
IGeometricObject objectB = contactSet.ObjectB.GeometricObject;
ConvexShape shapeB = objectB.Shape as ConvexShape;
Vector3F 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.
Vector3F 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.
Vector3F vA = poseA.ToWorldPosition(shapeA.InnerPoint * scaleA);
Vector3F 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 = Vector3F.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.
Vector3F p = poseA.ToWorldPosition(shapeA.GetSupportPoint(orientationAInverse * -v, scaleA));
Vector3F q = poseB.ToWorldPosition(shapeB.GetSupportPoint(orientationBInverse * v, scaleB));
Vector3F w = p - q;
// w projected onto the separating axis.
float delta = Vector3F.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.
Vector3F 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.
Vector3F innerA = poseA.ToWorldPosition(shapeA.InnerPoint * scaleA);
normal = simplex.ClosestPointOnA - innerA;
if (!normal.TryNormalize())
{
Vector3F innerB = poseB.ToWorldPosition(shapeB.InnerPoint * scaleB);
normal = innerB - innerA;
if (!normal.TryNormalize())
{
normal = Vector3F.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...
Vector3F innerA = poseA.ToWorldPosition(shapeA.InnerPoint * scaleA);
if (Vector3F.Dot(simplex.ClosestPointOnA - innerA, normal) < 0)
{
normal = -normal;
}
}
}
}
Debug.Assert(normal.IsNumericallyZero == false);
if (type != CollisionQueryType.Boolean)
{
Vector3F 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();
}
}
private void ComputeCollisionFast(ContactSet contactSet, CollisionQueryType type,
IGeometricObject heightFieldGeometricObject, IGeometricObject convexGeometricObject,
HeightField heightField, ConvexShape convex, bool swapped)
{
Debug.Assert(type != CollisionQueryType.ClosestPoints);
// Assume no contact.
contactSet.HaveContact = false;
// Get scales and poses
Pose heightFieldPose = heightFieldGeometricObject.Pose;
Vector3F heightFieldScale = heightFieldGeometricObject.Scale;
if (heightFieldScale.X < 0 || heightFieldScale.Y < 0 || heightFieldScale.Z < 0)
{
throw new NotSupportedException("Computing collisions for height fields with a negative scaling is not supported.");
}
Pose convexPose = convexGeometricObject.Pose;
Vector3F convexScale = convexGeometricObject.Scale;
// Get a point in the convex. (Could also use center of AABB.)
var convexPoint = convexPose.ToWorldPosition(convex.InnerPoint * convexScale);
// Get height field coordinates.
convexPoint = heightFieldPose.ToLocalPosition(convexPoint);
float xUnscaled = convexPoint.X / heightFieldScale.X;
float zUnscaled = convexPoint.Z / heightFieldScale.Z;
// If convex point is outside height field, abort.
var originX = heightField.OriginX;
var originZ = heightField.OriginZ;
if (xUnscaled < originX || xUnscaled > originX + heightField.WidthX ||
zUnscaled < originZ || zUnscaled > originZ + heightField.WidthZ)
{
return;
}
// Get height and normal.
float height;
Vector3F normal;
int featureIndex;
GetHeight(heightField, xUnscaled, zUnscaled, out height, out normal, out featureIndex);
// Check for holes.
if (Numeric.IsNaN(height))
{
return;
}
// Apply scaling.
height *= heightFieldScale.Y;
// Normals are transformed with the inverse transposed matrix --> 1 / scale.
normal = normal / heightFieldScale;
normal.Normalize();
// ----- Now we test convex vs. plane.
// Convert normal to convex space.
normal = heightFieldPose.ToWorldDirection(normal);
var normalInConvex = convexPose.ToLocalDirection(normal);
// Convert plane point to convex space.
Vector3F planePoint = new Vector3F(convexPoint.X, height, convexPoint.Z);
planePoint = heightFieldPose.ToWorldPosition(planePoint);
planePoint = convexPose.ToLocalPosition(planePoint);
// Get convex support point in plane normal direction.
Vector3F supportPoint = convex.GetSupportPoint(-normalInConvex, convexScale);
// Get penetration depth.
float penetrationDepth = Vector3F.Dot((planePoint - supportPoint), normalInConvex);
// Abort if there is no contact.
if (penetrationDepth < 0)
{
return;
}
// Abort if object is too deep under the height field.
// This is important for height fields with holes/caves. Without this check
// no objects could enter the cave.
if (penetrationDepth > heightField.Depth)
{
return;
}
// We have contact.
contactSet.HaveContact = true;
// Return for boolean queries.
if (type == CollisionQueryType.Boolean)
{
return;
}
// Contact position is in the "middle of the penetration".
Vector3F position = convexPose.ToWorldPosition(supportPoint + normalInConvex * (penetrationDepth / 2));
if (swapped)
{
normal = -normal;
}
// Add contact
var contact = ContactHelper.CreateContact(contactSet, position, normal, penetrationDepth, false);
if (swapped)
{
contact.FeatureB = featureIndex;
}
else
{
contact.FeatureA = featureIndex;
}
ContactHelper.Merge(contactSet, contact, type, CollisionDetection.ContactPositionTolerance);
if (CollisionDetection.FullContactSetPerFrame &&
contactSet.Count < 3)
{
// Trying to create a full contact set.
// We use arbitrary orthonormal values to perturb the normal direction.
var ortho1 = normalInConvex.Orthonormal1;
var ortho2 = normalInConvex.Orthonormal2;
// Test 4 perturbed support directions.
for (int i = 0; i < 4; i++)
{
Vector3F direction;
switch (i)
{
case 0:
direction = -normalInConvex + ortho1;
break;
case 1:
direction = -normalInConvex - ortho1;
break;
case 2:
direction = -normalInConvex + ortho2;
break;
default:
direction = -normalInConvex - ortho2;
break;
}
// Support point vs. plane test as above:
supportPoint = convex.GetSupportPoint(direction, convexScale);
penetrationDepth = Vector3F.Dot((planePoint - supportPoint), normalInConvex);
if (penetrationDepth >= 0)
{
position = convexPose.ToWorldPosition(supportPoint + normalInConvex * (penetrationDepth / 2));
contact = ContactHelper.CreateContact(contactSet, position, normal, penetrationDepth, false);
ContactHelper.Merge(contactSet, contact, type, CollisionDetection.ContactPositionTolerance);
}
}
}
}
/// <summary>
/// Guesses the closest pair.
/// </summary>
/// <param name="contactSet">The contact set.</param>
/// <param name="swapped">
/// Object A in <paramref name="contactSet"/> should be the height field. This parameter
/// indicates whether object A and object B in the contact set are swapped.
/// </param>
/// <param name="isOverHole">
/// <see langword="true"/> if the guessed contact is over a hole and probably shouldn't be used.
/// </param>
/// <returns>Guess for closest pair.</returns>
/// <remarks>
/// For general shapes: Inner point of B to height field point "under" inner point of B.
/// For convex shapes: Support point of B in the "down" direction to height field point "under"
/// this support point.
/// </remarks>
private static Contact GuessClosestPair(ContactSet contactSet, bool swapped, out bool isOverHole)
{
// Object A should be the height field.
IGeometricObject objectA = contactSet.ObjectA.GeometricObject;
IGeometricObject objectB = contactSet.ObjectB.GeometricObject;
// Swap if necessary.
if (swapped)
{
MathHelper.Swap(ref objectA, ref objectB);
}
HeightField heightFieldA = (HeightField)objectA.Shape;
Shape shapeB = objectB.Shape;
Vector3 scaleA = objectA.Scale;
Vector3 scaleB = objectB.Scale;
Pose poseA = objectA.Pose;
Pose poseB = objectB.Pose;
// Get the height field up-axis in world space.
Vector3 heightFieldUpAxis = poseA.ToWorldDirection(Vector3.UnitY);
// Get point on other object.
Vector3 positionBLocal;
ConvexShape shapeBAsConvex = shapeB as ConvexShape;
if (shapeBAsConvex != null)
{
// Use support point for convex shapes.
positionBLocal = shapeBAsConvex.GetSupportPoint(poseB.ToLocalDirection(-heightFieldUpAxis), scaleB);
}
else
{
// Use inner point for general shapes.
positionBLocal = shapeB.InnerPoint * scaleB;
}
// Convert point (object B) into height field space (object A).
Vector3 positionB = poseB.ToWorldPosition(positionBLocal);
Vector3 positionBInA = poseA.ToLocalPosition(positionB);
// Get point on the surface of the height field (object A):
// Clamp x and z coordinate to height field widths.
// For y coordinate get the height of the height field at the x-z position.
float originX = heightFieldA.OriginX;
float originZ = heightFieldA.OriginZ;
float x = MathHelper.Clamp(positionBInA.X, originX * scaleA.X, (originX + heightFieldA.WidthX) * scaleA.X);
float z = MathHelper.Clamp(positionBInA.Z, originZ * scaleA.Z, (originZ + heightFieldA.WidthZ) * scaleA.Z);
float y = heightFieldA.GetHeight(x / scaleA.X, z / scaleA.Z) * scaleA.Y; // Inverse scale applied in GetHeight() parameters.
Vector3 positionALocal = new Vector3(x, y, z);
// Special handling of holes.
isOverHole = Numeric.IsNaN(y);
if (isOverHole)
{
positionALocal = heightFieldA.InnerPoint * scaleA;
}
// Convert point on height field to world space.
Vector3 positionA = poseA.ToWorldPosition(positionALocal);
// Use the world positions (positionA, positionB) as our closest-pair/contact guess.
// Compute contact information.
Vector3 position = (positionA + positionB) / 2;
float penetrationDepth = (positionA - positionB).Length;
bool haveContact = (positionALocal.Y >= positionBInA.Y);
Vector3 normal = positionA - positionB;
if (!normal.TryNormalize())
{
normal = heightFieldUpAxis;
}
if (swapped)
{
normal = -normal;
}
bool isRayHit = haveContact && shapeB is RayShape;
if (!haveContact)
{
// For separation: Switch the normal and make the penetration depth negative to indicate
// separation.
normal = -normal;
penetrationDepth = -penetrationDepth;
}
return(ContactHelper.CreateContact(contactSet, position, normal, penetrationDepth, isRayHit));
}
// 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);
}
internal static IList <Vector3F> SampleConvexShape(ConvexShape shape, float distanceThreshold, int iterationLimit)
{
if (shape == null)
{
throw new ArgumentNullException("shape");
}
if (distanceThreshold < 0)
{
throw new ArgumentOutOfRangeException("distanceThreshold", "distanceThreshold must be greater than or equal to 0.");
}
if (iterationLimit < 1)
{
throw new ArgumentOutOfRangeException("iterationLimit", "iterationLimit must be greater than or equal to 1.");
}
distanceThreshold = Math.Max(distanceThreshold, Numeric.EpsilonF);
PointCollector points = new PointCollector {
DistanceThreshold = distanceThreshold
};
// Start with 6 sensors and 8 sensor triangles (like a double pyramid).
List <Sensor> sensors = new List <Sensor>
{
new Sensor(new Vector3F(-1, 0, 0)),
new Sensor(new Vector3F(1, 0, 0)),
new Sensor(new Vector3F(0, -1, 0)),
new Sensor(new Vector3F(0, 1, 0)),
new Sensor(new Vector3F(0, 0, -1)),
new Sensor(new Vector3F(0, 0, 1))
};
List <SensorTriangle> triangles = new List <SensorTriangle>
{
new SensorTriangle(5, 1, 3, 1), // SensorTriangle.Iteration is set to 1.
new SensorTriangle(5, 2, 1, 1),
new SensorTriangle(1, 2, 4, 1),
new SensorTriangle(4, 2, 0, 1),
new SensorTriangle(0, 2, 5, 1),
new SensorTriangle(1, 4, 3, 1),
new SensorTriangle(3, 4, 0, 1),
new SensorTriangle(0, 5, 3, 1)
};
// Sample the first 6 sensors.
int numberOfSensors = sensors.Count;
for (int i = 0; i < numberOfSensors; i++)
{
Sensor sensor = sensors[i];
Vector3F sample = shape.GetSupportPoint(sensor.Direction);
int index = points.GetIndex(sample);
if (index == -1)
{
// Add new point.
index = points.Points.Count;
points.Points.Add(sample);
}
sensor.IndexOfPoint = index;
}
// Collector for new points.
PointCollector newPoints = new PointCollector();
bool finished = false;
while (!finished)
{
// Assume that we cannot refine. If we can refine, finished must be set to false.
finished = true;
// Reset collector for new points.
newPoints.Points.Clear();
int numberOfPoints = points.Points.Count;
int numberOfTriangles = triangles.Count;
for (int currentTriangle = 0; currentTriangle < numberOfTriangles; currentTriangle++)
{
// In each iteration refine the existing triangles by breaking up edges.
SensorTriangle triangle = triangles[currentTriangle];
int iteration = triangle.Iteration;
if (iteration >= 0 && iteration < iterationLimit)
{
// Sensor indices.
int indexOfSensor0 = triangle.IndexOfSensor0;
int indexOfSensor1 = triangle.IndexOfSensor1;
int indexOfSensor2 = triangle.IndexOfSensor2;
// Sensors.
Sensor sensor0 = sensors[indexOfSensor0];
Sensor sensor1 = sensors[indexOfSensor1];
Sensor sensor2 = sensors[indexOfSensor2];
// New sensors.
Sensor sensor01 = new Sensor((sensor0.Direction + sensor1.Direction).Normalized);
Sensor sensor12 = new Sensor((sensor1.Direction + sensor2.Direction).Normalized);
Sensor sensor20 = new Sensor((sensor2.Direction + sensor0.Direction).Normalized);
// New Support Points.
Vector3F v01 = shape.GetSupportPoint(sensor01.Direction);
Vector3F v12 = shape.GetSupportPoint(sensor12.Direction);
Vector3F v20 = shape.GetSupportPoint(sensor20.Direction);
// See if the points are new.
sensor01.IndexOfPoint = points.GetIndex(v01);
sensor12.IndexOfPoint = points.GetIndex(v12);
sensor20.IndexOfPoint = points.GetIndex(v20);
// Check if at least one point is new, and if we have improved more than the distance threshold.
// (The PointCollector.GetIndex() automatically takes care of the distance threshold.)
if (sensor01.IndexOfPoint == -1 || sensor12.IndexOfPoint == -1 || sensor20.IndexOfPoint == -1)
{
// We refine the sensor triangle into 4 triangles.
finished = false;
// Indices of the new sensors.
int indexOfSensor01 = sensors.Count;
int indexOfSensor12 = indexOfSensor01 + 1;
int indexOfSensor20 = indexOfSensor12 + 1;
// Add sensors.
sensors.Add(sensor01);
sensors.Add(sensor12);
sensors.Add(sensor20);
// Use the instance of the old triangle for the first new triangle.
triangle.IndexOfSensor0 = indexOfSensor01;
triangle.IndexOfSensor1 = indexOfSensor12;
triangle.IndexOfSensor2 = indexOfSensor20;
iteration++;
triangle.Iteration = iteration;
// Add other triangles, but only if they contain a new point.
if (sensor01.IndexOfPoint == -1 || sensor12.IndexOfPoint == -1)
{
triangles.Add(new SensorTriangle(indexOfSensor01, indexOfSensor1, indexOfSensor12, iteration));
}
if (sensor12.IndexOfPoint == -1 || sensor20.IndexOfPoint == -1)
{
triangles.Add(new SensorTriangle(indexOfSensor12, indexOfSensor2, indexOfSensor20, iteration));
}
if (sensor01.IndexOfPoint == -1 || sensor20.IndexOfPoint == -1)
{
triangles.Add(new SensorTriangle(indexOfSensor0, indexOfSensor01, indexOfSensor20, iteration));
}
// Add new points
if (sensor01.IndexOfPoint == -1)
{
int newPointsIndex = newPoints.GetIndex(v01);
if (newPointsIndex == -1)
{
newPointsIndex = newPoints.Points.Count;
newPoints.Points.Add(v01);
}
sensor01.IndexOfPoint = numberOfPoints + newPointsIndex;
}
if (sensor12.IndexOfPoint == -1)
{
int newPointsIndex = newPoints.GetIndex(v12);
if (newPointsIndex == -1)
{
newPointsIndex = newPoints.Points.Count;
newPoints.Points.Add(v12);
}
sensor12.IndexOfPoint = numberOfPoints + newPointsIndex;
}
if (sensor20.IndexOfPoint == -1)
{
int newPointsIndex = newPoints.GetIndex(v20);
if (newPointsIndex == -1)
{
newPointsIndex = newPoints.Points.Count;
newPoints.Points.Add(v20);
}
sensor20.IndexOfPoint = numberOfPoints + newPointsIndex;
}
}
else
{
// Fine enough.
triangle.Iteration = -1;
}
}
}
// Copy new points to point collector.
int numberOfNewPoints = newPoints.Points.Count;
for (int i = 0; i < numberOfNewPoints; i++)
{
points.Points.Add(newPoints.Points[i]);
}
}
return(points.Points);
}
