/// <summary>
/// Initializes a new instance of the <see cref="RayTriangleAlgorithm"/> class.
/// </summary>
/// <param name="collisionDetection">The collision detection service.</param>
public RayTriangleAlgorithm(CollisionDetection collisionDetection)
: base(collisionDetection)
{
_gjk = new Gjk(collisionDetection);
}
private Vector3F DoMpr(CollisionQueryType type, ContactSet contactSet, Vector3F v0)
{
int iterationCount = 0;
const int iterationLimit = 100;
CollisionObject collisionObjectA = contactSet.ObjectA;
IGeometricObject geometricObjectA = collisionObjectA.GeometricObject;
ConvexShape shapeA = (ConvexShape)geometricObjectA.Shape;
Vector3F scaleA = geometricObjectA.Scale;
Pose poseA = geometricObjectA.Pose;
CollisionObject collisionObjectB = contactSet.ObjectB;
IGeometricObject geometricObjectB = collisionObjectB.GeometricObject;
ConvexShape shapeB = (ConvexShape)geometricObjectB.Shape;
Vector3F scaleB = geometricObjectB.Scale;
Pose poseB = geometricObjectB.Pose;
// Cache inverted rotations.
var orientationAInverse = poseA.Orientation.Transposed;
var orientationBInverse = poseB.Orientation.Transposed;
Vector3F n = -v0; // Shoot from v0 to the origin.
Vector3F v1A = poseA.ToWorldPosition(shapeA.GetSupportPoint(orientationAInverse * -n, scaleA));
Vector3F v1B = poseB.ToWorldPosition(shapeB.GetSupportPoint(orientationBInverse * n, scaleB));
Vector3F v1 = v1B - v1A;
// Separating axis test:
if (Vector3F.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(Vector3F.Zero);
}
// Second support direction = perpendicular to plane of origin, v0 and v1.
n = Vector3F.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(Vector3F.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.)
Vector3F 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 = Vector3F.UnitY;
}
Vector3F 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(Vector3F.Zero);
}
Vector3F v2A = poseA.ToWorldPosition(shapeA.GetSupportPoint(orientationAInverse * -n, scaleA));
Vector3F v2B = poseB.ToWorldPosition(shapeB.GetSupportPoint(orientationBInverse * n, scaleB));
Vector3F v2 = v2B - v2A;
// Separating axis test:
if (Vector3F.Dot(v2, n) < 0)
{
return(Vector3F.Zero);
}
// Third support direction = perpendicular to plane of v0, v1 and v2.
n = Vector3F.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 (Vector3F.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(Vector3F.Zero);
}
// Search for a valid portal.
Vector3F v3, v3A, v3B;
while (true)
{
iterationCount++;
// Abort if we cannot find a valid portal.
if (iterationCount > iterationLimit)
{
return(Vector3F.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:
Vector3F 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);
Vector3F 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;
Vector3F 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;
Vector3F 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 (Vector3F.Dot(v3, n) < 0)
if (v3.X * n.X + v3.Y * n.Y + v3.Z * n.Z < 0)
{
return(Vector3F.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 (Vector3F.Dot(Vector3F.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 = Vector3F.Cross(v1 - v0, v3 - v0);
// ----- Optimized version:
Vector3F v1MinusV0;
v1MinusV0.X = v1.X - v0.X;
v1MinusV0.Y = v1.Y - v0.Y;
v1MinusV0.Z = v1.Z - v0.Z;
Vector3F 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 (Vector3F.Dot(Vector3F.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 = Vector3F.Cross(v3 - v0, v2 - v0);
// ----- Optimized version:
Vector3F v3MinusV0;
v3MinusV0.X = v3.X - v0.X;
v3MinusV0.Y = v3.Y - v0.Y;
v3MinusV0.Z = v3.Z - v0.Z;
Vector3F 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.
Vector3F oldN = n;
// Compute outward pointing normal of the portal
//n = Vector3F.Cross(v2 - v1, v3 - v1);
Vector3F v2MinusV1;
v2MinusV1.X = v2.X - v1.X;
v2MinusV1.Y = v2.Y - v1.Y;
v2MinusV1.Z = v2.Z - v1.Z;
Vector3F 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(Vector3F.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), Vector3F.Zero, out u, out v, out w);
Vector3F 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(Vector3F.Zero);
}
// The points on the objects have the same barycentric coordinates.
Vector3F pointOnA = u * v1A + v * v2A + w * v3A;
Vector3F pointOnB = u * v1B + v * v2B + w * v3B;
Vector3F normal = pointOnA - pointOnB;
if (!normal.TryNormalize())
{
if (contactSet.IsPreferredNormalAvailable)
{
normal = contactSet.PreferredNormal;
}
else
{
normal = Vector3F.UnitY;
}
}
Vector3F 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(Vector3F.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 && Vector3F.Dot(v1, n) > 0)
{
// Portal points aways from origin --> Origin is in the tetrahedron.
contactSet.HaveContact = true;
if (type == CollisionQueryType.Boolean)
{
return(Vector3F.Zero);
}
}
// Find new support point.
//Vector3F v4A = poseA.ToWorldPosition(shapeA.GetSupportPoint(orientationAInverse * -n, scaleA));
//Vector3F v4B = poseB.ToWorldPosition(shapeB.GetSupportPoint(orientationBInverse * n, scaleB));
//Vector3F v4 = v4B - v4A;
// ----- Optimized version:
Vector3F 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);
Vector3F supportPointA = shapeA.GetSupportPoint(supportDirectionA, scaleA);
Vector3F 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;
Vector3F 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;
Vector3F supportPointB = shapeB.GetSupportPoint(supportDirectionB, scaleB);
Vector3F 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;
Vector3F 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 && Vector3F.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(Vector3F.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 (Vector3F.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 ||
Vector3F.AreNumericallyEqual(n, oldN) ||
iterationCount >= iterationLimit)
{
// We have the final portal.
if (!contactSet.HaveContact)
{
return(Vector3F.Zero);
}
if (type == CollisionQueryType.Boolean)
{
return(Vector3F.Zero);
}
// Find the point closest to the origin.
float u, v, w;
GeometryHelper.GetClosestPoint(new Triangle(v1, v2, v3), Vector3F.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.
Vector3F closest = u * v1 + v * v2 + w * v3;
// The points on the objects have the same barycentric coordinates.
Vector3F pointOnA = u * v1A + v * v2A + w * v3A;
Vector3F pointOnB = u * v1B + v * v2B + w * v3B;
// Use difference between points as normal direction, only if it can be normalized.
Vector3F normal = pointOnA - pointOnB;
if (!normal.TryNormalize())
{
normal = -n; // Else use the inverted normal of the portal.
}
Vector3F 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(Vector3F.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.
//Vector3F cross = Vector3F.Cross(v4, v0);
// ----- Optimized version:
Vector3F 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 (Vector3F.Dot(v1, cross) > 0)
if (v1.X * cross.X + v1.Y * cross.Y + v1.Z * cross.Z > 0)
{
// Eliminate v3 or v1.
//if (Vector3F.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 (Vector3F.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 CollisionAlgorithmMatrix(CollisionDetection collisionDetection)
{
// Initialize with dummy collision algorithms.
var noAlgo = new NoCollisionAlgorithm(collisionDetection); // Definitely no collision wanted.
var infiniteAlgo = new InfiniteShapeAlgorithm(collisionDetection); // Returns always a collision.
// Build default configuration:
var gjk = new Gjk(collisionDetection);
var gjkBoxAlgorithm = new CombinedCollisionAlgorithm(collisionDetection, gjk, new BoxBoxAlgorithm(collisionDetection));
var gjkMprAlgorithm = new CombinedCollisionAlgorithm(collisionDetection, gjk, new MinkowskiPortalRefinement(collisionDetection));
var gjkTriTriAlgorithm = new CombinedCollisionAlgorithm(collisionDetection, gjk, new TriangleTriangleAlgorithm(collisionDetection));
BoxSphereAlgorithm boxSphereAlgorithm = new BoxSphereAlgorithm(collisionDetection);
CompositeShapeAlgorithm compositeAlgorithm = new CompositeShapeAlgorithm(collisionDetection);
HeightFieldAlgorithm heightFieldAlgorithm = new HeightFieldAlgorithm(collisionDetection);
LineAlgorithm lineAlgorithm = new LineAlgorithm(collisionDetection);
PlaneBoxAlgorithm planeBoxAlgorithm = new PlaneBoxAlgorithm(collisionDetection);
PlaneConvexAlgorithm planeConvexAlgorithm = new PlaneConvexAlgorithm(collisionDetection);
PlaneRayAlgorithm planeRayAlgorithm = new PlaneRayAlgorithm(collisionDetection);
PlaneSphereAlgorithm planeSphereAlgorithm = new PlaneSphereAlgorithm(collisionDetection);
RayBoxAlgorithm rayBoxAlgorithm = new RayBoxAlgorithm(collisionDetection);
RayConvexAlgorithm rayConvexAlgorithm = new RayConvexAlgorithm(collisionDetection);
RaySphereAlgorithm raySphereAlgorithm = new RaySphereAlgorithm(collisionDetection);
RayTriangleAlgorithm rayTriangleAlgorithm = new RayTriangleAlgorithm(collisionDetection);
SphereSphereAlgorithm sphereSphereAlgorithm = new SphereSphereAlgorithm(collisionDetection);
TransformedShapeAlgorithm transformedShapeAlgorithm = new TransformedShapeAlgorithm(collisionDetection);
TriangleMeshAlgorithm triangleMeshAlgorithm = new TriangleMeshAlgorithm(collisionDetection);
RayCompositeAlgorithm rayCompositeAlgorithm = new RayCompositeAlgorithm(collisionDetection);
RayTriangleMeshAlgorithm rayTriangleMeshAlgorithm = new RayTriangleMeshAlgorithm(collisionDetection);
RayHeightFieldAlgorithm rayHeightFieldAlgorithm = new RayHeightFieldAlgorithm(collisionDetection);
this[typeof(PointShape), typeof(PointShape)] = gjkMprAlgorithm;
this[typeof(PointShape), typeof(LineShape)] = lineAlgorithm;
this[typeof(PointShape), typeof(RayShape)] = rayConvexAlgorithm;
this[typeof(PointShape), typeof(LineSegmentShape)] = gjkMprAlgorithm;
this[typeof(PointShape), typeof(TriangleShape)] = gjkMprAlgorithm;
this[typeof(PointShape), typeof(RectangleShape)] = gjkMprAlgorithm;
this[typeof(PointShape), typeof(BoxShape)] = gjkMprAlgorithm;
this[typeof(PointShape), typeof(ConvexShape)] = gjkMprAlgorithm;
this[typeof(PointShape), typeof(ScaledConvexShape)] = gjkMprAlgorithm;
this[typeof(PointShape), typeof(CircleShape)] = gjkMprAlgorithm;
this[typeof(PointShape), typeof(SphereShape)] = gjkMprAlgorithm;
this[typeof(PointShape), typeof(CapsuleShape)] = gjkMprAlgorithm;
this[typeof(PointShape), typeof(ConeShape)] = gjkMprAlgorithm;
this[typeof(PointShape), typeof(CylinderShape)] = gjkMprAlgorithm;
this[typeof(PointShape), typeof(EmptyShape)] = noAlgo;
this[typeof(PointShape), typeof(InfiniteShape)] = infiniteAlgo;
this[typeof(PointShape), typeof(PlaneShape)] = planeConvexAlgorithm;
this[typeof(PointShape), typeof(HeightField)] = heightFieldAlgorithm;
this[typeof(PointShape), typeof(TriangleMeshShape)] = triangleMeshAlgorithm;
this[typeof(PointShape), typeof(TransformedShape)] = transformedShapeAlgorithm;
this[typeof(PointShape), typeof(CompositeShape)] = compositeAlgorithm;
this[typeof(LineShape), typeof(LineShape)] = lineAlgorithm;
this[typeof(LineShape), typeof(RayShape)] = lineAlgorithm;
this[typeof(LineShape), typeof(LineSegmentShape)] = lineAlgorithm;
this[typeof(LineShape), typeof(TriangleShape)] = lineAlgorithm;
this[typeof(LineShape), typeof(RectangleShape)] = lineAlgorithm;
this[typeof(LineShape), typeof(BoxShape)] = lineAlgorithm;
this[typeof(LineShape), typeof(ConvexShape)] = lineAlgorithm;
this[typeof(LineShape), typeof(ScaledConvexShape)] = lineAlgorithm;
this[typeof(LineShape), typeof(CircleShape)] = lineAlgorithm;
this[typeof(LineShape), typeof(SphereShape)] = lineAlgorithm;
this[typeof(LineShape), typeof(CapsuleShape)] = lineAlgorithm;
this[typeof(LineShape), typeof(ConeShape)] = lineAlgorithm;
this[typeof(LineShape), typeof(CylinderShape)] = lineAlgorithm;
this[typeof(LineShape), typeof(EmptyShape)] = noAlgo;
this[typeof(LineShape), typeof(InfiniteShape)] = infiniteAlgo;
this[typeof(LineShape), typeof(PlaneShape)] = noAlgo;
this[typeof(LineShape), typeof(HeightField)] = noAlgo;
this[typeof(LineShape), typeof(TriangleMeshShape)] = lineAlgorithm;
this[typeof(LineShape), typeof(TransformedShape)] = lineAlgorithm;
this[typeof(LineShape), typeof(CompositeShape)] = lineAlgorithm;
this[typeof(RayShape), typeof(RayShape)] = noAlgo;
this[typeof(RayShape), typeof(LineSegmentShape)] = rayConvexAlgorithm;
this[typeof(RayShape), typeof(TriangleShape)] = rayTriangleAlgorithm;
this[typeof(RayShape), typeof(RectangleShape)] = rayConvexAlgorithm;
this[typeof(RayShape), typeof(BoxShape)] = rayBoxAlgorithm;
this[typeof(RayShape), typeof(ConvexShape)] = rayConvexAlgorithm;
this[typeof(RayShape), typeof(ScaledConvexShape)] = rayConvexAlgorithm;
this[typeof(RayShape), typeof(CircleShape)] = rayConvexAlgorithm;
this[typeof(RayShape), typeof(SphereShape)] = raySphereAlgorithm;
this[typeof(RayShape), typeof(CapsuleShape)] = rayConvexAlgorithm;
this[typeof(RayShape), typeof(ConeShape)] = rayConvexAlgorithm;
this[typeof(RayShape), typeof(CylinderShape)] = rayConvexAlgorithm;
this[typeof(RayShape), typeof(EmptyShape)] = noAlgo;
this[typeof(RayShape), typeof(InfiniteShape)] = infiniteAlgo;
this[typeof(RayShape), typeof(PlaneShape)] = planeRayAlgorithm;
this[typeof(RayShape), typeof(HeightField)] = rayHeightFieldAlgorithm;
this[typeof(RayShape), typeof(TriangleMeshShape)] = rayTriangleMeshAlgorithm;
this[typeof(RayShape), typeof(TransformedShape)] = transformedShapeAlgorithm;
this[typeof(RayShape), typeof(CompositeShape)] = rayCompositeAlgorithm;
this[typeof(LineSegmentShape), typeof(LineSegmentShape)] = gjkMprAlgorithm;
this[typeof(LineSegmentShape), typeof(TriangleShape)] = gjkMprAlgorithm;
this[typeof(LineSegmentShape), typeof(RectangleShape)] = gjkMprAlgorithm;
this[typeof(LineSegmentShape), typeof(BoxShape)] = gjkMprAlgorithm;
this[typeof(LineSegmentShape), typeof(ConvexShape)] = gjkMprAlgorithm;
this[typeof(LineSegmentShape), typeof(ScaledConvexShape)] = gjkMprAlgorithm;
this[typeof(LineSegmentShape), typeof(CircleShape)] = gjkMprAlgorithm;
this[typeof(LineSegmentShape), typeof(SphereShape)] = gjkMprAlgorithm;
this[typeof(LineSegmentShape), typeof(CapsuleShape)] = gjkMprAlgorithm;
this[typeof(LineSegmentShape), typeof(ConeShape)] = gjkMprAlgorithm;
this[typeof(LineSegmentShape), typeof(CylinderShape)] = gjkMprAlgorithm;
this[typeof(LineSegmentShape), typeof(EmptyShape)] = noAlgo;
this[typeof(LineSegmentShape), typeof(InfiniteShape)] = infiniteAlgo;
this[typeof(LineSegmentShape), typeof(PlaneShape)] = planeConvexAlgorithm;
this[typeof(LineSegmentShape), typeof(HeightField)] = heightFieldAlgorithm;
this[typeof(LineSegmentShape), typeof(TriangleMeshShape)] = triangleMeshAlgorithm;
this[typeof(LineSegmentShape), typeof(TransformedShape)] = transformedShapeAlgorithm;
this[typeof(LineSegmentShape), typeof(CompositeShape)] = compositeAlgorithm;
this[typeof(TriangleShape), typeof(TriangleShape)] = gjkTriTriAlgorithm;
this[typeof(TriangleShape), typeof(RectangleShape)] = gjkMprAlgorithm;
this[typeof(TriangleShape), typeof(BoxShape)] = gjkMprAlgorithm;
this[typeof(TriangleShape), typeof(ConvexShape)] = gjkMprAlgorithm;
this[typeof(TriangleShape), typeof(ScaledConvexShape)] = gjkMprAlgorithm;
this[typeof(TriangleShape), typeof(CircleShape)] = gjkMprAlgorithm;
this[typeof(TriangleShape), typeof(SphereShape)] = gjkMprAlgorithm;
this[typeof(TriangleShape), typeof(CapsuleShape)] = gjkMprAlgorithm;
this[typeof(TriangleShape), typeof(ConeShape)] = gjkMprAlgorithm;
this[typeof(TriangleShape), typeof(CylinderShape)] = gjkMprAlgorithm;
this[typeof(TriangleShape), typeof(EmptyShape)] = noAlgo;
this[typeof(TriangleShape), typeof(InfiniteShape)] = infiniteAlgo;
this[typeof(TriangleShape), typeof(PlaneShape)] = planeConvexAlgorithm;
this[typeof(TriangleShape), typeof(HeightField)] = heightFieldAlgorithm;
this[typeof(TriangleShape), typeof(TriangleMeshShape)] = triangleMeshAlgorithm;
this[typeof(TriangleShape), typeof(TransformedShape)] = transformedShapeAlgorithm;
this[typeof(TriangleShape), typeof(CompositeShape)] = compositeAlgorithm;
this[typeof(RectangleShape), typeof(RectangleShape)] = gjkMprAlgorithm;
this[typeof(RectangleShape), typeof(BoxShape)] = gjkMprAlgorithm;
this[typeof(RectangleShape), typeof(ConvexShape)] = gjkMprAlgorithm;
this[typeof(RectangleShape), typeof(ScaledConvexShape)] = gjkMprAlgorithm;
this[typeof(RectangleShape), typeof(CircleShape)] = gjkMprAlgorithm;
this[typeof(RectangleShape), typeof(SphereShape)] = gjkMprAlgorithm;
this[typeof(RectangleShape), typeof(CapsuleShape)] = gjkMprAlgorithm;
this[typeof(RectangleShape), typeof(ConeShape)] = gjkMprAlgorithm;
this[typeof(RectangleShape), typeof(CylinderShape)] = gjkMprAlgorithm;
this[typeof(RectangleShape), typeof(EmptyShape)] = noAlgo;
this[typeof(RectangleShape), typeof(InfiniteShape)] = infiniteAlgo;
this[typeof(RectangleShape), typeof(PlaneShape)] = planeConvexAlgorithm;
this[typeof(RectangleShape), typeof(HeightField)] = heightFieldAlgorithm;
this[typeof(RectangleShape), typeof(TriangleMeshShape)] = triangleMeshAlgorithm;
this[typeof(RectangleShape), typeof(TransformedShape)] = transformedShapeAlgorithm;
this[typeof(RectangleShape), typeof(CompositeShape)] = compositeAlgorithm;
this[typeof(BoxShape), typeof(BoxShape)] = gjkBoxAlgorithm;
this[typeof(BoxShape), typeof(ConvexShape)] = gjkMprAlgorithm;
this[typeof(BoxShape), typeof(ScaledConvexShape)] = gjkMprAlgorithm;
this[typeof(BoxShape), typeof(CircleShape)] = gjkMprAlgorithm;
this[typeof(BoxShape), typeof(SphereShape)] = boxSphereAlgorithm;
this[typeof(BoxShape), typeof(CapsuleShape)] = gjkMprAlgorithm;
this[typeof(BoxShape), typeof(ConeShape)] = gjkMprAlgorithm;
this[typeof(BoxShape), typeof(CylinderShape)] = gjkMprAlgorithm;
this[typeof(BoxShape), typeof(EmptyShape)] = noAlgo;
this[typeof(BoxShape), typeof(InfiniteShape)] = infiniteAlgo;
this[typeof(BoxShape), typeof(PlaneShape)] = planeBoxAlgorithm;
this[typeof(BoxShape), typeof(HeightField)] = heightFieldAlgorithm;
this[typeof(BoxShape), typeof(TriangleMeshShape)] = triangleMeshAlgorithm;
this[typeof(BoxShape), typeof(TransformedShape)] = transformedShapeAlgorithm;
this[typeof(BoxShape), typeof(CompositeShape)] = compositeAlgorithm;
this[typeof(ConvexShape), typeof(ConvexShape)] = gjkMprAlgorithm;
this[typeof(ConvexShape), typeof(ScaledConvexShape)] = gjkMprAlgorithm;
this[typeof(ConvexShape), typeof(CircleShape)] = gjkMprAlgorithm;
this[typeof(ConvexShape), typeof(SphereShape)] = gjkMprAlgorithm;
this[typeof(ConvexShape), typeof(CapsuleShape)] = gjkMprAlgorithm;
this[typeof(ConvexShape), typeof(ConeShape)] = gjkMprAlgorithm;
this[typeof(ConvexShape), typeof(CylinderShape)] = gjkMprAlgorithm;
this[typeof(ConvexShape), typeof(EmptyShape)] = noAlgo;
this[typeof(ConvexShape), typeof(InfiniteShape)] = infiniteAlgo;
this[typeof(ConvexShape), typeof(PlaneShape)] = planeConvexAlgorithm;
this[typeof(ConvexShape), typeof(HeightField)] = heightFieldAlgorithm;
this[typeof(ConvexShape), typeof(TriangleMeshShape)] = triangleMeshAlgorithm;
this[typeof(ConvexShape), typeof(TransformedShape)] = transformedShapeAlgorithm;
this[typeof(ConvexShape), typeof(CompositeShape)] = compositeAlgorithm;
this[typeof(ScaledConvexShape), typeof(ScaledConvexShape)] = gjkMprAlgorithm;
this[typeof(ScaledConvexShape), typeof(CircleShape)] = gjkMprAlgorithm;
this[typeof(ScaledConvexShape), typeof(SphereShape)] = gjkMprAlgorithm;
this[typeof(ScaledConvexShape), typeof(CapsuleShape)] = gjkMprAlgorithm;
this[typeof(ScaledConvexShape), typeof(ConeShape)] = gjkMprAlgorithm;
this[typeof(ScaledConvexShape), typeof(CylinderShape)] = gjkMprAlgorithm;
this[typeof(ScaledConvexShape), typeof(EmptyShape)] = noAlgo;
this[typeof(ScaledConvexShape), typeof(InfiniteShape)] = infiniteAlgo;
this[typeof(ScaledConvexShape), typeof(PlaneShape)] = planeConvexAlgorithm;
this[typeof(ScaledConvexShape), typeof(HeightField)] = heightFieldAlgorithm;
this[typeof(ScaledConvexShape), typeof(TriangleMeshShape)] = triangleMeshAlgorithm;
this[typeof(ScaledConvexShape), typeof(TransformedShape)] = transformedShapeAlgorithm;
this[typeof(ScaledConvexShape), typeof(CompositeShape)] = compositeAlgorithm;
this[typeof(CircleShape), typeof(CircleShape)] = gjkMprAlgorithm;
this[typeof(CircleShape), typeof(SphereShape)] = gjkMprAlgorithm;
this[typeof(CircleShape), typeof(CapsuleShape)] = gjkMprAlgorithm;
this[typeof(CircleShape), typeof(ConeShape)] = gjkMprAlgorithm;
this[typeof(CircleShape), typeof(CylinderShape)] = gjkMprAlgorithm;
this[typeof(CircleShape), typeof(EmptyShape)] = noAlgo;
this[typeof(CircleShape), typeof(InfiniteShape)] = infiniteAlgo;
this[typeof(CircleShape), typeof(PlaneShape)] = planeConvexAlgorithm;
this[typeof(CircleShape), typeof(HeightField)] = heightFieldAlgorithm;
this[typeof(CircleShape), typeof(TriangleMeshShape)] = triangleMeshAlgorithm;
this[typeof(CircleShape), typeof(TransformedShape)] = transformedShapeAlgorithm;
this[typeof(CircleShape), typeof(CompositeShape)] = compositeAlgorithm;
this[typeof(SphereShape), typeof(SphereShape)] = sphereSphereAlgorithm;
this[typeof(SphereShape), typeof(CapsuleShape)] = gjkMprAlgorithm;
this[typeof(SphereShape), typeof(ConeShape)] = gjkMprAlgorithm;
this[typeof(SphereShape), typeof(CylinderShape)] = gjkMprAlgorithm;
this[typeof(SphereShape), typeof(EmptyShape)] = noAlgo;
this[typeof(SphereShape), typeof(InfiniteShape)] = infiniteAlgo;
this[typeof(SphereShape), typeof(PlaneShape)] = planeSphereAlgorithm;
this[typeof(SphereShape), typeof(HeightField)] = heightFieldAlgorithm;
this[typeof(SphereShape), typeof(TriangleMeshShape)] = triangleMeshAlgorithm;
this[typeof(SphereShape), typeof(TransformedShape)] = transformedShapeAlgorithm;
this[typeof(SphereShape), typeof(CompositeShape)] = compositeAlgorithm;
this[typeof(CapsuleShape), typeof(CapsuleShape)] = gjkMprAlgorithm;
this[typeof(CapsuleShape), typeof(ConeShape)] = gjkMprAlgorithm;
this[typeof(CapsuleShape), typeof(CylinderShape)] = gjkMprAlgorithm;
this[typeof(CapsuleShape), typeof(EmptyShape)] = noAlgo;
this[typeof(CapsuleShape), typeof(InfiniteShape)] = infiniteAlgo;
this[typeof(CapsuleShape), typeof(PlaneShape)] = planeConvexAlgorithm;
this[typeof(CapsuleShape), typeof(HeightField)] = heightFieldAlgorithm;
this[typeof(CapsuleShape), typeof(TriangleMeshShape)] = triangleMeshAlgorithm;
this[typeof(CapsuleShape), typeof(TransformedShape)] = transformedShapeAlgorithm;
this[typeof(CapsuleShape), typeof(CompositeShape)] = compositeAlgorithm;
this[typeof(ConeShape), typeof(ConeShape)] = gjkMprAlgorithm;
this[typeof(ConeShape), typeof(CylinderShape)] = gjkMprAlgorithm;
this[typeof(ConeShape), typeof(EmptyShape)] = noAlgo;
this[typeof(ConeShape), typeof(InfiniteShape)] = infiniteAlgo;
this[typeof(ConeShape), typeof(PlaneShape)] = planeConvexAlgorithm;
this[typeof(ConeShape), typeof(HeightField)] = heightFieldAlgorithm;
this[typeof(ConeShape), typeof(TriangleMeshShape)] = triangleMeshAlgorithm;
this[typeof(ConeShape), typeof(TransformedShape)] = transformedShapeAlgorithm;
this[typeof(ConeShape), typeof(CompositeShape)] = compositeAlgorithm;
this[typeof(CylinderShape), typeof(CylinderShape)] = gjkMprAlgorithm;
this[typeof(CylinderShape), typeof(EmptyShape)] = noAlgo;
this[typeof(CylinderShape), typeof(InfiniteShape)] = infiniteAlgo;
this[typeof(CylinderShape), typeof(PlaneShape)] = planeConvexAlgorithm;
this[typeof(CylinderShape), typeof(HeightField)] = heightFieldAlgorithm;
this[typeof(CylinderShape), typeof(TriangleMeshShape)] = triangleMeshAlgorithm;
this[typeof(CylinderShape), typeof(TransformedShape)] = transformedShapeAlgorithm;
this[typeof(CylinderShape), typeof(CompositeShape)] = compositeAlgorithm;
this[typeof(EmptyShape), typeof(EmptyShape)] = noAlgo;
this[typeof(EmptyShape), typeof(InfiniteShape)] = noAlgo; // No collision between Empty and Infinite.
this[typeof(EmptyShape), typeof(PlaneShape)] = noAlgo;
this[typeof(EmptyShape), typeof(HeightField)] = noAlgo;
this[typeof(EmptyShape), typeof(TriangleMeshShape)] = noAlgo;
this[typeof(EmptyShape), typeof(TransformedShape)] = noAlgo;
this[typeof(EmptyShape), typeof(CompositeShape)] = noAlgo;
this[typeof(InfiniteShape), typeof(InfiniteShape)] = infiniteAlgo;
this[typeof(InfiniteShape), typeof(PlaneShape)] = infiniteAlgo;
this[typeof(InfiniteShape), typeof(HeightField)] = infiniteAlgo;
this[typeof(InfiniteShape), typeof(TriangleMeshShape)] = infiniteAlgo;
this[typeof(InfiniteShape), typeof(TransformedShape)] = infiniteAlgo;
this[typeof(InfiniteShape), typeof(CompositeShape)] = infiniteAlgo;
this[typeof(PlaneShape), typeof(PlaneShape)] = noAlgo;
this[typeof(PlaneShape), typeof(HeightField)] = noAlgo;
this[typeof(PlaneShape), typeof(TriangleMeshShape)] = triangleMeshAlgorithm;
this[typeof(PlaneShape), typeof(TransformedShape)] = transformedShapeAlgorithm;
this[typeof(PlaneShape), typeof(CompositeShape)] = compositeAlgorithm;
this[typeof(HeightField), typeof(HeightField)] = noAlgo;
// We could also call triangleMeshAlgorithm. But since HeightField has usually larger parts it
// is better to call the heightFieldAlgorithm. The heightFieldAlgorithm will cull all but a
// few height field cells very quickly.
this[typeof(HeightField), typeof(TriangleMeshShape)] = heightFieldAlgorithm;
this[typeof(HeightField), typeof(TransformedShape)] = transformedShapeAlgorithm;
// Same as for triangle meshes: Call height field algorithm first.
this[typeof(HeightField), typeof(CompositeShape)] = heightFieldAlgorithm;
this[typeof(TriangleMeshShape), typeof(TriangleMeshShape)] = triangleMeshAlgorithm;
this[typeof(TriangleMeshShape), typeof(TransformedShape)] = transformedShapeAlgorithm;
this[typeof(TriangleMeshShape), typeof(CompositeShape)] = compositeAlgorithm;
this[typeof(TransformedShape), typeof(TransformedShape)] = transformedShapeAlgorithm;
this[typeof(TransformedShape), typeof(CompositeShape)] = transformedShapeAlgorithm;
this[typeof(CompositeShape), typeof(CompositeShape)] = compositeAlgorithm;
}
private Vector3F DoMpr(CollisionQueryType type, ContactSet contactSet, Vector3F v0)
{
int iterationCount = 0;
const int iterationLimit = 100;
CollisionObject collisionObjectA = contactSet.ObjectA;
IGeometricObject geometricObjectA = collisionObjectA.GeometricObject;
ConvexShape shapeA = (ConvexShape)geometricObjectA.Shape;
Vector3F scaleA = geometricObjectA.Scale;
Pose poseA = geometricObjectA.Pose;
CollisionObject collisionObjectB = contactSet.ObjectB;
IGeometricObject geometricObjectB = collisionObjectB.GeometricObject;
ConvexShape shapeB = (ConvexShape)geometricObjectB.Shape;
Vector3F scaleB = geometricObjectB.Scale;
Pose poseB = geometricObjectB.Pose;
// Cache inverted rotations.
var orientationAInverse = poseA.Orientation.Transposed;
var orientationBInverse = poseB.Orientation.Transposed;
Vector3F n = -v0; // Shoot from v0 to the origin.
Vector3F v1A = poseA.ToWorldPosition(shapeA.GetSupportPoint(orientationAInverse * -n, scaleA));
Vector3F v1B = poseB.ToWorldPosition(shapeB.GetSupportPoint(orientationBInverse * n, scaleB));
Vector3F v1 = v1B - v1A;
// Separating axis test:
if (Vector3F.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 Vector3F.Zero;
}
// Second support direction = perpendicular to plane of origin, v0 and v1.
n = Vector3F.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 Vector3F.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.)
Vector3F 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 = Vector3F.UnitY;
}
Vector3F 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 Vector3F.Zero;
}
Vector3F v2A = poseA.ToWorldPosition(shapeA.GetSupportPoint(orientationAInverse * -n, scaleA));
Vector3F v2B = poseB.ToWorldPosition(shapeB.GetSupportPoint(orientationBInverse * n, scaleB));
Vector3F v2 = v2B - v2A;
// Separating axis test:
if (Vector3F.Dot(v2, n) < 0)
return Vector3F.Zero;
// Third support direction = perpendicular to plane of v0, v1 and v2.
n = Vector3F.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 (Vector3F.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 Vector3F.Zero;
}
// Search for a valid portal.
Vector3F v3, v3A, v3B;
while (true)
{
iterationCount++;
// Abort if we cannot find a valid portal.
if (iterationCount > iterationLimit)
return Vector3F.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:
Vector3F 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);
Vector3F 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;
Vector3F 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;
Vector3F 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 (Vector3F.Dot(v3, n) < 0)
if (v3.X * n.X + v3.Y * n.Y + v3.Z * n.Z < 0)
return Vector3F.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 (Vector3F.Dot(Vector3F.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 = Vector3F.Cross(v1 - v0, v3 - v0);
// ----- Optimized version:
Vector3F v1MinusV0;
v1MinusV0.X = v1.X - v0.X;
v1MinusV0.Y = v1.Y - v0.Y;
v1MinusV0.Z = v1.Z - v0.Z;
Vector3F 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 (Vector3F.Dot(Vector3F.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 = Vector3F.Cross(v3 - v0, v2 - v0);
// ----- Optimized version:
Vector3F v3MinusV0;
v3MinusV0.X = v3.X - v0.X;
v3MinusV0.Y = v3.Y - v0.Y;
v3MinusV0.Z = v3.Z - v0.Z;
Vector3F 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.
Vector3F oldN = n;
// Compute outward pointing normal of the portal
//n = Vector3F.Cross(v2 - v1, v3 - v1);
Vector3F v2MinusV1;
v2MinusV1.X = v2.X - v1.X;
v2MinusV1.Y = v2.Y - v1.Y;
v2MinusV1.Z = v2.Z - v1.Z;
Vector3F 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 Vector3F.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), Vector3F.Zero, out u, out v, out w);
Vector3F 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 Vector3F.Zero;
// The points on the objects have the same barycentric coordinates.
Vector3F pointOnA = u * v1A + v * v2A + w * v3A;
Vector3F pointOnB = u * v1B + v * v2B + w * v3B;
Vector3F normal = pointOnA - pointOnB;
if (!normal.TryNormalize())
{
if (contactSet.IsPreferredNormalAvailable)
normal = contactSet.PreferredNormal;
else
normal = Vector3F.UnitY;
}
Vector3F 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 Vector3F.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 && Vector3F.Dot(v1, n) > 0)
{
// Portal points aways from origin --> Origin is in the tetrahedron.
contactSet.HaveContact = true;
if (type == CollisionQueryType.Boolean)
return Vector3F.Zero;
}
// Find new support point.
//Vector3F v4A = poseA.ToWorldPosition(shapeA.GetSupportPoint(orientationAInverse * -n, scaleA));
//Vector3F v4B = poseB.ToWorldPosition(shapeB.GetSupportPoint(orientationBInverse * n, scaleB));
//Vector3F v4 = v4B - v4A;
// ----- Optimized version:
Vector3F 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);
Vector3F supportPointA = shapeA.GetSupportPoint(supportDirectionA, scaleA);
Vector3F 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;
Vector3F 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;
Vector3F supportPointB = shapeB.GetSupportPoint(supportDirectionB, scaleB);
Vector3F 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;
Vector3F 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 && Vector3F.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 Vector3F.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 (Vector3F.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
|| Vector3F.AreNumericallyEqual(n, oldN)
|| iterationCount >= iterationLimit)
{
// We have the final portal.
if (!contactSet.HaveContact)
return Vector3F.Zero;
if (type == CollisionQueryType.Boolean)
return Vector3F.Zero;
// Find the point closest to the origin.
float u, v, w;
GeometryHelper.GetClosestPoint(new Triangle(v1, v2, v3), Vector3F.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.
Vector3F closest = u * v1 + v * v2 + w * v3;
// The points on the objects have the same barycentric coordinates.
Vector3F pointOnA = u * v1A + v * v2A + w * v3A;
Vector3F pointOnB = u * v1B + v * v2B + w * v3B;
// Use difference between points as normal direction, only if it can be normalized.
Vector3F normal = pointOnA - pointOnB;
if (!normal.TryNormalize())
normal = -n; // Else use the inverted normal of the portal.
Vector3F 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 Vector3F.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.
//Vector3F cross = Vector3F.Cross(v4, v0);
// ----- Optimized version:
Vector3F 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 (Vector3F.Dot(v1, cross) > 0)
if (v1.X * cross.X + v1.Y * cross.Y + v1.Z * cross.Z > 0)
{
// Eliminate v3 or v1.
//if (Vector3F.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 (Vector3F.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;
}
}
}
}
/// <summary>
/// Initializes a new instance of the <see cref="RayConvexAlgorithm"/> class.
/// </summary>
/// <param name="collisionDetection">The collision detection service.</param>
public RayConvexAlgorithm(CollisionDetection collisionDetection)
: base(collisionDetection)
{
_gjk = new Gjk(CollisionDetection);
}
private readonly Gjk _gjk; // A cached GJK instance.
#endregion Fields
#region Constructors
/// <summary>
/// Initializes a new instance of the <see cref="RayBoxAlgorithm"/> class.
/// </summary>
/// <param name="collisionDetection">The collision detection service.</param>
public RayBoxAlgorithm(CollisionDetection collisionDetection)
: base(collisionDetection)
{
_gjk = new Gjk(collisionDetection);
}
