/// <summary>
/// Computes the collision between line vs. line.
/// </summary>
/// <param name="contactSet">The contact set.</param>
/// <param name="type">The type of collision query.</param>
private void ComputeLineVsLine(ContactSet contactSet, CollisionQueryType type)
{
IGeometricObject objectA = contactSet.ObjectA.GeometricObject;
IGeometricObject objectB = contactSet.ObjectB.GeometricObject;
Debug.Assert(objectA.Shape is LineShape && objectB.Shape is LineShape, "LineAlgorithm.ComputeLineVsLine should only be called for 2 line shapes.");
Debug.Assert(contactSet.Count <= 1, "Two lines should have at max 1 contact point.");
// Get transformations.
Vector3F scaleA = objectA.Scale;
Vector3F scaleB = objectB.Scale;
Pose poseA = objectA.Pose;
Pose poseB = objectB.Pose;
// Create two line objects in world space.
var lineA = new Line((LineShape)objectA.Shape);
lineA.Scale(ref scaleA);
lineA.ToWorld(ref poseA);
var lineB = new Line((LineShape)objectB.Shape);
lineB.Scale(ref scaleB);
lineB.ToWorld(ref poseB);
// Get closest points.
Vector3F pointA;
Vector3F pointB;
contactSet.HaveContact = GeometryHelper.GetClosestPoints(lineA, lineB, out pointA, out pointB);
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;
}
// Create contact information.
Vector3F position = (pointA + pointB) / 2;
Vector3F normal = pointB - pointA;
float length = normal.Length;
if (Numeric.IsZero(length))
{
// Create normal from cross product of both lines.
normal = Vector3F.Cross(lineA.Direction, lineB.Direction);
if (!normal.TryNormalize())
normal = Vector3F.UnitY;
}
else
{
// Normalize vector
normal = normal / length;
}
Contact contact = ContactHelper.CreateContact(contactSet, position, normal, -length, false);
ContactHelper.Merge(contactSet, contact, type, CollisionDetection.ContactPositionTolerance);
}
public override void ComputeCollision(ContactSet contactSet, CollisionQueryType type)
{
if (type == CollisionQueryType.ClosestPoints)
{
// Closest point queries.
_closestPointsAlgorithm.ComputeCollision(contactSet, type);
if (contactSet.HaveContact)
{
// Penetration.
// Remember the closest point info in case we run into troubles.
// Get the last contact. We assume that this is the newest. In most cases there will
// only be one contact in the contact set.
Contact fallbackContact = (contactSet.Count > 0)
? contactSet[contactSet.Count - 1]
: null;
// Call the contact query algorithm.
_contactAlgorithm.ComputeCollision(contactSet, CollisionQueryType.Contacts);
if (!contactSet.HaveContact)
{
// Problem!
// The closest-point algorithm reported contact. The contact algorithm didn't find a contact.
// This can happen, for example, because of numerical inaccuracies in GJK vs. MPR.
// Keep the result of the closest-point computation, but decrease the penetration depth
// to indicate separation.
if (fallbackContact != null)
{
Debug.Assert(fallbackContact.PenetrationDepth == 0);
fallbackContact.PenetrationDepth = -Math.Min(10 * Numeric.EpsilonF, CollisionDetection.Epsilon);
foreach (var contact in contactSet)
if (contact != fallbackContact)
contact.Recycle();
contactSet.Clear();
contactSet.Add(fallbackContact);
}
contactSet.HaveContact = false;
}
}
}
else
{
// Boolean or contact queries.
_contactAlgorithm.ComputeCollision(contactSet, type);
}
}
public override void ComputeCollision(ContactSet contactSet, CollisionQueryType type)
{
bool isLineA = contactSet.ObjectA.GeometricObject.Shape is LineShape;
bool isLineB = contactSet.ObjectB.GeometricObject.Shape is LineShape;
if (isLineA && isLineB)
{
ComputeLineVsLine(contactSet, type);
}
else if (isLineA || isLineB)
{
ComputeLineVsOther(contactSet, type, isLineA);
}
else
{
throw new ArgumentException("The contact set must contain a line.", "contactSet");
}
}
public override void ComputeCollision(ContactSet contactSet, CollisionQueryType type)
{
// Object A should be the height field.
CollisionObject heightFieldCollisionObject = contactSet.ObjectA;
CollisionObject otherCollisionObject = contactSet.ObjectB;
// Swap objects if necessary.
bool swapped = !(heightFieldCollisionObject.GeometricObject.Shape is HeightField);
if (swapped)
MathHelper.Swap(ref heightFieldCollisionObject, ref otherCollisionObject);
IGeometricObject heightFieldGeometricObject = heightFieldCollisionObject.GeometricObject;
IGeometricObject otherGeometricObject = otherCollisionObject.GeometricObject;
HeightField heightField = heightFieldGeometricObject.Shape as HeightField;
Shape otherShape = otherGeometricObject.Shape;
// Check if collision object shapes are correct.
if (heightField == null)
throw new ArgumentException("The contact set must contain a height field.", "contactSet");
if (heightField.UseFastCollisionApproximation && type != CollisionQueryType.ClosestPoints)
{
// If other object is convex, use the new fast collision detection algorithm.
ConvexShape convex = otherShape as ConvexShape;
if (convex != null)
{
ComputeCollisionFast(
contactSet,
type,
heightFieldGeometricObject,
otherGeometricObject,
heightField,
convex,
swapped);
return;
}
}
#region ----- Precomputations -----
Vector3F scaleHeightField = heightFieldGeometricObject.Scale;
Vector3F scaleOther = otherGeometricObject.Scale;
Pose heightFieldPose = heightFieldGeometricObject.Pose;
// We do not support negative scaling. It is not clear what should happen when y is
// scaled with a negative factor and triangle orders would be wrong... Not worth the trouble.
if (scaleHeightField.X < 0 || scaleHeightField.Y < 0 || scaleHeightField.Z < 0)
throw new NotSupportedException("Computing collisions for height fields with a negative scaling is not supported.");
// Get height field and basic info.
Vector3F heightFieldUpAxis = heightFieldPose.ToWorldDirection(Vector3F.UnitY);
int arrayLengthX = heightField.NumberOfSamplesX;
int arrayLengthZ = heightField.NumberOfSamplesZ;
Debug.Assert(arrayLengthX > 1 && arrayLengthZ > 1, "A height field should contain at least 2 x 2 elements (= 1 cell).");
float cellWidthX = heightField.WidthX * scaleHeightField.X / (arrayLengthX - 1);
float cellWidthZ = heightField.WidthZ * scaleHeightField.Z / (arrayLengthZ - 1);
// The search-space is the rectangular region on the height field where the closest points
// must lie in. For contacts we do not have to search neighbor cells. For closest-point
// queries and separation we have to search neighbor cells.
// We compute the search-space using a current maximum search distance.
float currentSearchDistance = 0;
Contact guessedClosestPair = null;
if (!contactSet.HaveContact && type == CollisionQueryType.ClosestPoints)
{
// Make a guess for the closest pair using SupportMapping or InnerPoints.
bool isOverHole;
guessedClosestPair = GuessClosestPair(contactSet, swapped, out isOverHole);
if (isOverHole)
{
// Guesses over holes are useless. --> Check the whole terrain.
currentSearchDistance = heightFieldGeometricObject.Aabb.Extent.Length;
}
else if (guessedClosestPair.PenetrationDepth < 0)
{
currentSearchDistance = -guessedClosestPair.PenetrationDepth;
}
else
{
contactSet.HaveContact = true;
}
}
else
{
// Assume no contact.
contactSet.HaveContact = false;
}
// Get AABB of the other object in local space of the height field.
Aabb aabbOfOther = otherShape.GetAabb(scaleOther, heightFieldPose.Inverse * otherGeometricObject.Pose);
float originX = heightField.OriginX * scaleHeightField.X;
float originZ = heightField.OriginZ * scaleHeightField.Z;
// ----- Compute the cell indices of the search-space.
// Estimate start and end indices from our search distance.
int xIndexStartEstimated = (int)((aabbOfOther.Minimum.X - currentSearchDistance - originX) / cellWidthX);
int xIndexEndEstimated = (int)((aabbOfOther.Maximum.X + currentSearchDistance - originX) / cellWidthX);
int zIndexStartEstimated = (int)((aabbOfOther.Minimum.Z - currentSearchDistance - originZ) / cellWidthZ);
int zIndexEndEstimated = (int)((aabbOfOther.Maximum.Z + currentSearchDistance - originZ) / cellWidthZ);
// Clamp indices to valid range.
int xIndexMax = arrayLengthX - 2;
int zIndexMax = arrayLengthZ - 2;
int xIndexStart = Math.Max(xIndexStartEstimated, 0);
int xIndexEnd = Math.Min(xIndexEndEstimated, xIndexMax);
int zIndexStart = Math.Max(zIndexStartEstimated, 0);
int zIndexEnd = Math.Min(zIndexEndEstimated, zIndexMax);
// Find collision algorithm for MinkowskiSum vs. other object's shape.
CollisionAlgorithm collisionAlgorithm = CollisionDetection.AlgorithmMatrix[typeof(ConvexShape), otherShape.GetType()];
int numberOfContactsInLastFrame = contactSet.Count;
#endregion
#region ----- Test all height field cells in the search space. -----
// Create several temporary test objects:
// Instead of the original height field geometric object, we test against a shape for each
// height field triangle. For the test shape we "extrude" the triangle under the height field.
// To create the extrusion we "add" a line segment to the triangle using a Minkowski sum.
// TODO: We can make this faster with a special shape that knows that the child poses are Identity (instead of the standard MinkowskiSumShape).
// This special shape could compute its InnerPoint without applying the poses.
var triangleShape = ResourcePools.TriangleShapes.Obtain();
// (Vertices will be set in the loop below.)
var triangleGeometricObject = TestGeometricObject.Create();
triangleGeometricObject.Shape = triangleShape;
var lineSegment = ResourcePools.LineSegmentShapes.Obtain();
lineSegment.Start = Vector3F.Zero;
lineSegment.End = -heightField.Depth * Vector3F.UnitY;
var lineSegmentGeometricObject = TestGeometricObject.Create();
lineSegmentGeometricObject.Shape = lineSegment;
var extrudedTriangleShape = TestMinkowskiSumShape.Create();
extrudedTriangleShape.ObjectA = triangleGeometricObject;
extrudedTriangleShape.ObjectB = lineSegmentGeometricObject;
var extrudedTriangleGeometricObject = TestGeometricObject.Create();
extrudedTriangleGeometricObject.Shape = extrudedTriangleShape;
extrudedTriangleGeometricObject.Pose = heightFieldPose;
var testCollisionObject = ResourcePools.TestCollisionObjects.Obtain();
testCollisionObject.SetInternal(heightFieldCollisionObject, extrudedTriangleGeometricObject);
var testContactSet = swapped ? ContactSet.Create(contactSet.ObjectA, testCollisionObject)
: ContactSet.Create(testCollisionObject, contactSet.ObjectB);
testContactSet.IsPerturbationTestAllowed = false;
// We compute closest points with a preferred normal direction: the height field up-axis.
// Loop over the cells in the search space.
// (The inner loop can reduce the search space. Therefore, when we increment the indices
// xIndex and zIndex we also check if the start indices have changed.)
for (int xIndex = xIndexStart; xIndex <= xIndexEnd; xIndex = Math.Max(xIndexStart, xIndex + 1))
{
for (int zIndex = zIndexStart; zIndex <= zIndexEnd; zIndex = Math.Max(zIndexStart, zIndex + 1))
{
// Test the two cell triangles.
for (int triangleIndex = 0; triangleIndex < 2; triangleIndex++)
{
// Get triangle 0 or 1.
var triangle = heightField.GetTriangle(xIndex, zIndex, triangleIndex != 0);
var triangleIsHole = Numeric.IsNaN(triangle.Vertex0.Y * triangle.Vertex1.Y * triangle.Vertex2.Y);
if (triangleIsHole)
continue;
triangleShape.Vertex0 = triangle.Vertex0 * scaleHeightField;
triangleShape.Vertex1 = triangle.Vertex1 * scaleHeightField;
triangleShape.Vertex2 = triangle.Vertex2 * scaleHeightField;
if (type == CollisionQueryType.Boolean)
{
collisionAlgorithm.ComputeCollision(testContactSet, CollisionQueryType.Boolean);
contactSet.HaveContact = contactSet.HaveContact || testContactSet.HaveContact;
if (contactSet.HaveContact)
{
// We can stop tests here for boolean queries.
// Update end indices to exit the outer loops.
xIndexEnd = -1;
zIndexEnd = -1;
break;
}
}
else
{
Debug.Assert(testContactSet.Count == 0, "testContactSet needs to be cleared.");
// If we know that we have a contact, then we can make a faster contact query
// instead of a closest-point query.
CollisionQueryType queryType = (contactSet.HaveContact) ? CollisionQueryType.Contacts : type;
collisionAlgorithm.ComputeCollision(testContactSet, queryType);
if (testContactSet.HaveContact)
{
contactSet.HaveContact = true;
if (testContactSet.Count > 0)
{
// Get neighbor triangle.
// To compute the triangle normal we take the normal of the unscaled triangle and transform
// the normal with: (M^-1)^T = 1 / scale
// Note: We cannot use the scaled vertices because negative scalings change the
// face-order of the vertices.
Vector3F triangleNormal = triangle.Normal / scaleHeightField;
triangleNormal = heightFieldPose.ToWorldDirection(triangleNormal);
triangleNormal.TryNormalize();
// Assuming the last contact is the newest. (With closest-point queries
// and the CombinedCollisionAlgo, testContactSet[0] could be a (not so useful)
// closest-point result, and testContactSet[1] the better contact query result.)
var testContact = testContactSet[testContactSet.Count - 1];
var contactNormal = swapped ? -testContact.Normal : testContact.Normal;
if (Vector3F.Dot(contactNormal, triangleNormal) < WeldingLimit)
{
// Contact normal deviates by more than the welding limit. --> Check the contact.
// If we do not find a neighbor, we assume the neighbor has the same normal.
var neighborNormal = triangleNormal;
#region ----- Get Neighbor Triangle Normal -----
// Get barycentric coordinates of contact position.
Vector3F contactPositionOnHeightField = swapped ? testContact.PositionBLocal / scaleHeightField : testContact.PositionALocal / scaleHeightField;
float u, v, w;
// TODO: GetBaryCentricFromPoint computes the triangle normal, which we already know - optimize.
GeometryHelper.GetBarycentricFromPoint(triangle, contactPositionOnHeightField, out u, out v, out w);
// If one coordinate is near 0, the contact is near an edge.
if (u < 0.05f || v < 0.05f || w < 0.05f)
{
if (triangleIndex == 0)
{
if (u < v && u < w)
{
neighborNormal = heightFieldPose.ToWorldDirection(heightField.GetTriangle(xIndex, zIndex, true).Normal / scaleHeightField);
neighborNormal.TryNormalize();
}
else if (v < w)
{
if (zIndex > 0)
{
neighborNormal = heightFieldPose.ToWorldDirection(heightField.GetTriangle(xIndex, zIndex - 1, true).Normal / scaleHeightField);
neighborNormal.TryNormalize();
}
else
{
// The contact is at the border of the whole height field. Set a normal which disables all bad contact filtering.
neighborNormal = new Vector3F(float.NaN);
}
}
else
{
if (xIndex > 0)
{
neighborNormal = heightFieldPose.ToWorldDirection(heightField.GetTriangle(xIndex - 1, zIndex, true).Normal / scaleHeightField);
neighborNormal.TryNormalize();
}
else
{
neighborNormal = new Vector3F(float.NaN);
}
}
}
else
{
if (u < v && u < w)
{
if (xIndex + 2 < arrayLengthX)
{
neighborNormal = heightFieldPose.ToWorldDirection(heightField.GetTriangle(xIndex + 1, zIndex, false).Normal / scaleHeightField);
neighborNormal.TryNormalize();
}
else
{
neighborNormal = new Vector3F(float.NaN);
}
}
else if (v < w)
{
neighborNormal = heightFieldPose.ToWorldDirection(heightField.GetTriangle(xIndex, zIndex, false).Normal / scaleHeightField);
neighborNormal.TryNormalize();
}
else
{
if (zIndex + 2 < arrayLengthZ)
{
neighborNormal = heightFieldPose.ToWorldDirection(heightField.GetTriangle(xIndex, zIndex + 1, true).Normal / scaleHeightField);
neighborNormal.TryNormalize();
}
else
{
neighborNormal = new Vector3F(float.NaN);
}
}
}
}
#endregion
// Contact normals in the range triangleNormal - neighborNormal are allowed.
// Others, especially vertical contacts in slopes or horizontal normals are not
// allowed.
var cosMinAngle = Vector3F.Dot(neighborNormal, triangleNormal) - CollisionDetection.Epsilon;
RemoveBadContacts(swapped, testContactSet, triangleNormal, cosMinAngle);
// If we have no contact yet, we retry with a preferred normal identical to the up axis.
// (Note: contactSet.Count will be > 0 for closest-point queries but will
// probably constraint separated contacts and not real contacts.)
if (testContactSet.Count == 0
&& (contactSet.Count == 0 || type == CollisionQueryType.ClosestPoints))
{
testContactSet.PreferredNormal = (swapped) ? -heightFieldUpAxis : heightFieldUpAxis;
collisionAlgorithm.ComputeCollision(testContactSet, CollisionQueryType.Contacts);
testContactSet.PreferredNormal = Vector3F.Zero;
RemoveBadContacts(swapped, testContactSet, triangleNormal, cosMinAngle);
}
// If we have no contact yet, we retry with a preferred normal identical to the triangle normal.
// But only if the triangle normal differs significantly from the up axis.
if (testContactSet.Count == 0
&& (contactSet.Count == 0 || type == CollisionQueryType.ClosestPoints)
&& Vector3F.Dot(heightFieldUpAxis, triangleNormal) < WeldingLimit)
{
testContactSet.PreferredNormal = (swapped) ? -triangleNormal : triangleNormal;
collisionAlgorithm.ComputeCollision(testContactSet, CollisionQueryType.Contacts);
testContactSet.PreferredNormal = Vector3F.Zero;
RemoveBadContacts(swapped, testContactSet, triangleNormal, cosMinAngle);
}
}
}
}
if (testContactSet.Count > 0)
{
// Remember separation distance for later.
float separationDistance = -testContactSet[0].PenetrationDepth;
// Set the shape feature of the new contacts.
// The features is the height field triangle index (see HeightField documentation).
int numberOfContacts = testContactSet.Count;
for (int i = 0; i < numberOfContacts; i++)
{
Contact contact = testContactSet[i];
int featureIndex = (zIndex * (arrayLengthX - 1) + xIndex) * 2 + triangleIndex;
if (swapped)
contact.FeatureB = featureIndex;
else
contact.FeatureA = featureIndex;
}
// Merge the contact info. (Contacts in testContactSet are recycled!)
ContactHelper.Merge(contactSet, testContactSet, type, CollisionDetection.ContactPositionTolerance);
#region ----- Update search space -----
// Update search space if possible.
// The best search distance is 0. For separation we can use the current smallest
// separation as search distance. As soon as we have a contact, we set the
// search distance to 0.
if (currentSearchDistance > 0 // No need to update search space if search distance is already 0.
&& (contactSet.HaveContact // If we have a contact, we set the search distance to 0.
|| separationDistance < currentSearchDistance)) // If we have closer separation, we use this.
{
// Note: We only check triangleContactSet[0] in the if condition.
// triangleContactSet could contain several contacts, but we don't bother with
// this special case.
// Update search distance.
if (contactSet.HaveContact)
currentSearchDistance = 0;
else
currentSearchDistance = Math.Max(0, separationDistance);
// Update search space indices.
xIndexStartEstimated = (int)((aabbOfOther.Minimum.X - currentSearchDistance - originX) / cellWidthX);
xIndexEndEstimated = (int)((aabbOfOther.Maximum.X + currentSearchDistance - originX) / cellWidthX);
zIndexStartEstimated = (int)((aabbOfOther.Minimum.Z - currentSearchDistance - originZ) / cellWidthZ);
zIndexEndEstimated = (int)((aabbOfOther.Maximum.Z + currentSearchDistance - originZ) / cellWidthZ);
xIndexStart = Math.Max(xIndexStart, xIndexStartEstimated);
xIndexEnd = Math.Min(xIndexEndEstimated, xIndexMax);
zIndexStart = Math.Max(zIndexStart, zIndexStartEstimated);
zIndexEnd = Math.Min(zIndexEndEstimated, zIndexMax);
}
#endregion
}
}
}
}
}
// Recycle temporary objects.
testContactSet.Recycle();
ResourcePools.TestCollisionObjects.Recycle(testCollisionObject);
extrudedTriangleGeometricObject.Recycle();
extrudedTriangleShape.Recycle();
lineSegmentGeometricObject.Recycle();
ResourcePools.LineSegmentShapes.Recycle(lineSegment);
triangleGeometricObject.Recycle();
ResourcePools.TriangleShapes.Recycle(triangleShape);
#endregion
#region ----- Handle missing contact info -----
if (contactSet.Count == 0
&& (contactSet.HaveContact && type == CollisionQueryType.Contacts || type == CollisionQueryType.ClosestPoints))
{
// ----- Bad contact info:
// We should have contact data because this is either a contact query and the objects touch
// or this is a closest-point query.
// Use our guess as the contact info.
Contact closestPair = guessedClosestPair;
bool isOverHole = false;
if (closestPair == null)
closestPair = GuessClosestPair(contactSet, swapped, out isOverHole);
// Guesses over holes are useless. :-(
if (!isOverHole)
ContactHelper.Merge(contactSet, closestPair, type, CollisionDetection.ContactPositionTolerance);
}
#endregion
if (CollisionDetection.FullContactSetPerFrame
&& type == CollisionQueryType.Contacts
&& numberOfContactsInLastFrame == 0
&& contactSet.Count > 0
&& contactSet.Count < 4)
{
// Try to find full contact set.
// TODO: This can be optimized by not doing the whole overhead of ComputeCollision again.
ContactHelper.TestWithPerturbations(
CollisionDetection,
contactSet,
!swapped, // Perturb objectB not the height field.
_computeContactsMethod);
}
}
public override void ComputeCollision(ContactSet contactSet, CollisionQueryType type)
{
Debug.Assert(contactSet.Count <= 1, "Ray vs. plane should have at max 1 contact.");
// Object A should be the plane.
// Object B should be the ray.
IGeometricObject planeObject = contactSet.ObjectA.GeometricObject;
IGeometricObject rayObject = contactSet.ObjectB.GeometricObject;
// Swap objects if necessary.
bool swapped = (rayObject.Shape is PlaneShape);
if (swapped)
{
MathHelper.Swap(ref planeObject, ref rayObject);
}
PlaneShape planeShape = planeObject.Shape as PlaneShape;
RayShape rayShape = rayObject.Shape as RayShape;
// Check if A is really a plane and B is a ray.
if (planeShape == null || rayShape == null)
{
throw new ArgumentException("The contact set must contain a plane and a ray.", "contactSet");
}
// Get transformations.
Vector3 planeScale = planeObject.Scale;
Vector3 rayScale = rayObject.Scale;
Pose rayPose = rayObject.Pose;
Pose planePose = planeObject.Pose;
// Apply scale to plane.
Plane plane = new Plane(planeShape);
plane.Scale(ref planeScale);
// Apply scale to ray and transform ray into local space of plane.
Ray ray = new Ray(rayShape);
ray.Scale(ref rayScale); // Scale ray.
ray.ToWorld(ref rayPose); // Transform ray to world space.
ray.ToLocal(ref planePose); // Transform ray to local space of plane.
// Convert ray into a line segment.
LineSegment segment = new LineSegment {
Start = ray.Origin, End = ray.Origin + ray.Direction * ray.Length
};
// Check if ray origin is inside the plane. Otherwise call plane vs. ray query.
Vector3 linePoint;
Vector3 planePoint = Vector3.Zero;
if (Vector3.Dot(segment.Start, plane.Normal) <= plane.DistanceFromOrigin)
{
// The origin of the ray is below the plane.
linePoint = segment.Start;
contactSet.HaveContact = true;
}
else
{
// The origin of the ray is above the plane.
contactSet.HaveContact = GeometryHelper.GetClosestPoints(plane, segment, out linePoint, out planePoint);
}
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;
}
// ----- Create contact info.
Vector3 position;
float penetrationDepth;
if (contactSet.HaveContact)
{
// We have a contact.
position = planePose.ToWorldPosition(linePoint);
penetrationDepth = (linePoint - segment.Start).Length;
}
else
{
// Closest points, but separated.
position = planePose.ToWorldPosition((planePoint + linePoint) / 2);
penetrationDepth = -(linePoint - planePoint).Length;
}
Vector3 normal = planePose.ToWorldDirection(plane.Normal);
if (swapped)
{
normal = -normal;
}
Contact contact = ContactHelper.CreateContact(contactSet, position, normal, penetrationDepth, contactSet.HaveContact);
ContactHelper.Merge(contactSet, contact, type, CollisionDetection.ContactPositionTolerance);
}
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);
}
}
}
}
public override void ComputeCollision(ContactSet contactSet, CollisionQueryType type)
{
if (type == CollisionQueryType.ClosestPoints)
{
// Just use normal composite shape algorithm.
_triangleMeshAlgorithm.ComputeCollision(contactSet, type);
return;
}
Debug.Assert(type != CollisionQueryType.ClosestPoints, "Closest point queries should have already been handled!");
// Mesh = A, Ray = B
IGeometricObject meshObject = contactSet.ObjectA.GeometricObject;
IGeometricObject rayObject = contactSet.ObjectB.GeometricObject;
// Object A should be the mesh, swap objects if necessary.
bool swapped = (meshObject.Shape is RayShape);
if (swapped)
MathHelper.Swap(ref rayObject, ref meshObject);
RayShape rayShape = rayObject.Shape as RayShape;
TriangleMeshShape meshShape = meshObject.Shape as TriangleMeshShape;
// Check if shapes are correct.
if (rayShape == null || meshShape == null)
throw new ArgumentException("The contact set must contain a ray and a triangle mesh shape.", "contactSet");
// Assume no contact.
contactSet.HaveContact = false;
// Get transformations.
Vector3F rayScale = rayObject.Scale;
Pose rayPose = rayObject.Pose;
Vector3F meshScale = meshObject.Scale;
Pose meshPose = meshObject.Pose;
// Ray in world space.
Ray rayWorld = new Ray(rayShape);
rayWorld.Scale(ref rayScale); // Scale ray.
rayWorld.ToWorld(ref rayPose); // Transform ray to world space.
// Ray in local scaled space of the mesh.
Ray ray = rayWorld;
ray.ToLocal(ref meshPose); // Transform ray to local space of composite.
// Ray in local unscaled space of the mesh.
Ray rayUnscaled = ray;
var inverseCompositeScale = Vector3F.One / meshScale;
rayUnscaled.Scale(ref inverseCompositeScale);
ITriangleMesh triangleMesh = meshShape.Mesh;
bool isTwoSided = meshShape.IsTwoSided;
if (meshShape.Partition != null)
{
// ----- Mesh with BVH vs. Ray -----
foreach (var childIndex in meshShape.Partition.GetOverlaps(rayUnscaled))
{
Triangle triangle = triangleMesh.GetTriangle(childIndex);
AddContact(contactSet, swapped, type, ref rayWorld, ref ray, ref triangle, childIndex, ref meshPose, ref meshScale, isTwoSided);
if (type == CollisionQueryType.Boolean && contactSet.HaveContact)
break; // We can abort early.
}
}
else
{
// ----- Mesh vs. Ray -----
var rayUnscaledDirectionInverse = new Vector3F(
1 / rayUnscaled.Direction.X,
1 / rayUnscaled.Direction.Y,
1 / rayUnscaled.Direction.Z);
float epsilon = Numeric.EpsilonF * (1 + meshObject.Aabb.Extent.Length);
int numberOfTriangles = triangleMesh.NumberOfTriangles;
for (int i = 0; i < numberOfTriangles; i++)
{
Triangle triangle = triangleMesh.GetTriangle(i);
// Make ray vs AABB check first. We could skip this because the ray vs. triangle test
// is also fast. But experiments (ray vs sphere mesh) have shown that making an
// additional ray vs. AABB test first makes the worst case more than 20% faster.
if (GeometryHelper.HaveContact(triangle.Aabb, rayUnscaled.Origin, rayUnscaledDirectionInverse, rayUnscaled.Length, epsilon))
{
AddContact(contactSet, swapped, type, ref rayWorld, ref ray, ref triangle, i, ref meshPose, ref meshScale, isTwoSided);
// We have contact and stop for boolean queries.
if (contactSet.HaveContact && type == CollisionQueryType.Boolean)
break;
}
}
}
}
public override void ComputeCollision(ContactSet contactSet, CollisionQueryType type)
{
contactSet.Clear();
contactSet.Add(ContactHelper.CreateContact(contactSet, new Vector3F(1, 2, 3), Vector3F.UnitZ, 1, false));
if (type == CollisionQueryType.Contacts)
contactSet.Add(ContactHelper.CreateContact(contactSet, new Vector3F(2, 2, 3), Vector3F.UnitZ, 1.2f, false));
contactSet.HaveContact = true;
}
public static void Merge(ContactSet target, ContactSet newContacts, CollisionQueryType type, float contactPositionTolerance)
{
Debug.Assert(target != null);
Debug.Assert(newContacts != null);
Debug.Assert(contactPositionTolerance >= 0, "The contact position tolerance must be greater than or equal to 0");
int numberOfNewContacts = newContacts.Count;
for (int i = 0; i < numberOfNewContacts; i++)
Merge(target, newContacts[i], type, contactPositionTolerance);
newContacts.Clear();
}
// Compute contacts between child shapes of two <see cref="CompositeShape"/>s.
// testXxx are initialized objects which are re-used to avoid a lot of GC garbage.
private void AddChildChildContacts(ContactSet contactSet,
int childIndexA,
int childIndexB,
CollisionQueryType type,
ContactSet testContactSet,
CollisionObject testCollisionObjectA,
TestGeometricObject testGeometricObjectA,
CollisionObject testCollisionObjectB,
TestGeometricObject testGeometricObjectB)
{
CollisionObject collisionObjectA = contactSet.ObjectA;
CollisionObject collisionObjectB = contactSet.ObjectB;
IGeometricObject geometricObjectA = collisionObjectA.GeometricObject;
IGeometricObject geometricObjectB = collisionObjectB.GeometricObject;
CompositeShape shapeA = (CompositeShape)geometricObjectA.Shape;
CompositeShape shapeB = (CompositeShape)geometricObjectB.Shape;
Vector3F scaleA = geometricObjectA.Scale;
Vector3F scaleB = geometricObjectB.Scale;
IGeometricObject childA = shapeA.Children[childIndexA];
IGeometricObject childB = shapeB.Children[childIndexB];
// Find collision algorithm.
CollisionAlgorithm collisionAlgorithm = CollisionDetection.AlgorithmMatrix[childA, childB];
// ----- Set the shape temporarily to the current children.
// (Note: The scaling is either uniform or the child has no local rotation. Therefore, we only
// need to apply the scale of the parent to the scale and translation of the child. We can
// ignore the rotation.)
Debug.Assert(
(scaleA.X == scaleA.Y && scaleA.Y == scaleA.Z) || !childA.Pose.HasRotation,
"CompositeShapeAlgorithm should have thrown an exception. Non-uniform scaling is not supported for rotated children.");
Debug.Assert(
(scaleB.X == scaleB.Y && scaleB.Y == scaleB.Z) || !childB.Pose.HasRotation,
"CompositeShapeAlgorithm should have thrown an exception. Non-uniform scaling is not supported for rotated children.");
var childAPose = childA.Pose;
childAPose.Position *= scaleA; // Apply scaling to local translation.
testGeometricObjectA.Pose = geometricObjectA.Pose * childAPose;
testGeometricObjectA.Shape = childA.Shape;
testGeometricObjectA.Scale = scaleA * childA.Scale; // Apply scaling to local scale.
testCollisionObjectA.SetInternal(collisionObjectA, testGeometricObjectA);
var childBPose = childB.Pose;
childBPose.Position *= scaleB; // Apply scaling to local translation.
testGeometricObjectB.Pose = geometricObjectB.Pose * childBPose;
testGeometricObjectB.Shape = childB.Shape;
testGeometricObjectB.Scale = scaleB * childB.Scale; // Apply scaling to local scale.
testCollisionObjectB.SetInternal(collisionObjectB, testGeometricObjectB);
Debug.Assert(testContactSet.Count == 0, "testContactSet needs to be cleared.");
testContactSet.Reset(testCollisionObjectA, testCollisionObjectB);
if (type == CollisionQueryType.Boolean)
{
// Boolean queries.
collisionAlgorithm.ComputeCollision(testContactSet, CollisionQueryType.Boolean);
contactSet.HaveContact = (contactSet.HaveContact || testContactSet.HaveContact);
}
else
{
// TODO: We could add existing contacts with the same child shape to childContactSet.
// No perturbation test. Most composite shapes are either complex and automatically
// have more contacts. Or they are complex and will not be used for stacking
// where full contact sets would be needed.
testContactSet.IsPerturbationTestAllowed = false;
// Make collision check. As soon as we have found contact, we can make faster
// contact queries instead of closest-point queries.
CollisionQueryType queryType = (contactSet.HaveContact) ? CollisionQueryType.Contacts : type;
collisionAlgorithm.ComputeCollision(testContactSet, queryType);
contactSet.HaveContact = (contactSet.HaveContact || testContactSet.HaveContact);
// Transform contacts into space of composite shape.
// And set the shape feature of the contact.
int numberOfContacts = testContactSet.Count;
for (int i = 0; i < numberOfContacts; i++)
{
Contact contact = testContactSet[i];
contact.PositionALocal = childAPose.ToWorldPosition(contact.PositionALocal);
//if (contact.Lifetime.Ticks == 0) // Currently, all contacts are new, so this check is not necessary.
//{
contact.FeatureA = childIndexA;
//}
contact.PositionBLocal = childBPose.ToWorldPosition(contact.PositionBLocal);
//if (contact.Lifetime.Ticks == 0) // Currently, all contacts are new, so this check is not necessary.
//{
contact.FeatureB = childIndexB;
//}
}
// Merge child contacts.
ContactHelper.Merge(contactSet, testContactSet, type, CollisionDetection.ContactPositionTolerance);
}
}
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.
Vector3 scalePlane = planeObject.Scale;
Vector3 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.
Vector3 planeNormalLocalB = poseB.ToLocalDirection(planeWorld.Normal);
// Get support vertex nearest to the plane.
Vector3 supportVertexBLocal = convexShape.GetSupportPoint(-planeNormalLocalB, scaleB);
// Transform support vertex into world space.
Vector3 supportVertexBWorld = poseB.ToWorldPosition(supportVertexBLocal);
// Project vertex onto separating axis (given by plane normal).
float distance = Vector3.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.
Vector3 position = supportVertexBWorld + planeWorld.Normal * (penetrationDepth / 2);
Vector3 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 <Vector3> 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];
Vector3 scaledVertex = vertex * scaleB;
if (scaledVertex != supportVertexBLocal) // supportVertexBLocal has already been added.
{
Vector3 vertexWorld = poseB.ToWorldPosition(scaledVertex);
distance = Vector3.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);
}
}
}
// Returns true if a contact was added.
private bool AddContact(ContactSet contactSet,
bool swapped,
CollisionQueryType type,
ref Ray rayWorld, // The ray in world space.
ref Ray rayInField, // The ray in the scaled height field space.
ref Triangle triangle, // The unscaled triangle in the mesh space.
int triangleIndex,
ref Pose trianglePose,
ref Vector3 triangleScale)
{
// This code is from GeometryHelper_Triangles.cs. Sync changes!
Vector3 v0 = triangle.Vertex0 * triangleScale;
Vector3 v1 = triangle.Vertex1 * triangleScale;
Vector3 v2 = triangle.Vertex2 * triangleScale;
Vector3 d1 = (v1 - v0);
Vector3 d2 = (v2 - v0);
Vector3 n = Vector3.Cross(d1, d2);
// Tolerance value, see SOLID, Bergen: "Collision Detection in Interactive 3D Environments".
float ε = n.Length * Numeric.EpsilonFSquared;
Vector3 r = rayInField.Direction * rayInField.Length;
float δ = -Vector3.Dot(r, n);
// Degenerate triangle --> No hit.
if (ε == 0.0f || Numeric.IsZero(δ, ε))
{
return(false);
}
Vector3 triangleToRayOrigin = rayInField.Origin - v0;
float λ = Vector3.Dot(triangleToRayOrigin, n) / δ;
if (λ < 0 || λ > 1)
{
return(false);
}
// The ray hit the triangle plane.
Vector3 u = Vector3.Cross(triangleToRayOrigin, r);
float μ1 = Vector3.Dot(d2, u) / δ;
float μ2 = Vector3.Dot(-d1, u) / δ;
if (μ1 + μ2 <= 1 + ε && μ1 >= -ε && μ2 >= -ε)
{
// Hit!
contactSet.HaveContact = true;
if (type == CollisionQueryType.Boolean)
{
return(false);
}
if (δ < 0)
{
return(false); // Shooting into the back of a one-sided triangle - no contact.
}
float penetrationDepth = λ * rayInField.Length;
// Create contact info.
Vector3 position = rayWorld.Origin + rayWorld.Direction * penetrationDepth;
n = trianglePose.ToWorldDirection(n);
Debug.Assert(!n.IsNumericallyZero, "Degenerate cases of ray vs. triangle should be treated above.");
n.Normalize();
if (δ < 0)
{
n = -n;
}
if (swapped)
{
n = -n;
}
Contact contact = ContactHelper.CreateContact(contactSet, position, n, penetrationDepth, true);
if (swapped)
{
contact.FeatureB = triangleIndex;
}
else
{
contact.FeatureA = triangleIndex;
}
Debug.Assert(
contactSet.ObjectA.GeometricObject.Shape is RayShape && contact.FeatureA == -1 ||
contactSet.ObjectB.GeometricObject.Shape is RayShape && contact.FeatureB == -1,
"RayHeightFieldAlgorithm has set the wrong feature property.");
ContactHelper.Merge(contactSet, contact, type, CollisionDetection.ContactPositionTolerance);
return(true);
}
return(false);
}
public override void ComputeCollision(ContactSet contactSet, CollisionQueryType type)
{
if (type == CollisionQueryType.ClosestPoints)
{
// Just use normal height field shape algorithm.
_heightFieldAlgorithm.ComputeCollision(contactSet, type);
return;
}
Debug.Assert(type != CollisionQueryType.ClosestPoints, "Closest point queries should have already been handled!");
// HeightField = A, Ray = B
IGeometricObject heightFieldObject = contactSet.ObjectA.GeometricObject;
IGeometricObject rayObject = contactSet.ObjectB.GeometricObject;
// Object A should be the height field, swap objects if necessary.
bool swapped = (heightFieldObject.Shape is RayShape);
if (swapped)
{
MathHelper.Swap(ref rayObject, ref heightFieldObject);
}
RayShape rayShape = rayObject.Shape as RayShape;
HeightField heightField = heightFieldObject.Shape as HeightField;
// Check if shapes are correct.
if (rayShape == null || heightField == null)
{
throw new ArgumentException("The contact set must contain a ray and a height field.", "contactSet");
}
// Assume no contact.
contactSet.HaveContact = false;
// Get transformations.
Vector3 rayScale = rayObject.Scale;
Pose rayPose = rayObject.Pose;
Vector3 heightFieldScale = heightFieldObject.Scale;
Pose heightFieldPose = heightFieldObject.Pose;
// We do not support negative scaling. It is not clear what should happen when y is
// scaled with a negative factor and triangle orders would be wrong... Not worth the trouble.
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.");
}
// Ray in world space.
Ray rayWorld = new Ray(rayShape);
rayWorld.Scale(ref rayScale);
rayWorld.ToWorld(ref rayPose);
// Ray in local scaled space of the height field.
Ray rayScaled = rayWorld;
rayScaled.ToLocal(ref heightFieldPose);
// Ray in local unscaled space of the mesh.
Ray rayUnscaled = rayScaled;
var inverseCompositeScale = Vector3.One / heightFieldScale;
rayUnscaled.Scale(ref inverseCompositeScale);
// Get height field and basic info.
int arrayLengthX = heightField.NumberOfSamplesX;
int arrayLengthZ = heightField.NumberOfSamplesZ;
int numberOfCellsX = arrayLengthX - 1;
int numberOfCellsZ = arrayLengthZ - 1;
Debug.Assert(arrayLengthX > 1 && arrayLengthZ > 1, "A height field should contain at least 2 x 2 elements (= 1 cell).");
float cellWidthX = heightField.WidthX / numberOfCellsX; // Unscaled!
float cellWidthZ = heightField.WidthZ / numberOfCellsZ; // Unscaled!
// We use a 2D-DDA traversal of the height field cells. In other words: Look at it from
// above. The height field is our screen and we will select the cells as if we draw
// a pixel line. This could be made more efficient when we do not recompute values and
// reuse values and make incremental steps Bresenham-style.
// See GeometryHelper_Casts.cs method HaveContact(Aabb, ray) for explanation of the
// ray parameter formula.
var rayUnscaledDirectionInverse = new Vector3(
1 / rayUnscaled.Direction.X,
1 / rayUnscaled.Direction.Y,
1 / rayUnscaled.Direction.Z);
// The position where the ray enters the current cell.
var cellEnter = rayUnscaled.Origin; // Unscaled!!!
var originX = heightField.OriginX;
var originZ = heightField.OriginZ;
// ----- Find first cell.
int indexX = (cellEnter.X >= originX) ? (int)((cellEnter.X - originX) / cellWidthX) : -1; // (int)(...) does not return the desired result for negative values!
if (indexX < 0)
{
if (rayUnscaled.Direction.X <= 0)
{
return;
}
float parameter = (originX - rayUnscaled.Origin.X) * rayUnscaledDirectionInverse.X;
if (parameter > rayUnscaled.Length)
{
return; // The ray does not reach the height field.
}
cellEnter = rayUnscaled.Origin + parameter * rayUnscaled.Direction;
indexX = 0;
}
else if (indexX >= numberOfCellsX)
{
if (rayUnscaled.Direction.X >= 0)
{
return;
}
float parameter = (originX + heightField.WidthX - rayUnscaled.Origin.X) * rayUnscaledDirectionInverse.X;
if (parameter > rayUnscaled.Length)
{
return; // The ray does not reach the height field.
}
cellEnter = rayUnscaled.Origin + parameter * rayUnscaled.Direction;
indexX = numberOfCellsX - 1;
}
int indexZ = (cellEnter.Z >= originZ) ? (int)((cellEnter.Z - originZ) / cellWidthZ) : -1;
if (indexZ < 0)
{
if (rayUnscaled.Direction.Z <= 0)
{
return;
}
float parameter = (originZ - rayUnscaled.Origin.Z) * rayUnscaledDirectionInverse.Z;
if (parameter > rayUnscaled.Length)
{
return; // The ray does not reach the next height field.
}
cellEnter = rayUnscaled.Origin + parameter * rayUnscaled.Direction;
// We also have to correct the indexX!
indexX = (cellEnter.X >= originX) ? (int)((cellEnter.X - originX) / cellWidthX) : -1;
indexZ = 0;
}
else if (indexZ >= numberOfCellsZ)
{
if (rayUnscaled.Direction.Z >= 0)
{
return;
}
float parameter = (originZ + heightField.WidthZ - rayUnscaled.Origin.Z) * rayUnscaledDirectionInverse.Z;
if (parameter > rayUnscaled.Length)
{
return; // The ray does not reach the next height field.
}
cellEnter = rayUnscaled.Origin + parameter * rayUnscaled.Direction;
indexX = (cellEnter.X >= originX) ? (int)((cellEnter.X - originX) / cellWidthX) : -1;
indexZ = numberOfCellsZ - 1;
}
if (indexX < 0 || indexX >= numberOfCellsX || indexZ < 0 || indexZ >= numberOfCellsZ)
{
return;
}
while (true)
{
// ----- Get triangles of current cell.
var triangle0 = heightField.GetTriangle(indexX, indexZ, false);
var triangle1 = heightField.GetTriangle(indexX, indexZ, true);
// Index of first triangle.
var triangleIndex = (indexZ * numberOfCellsX + indexX) * 2;
float xRelative = (cellEnter.X - originX) / cellWidthX - indexX;
float zRelative = (cellEnter.Z - originZ) / cellWidthZ - indexZ;
bool enterSecondTriangle = (xRelative + zRelative) > 1; // The diagonal is where xRel + zRel == 1.
// ----- Find cell exit and move indices to next cell.
// The position where the ray leaves the current cell.
Vector3 cellExit;
float nextXParameter = float.PositiveInfinity;
if (rayUnscaled.Direction.X > 0)
{
nextXParameter = (originX + (indexX + 1) * cellWidthX - rayUnscaled.Origin.X) * rayUnscaledDirectionInverse.X;
}
else if (rayUnscaled.Direction.X < 0)
{
nextXParameter = (originX + indexX * cellWidthX - rayUnscaled.Origin.X) * rayUnscaledDirectionInverse.X;
}
float nextZParameter = float.PositiveInfinity;
if (rayUnscaled.Direction.Z > 0)
{
nextZParameter = (originZ + (indexZ + 1) * cellWidthZ - rayUnscaled.Origin.Z) * rayUnscaledDirectionInverse.Z;
}
else if (rayUnscaled.Direction.Z < 0)
{
nextZParameter = (originZ + indexZ * cellWidthZ - rayUnscaled.Origin.Z) * rayUnscaledDirectionInverse.Z;
}
bool isLastCell = false;
if (nextXParameter < nextZParameter)
{
if (rayUnscaled.Direction.X > 0)
{
indexX++;
if (indexX >= numberOfCellsX) // Abort if we have left the height field.
{
isLastCell = true;
}
}
else
{
indexX--;
if (indexX < 0)
{
isLastCell = true;
}
}
if (nextXParameter > rayUnscaled.Length)
{
isLastCell = true; // The ray does not reach the next cell.
nextXParameter = rayUnscaled.Length;
}
cellExit = rayUnscaled.Origin + nextXParameter * rayUnscaled.Direction;
}
else
{
if (rayUnscaled.Direction.Z > 0)
{
indexZ++;
if (indexZ >= numberOfCellsZ)
{
isLastCell = true;
}
}
else
{
indexZ--;
if (indexZ < 0)
{
isLastCell = true;
}
}
if (nextZParameter > rayUnscaled.Length)
{
isLastCell = true;
nextZParameter = rayUnscaled.Length;
}
cellExit = rayUnscaled.Origin + nextZParameter * rayUnscaled.Direction;
}
// ----- We can skip cell if cell AABB is below the ray.
var rayMinY = Math.Min(cellEnter.Y, cellExit.Y) - CollisionDetection.Epsilon; // Apply to avoid missing collisions when ray hits a cell border.
// The ray is above if no height field height is higher the ray height.
// (This check handles NaN height values (holes) correctly.)
bool rayIsAbove = !(triangle0.Vertex0.Y >= rayMinY ||
triangle0.Vertex1.Y >= rayMinY ||
triangle0.Vertex2.Y >= rayMinY ||
triangle1.Vertex1.Y >= rayMinY); // Vertex1 of triangle1 is the fourth quad vertex!
// ----- Test ray against the 2 triangles of the cell.
bool triangle0IsHole = false;
bool triangle1IsHole = false;
if (!rayIsAbove)
{
// Abort if a height value is NaN (hole).
triangle0IsHole = Numeric.IsNaN(triangle0.Vertex0.Y * triangle0.Vertex1.Y * triangle0.Vertex2.Y);
triangle1IsHole = Numeric.IsNaN(triangle1.Vertex0.Y * triangle1.Vertex1.Y * triangle1.Vertex2.Y);
bool contactAdded = false;
if (enterSecondTriangle)
{
// Test second triangle first.
if (!triangle1IsHole)
{
contactAdded = AddContact(contactSet, swapped, type, ref rayWorld, ref rayScaled, ref triangle1, triangleIndex + 1, ref heightFieldPose, ref heightFieldScale);
}
if (!contactAdded && !triangle0IsHole)
{
contactAdded = AddContact(contactSet, swapped, type, ref rayWorld, ref rayScaled, ref triangle0, triangleIndex, ref heightFieldPose, ref heightFieldScale);
}
}
else
{
// Test first triangle first.
if (!triangle0IsHole)
{
contactAdded = AddContact(contactSet, swapped, type, ref rayWorld, ref rayScaled, ref triangle0, triangleIndex, ref heightFieldPose, ref heightFieldScale);
}
if (!contactAdded && !triangle1IsHole)
{
contactAdded = AddContact(contactSet, swapped, type, ref rayWorld, ref rayScaled, ref triangle1, triangleIndex + 1, ref heightFieldPose, ref heightFieldScale);
}
}
if (contactAdded)
{
return;
}
// We have contact and stop for boolean queries.
if (contactSet.HaveContact && type == CollisionQueryType.Boolean)
{
return;
}
}
// ----- Return simplified contact if cellEnter is below the cell.
if (!rayIsAbove)
{
if (!enterSecondTriangle && !triangle0IsHole && GeometryHelper.IsInFront(triangle0, cellEnter) < 0 ||
enterSecondTriangle && !triangle1IsHole && GeometryHelper.IsInFront(triangle1, cellEnter) < 0)
{
contactSet.HaveContact = true;
if (type == CollisionQueryType.Boolean)
{
return;
}
var position = heightFieldPose.ToWorldPosition(cellEnter * heightFieldScale);
var normal = heightFieldPose.ToWorldDirection(Vector3.UnitY);
if (swapped)
{
normal = -normal;
}
float penetrationDepth = (position - rayWorld.Origin).Length;
Contact contact = ContactHelper.CreateContact(contactSet, position, normal, penetrationDepth, true);
ContactHelper.Merge(contactSet, contact, type, CollisionDetection.ContactPositionTolerance);
return;
}
}
// ----- Move to next cell.
if (isLastCell)
{
return;
}
cellEnter = cellExit;
}
}
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)
{
CollisionObject collisionObjectA = contactSet.ObjectA;
CollisionObject collisionObjectB = contactSet.ObjectB;
IGeometricObject geometricObjectA = collisionObjectA.GeometricObject;
IGeometricObject geometricObjectB = collisionObjectB.GeometricObject;
// Object A should be the composite, swap objects if necessary.
// When testing CompositeShape vs. CompositeShape with BVH, object A should be the
// CompositeShape with BVH.
CompositeShape compositeShapeA = geometricObjectA.Shape as CompositeShape;
CompositeShape compositeShapeB = geometricObjectB.Shape as CompositeShape;
bool swapped = false;
if (compositeShapeA == null)
{
// Object A is something else. Object B must be a composite shape.
swapped = true;
}
else if (compositeShapeA.Partition == null &&
compositeShapeB != null &&
compositeShapeB.Partition != null)
{
// Object A has no BVH, object B is CompositeShape with BVH.
swapped = true;
}
if (swapped)
{
MathHelper.Swap(ref collisionObjectA, ref collisionObjectB);
MathHelper.Swap(ref geometricObjectA, ref geometricObjectB);
MathHelper.Swap(ref compositeShapeA, ref compositeShapeB);
}
// Check if collision objects shapes are correct.
if (compositeShapeA == null)
{
throw new ArgumentException("The contact set must contain a composite shape.", "contactSet");
}
// Assume no contact.
contactSet.HaveContact = false;
Vector3F scaleA = geometricObjectA.Scale;
Vector3F scaleB = geometricObjectB.Scale;
// Check if transforms are supported.
if (compositeShapeA != null && // When object A is a CompositeShape
(scaleA.X != scaleA.Y || scaleA.Y != scaleA.Z) && // non-uniform scaling is not supported
compositeShapeA.Children.Any(child => child.Pose.HasRotation)) // when a child has a local rotation.
{ // Note: Any() creates garbage, but non-uniform scalings should not be used anyway.
throw new NotSupportedException("Computing collisions for composite shapes with non-uniform scaling and rotated children is not supported.");
}
// Same check for object B.
if (compositeShapeB != null &&
(scaleB.X != scaleB.Y || scaleB.Y != scaleB.Z) &&
compositeShapeB.Children.Any(child => child.Pose.HasRotation)) // Note: Any() creates garbage, but non-uniform scalings should not be used anyway.
{
throw new NotSupportedException("Computing collisions for composite shapes with non-uniform scaling and rotated children is not supported.");
}
// ----- A few fixed objects which are reused to avoid GC garbage.
var testCollisionObjectA = ResourcePools.TestCollisionObjects.Obtain();
var testCollisionObjectB = ResourcePools.TestCollisionObjects.Obtain();
// Create a test contact set and initialize with dummy objects.
// (The actual collision objects are set below.)
var testContactSet = ContactSet.Create(testCollisionObjectA, testCollisionObjectB);
var testGeometricObjectA = TestGeometricObject.Create();
var testGeometricObjectB = TestGeometricObject.Create();
try
{
if (compositeShapeA.Partition != null &&
(type != CollisionQueryType.ClosestPoints ||
compositeShapeA.Partition is ISupportClosestPointQueries <int>))
{
if (compositeShapeB != null && compositeShapeB.Partition != null)
{
#region ----- Composite with BVH vs. Composite with BVH -----
Debug.Assert(swapped == false, "Why did we swap the objects? Order of objects is fine.");
if (type != CollisionQueryType.ClosestPoints)
{
// ----- Boolean or Contact Query
// Heuristic: Test large BVH vs. small BVH.
Aabb aabbOfA = geometricObjectA.Aabb;
Aabb aabbOfB = geometricObjectB.Aabb;
float largestExtentA = aabbOfA.Extent.LargestComponent;
float largestExtentB = aabbOfB.Extent.LargestComponent;
IEnumerable <Pair <int> > overlaps;
bool overlapsSwapped = largestExtentA < largestExtentB;
if (overlapsSwapped)
{
overlaps = compositeShapeB.Partition.GetOverlaps(
scaleB,
geometricObjectB.Pose,
compositeShapeA.Partition,
scaleA,
geometricObjectA.Pose);
}
else
{
overlaps = compositeShapeA.Partition.GetOverlaps(
scaleA,
geometricObjectA.Pose,
compositeShapeB.Partition,
scaleB,
geometricObjectB.Pose);
}
foreach (var overlap in overlaps)
{
if (type == CollisionQueryType.Boolean && contactSet.HaveContact)
{
break; // We can abort early.
}
AddChildChildContacts(
contactSet,
overlapsSwapped ? overlap.Second : overlap.First,
overlapsSwapped ? overlap.First : overlap.Second,
type,
testContactSet,
testCollisionObjectA,
testGeometricObjectA,
testCollisionObjectB,
testGeometricObjectB);
}
}
else
{
// Closest-Point Query
var callback = ClosestPointsCallbacks.Obtain();
callback.CollisionAlgorithm = this;
callback.Swapped = false;
callback.ContactSet = contactSet;
callback.TestCollisionObjectA = testCollisionObjectA;
callback.TestCollisionObjectB = testCollisionObjectB;
callback.TestGeometricObjectA = testGeometricObjectA;
callback.TestGeometricObjectB = testGeometricObjectB;
callback.TestContactSet = testContactSet;
((ISupportClosestPointQueries <int>)compositeShapeA.Partition)
.GetClosestPointCandidates(
scaleA,
geometricObjectA.Pose,
compositeShapeB.Partition,
scaleB,
geometricObjectB.Pose,
callback.HandlePair);
ClosestPointsCallbacks.Recycle(callback);
}
#endregion
}
else
{
#region ----- Composite with BVH vs. * -----
// Compute AABB of B in local space of the CompositeShape.
Aabb aabbBInA = geometricObjectB.Shape.GetAabb(
scaleB, geometricObjectA.Pose.Inverse * geometricObjectB.Pose);
// Apply inverse scaling to do the AABB checks in the unscaled local space of A.
aabbBInA.Scale(Vector3F.One / scaleA);
if (type != CollisionQueryType.ClosestPoints)
{
// Boolean or Contact Query
foreach (var childIndex in compositeShapeA.Partition.GetOverlaps(aabbBInA))
{
if (type == CollisionQueryType.Boolean && contactSet.HaveContact)
{
break; // We can abort early.
}
AddChildContacts(
contactSet,
swapped,
childIndex,
type,
testContactSet,
testCollisionObjectA,
testGeometricObjectA);
}
}
else if (type == CollisionQueryType.ClosestPoints)
{
// Closest-Point Query
var callback = ClosestPointsCallbacks.Obtain();
callback.CollisionAlgorithm = this;
callback.Swapped = swapped;
callback.ContactSet = contactSet;
callback.TestCollisionObjectA = testCollisionObjectA;
callback.TestCollisionObjectB = testCollisionObjectB;
callback.TestGeometricObjectA = testGeometricObjectA;
callback.TestGeometricObjectB = testGeometricObjectB;
callback.TestContactSet = testContactSet;
((ISupportClosestPointQueries <int>)compositeShapeA.Partition)
.GetClosestPointCandidates(
aabbBInA,
float.PositiveInfinity,
callback.HandleItem);
ClosestPointsCallbacks.Recycle(callback);
}
#endregion
}
}
else
{
#region ----- Composite vs. *-----
// Compute AABB of B in local space of the composite.
Aabb aabbBInA = geometricObjectB.Shape.GetAabb(scaleB, geometricObjectA.Pose.Inverse * geometricObjectB.Pose);
// Apply inverse scaling to do the AABB checks in the unscaled local space of A.
aabbBInA.Scale(Vector3F.One / scaleA);
// Go through list of children and find contacts.
int numberOfChildGeometries = compositeShapeA.Children.Count;
for (int i = 0; i < numberOfChildGeometries; i++)
{
IGeometricObject child = compositeShapeA.Children[i];
// NOTE: For closest-point queries we could be faster estimating a search space.
// See TriangleMeshAlgorithm or BVH queries.
// But the current implementation is sufficient. If the CompositeShape is more complex
// the user should be using spatial partitions anyway.
// For boolean or contact queries, we make an AABB test first.
// For closest points where we have not found a contact yet, we have to search
// all children.
if ((type == CollisionQueryType.ClosestPoints && !contactSet.HaveContact) ||
GeometryHelper.HaveContact(aabbBInA, child.Shape.GetAabb(child.Scale, child.Pose)))
{
// TODO: We could compute the minDistance of the child AABB and the AABB of the
// other shape. If the minDistance is greater than the current closestPairDistance
// we can ignore this pair. - This could be a performance boost.
// Get contacts/closest pairs of this child.
AddChildContacts(
contactSet,
swapped,
i,
type,
testContactSet,
testCollisionObjectA,
testGeometricObjectA);
// We have contact and stop for boolean queries.
if (contactSet.HaveContact && type == CollisionQueryType.Boolean)
{
break;
}
}
}
#endregion
}
}
finally
{
Debug.Assert(collisionObjectA.GeometricObject.Shape == compositeShapeA, "Shape was altered and not restored.");
testContactSet.Recycle();
ResourcePools.TestCollisionObjects.Recycle(testCollisionObjectA);
ResourcePools.TestCollisionObjects.Recycle(testCollisionObjectB);
testGeometricObjectB.Recycle();
testGeometricObjectA.Recycle();
}
}
public override void ComputeCollision(ContactSet contactSet, CollisionQueryType type)
{
// Note: When comparing this implementation with the GJK (see Gjk.cs), be aware that the
// GJK implementation computes the CSO as the Minkowski difference A-B whereas the MPR uses
// B-A. Both representations of the CSO are equivalent, we just have to invert the vectors
// here and there. (B-A was chosen because the original description of the MPR used B-A.)
if (type == CollisionQueryType.ClosestPoints)
throw new GeometryException("MPR cannot handle closest-point queries. Use GJK instead.");
CollisionObject collisionObjectA = contactSet.ObjectA;
IGeometricObject geometricObjectA = collisionObjectA.GeometricObject;
ConvexShape shapeA = geometricObjectA.Shape as ConvexShape;
Vector3F scaleA = geometricObjectA.Scale;
Pose poseA = geometricObjectA.Pose;
CollisionObject collisionObjectB = contactSet.ObjectB;
IGeometricObject geometricObjectB = collisionObjectB.GeometricObject;
ConvexShape shapeB = geometricObjectB.Shape as ConvexShape;
Vector3F scaleB = geometricObjectB.Scale;
Pose poseB = geometricObjectB.Pose;
if (shapeA == null || shapeB == null)
throw new ArgumentException("The contact set must contain convex shapes.", "contactSet");
// Assume no contact.
contactSet.HaveContact = false;
Vector3F v0;
if (contactSet.IsPreferredNormalAvailable && type == CollisionQueryType.Contacts)
{
// Set v0, so to shoot into preferred direction.
v0 = contactSet.PreferredNormal;
// Perform only 1 MPR iteration.
DoMpr(type, contactSet, v0);
return;
}
// Find first point v0 (which determines the ray direction).
// Inner point in CSO (Minkowski difference B-A).
Vector3F v0A = poseA.ToWorldPosition(shapeA.InnerPoint * scaleA);
Vector3F v0B = poseB.ToWorldPosition(shapeB.InnerPoint * scaleB);
v0 = v0B - v0A;
// If v0 == origin then we have contact.
if (v0.IsNumericallyZero)
{
// The inner points overlap. Probably there are two objects centered on the same point.
contactSet.HaveContact = true;
if (type == CollisionQueryType.Boolean)
return;
// Choose a v0 different from Zero. Any direction is ok.
// The point should still be in the Minkowski difference.
v0.X = CollisionDetection.Epsilon / 10;
}
// Call MPR in iteration until the MPR ray has converged.
int iterationCount = 0;
const int iterationLimit = 10;
Vector3F oldMprRay;
// Use a temporary contact set.
var testContactSet = ContactSet.Create(collisionObjectA, collisionObjectB);
testContactSet.IsPerturbationTestAllowed = contactSet.IsPerturbationTestAllowed;
testContactSet.PreferredNormal = contactSet.PreferredNormal;
Contact oldContact = null;
do
{
oldMprRay = v0;
if (iterationCount == 0)
oldMprRay.TryNormalize();
// Call MPR. v0 of the next iteration is simply -returned portal normal.
Debug.Assert(testContactSet.Count == 0 || testContactSet.Count == 1, "testContactSet in MPR should have 0 or 1 contacts.");
Debug.Assert(testContactSet.Count == 0 || testContactSet[0] == oldContact);
testContactSet.Clear();
// Because of numerical problems (for example with long thin ellipse vs. capsule)
// it is possible that the last iteration was a contact but in this iteration
// no contact is found. Therefore we also reset the HaveContact flag to avoid
// an end result where HaveContact is set but no Contact is in the ContactSet.
testContactSet.HaveContact = false;
v0 = -DoMpr(type, testContactSet, v0);
if (testContactSet.Count > 0)
{
var newContact = testContactSet[0];
if (oldContact != null)
{
if (oldContact.PenetrationDepth < newContact.PenetrationDepth)
{
// The new penetration depth is larger then the old penetration depth.
// In this case we keep the old contact.
// This can happen for nearly parallel boxes. First we get a good contact.
// Then we get a contact another side. Normal has changed 90�. The new
// penetration depth can be nearly the whole box side length :-(.
newContact.Recycle();
testContactSet[0] = oldContact;
break;
}
}
if (newContact != oldContact)
{
if (oldContact != null)
oldContact.Recycle();
oldContact = newContact;
}
}
iterationCount++;
} while (testContactSet.HaveContact // Separation? - No contact which we could refine.
&& iterationCount < iterationLimit // Iteration limit reached?
&& v0 != Vector3F.Zero // Is normal useful to go on?
&& !Vector3F.AreNumericallyEqual(-v0, oldMprRay, CollisionDetection.Epsilon));
// Normal hasn't converged yet?
if (testContactSet.Count > 0)
{
// Recycle oldContact if not used.
if (testContactSet[0] != oldContact)
{
if (oldContact != null)
{
oldContact.Recycle();
oldContact = null;
}
}
}
if (CollisionDetection.FullContactSetPerFrame
&& type == CollisionQueryType.Contacts
&& testContactSet.Count > 0
&& contactSet.Count < 3)
{
// Try to find full contact set.
var wrapper = TestMethodWrappers.Obtain();
wrapper.OriginalMethod = _doMprMethod;
wrapper.V0 = testContactSet[0].Normal; // The MPR ray will point along the normal of the first contact.
ContactHelper.TestWithPerturbations(
CollisionDetection,
testContactSet,
true,
wrapper.Method);
TestMethodWrappers.Recycle(wrapper);
}
contactSet.HaveContact = testContactSet.HaveContact;
ContactHelper.Merge(contactSet, testContactSet, type, CollisionDetection.ContactPositionTolerance);
// Recycle temporary objects.
testContactSet.Recycle();
}
public override void ComputeCollision(ContactSet contactSet, CollisionQueryType type)
{
IGeometricObject sphereObjectA = contactSet.ObjectA.GeometricObject;
IGeometricObject sphereObjectB = contactSet.ObjectB.GeometricObject;
SphereShape sphereShapeA = sphereObjectA.Shape as SphereShape;
SphereShape sphereShapeB = sphereObjectB.Shape as SphereShape;
// Check if collision objects are spheres.
if (sphereShapeA == null || sphereShapeB == null)
throw new ArgumentException("The contact set must contain sphere shapes.", "contactSet");
Vector3F scaleA = Vector3F.Absolute(sphereObjectA.Scale);
Vector3F scaleB = Vector3F.Absolute(sphereObjectB.Scale);
// Call MPR for non-uniformly scaled spheres.
if (scaleA.X != scaleA.Y || scaleA.Y != scaleA.Z
|| scaleB.X != scaleB.Y || scaleB.Y != scaleB.Z)
{
if (_fallbackAlgorithm == null)
_fallbackAlgorithm = CollisionDetection.AlgorithmMatrix[typeof(ConvexShape), typeof(ConvexShape)];
_fallbackAlgorithm.ComputeCollision(contactSet, type);
return;
}
// Apply uniform scale.
float radiusA = sphereShapeA.Radius * scaleA.X;
float radiusB = sphereShapeB.Radius * scaleB.X;
// Vector from center of A to center of B.
Vector3F centerA = sphereObjectA.Pose.Position;
Vector3F centerB = sphereObjectB.Pose.Position;
Vector3F aToB = centerB - centerA;
float lengthAToB = aToB.Length;
// Check radius of spheres.
float penetrationDepth = radiusA + radiusB - lengthAToB;
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;
}
// ----- Create contact information.
Vector3F normal;
if (Numeric.IsZero(lengthAToB))
{
// Spheres are on the same position, we can choose any normal vector.
// Possibly it would be better to consider the object movement (velocities), but
// it is not important since this case should be VERY rare.
normal = Vector3F.UnitY;
}
else
{
normal = aToB.Normalized;
}
// The contact point lies in the middle of the intersecting volume.
Vector3F position = centerA + normal * (radiusA - penetrationDepth / 2);
// Update contact set.
Contact contact = ContactHelper.CreateContact(contactSet, position, normal, penetrationDepth, false);
ContactHelper.Merge(contactSet, contact, type, CollisionDetection.ContactPositionTolerance);
}
public override void ComputeCollision(ContactSet contactSet, CollisionQueryType type)
{
Debug.Assert(contactSet.Count <= 1, "Ray vs. plane should have at max 1 contact.");
// Object A should be the plane.
// Object B should be the ray.
IGeometricObject planeObject = contactSet.ObjectA.GeometricObject;
IGeometricObject rayObject = contactSet.ObjectB.GeometricObject;
// Swap objects if necessary.
bool swapped = (rayObject.Shape is PlaneShape);
if (swapped)
MathHelper.Swap(ref planeObject, ref rayObject);
PlaneShape planeShape = planeObject.Shape as PlaneShape;
RayShape rayShape = rayObject.Shape as RayShape;
// Check if A is really a plane and B is a ray.
if (planeShape == null || rayShape == null)
throw new ArgumentException("The contact set must contain a plane and a ray.", "contactSet");
// Get transformations.
Vector3F planeScale = planeObject.Scale;
Vector3F rayScale = rayObject.Scale;
Pose rayPose = rayObject.Pose;
Pose planePose = planeObject.Pose;
// Apply scale to plane.
Plane plane = new Plane(planeShape);
plane.Scale(ref planeScale);
// Apply scale to ray and transform ray into local space of plane.
Ray ray = new Ray(rayShape);
ray.Scale(ref rayScale); // Scale ray.
ray.ToWorld(ref rayPose); // Transform ray to world space.
ray.ToLocal(ref planePose); // Transform ray to local space of plane.
// Convert ray into a line segment.
LineSegment segment = new LineSegment { Start = ray.Origin, End = ray.Origin + ray.Direction * ray.Length };
// Check if ray origin is inside the plane. Otherwise call plane vs. ray query.
Vector3F linePoint;
Vector3F planePoint = Vector3F.Zero;
if (Vector3F.Dot(segment.Start, plane.Normal) <= plane.DistanceFromOrigin)
{
// The origin of the ray is below the plane.
linePoint = segment.Start;
contactSet.HaveContact = true;
}
else
{
// The origin of the ray is above the plane.
contactSet.HaveContact = GeometryHelper.GetClosestPoints(plane, segment, out linePoint, out planePoint);
}
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;
}
// ----- Create contact info.
Vector3F position;
float penetrationDepth;
if (contactSet.HaveContact)
{
// We have a contact.
position = planePose.ToWorldPosition(linePoint);
penetrationDepth = (linePoint - segment.Start).Length;
}
else
{
// Closest points, but separated.
position = planePose.ToWorldPosition((planePoint + linePoint) / 2);
penetrationDepth = -(linePoint - planePoint).Length;
}
Vector3F normal = planePose.ToWorldDirection(plane.Normal);
if (swapped)
normal = -normal;
Contact contact = ContactHelper.CreateContact(contactSet, position, normal, penetrationDepth, contactSet.HaveContact);
ContactHelper.Merge(contactSet, contact, type, CollisionDetection.ContactPositionTolerance);
}
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;
Vector3 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;
Vector3 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;
Vector3 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.
Vector3 planePoint = new Vector3(convexPoint.X, height, convexPoint.Z);
planePoint = heightFieldPose.ToWorldPosition(planePoint);
planePoint = convexPose.ToLocalPosition(planePoint);
// Get convex support point in plane normal direction.
Vector3 supportPoint = convex.GetSupportPoint(-normalInConvex, convexScale);
// Get penetration depth.
float penetrationDepth = Vector3.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".
Vector3 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++)
{
Vector3 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 = Vector3.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);
}
}
}
}
// Compute contacts between child shapes of two <see cref="CompositeShape"/>s.
// testXxx are initialized objects which are re-used to avoid a lot of GC garbage.
private void AddChildChildContacts(ContactSet contactSet,
int childIndexA,
int childIndexB,
CollisionQueryType type,
ContactSet testContactSet,
CollisionObject testCollisionObjectA,
TestGeometricObject testGeometricObjectA,
CollisionObject testCollisionObjectB,
TestGeometricObject testGeometricObjectB)
{
CollisionObject collisionObjectA = contactSet.ObjectA;
CollisionObject collisionObjectB = contactSet.ObjectB;
IGeometricObject geometricObjectA = collisionObjectA.GeometricObject;
IGeometricObject geometricObjectB = collisionObjectB.GeometricObject;
CompositeShape shapeA = (CompositeShape)geometricObjectA.Shape;
CompositeShape shapeB = (CompositeShape)geometricObjectB.Shape;
Vector3F scaleA = geometricObjectA.Scale;
Vector3F scaleB = geometricObjectB.Scale;
IGeometricObject childA = shapeA.Children[childIndexA];
IGeometricObject childB = shapeB.Children[childIndexB];
// Find collision algorithm.
CollisionAlgorithm collisionAlgorithm = CollisionDetection.AlgorithmMatrix[childA, childB];
// ----- Set the shape temporarily to the current children.
// (Note: The scaling is either uniform or the child has no local rotation. Therefore, we only
// need to apply the scale of the parent to the scale and translation of the child. We can
// ignore the rotation.)
Debug.Assert(
(scaleA.X == scaleA.Y && scaleA.Y == scaleA.Z) || !childA.Pose.HasRotation,
"CompositeShapeAlgorithm should have thrown an exception. Non-uniform scaling is not supported for rotated children.");
Debug.Assert(
(scaleB.X == scaleB.Y && scaleB.Y == scaleB.Z) || !childB.Pose.HasRotation,
"CompositeShapeAlgorithm should have thrown an exception. Non-uniform scaling is not supported for rotated children.");
var childAPose = childA.Pose;
childAPose.Position *= scaleA; // Apply scaling to local translation.
testGeometricObjectA.Pose = geometricObjectA.Pose * childAPose;
testGeometricObjectA.Shape = childA.Shape;
testGeometricObjectA.Scale = scaleA * childA.Scale; // Apply scaling to local scale.
testCollisionObjectA.SetInternal(collisionObjectA, testGeometricObjectA);
var childBPose = childB.Pose;
childBPose.Position *= scaleB; // Apply scaling to local translation.
testGeometricObjectB.Pose = geometricObjectB.Pose * childBPose;
testGeometricObjectB.Shape = childB.Shape;
testGeometricObjectB.Scale = scaleB * childB.Scale; // Apply scaling to local scale.
testCollisionObjectB.SetInternal(collisionObjectB, testGeometricObjectB);
Debug.Assert(testContactSet.Count == 0, "testContactSet needs to be cleared.");
testContactSet.Reset(testCollisionObjectA, testCollisionObjectB);
if (type == CollisionQueryType.Boolean)
{
// Boolean queries.
collisionAlgorithm.ComputeCollision(testContactSet, CollisionQueryType.Boolean);
contactSet.HaveContact = (contactSet.HaveContact || testContactSet.HaveContact);
}
else
{
// TODO: We could add existing contacts with the same child shape to childContactSet.
// No perturbation test. Most composite shapes are either complex and automatically
// have more contacts. Or they are complex and will not be used for stacking
// where full contact sets would be needed.
testContactSet.IsPerturbationTestAllowed = false;
// Make collision check. As soon as we have found contact, we can make faster
// contact queries instead of closest-point queries.
CollisionQueryType queryType = (contactSet.HaveContact) ? CollisionQueryType.Contacts : type;
collisionAlgorithm.ComputeCollision(testContactSet, queryType);
contactSet.HaveContact = (contactSet.HaveContact || testContactSet.HaveContact);
// Transform contacts into space of composite shape.
// And set the shape feature of the contact.
int numberOfContacts = testContactSet.Count;
for (int i = 0; i < numberOfContacts; i++)
{
Contact contact = testContactSet[i];
contact.PositionALocal = childAPose.ToWorldPosition(contact.PositionALocal);
//if (contact.Lifetime.Ticks == 0) // Currently, all contacts are new, so this check is not necessary.
//{
contact.FeatureA = childIndexA;
//}
contact.PositionBLocal = childBPose.ToWorldPosition(contact.PositionBLocal);
//if (contact.Lifetime.Ticks == 0) // Currently, all contacts are new, so this check is not necessary.
//{
contact.FeatureB = childIndexB;
//}
}
// Merge child contacts.
ContactHelper.Merge(contactSet, testContactSet, type, CollisionDetection.ContactPositionTolerance);
}
}
/// <summary> /// Computes the collision. - This method should only be used by /// <see cref="CollisionAlgorithm"/> instances! /// </summary> /// <param name="contactSet">The contact set.</param> /// <param name="type">The type of collision query.</param> /// <remarks> /// <para> /// This method does the real work. It is called from the other public methods of a /// <see cref="CollisionAlgorithm"/>. Also, if one <see cref="CollisionAlgorithm"/> uses another /// <see cref="CollisionAlgorithm"/> internally, this method should be called directly instead /// of <see cref="CollisionAlgorithm.UpdateClosestPoints"/> or /// <see cref="CollisionAlgorithm.UpdateContacts"/>. /// </para> /// <para> /// <strong>Notes to Inheritors:</strong> This is the central method which has to be implemented /// in derived classes. <paramref name="contactSet"/> is never <see langword="null"/>. This /// method has to add new contact/closest-point info with /// <see cref="ContactHelper.Merge(ContactSet,Contact,CollisionQueryType,float)"/>. It is not /// necessary to remove old contacts. At the beginning of the method /// <see cref="ContactSet.HaveContact"/> in <paramref name="contactSet"/> indicates the result /// of the last narrow phase algorithm that was run on <paramref name="contactSet"/>. This /// method must set <see cref="ContactSet.HaveContact"/> to <see langword="false"/> if it /// doesn't find a contact or to <see langword="true"/> if it finds a contact. /// </para> /// </remarks> public abstract void ComputeCollision(ContactSet contactSet, CollisionQueryType type);
// The parameters 'testXxx' are initialized objects which are re-used to avoid a lot of GC garbage.
private void AddTriangleTriangleContacts(
ContactSet contactSet, int triangleIndexA, int triangleIndexB, CollisionQueryType type,
ContactSet testContactSet, CollisionObject testCollisionObjectA, TestGeometricObject testGeometricObjectA,
TriangleShape testTriangleA, CollisionObject testCollisionObjectB, TestGeometricObject testGeometricObjectB,
TriangleShape testTriangleB)
{
CollisionObject collisionObjectA = contactSet.ObjectA;
CollisionObject collisionObjectB = contactSet.ObjectB;
IGeometricObject geometricObjectA = collisionObjectA.GeometricObject;
IGeometricObject geometricObjectB = collisionObjectB.GeometricObject;
TriangleMeshShape triangleMeshShapeA = (TriangleMeshShape)geometricObjectA.Shape;
Triangle triangleA = triangleMeshShapeA.Mesh.GetTriangle(triangleIndexA);
TriangleMeshShape triangleMeshShapeB = (TriangleMeshShape)geometricObjectB.Shape;
Triangle triangleB = triangleMeshShapeB.Mesh.GetTriangle(triangleIndexB);
Pose poseA = geometricObjectA.Pose;
Pose poseB = geometricObjectB.Pose;
Vector3F scaleA = geometricObjectA.Scale;
Vector3F scaleB = geometricObjectB.Scale;
// Apply SRT.
Triangle transformedTriangleA;
transformedTriangleA.Vertex0 = poseA.ToWorldPosition(triangleA.Vertex0 * scaleA);
transformedTriangleA.Vertex1 = poseA.ToWorldPosition(triangleA.Vertex1 * scaleA);
transformedTriangleA.Vertex2 = poseA.ToWorldPosition(triangleA.Vertex2 * scaleA);
Triangle transformedTriangleB;
transformedTriangleB.Vertex0 = poseB.ToWorldPosition(triangleB.Vertex0 * scaleB);
transformedTriangleB.Vertex1 = poseB.ToWorldPosition(triangleB.Vertex1 * scaleB);
transformedTriangleB.Vertex2 = poseB.ToWorldPosition(triangleB.Vertex2 * scaleB);
// Make super-fast boolean check first. This is redundant if we have to compute
// a contact with SAT below. But in stochastic benchmarks it seems to be 10% faster.
bool haveContact = GeometryHelper.HaveContact(ref transformedTriangleA, ref transformedTriangleB);
if (type == CollisionQueryType.Boolean)
{
contactSet.HaveContact = (contactSet.HaveContact || haveContact);
return;
}
if (haveContact)
{
// Make sure the scaled triangles have the correct normal.
// (A negative scale changes the normal/winding order. See unit test in TriangleTest.cs.)
if (scaleA.X * scaleA.Y * scaleA.Z < 0)
MathHelper.Swap(ref transformedTriangleA.Vertex0, ref transformedTriangleA.Vertex1);
if (scaleB.X * scaleB.Y * scaleB.Z < 0)
MathHelper.Swap(ref transformedTriangleB.Vertex0, ref transformedTriangleB.Vertex1);
// Compute contact.
Vector3F position, normal;
float penetrationDepth;
haveContact = TriangleTriangleAlgorithm.GetContact(
ref transformedTriangleA, ref transformedTriangleB,
!triangleMeshShapeA.IsTwoSided, !triangleMeshShapeB.IsTwoSided,
out position, out normal, out penetrationDepth);
if (haveContact)
{
contactSet.HaveContact = true;
// In deep interpenetrations we might get no contact (penDepth = NaN).
if (!Numeric.IsNaN(penetrationDepth))
{
Contact contact = ContactHelper.CreateContact(contactSet, position, normal, penetrationDepth, false);
contact.FeatureA = triangleIndexA;
contact.FeatureB = triangleIndexB;
ContactHelper.Merge(contactSet, contact, type, CollisionDetection.ContactPositionTolerance);
}
return;
}
// We might come here if the boolean test reports contact but the SAT test
// does not because of numerical errors.
}
Debug.Assert(!haveContact);
if (type == CollisionQueryType.Contacts)
return;
Debug.Assert(type == CollisionQueryType.ClosestPoints);
if (contactSet.HaveContact)
{
// These triangles are separated but other parts of the meshes touches.
// --> Abort.
return;
}
// We do not have a specialized triangle-triangle closest points algorithm.
// Fall back to the default algorithm (GJK).
// Initialize temporary test contact set and test objects.
// Note: We assume the triangle-triangle does not care about front/back faces.
testTriangleA.Vertex0 = transformedTriangleA.Vertex0;
testTriangleA.Vertex1 = transformedTriangleA.Vertex1;
testTriangleA.Vertex2 = transformedTriangleA.Vertex2;
testGeometricObjectA.Shape = testTriangleA;
Debug.Assert(testGeometricObjectA.Scale == Vector3F.One);
Debug.Assert(testGeometricObjectA.Pose == Pose.Identity);
testCollisionObjectA.SetInternal(collisionObjectA, testGeometricObjectA);
testTriangleB.Vertex0 = transformedTriangleB.Vertex0;
testTriangleB.Vertex1 = transformedTriangleB.Vertex1;
testTriangleB.Vertex2 = transformedTriangleB.Vertex2;
testGeometricObjectB.Shape = testTriangleB;
Debug.Assert(testGeometricObjectB.Scale == Vector3F.One);
Debug.Assert(testGeometricObjectB.Pose == Pose.Identity);
testCollisionObjectB.SetInternal(collisionObjectB, testGeometricObjectB);
Debug.Assert(testContactSet.Count == 0, "testContactSet needs to be cleared.");
testContactSet.Reset(testCollisionObjectA, testCollisionObjectB);
testContactSet.IsPerturbationTestAllowed = false;
_triangleTriangleAlgorithm.ComputeCollision(testContactSet, type);
// Note: We expect no contact but because of numerical differences the triangle-triangle
// algorithm could find a shallow surface contact.
contactSet.HaveContact = (contactSet.HaveContact || testContactSet.HaveContact);
#region ----- Merge testContactSet into contactSet -----
if (testContactSet.Count > 0)
{
// Set the shape feature of the new contacts.
int numberOfContacts = testContactSet.Count;
for (int i = 0; i < numberOfContacts; i++)
{
Contact contact = testContactSet[i];
//if (contact.Lifetime.Ticks == 0) // Currently, this check is not necessary because triangleSet does not contain old contacts.
//{
contact.FeatureA = triangleIndexA;
contact.FeatureB = triangleIndexB;
//}
}
// Merge the contact info.
ContactHelper.Merge(contactSet, testContactSet, type, CollisionDetection.ContactPositionTolerance);
}
#endregion
}
// Compute contacts between a shape and the child shapes of a <see cref="CompositeShape"/>.
// testXxx are initialized objects which are re-used to avoid a lot of GC garbage.
private void AddChildContacts(ContactSet contactSet,
bool swapped,
int childIndex,
CollisionQueryType type,
ContactSet testContactSet,
CollisionObject testCollisionObject,
TestGeometricObject testGeometricObject)
{
// This method is taken from CompositeShapeAlgorithm.cs and slightly modified. Keep changes
// in sync with CompositeShapeAlgorithm.cs!
// !!! Object A should be the composite. - This is different then in ComputeContacts() above!!!
CollisionObject collisionObjectA = (swapped) ? contactSet.ObjectB : contactSet.ObjectA;
CollisionObject collisionObjectB = (swapped) ? contactSet.ObjectA : contactSet.ObjectB;
IGeometricObject geometricObjectA = collisionObjectA.GeometricObject;
IGeometricObject geometricObjectB = collisionObjectB.GeometricObject;
Vector3 scaleA = geometricObjectA.Scale;
IGeometricObject childA = ((CompositeShape)geometricObjectA.Shape).Children[childIndex];
// Find collision algorithm.
CollisionAlgorithm collisionAlgorithm = CollisionDetection.AlgorithmMatrix[childA, geometricObjectB];
// ----- Set the shape temporarily to the current child.
// (Note: The scaling is either uniform or the child has no local rotation. Therefore, we only
// need to apply the scale of the parent to the scale and translation of the child. We can
// ignore the rotation.)
Debug.Assert(
(scaleA.X == scaleA.Y && scaleA.Y == scaleA.Z) || !childA.Pose.HasRotation,
"CompositeShapeAlgorithm should have thrown an exception. Non-uniform scaling is not supported for rotated children.");
var childPose = childA.Pose;
childPose.Position *= scaleA; // Apply scaling to local translation.
testGeometricObject.Pose = geometricObjectA.Pose * childPose;
testGeometricObject.Shape = childA.Shape;
testGeometricObject.Scale = scaleA * childA.Scale; // Apply scaling to local scale.
testCollisionObject.SetInternal(collisionObjectA, testGeometricObject);
// Create a temporary contact set.
// (ObjectA and ObjectB should have the same order as in contactSet; otherwise we couldn't
// simply merge them.)
Debug.Assert(testContactSet.Count == 0, "testContactSet needs to be cleared.");
if (swapped)
{
testContactSet.Reset(collisionObjectB, testCollisionObject);
}
else
{
testContactSet.Reset(testCollisionObject, collisionObjectB);
}
if (type == CollisionQueryType.Boolean)
{
// Boolean queries.
collisionAlgorithm.ComputeCollision(testContactSet, CollisionQueryType.Boolean);
contactSet.HaveContact = (contactSet.HaveContact || testContactSet.HaveContact);
}
else
{
// No perturbation test. Most composite shapes are either complex and automatically
// have more contacts. Or they are complex and will not be used for stacking
// where full contact sets would be needed.
testContactSet.IsPerturbationTestAllowed = false;
// Make collision check. As soon as we have found contact, we can make faster
// contact queries instead of closest-point queries.
CollisionQueryType queryType = (contactSet.HaveContact) ? CollisionQueryType.Contacts : type;
collisionAlgorithm.ComputeCollision(testContactSet, queryType);
contactSet.HaveContact = (contactSet.HaveContact || testContactSet.HaveContact);
// Transform contacts into space of composite shape.
// And set the shape feature of the contact.
int numberOfContacts = testContactSet.Count;
for (int i = 0; i < numberOfContacts; i++)
{
Contact contact = testContactSet[i];
if (swapped)
{
contact.PositionBLocal = childPose.ToWorldPosition(contact.PositionBLocal);
contact.FeatureB = childIndex;
}
else
{
contact.PositionALocal = childPose.ToWorldPosition(contact.PositionALocal);
contact.FeatureA = childIndex;
}
}
// Merge child contacts.
ContactHelper.Merge(contactSet, testContactSet, type, CollisionDetection.ContactPositionTolerance);
}
}
public override void ComputeCollision(ContactSet contactSet, CollisionQueryType type)
{
contactSet.HaveContact = false;
}
public override void ComputeCollision(ContactSet contactSet, CollisionQueryType type)
{
if (type == CollisionQueryType.ClosestPoints)
{
// Just use normal composite shape algorithm.
_compositeAlgorithm.ComputeCollision(contactSet, type);
return;
}
Debug.Assert(type != CollisionQueryType.ClosestPoints, "Closest point queries should have already been handled!");
// Composite = A, Ray = B
IGeometricObject compositeObject = contactSet.ObjectA.GeometricObject;
IGeometricObject rayObject = contactSet.ObjectB.GeometricObject;
// Object A should be the composite, swap objects if necessary.
bool swapped = (compositeObject.Shape is RayShape);
if (swapped)
{
MathHelper.Swap(ref rayObject, ref compositeObject);
}
RayShape rayShape = rayObject.Shape as RayShape;
CompositeShape compositeShape = compositeObject.Shape as CompositeShape;
// Check if shapes are correct.
if (rayShape == null || compositeShape == null)
{
throw new ArgumentException("The contact set must contain a ray and a composite shape.", "contactSet");
}
// Assume no contact.
contactSet.HaveContact = false;
// Get transformations.
Vector3 rayScale = rayObject.Scale;
Pose rayPose = rayObject.Pose;
Vector3 compositeScale = compositeObject.Scale;
Pose compositePose = compositeObject.Pose;
// Check if transforms are supported.
// Same check for object B.
if (compositeShape != null &&
(compositeScale.X != compositeScale.Y || compositeScale.Y != compositeScale.Z) &&
compositeShape.Children.Any(child => child.Pose.HasRotation)) // Note: Any() creates garbage, but non-uniform scalings should not be used anyway.
{
throw new NotSupportedException("Computing collisions for composite shapes with non-uniform scaling and rotated children is not supported.");
}
// ----- A few fixed objects which are reused to avoid GC garbage.
var testCollisionObject = ResourcePools.TestCollisionObjects.Obtain();
var testGeometricObject = TestGeometricObject.Create();
// Create a test contact set and initialize with dummy objects.
// (The actual collision objects are set below.)
var testContactSet = ContactSet.Create(testCollisionObject, contactSet.ObjectB); // Dummy arguments! They are changed later.
// Scale ray and transform ray to local unscaled space of composite.
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 compositePose); // Transform ray to local space of composite.
var inverseCompositeScale = Vector3.One / compositeScale;
ray.Scale(ref inverseCompositeScale);
try
{
if (compositeShape.Partition != null)
{
foreach (var childIndex in compositeShape.Partition.GetOverlaps(ray))
{
if (type == CollisionQueryType.Boolean && contactSet.HaveContact)
{
break; // We can abort early.
}
AddChildContacts(
contactSet,
swapped,
childIndex,
type,
testContactSet,
testCollisionObject,
testGeometricObject);
}
}
else
{
var rayDirectionInverse = new Vector3(
1 / ray.Direction.X,
1 / ray.Direction.Y,
1 / ray.Direction.Z);
float epsilon = Numeric.EpsilonF * (1 + compositeObject.Aabb.Extent.Length);
// Go through list of children and find contacts.
int numberOfChildGeometries = compositeShape.Children.Count;
for (int i = 0; i < numberOfChildGeometries; i++)
{
IGeometricObject child = compositeShape.Children[i];
if (GeometryHelper.HaveContact(child.Shape.GetAabb(child.Scale, child.Pose), ray.Origin, rayDirectionInverse, ray.Length, epsilon))
{
AddChildContacts(
contactSet,
swapped,
i,
type,
testContactSet,
testCollisionObject,
testGeometricObject);
// We have contact and stop for boolean queries.
if (contactSet.HaveContact && type == CollisionQueryType.Boolean)
{
break;
}
}
}
}
}
finally
{
Debug.Assert(compositeObject.Shape == compositeShape, "Shape was altered and not restored.");
testContactSet.Recycle();
ResourcePools.TestCollisionObjects.Recycle(testCollisionObject);
testGeometricObject.Recycle();
}
}
public override void ComputeCollision(ContactSet contactSet, CollisionQueryType type)
{
// Ray vs. box has at max 1 contact.
Debug.Assert(contactSet.Count <= 1);
// Object A should be the ray.
// Object B should be the box.
IGeometricObject rayObject = contactSet.ObjectA.GeometricObject;
IGeometricObject boxObject = contactSet.ObjectB.GeometricObject;
// Swap objects if necessary.
bool swapped = (boxObject.Shape is RayShape);
if (swapped)
MathHelper.Swap(ref rayObject, ref boxObject);
RayShape rayShape = rayObject.Shape as RayShape;
BoxShape boxShape = boxObject.Shape as BoxShape;
// Check if shapes are correct.
if (rayShape == null || boxShape == null)
throw new ArgumentException("The contact set must contain a ray and a box.", "contactSet");
// Call line segment vs. box for closest points queries.
if (type == CollisionQueryType.ClosestPoints)
{
// Find point on ray closest to the box.
// Call GJK.
_gjk.ComputeCollision(contactSet, type);
if (contactSet.HaveContact == false)
return;
// Otherwise compute 1 contact ...
// GJK result is invalid for penetration.
foreach (var contact in contactSet)
contact.Recycle();
contactSet.Clear();
}
// Assume no contact.
contactSet.HaveContact = false;
// Get transformations.
Vector3F rayScale = rayObject.Scale;
Vector3F boxScale = Vector3F.Absolute(boxObject.Scale);
Pose rayPose = rayObject.Pose;
Pose boxPose = boxObject.Pose;
// Apply scale to box.
Vector3F boxExtent = boxShape.Extent * boxScale;
// See SOLID and Bergen: "Collision Detection in Interactive 3D Environments", p. 75.
// Note: Compute in box local space.
// Apply scale to ray and transform to local space of box.
Ray rayWorld = new Ray(rayShape);
rayWorld.Scale(ref rayScale); // Apply scale to ray.
rayWorld.ToWorld(ref rayPose); // Transform ray to world space.
Ray ray = rayWorld;
ray.ToLocal(ref boxPose); // Transform ray from world to local space.
uint startOutcode = GeometryHelper.GetOutcode(boxExtent, ray.Origin);
uint endOutcode = GeometryHelper.GetOutcode(boxExtent, ray.Origin + ray.Direction * ray.Length);
if ((startOutcode & endOutcode) != 0)
{
// A face of the box is a separating plane.
return;
}
// Assertion: The ray can intersect with the box but may not...
float λEnter = 0; // ray parameter where ray enters box
float λExit = 1; // ray parameter where ray exits box
uint bit = 1;
Vector3F r = ray.Direction * ray.Length; // ray vector
Vector3F halfExtent = 0.5f * boxExtent; // Box half-extent vector.
Vector3F normal = Vector3F.Zero; // normal vector
for (int i = 0; i < 3; i++)
{
if ((startOutcode & bit) != 0)
{
// Intersection is an entering point.
float λ = (-ray.Origin[i] - halfExtent[i]) / r[i];
if (λEnter < λ)
{
λEnter = λ;
normal = new Vector3F();
normal[i] = 1;
}
}
else if ((endOutcode & bit) != 0)
{
// Intersection is an exciting point.
float λ = (-ray.Origin[i] - halfExtent[i]) / r[i];
if (λExit > λ)
λExit = λ;
}
bit <<= 1;
if ((startOutcode & bit) != 0)
{
// Intersection is an entering point.
float λ = (-ray.Origin[i] + halfExtent[i]) / r[i];
if (λEnter < λ)
{
λEnter = λ;
normal = new Vector3F();
normal[i] = -1;
}
}
else if ((endOutcode & bit) != 0)
{
// Intersection is an exciting point.
float λ = (-ray.Origin[i] + halfExtent[i]) / r[i];
if (λExit > λ)
λExit = λ;
}
bit <<= 1;
}
if (λEnter <= λExit)
{
// The ray intersects the box.
contactSet.HaveContact = true;
if (type == CollisionQueryType.Boolean)
return;
float penetrationDepth = λEnter * ray.Length;
if (normal == Vector3F.Zero)
normal = Vector3F.UnitX;
// Create contact info.
Vector3F position = rayWorld.Origin + rayWorld.Direction * penetrationDepth;
normal = boxPose.ToWorldDirection(normal);
if (swapped)
normal = -normal;
// Update contact set.
Contact contact = ContactHelper.CreateContact(contactSet, position, normal, penetrationDepth, true);
ContactHelper.Merge(contactSet, contact, type, CollisionDetection.ContactPositionTolerance);
}
}
private void ComputeLineVsOther(ContactSet contactSet, CollisionQueryType type, bool objectAIsLine)
{
CollisionObject collisionObjectA = contactSet.ObjectA;
CollisionObject collisionObjectB = contactSet.ObjectB;
IGeometricObject geometricObjectA = collisionObjectA.GeometricObject;
IGeometricObject geometricObjectB = collisionObjectB.GeometricObject;
Shape shapeA = geometricObjectA.Shape;
Shape shapeB = geometricObjectB.Shape;
Debug.Assert(
shapeA is LineShape && !(shapeB is LineShape) ||
shapeB is LineShape && !(shapeA is LineShape),
"LineAlgorithm.ComputeLineVsOther should only be called for a line and another shape.");
CollisionObject lineCollisionObject;
IGeometricObject lineGeometricObject;
IGeometricObject otherGeometricObject;
LineShape lineShape;
Shape otherShape;
if (objectAIsLine)
{
lineCollisionObject = collisionObjectA;
lineGeometricObject = geometricObjectA;
lineShape = (LineShape)shapeA;
otherGeometricObject = geometricObjectB;
otherShape = shapeB;
}
else
{
lineCollisionObject = collisionObjectB;
lineGeometricObject = geometricObjectB;
lineShape = (LineShape)shapeB;
otherGeometricObject = geometricObjectA;
otherShape = shapeA;
}
// Apply scaling to line.
Line line = new Line(lineShape);
Vector3 lineScale = lineGeometricObject.Scale;
line.Scale(ref lineScale);
// Step 1: Get any bounding sphere that encloses the other object.
Aabb aabb = otherGeometricObject.Aabb;
Vector3 center = (aabb.Minimum + aabb.Maximum) / 2;
float radius = (aabb.Maximum - aabb.Minimum).Length; // A large safe radius. (Exact size does not matter.)
// Step 2: Get the closest point of line vs. center.
// All computations in local space of the line.
Vector3 closestPointOnLine;
Pose linePose = lineGeometricObject.Pose;
GeometryHelper.GetClosestPoint(line, linePose.ToLocalPosition(center), out closestPointOnLine);
// Step 3: Crop the line to a line segment that will contain the closest point.
var lineSegment = ResourcePools.LineSegmentShapes.Obtain();
lineSegment.Start = closestPointOnLine - line.Direction * radius;
lineSegment.End = closestPointOnLine + line.Direction * radius;
// Use temporary test objects.
var testGeometricObject = TestGeometricObject.Create();
testGeometricObject.Shape = lineSegment;
testGeometricObject.Scale = Vector3.One;
testGeometricObject.Pose = linePose;
var testCollisionObject = ResourcePools.TestCollisionObjects.Obtain();
testCollisionObject.SetInternal(lineCollisionObject, testGeometricObject);
var testContactSet = objectAIsLine ? ContactSet.Create(testCollisionObject, collisionObjectB)
: ContactSet.Create(collisionObjectA, testCollisionObject);
testContactSet.IsPerturbationTestAllowed = contactSet.IsPerturbationTestAllowed;
// Step 4: Call another collision algorithm.
CollisionAlgorithm collisionAlgorithm = CollisionDetection.AlgorithmMatrix[lineSegment, otherShape];
// Step 5: Manually chosen preferred direction for MPR.
// For the MPR we choose the best ray direction ourselves. The ray should be normal
// to the line, otherwise MPR could try to push the line segment out of the other object
// in the line direction - this cannot work for infinite lines.
// Results without a manual MPR ray were ok for normal cases. Problems were only observed
// for cases where the InnerPoints overlap or for deep interpenetrations.
Vector3 v0A = geometricObjectA.Pose.ToWorldPosition(shapeA.InnerPoint * geometricObjectA.Scale);
Vector3 v0B = geometricObjectB.Pose.ToWorldPosition(shapeB.InnerPoint * geometricObjectB.Scale);
Vector3 n = v0B - v0A; // This is the default MPR ray direction.
// Make n normal to the line.
n = n - Vector3.ProjectTo(n, linePose.ToWorldDirection(lineShape.Direction));
if (!n.TryNormalize())
{
n = lineShape.Direction.Orthonormal1;
}
testContactSet.PreferredNormal = n;
collisionAlgorithm.ComputeCollision(testContactSet, type);
if (testContactSet.HaveContact)
{
contactSet.HaveContact = true;
}
ContactHelper.Merge(contactSet, testContactSet, type, CollisionDetection.ContactPositionTolerance);
// Recycle temporary objects.
testContactSet.Recycle();
ResourcePools.TestCollisionObjects.Recycle(testCollisionObject);
testGeometricObject.Recycle();
ResourcePools.LineSegmentShapes.Recycle(lineSegment);
}
public override void ComputeCollision(ContactSet contactSet, CollisionQueryType type)
{
// Ray vs. sphere has at max 1 contact.
Debug.Assert(contactSet.Count <= 1);
// Object A should be the ray.
// Object B should be the sphere.
IGeometricObject rayObject = contactSet.ObjectA.GeometricObject;
IGeometricObject sphereObject = contactSet.ObjectB.GeometricObject;
// Swap if necessary.
bool swapped = (sphereObject.Shape is RayShape);
if (swapped)
MathHelper.Swap(ref rayObject, ref sphereObject);
RayShape rayShape = rayObject.Shape as RayShape;
SphereShape sphereShape = sphereObject.Shape as SphereShape;
// Check if shapes are correct.
if (rayShape == null || sphereShape == null)
throw new ArgumentException("The contact set must contain a ray and a sphere.", "contactSet");
// Get transformations.
Vector3F rayScale = rayObject.Scale;
Vector3F sphereScale = Vector3F.Absolute(sphereObject.Scale);
Pose rayPose = rayObject.Pose;
Pose spherePose = sphereObject.Pose;
// Call other algorithm for non-uniformly scaled spheres.
if (sphereScale.X != sphereScale.Y || sphereScale.Y != sphereScale.Z)
{
if (_fallbackAlgorithm == null)
_fallbackAlgorithm = CollisionDetection.AlgorithmMatrix[typeof(RayShape), typeof(ConvexShape)];
_fallbackAlgorithm.ComputeCollision(contactSet, type);
return;
}
// Scale ray and transform ray to world space.
Ray rayWorld = new Ray(rayShape);
rayWorld.Scale(ref rayScale);
rayWorld.ToWorld(ref rayPose);
// Scale sphere.
float sphereRadius = sphereShape.Radius * sphereScale.X;
float sphereRadiusSquared = sphereRadius * sphereRadius;
// Call line segment vs. sphere for closest points queries.
if (type == CollisionQueryType.ClosestPoints || type == CollisionQueryType.Boolean)
{
// ----- Find point on ray closest to the sphere.
// Make a line segment vs. point (sphere center) check.
Debug.Assert(rayWorld.Direction.IsNumericallyNormalized, "The ray direction should be normalized.");
LineSegment segment = new LineSegment(rayWorld.Origin, rayWorld.Origin + rayWorld.Direction * rayWorld.Length);
Vector3F segmentPoint;
Vector3F sphereCenter = spherePose.Position;
GeometryHelper.GetClosestPoints(segment, sphereCenter, out segmentPoint);
Vector3F normal = sphereCenter - segmentPoint;
float distanceSquared = normal.LengthSquared;
float penetrationDepthSquared = sphereRadiusSquared - distanceSquared;
contactSet.HaveContact = penetrationDepthSquared >= 0;
if (type == CollisionQueryType.Boolean)
return;
if (penetrationDepthSquared <= 0)
{
// Separated or touching objects.
Vector3F position;
if (normal.TryNormalize())
{
Vector3F spherePoint = sphereCenter - normal * sphereRadius;
position = (spherePoint + segmentPoint) / 2;
}
else
{
position = segmentPoint;
normal = Vector3F.UnitY;
}
if (swapped)
normal = -normal;
float penetrationDepth = -((float)Math.Sqrt(distanceSquared) - sphereRadius);
// Update contact set.
Contact contact = ContactHelper.CreateContact(contactSet, position, normal, penetrationDepth, contactSet.HaveContact);
ContactHelper.Merge(contactSet, contact, type, CollisionDetection.ContactPositionTolerance);
return;
}
// Otherwise, we have a penetrating contact.
// Compute 1 contact ...
}
// First assume no contact.
contactSet.HaveContact = false;
// See SOLID and Bergen: "Collision Detection in Interactive 3D Environments", pp. 70.
// Compute in sphere local space.
// Transform ray to local space of sphere.
Ray ray = rayWorld;
ray.ToLocal(ref spherePose);
Vector3F s = ray.Origin; // ray source
Vector3F d = ray.Direction; // ray direction
Vector3F r = d * ray.Length; // ray
float rayLengthSquared = ray.Length * ray.Length; // ||r||²
float δ = -Vector3F.Dot(s, r); // -s∙r
float σ = δ * δ - rayLengthSquared * (s.LengthSquared - sphereRadiusSquared);
if (σ >= 0)
{
// The infinite ray intersects.
float sqrtσ = (float)Math.Sqrt(σ);
float λ2 = (δ + sqrtσ) /* / rayLengthSquared */; // Division can be ignored. Only sign is relevant.
if (λ2 >= 0)
{
// Ray shoots to sphere.
float λ1 = (δ - sqrtσ) / rayLengthSquared;
if (λ1 <= 1)
{
// Ray hits sphere.
contactSet.HaveContact = true;
Debug.Assert(type != CollisionQueryType.Boolean); // Was handled before.
float penetrationDepth;
Vector3F normal;
if (λ1 > 0)
{
// λ1 shows the entry point. λ2 shows the exit point.
penetrationDepth = λ1 * ray.Length; // Distance from ray origin to entry point (hit).
normal = -(s + r * λ1); // Entry point (hit).
}
else
{
// Ray origin is in the sphere.
penetrationDepth = 0;
normal = -s;
}
Vector3F position = rayWorld.Origin + rayWorld.Direction * penetrationDepth;
normal = spherePose.ToWorldDirection(normal);
if (!normal.TryNormalize())
normal = Vector3F.UnitY;
if (swapped)
normal = -normal;
// Update contact set.
Contact contact = ContactHelper.CreateContact(contactSet, position, normal, penetrationDepth, true);
ContactHelper.Merge(contactSet, contact, type, CollisionDetection.ContactPositionTolerance);
}
}
}
}
/// <summary>
/// Computes the collision between line vs. line.
/// </summary>
/// <param name="contactSet">The contact set.</param>
/// <param name="type">The type of collision query.</param>
private void ComputeLineVsLine(ContactSet contactSet, CollisionQueryType type)
{
IGeometricObject objectA = contactSet.ObjectA.GeometricObject;
IGeometricObject objectB = contactSet.ObjectB.GeometricObject;
Debug.Assert(objectA.Shape is LineShape && objectB.Shape is LineShape, "LineAlgorithm.ComputeLineVsLine should only be called for 2 line shapes.");
Debug.Assert(contactSet.Count <= 1, "Two lines should have at max 1 contact point.");
// Get transformations.
Vector3 scaleA = objectA.Scale;
Vector3 scaleB = objectB.Scale;
Pose poseA = objectA.Pose;
Pose poseB = objectB.Pose;
// Create two line objects in world space.
var lineA = new Line((LineShape)objectA.Shape);
lineA.Scale(ref scaleA);
lineA.ToWorld(ref poseA);
var lineB = new Line((LineShape)objectB.Shape);
lineB.Scale(ref scaleB);
lineB.ToWorld(ref poseB);
// Get closest points.
Vector3 pointA;
Vector3 pointB;
contactSet.HaveContact = GeometryHelper.GetClosestPoints(lineA, lineB, out pointA, out pointB);
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;
}
// Create contact information.
Vector3 position = (pointA + pointB) / 2;
Vector3 normal = pointB - pointA;
float length = normal.Length;
if (Numeric.IsZero(length))
{
// Create normal from cross product of both lines.
normal = Vector3.Cross(lineA.Direction, lineB.Direction);
if (!normal.TryNormalize())
{
normal = Vector3.UnitY;
}
}
else
{
// Normalize vector
normal = normal / length;
}
Contact contact = ContactHelper.CreateContact(contactSet, position, normal, -length, false);
ContactHelper.Merge(contactSet, contact, type, CollisionDetection.ContactPositionTolerance);
}
public override void ComputeCollision(ContactSet contactSet, CollisionQueryType type)
{
// Invoke GJK for closest points.
if (type == CollisionQueryType.ClosestPoints)
throw new GeometryException("This collision algorithm cannot handle closest-point queries. Use GJK instead.");
//if (UseMpr)
//{
// if (_mpr == null)
// _mpr = new MinkowskiPortalRefinement(CollisionDetection);
// _mpr.ComputeCollision(contactSet, type);
// return;
//}
CollisionObject collisionObjectA = contactSet.ObjectA;
CollisionObject collisionObjectB = contactSet.ObjectB;
IGeometricObject geometricObjectA = collisionObjectA.GeometricObject;
IGeometricObject geometricObjectB = collisionObjectB.GeometricObject;
TriangleShape triangleShapeA = geometricObjectA.Shape as TriangleShape;
TriangleShape triangleShapeB = geometricObjectB.Shape as TriangleShape;
// Check if collision objects shapes are correct.
if (triangleShapeA == null || triangleShapeB == null)
throw new ArgumentException("The contact set must contain triangle shapes.", "contactSet");
Vector3F scaleA = Vector3F.Absolute(geometricObjectA.Scale);
Vector3F scaleB = Vector3F.Absolute(geometricObjectB.Scale);
Pose poseA = geometricObjectA.Pose;
Pose poseB = geometricObjectB.Pose;
// Get triangles in world space.
Triangle triangleA;
triangleA.Vertex0 = poseA.ToWorldPosition(triangleShapeA.Vertex0 * scaleA);
triangleA.Vertex1 = poseA.ToWorldPosition(triangleShapeA.Vertex1 * scaleA);
triangleA.Vertex2 = poseA.ToWorldPosition(triangleShapeA.Vertex2 * scaleA);
Triangle triangleB;
triangleB.Vertex0 = poseB.ToWorldPosition(triangleShapeB.Vertex0 * scaleB);
triangleB.Vertex1 = poseB.ToWorldPosition(triangleShapeB.Vertex1 * scaleB);
triangleB.Vertex2 = poseB.ToWorldPosition(triangleShapeB.Vertex2 * scaleB);
if (type == CollisionQueryType.Boolean)
{
contactSet.HaveContact = GeometryHelper.HaveContact(ref triangleA, ref triangleB);
return;
}
Debug.Assert(type == CollisionQueryType.Contacts, "TriangleTriangleAlgorithm cannot handle closest point queries.");
// Assume no contact.
contactSet.HaveContact = false;
Vector3F position, normal;
float penetrationDepth;
if (!GetContact(ref triangleA, ref triangleB, false, false, out position, out normal, out penetrationDepth))
return;
contactSet.HaveContact = true;
Contact contact = ContactHelper.CreateContact(contactSet, position, normal, penetrationDepth, false);
ContactHelper.Merge(contactSet, contact, type, CollisionDetection.ContactPositionTolerance);
}
// Adds a contact to the contact set.
private void AddContact(ref Vector3F vertex, ref Plane planeWorld, float penetrationDepth, bool swapped, ContactSet contactSet, CollisionQueryType type)
{
Vector3F position = vertex + 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);
}
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 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 boxObject = contactSet.ObjectB.GeometricObject;
// Swap objects if necessary.
bool swapped = (boxObject.Shape is PlaneShape);
if (swapped)
{
MathHelper.Swap(ref planeObject, ref boxObject);
}
PlaneShape planeShape = planeObject.Shape as PlaneShape;
BoxShape boxShape = boxObject.Shape as BoxShape;
// Check if shapes are correct.
if (planeShape == null || boxShape == null)
{
throw new ArgumentException("The contact set must contain a plane and a box shape.", "contactSet");
}
// Get transformations.
Vector3F scalePlane = planeObject.Scale;
Vector3F scaleBox = boxObject.Scale;
Pose posePlane = planeObject.Pose;
Pose poseBox = boxObject.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 posePlane); // Transform plane to world space.
// Transform plane normal to local space of box.
Vector3F planeNormalLocalB = poseBox.ToLocalDirection(planeWorld.Normal);
// Get support vertex nearest to the plane.
Vector3F supportVertexBLocal = boxShape.GetSupportPoint(-planeNormalLocalB, scaleBox);
// Transform support vertex into world space.
Vector3F supportVertexBWorld = poseBox.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;
}
// We have contact.
if (!CollisionDetection.FullContactSetPerFrame || type == CollisionQueryType.ClosestPoints)
{
// Use only the already known contact.
AddContact(ref supportVertexBWorld, ref planeWorld, penetrationDepth, swapped, contactSet, type);
}
else
{
// Apply scale to box extent.
Vector3F boxHalfExtent = boxShape.Extent * scaleBox * 0.5f;
// Check all box vertices against plane.
CheckContact(ref planeWorld, new Vector3F(-boxHalfExtent.X, -boxHalfExtent.Y, -boxHalfExtent.Z), ref poseBox, swapped, contactSet);
CheckContact(ref planeWorld, new Vector3F(-boxHalfExtent.X, -boxHalfExtent.Y, boxHalfExtent.Z), ref poseBox, swapped, contactSet);
CheckContact(ref planeWorld, new Vector3F(-boxHalfExtent.X, boxHalfExtent.Y, -boxHalfExtent.Z), ref poseBox, swapped, contactSet);
CheckContact(ref planeWorld, new Vector3F(-boxHalfExtent.X, boxHalfExtent.Y, boxHalfExtent.Z), ref poseBox, swapped, contactSet);
CheckContact(ref planeWorld, new Vector3F(boxHalfExtent.X, -boxHalfExtent.Y, -boxHalfExtent.Z), ref poseBox, swapped, contactSet);
CheckContact(ref planeWorld, new Vector3F(boxHalfExtent.X, -boxHalfExtent.Y, boxHalfExtent.Z), ref poseBox, swapped, contactSet);
CheckContact(ref planeWorld, new Vector3F(boxHalfExtent.X, boxHalfExtent.Y, -boxHalfExtent.Z), ref poseBox, swapped, contactSet);
CheckContact(ref planeWorld, new Vector3F(boxHalfExtent.X, boxHalfExtent.Y, boxHalfExtent.Z), ref poseBox, swapped, contactSet);
}
}
public override void ComputeCollision(ContactSet contactSet, CollisionQueryType type)
{
// Invoke GJK for closest points.
if (type == CollisionQueryType.ClosestPoints)
throw new GeometryException("This collision algorithm cannot handle closest-point queries. Use GJK instead.");
CollisionObject collisionObjectA = contactSet.ObjectA;
CollisionObject collisionObjectB = contactSet.ObjectB;
IGeometricObject geometricObjectA = collisionObjectA.GeometricObject;
IGeometricObject geometricObjectB = collisionObjectB.GeometricObject;
BoxShape boxA = geometricObjectA.Shape as BoxShape;
BoxShape boxB = geometricObjectB.Shape as BoxShape;
// Check if collision objects shapes are correct.
if (boxA == null || boxB == null)
throw new ArgumentException("The contact set must contain box shapes.", "contactSet");
Vector3F scaleA = Vector3F.Absolute(geometricObjectA.Scale);
Vector3F scaleB = Vector3F.Absolute(geometricObjectB.Scale);
Pose poseA = geometricObjectA.Pose;
Pose poseB = geometricObjectB.Pose;
// We perform the separating axis test in the local space of A.
// The following variables are in local space of A.
// Center of box B.
Vector3F cB = poseA.ToLocalPosition(poseB.Position);
// Orientation matrix of box B
Matrix33F mB = poseA.Orientation.Transposed * poseB.Orientation;
// Absolute of mB.
Matrix33F aMB = Matrix33F.Absolute(mB);
// Half extent vectors of the boxes.
Vector3F eA = 0.5f * boxA.Extent * scaleA;
Vector3F eB = 0.5f * boxB.Extent * scaleB;
// ----- Separating Axis tests
// If the boxes are separated, we immediately return.
// For the case of interpenetration, we store the smallest penetration depth.
float smallestPenetrationDepth = float.PositiveInfinity;
int separatingAxisNumber = 0;
Vector3F normal = Vector3F.UnitX;
bool isNormalInverted = false;
contactSet.HaveContact = false; // Assume no contact.
#region ----- Case 1: Separating Axis: (1, 0, 0) -----
float separation = Math.Abs(cB.X) - (eA.X + eB.X * aMB.M00 + eB.Y * aMB.M01 + eB.Z * aMB.M02);
if (separation > 0)
return;
if (type != CollisionQueryType.Boolean && -separation < smallestPenetrationDepth)
{
normal = Vector3F.UnitX;
smallestPenetrationDepth = -separation;
isNormalInverted = cB.X < 0;
separatingAxisNumber = 1;
}
#endregion
#region ----- Case 2: Separating Axis: (0, 1, 0) -----
separation = Math.Abs(cB.Y) - (eA.Y + eB.X * aMB.M10 + eB.Y * aMB.M11 + eB.Z * aMB.M12);
if (separation > 0)
return;
if (type != CollisionQueryType.Boolean && -separation < smallestPenetrationDepth)
{
normal = Vector3F.UnitY;
smallestPenetrationDepth = -separation;
isNormalInverted = cB.Y < 0;
separatingAxisNumber = 2;
}
#endregion
#region ----- Case 3: Separating Axis: (0, 0, 1) -----
separation = Math.Abs(cB.Z) - (eA.Z + eB.X * aMB.M20 + eB.Y * aMB.M21 + eB.Z * aMB.M22);
if (separation > 0)
return;
if (type != CollisionQueryType.Boolean && -separation < smallestPenetrationDepth)
{
normal = Vector3F.UnitZ;
smallestPenetrationDepth = -separation;
isNormalInverted = cB.Z < 0;
separatingAxisNumber = 3;
}
#endregion
#region ----- Case 4: Separating Axis: OrientationB * (1, 0, 0) -----
float expression = cB.X * mB.M00 + cB.Y * mB.M10 + cB.Z * mB.M20;
separation = Math.Abs(expression) - (eB.X + eA.X * aMB.M00 + eA.Y * aMB.M10 + eA.Z * aMB.M20);
if (separation > 0)
return;
if (type != CollisionQueryType.Boolean && -separation < smallestPenetrationDepth)
{
normal = mB.GetColumn(0);
smallestPenetrationDepth = -separation;
isNormalInverted = expression < 0;
separatingAxisNumber = 4;
}
#endregion
#region ----- Case 5: Separating Axis: OrientationB * (0, 1, 0) -----
expression = cB.X * mB.M01 + cB.Y * mB.M11 + cB.Z * mB.M21;
separation = Math.Abs(expression) - (eB.Y + eA.X * aMB.M01 + eA.Y * aMB.M11 + eA.Z * aMB.M21);
if (separation > 0)
return;
if (type != CollisionQueryType.Boolean && -separation < smallestPenetrationDepth)
{
normal = mB.GetColumn(1);
smallestPenetrationDepth = -separation;
isNormalInverted = expression < 0;
separatingAxisNumber = 5;
}
#endregion
#region ----- Case 6: Separating Axis: OrientationB * (0, 0, 1) -----
expression = cB.X * mB.M02 + cB.Y * mB.M12 + cB.Z * mB.M22;
separation = Math.Abs(expression) - (eB.Z + eA.X * aMB.M02 + eA.Y * aMB.M12 + eA.Z * aMB.M22);
if (separation > 0)
return;
if (type != CollisionQueryType.Boolean && -separation < smallestPenetrationDepth)
{
normal = mB.GetColumn(2);
smallestPenetrationDepth = -separation;
isNormalInverted = expression < 0;
separatingAxisNumber = 6;
}
#endregion
// The next 9 tests are edge-edge cases. The normal vector has to be normalized
// to get the right penetration depth.
// normal = Normalize(edgeA x edgeB)
Vector3F separatingAxis;
float length;
#region ----- Case 7: Separating Axis: (1, 0, 0) x (OrientationB * (1, 0, 0)) -----
expression = cB.Z * mB.M10 - cB.Y * mB.M20;
separation = Math.Abs(expression) - (eA.Y * aMB.M20 + eA.Z * aMB.M10 + eB.Y * aMB.M02 + eB.Z * aMB.M01);
if (separation > 0)
return;
if (type != CollisionQueryType.Boolean)
{
separatingAxis = new Vector3F(0, -mB.M20, mB.M10);
length = separatingAxis.Length;
separation /= length;
if (-separation < smallestPenetrationDepth)
{
normal = separatingAxis / length;
smallestPenetrationDepth = -separation;
isNormalInverted = expression < 0;
separatingAxisNumber = 7;
}
}
#endregion
#region ----- Case 8: Separating Axis: (1, 0, 0) x (OrientationB * (0, 1, 0)) -----
expression = cB.Z * mB.M11 - cB.Y * mB.M21;
separation = Math.Abs(expression) - (eA.Y * aMB.M21 + eA.Z * aMB.M11 + eB.X * aMB.M02 + eB.Z * aMB.M00);
if (separation > 0)
return;
if (type != CollisionQueryType.Boolean)
{
separatingAxis = new Vector3F(0, -mB.M21, mB.M11);
length = separatingAxis.Length;
separation /= length;
if (-separation < smallestPenetrationDepth)
{
normal = separatingAxis / length;
smallestPenetrationDepth = -separation;
isNormalInverted = expression < 0;
separatingAxisNumber = 8;
}
}
#endregion
#region ----- Case 9: Separating Axis: (1, 0, 0) x (OrientationB * (0, 0, 1)) -----
expression = cB.Z * mB.M12 - cB.Y * mB.M22;
separation = Math.Abs(expression) - (eA.Y * aMB.M22 + eA.Z * aMB.M12 + eB.X * aMB.M01 + eB.Y * aMB.M00);
if (separation > 0)
return;
if (type != CollisionQueryType.Boolean)
{
separatingAxis = new Vector3F(0, -mB.M22, mB.M12);
length = separatingAxis.Length;
separation /= length;
if (-separation < smallestPenetrationDepth)
{
normal = separatingAxis / length;
smallestPenetrationDepth = -separation;
isNormalInverted = expression < 0;
separatingAxisNumber = 9;
}
}
#endregion
#region ----- Case 10: Separating Axis: (0, 1, 0) x (OrientationB * (1, 0, 0)) -----
expression = cB.X * mB.M20 - cB.Z * mB.M00;
separation = Math.Abs(expression) - (eA.X * aMB.M20 + eA.Z * aMB.M00 + eB.Y * aMB.M12 + eB.Z * aMB.M11);
if (separation > 0)
return;
if (type != CollisionQueryType.Boolean)
{
separatingAxis = new Vector3F(mB.M20, 0, -mB.M00);
length = separatingAxis.Length;
separation /= length;
if (-separation < smallestPenetrationDepth)
{
normal = separatingAxis / length;
smallestPenetrationDepth = -separation;
isNormalInverted = expression < 0;
separatingAxisNumber = 10;
}
}
#endregion
#region ----- Case 11: Separating Axis: (0, 1, 0) x (OrientationB * (0, 1, 0)) -----
expression = cB.X * mB.M21 - cB.Z * mB.M01;
separation = Math.Abs(expression) - (eA.X * aMB.M21 + eA.Z * aMB.M01 + eB.X * aMB.M12 + eB.Z * aMB.M10);
if (separation > 0)
return;
if (type != CollisionQueryType.Boolean)
{
separatingAxis = new Vector3F(mB.M21, 0, -mB.M01);
length = separatingAxis.Length;
separation /= length;
if (-separation < smallestPenetrationDepth)
{
normal = separatingAxis / length;
smallestPenetrationDepth = -separation;
isNormalInverted = expression < 0;
separatingAxisNumber = 11;
}
}
#endregion
#region ----- Case 12: Separating Axis: (0, 1, 0) x (OrientationB * (0, 0, 1)) -----
expression = cB.X * mB.M22 - cB.Z * mB.M02;
separation = Math.Abs(expression) - (eA.X * aMB.M22 + eA.Z * aMB.M02 + eB.X * aMB.M11 + eB.Y * aMB.M10);
if (separation > 0)
return;
if (type != CollisionQueryType.Boolean)
{
separatingAxis = new Vector3F(mB.M22, 0, -mB.M02);
length = separatingAxis.Length;
separation /= length;
if (-separation < smallestPenetrationDepth)
{
normal = separatingAxis / length;
smallestPenetrationDepth = -separation;
isNormalInverted = expression < 0;
separatingAxisNumber = 12;
}
}
#endregion
#region ----- Case 13: Separating Axis: (0, 0, 1) x (OrientationB * (1, 0, 0)) -----
expression = cB.Y * mB.M00 - cB.X * mB.M10;
separation = Math.Abs(expression) - (eA.X * aMB.M10 + eA.Y * aMB.M00 + eB.Y * aMB.M22 + eB.Z * aMB.M21);
if (separation > 0)
return;
if (type != CollisionQueryType.Boolean)
{
separatingAxis = new Vector3F(-mB.M10, mB.M00, 0);
length = separatingAxis.Length;
separation /= length;
if (-separation < smallestPenetrationDepth)
{
normal = separatingAxis / length;
smallestPenetrationDepth = -separation;
isNormalInverted = expression < 0;
separatingAxisNumber = 13;
}
}
#endregion
#region ----- Case 14: Separating Axis: (0, 0, 1) x (OrientationB * (0, 1, 0)) -----
expression = cB.Y * mB.M01 - cB.X * mB.M11;
separation = Math.Abs(expression) - (eA.X * aMB.M11 + eA.Y * aMB.M01 + eB.X * aMB.M22 + eB.Z * aMB.M20);
if (separation > 0)
return;
if (type != CollisionQueryType.Boolean)
{
separatingAxis = new Vector3F(-mB.M11, mB.M01, 0);
length = separatingAxis.Length;
separation /= length;
if (-separation < smallestPenetrationDepth)
{
normal = separatingAxis / length;
smallestPenetrationDepth = -separation;
isNormalInverted = expression < 0;
separatingAxisNumber = 14;
}
}
#endregion
#region ----- Case 15: Separating Axis: (0, 0, 1) x (OrientationB * (0, 0, 1)) -----
expression = cB.Y * mB.M02 - cB.X * mB.M12;
separation = Math.Abs(expression) - (eA.X * aMB.M12 + eA.Y * aMB.M02 + eB.X * aMB.M21 + eB.Y * aMB.M20);
if (separation > 0)
return;
if (type != CollisionQueryType.Boolean)
{
separatingAxis = new Vector3F(-mB.M12, mB.M02, 0);
length = separatingAxis.Length;
separation /= length;
if (-separation < smallestPenetrationDepth)
{
normal = separatingAxis / length;
smallestPenetrationDepth = -separation;
isNormalInverted = expression < 0;
separatingAxisNumber = 15;
}
}
#endregion
// We have a contact.
contactSet.HaveContact = true;
// HaveContact queries can exit here.
if (type == CollisionQueryType.Boolean)
return;
// Lets find the contact info.
Debug.Assert(smallestPenetrationDepth >= 0, "The smallest penetration depth should be greater than or equal to 0.");
if (isNormalInverted)
normal = -normal;
// Transform normal from local space of A to world space.
Vector3F normalWorld = poseA.ToWorldDirection(normal);
if (separatingAxisNumber > 6)
{
// The intersection was detected by an edge-edge test.
// Get the intersecting edges.
// Separating axes:
// 7 = x edge on A, x edge on B
// 8 = x edge on A, y edge on B
// 9 = x edge on A, Z edge on B
// 10 = y edge on A, x edge on B
// ...
// 15 = z edge on A, z edge on B
var edgeA = boxA.GetEdge((separatingAxisNumber - 7) / 3, normal, scaleA);
var edgeB = boxB.GetEdge((separatingAxisNumber - 7) % 3, Matrix33F.MultiplyTransposed(mB, -normal), scaleB);
edgeB.Start = mB * edgeB.Start + cB;
edgeB.End = mB * edgeB.End + cB;
Vector3F position;
Vector3F dummy;
GeometryHelper.GetClosestPoints(edgeA, edgeB, out position, out dummy);
position = position - normal * (smallestPenetrationDepth / 2); // Position is between the positions of the box surfaces.
// Convert back position from local space of A to world space;
position = poseA.ToWorldPosition(position);
Contact contact = ContactHelper.CreateContact(contactSet, position, normalWorld, smallestPenetrationDepth, false);
ContactHelper.Merge(contactSet, contact, type, CollisionDetection.ContactPositionTolerance);
}
else if (1 <= separatingAxisNumber && separatingAxisNumber <= 6)
{
// The intersection was detected by a face vs. * test.
// The separating axis is perpendicular to a face.
#region ----- Credits -----
// The face vs. * test is based on the algorithm of the Bullet Continuous Collision
// Detection and Physics Library. DigitalRune Geometry contains a new and improved
// implementation of the original algorithm.
//
// The box-box detector in Bullet contains the following remarks:
//
// Box-Box collision detection re-distributed under the ZLib license with permission from Russell L. Smith
// Original version is from Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith.
// All rights reserved. Email: [email protected] Web: www.q12.org
//
// Bullet Continuous Collision Detection and Physics Library
// Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
//
// This software is provided 'as-is', without any express or implied warranty.
// In no event will the authors be held liable for any damages arising from the use of this software.
// Permission is granted to anyone to use this software for any purpose,
// including commercial applications, and to alter it and redistribute it freely,
// subject to the following restrictions:
//
// 1. The origin of this software must not be misrepresented; you must not claim that you wrote the
// original software. If you use this software in a product, an acknowledgment in the product
// documentation would be appreciated but is not required.
// 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being
// the original software.
// 3. This notice may not be removed or altered from any source distribution.
#endregion
// We define the face perpendicular to the separating axis to be the "reference face".
// The face of the other box closest to the reference face is called the "incident face".
// Accordingly, we will call the box containing the reference face the "reference box" and
// the box containing the incident face the "incident box".
//
// We will transform the incident face into the 2D space of reference face. Then we will
// clip the incident face against the reference face. The polygon resulting from the
// intersection will be transformed back into world space and the points of the polygon will
// be the candidates for the contact points.
Pose poseR; // Pose of reference box.
Pose poseI; // Pose of incident box.
Vector3F boxExtentR; // Half extent of reference box.
Vector3F boxExtentI; // Half extent of incident box.
// Contact normal (= normal of reference face) in world space.
if (separatingAxisNumber <= 3)
{
poseR = poseA;
poseI = poseB;
boxExtentR = eA;
boxExtentI = eB;
isNormalInverted = false;
}
else
{
poseR = poseB;
poseI = poseA;
boxExtentR = eB;
boxExtentI = eA;
normalWorld = -normalWorld;
isNormalInverted = true;
}
// Contact normal in local space of incident box.
Vector3F normalI = poseI.ToLocalDirection(normalWorld);
Vector3F absNormal = normalI;
absNormal.Absolute();
Vector3F xAxisInc, yAxisInc; // The basis of the incident-face space.
float absFaceOffsetI; // The offset of the incident face to the center of the box.
Vector2F faceExtentI; // The half extent of the incident face.
Vector3F faceNormal; // The normal of the incident face in world space.
float faceDirection; // A value indicating the direction of the incident face.
// Find the largest component of the normal. The largest component indicates which face is
// the incident face.
switch (Vector3F.Absolute(normalI).IndexOfLargestComponent)
{
case 0:
faceExtentI.X = boxExtentI.Y;
faceExtentI.Y = boxExtentI.Z;
absFaceOffsetI = boxExtentI.X;
faceNormal = poseI.Orientation.GetColumn(0);
xAxisInc = poseI.Orientation.GetColumn(1);
yAxisInc = poseI.Orientation.GetColumn(2);
faceDirection = normalI.X;
break;
case 1:
faceExtentI.X = boxExtentI.X;
faceExtentI.Y = boxExtentI.Z;
absFaceOffsetI = boxExtentI.Y;
faceNormal = poseI.Orientation.GetColumn(1);
xAxisInc = poseI.Orientation.GetColumn(0);
yAxisInc = poseI.Orientation.GetColumn(2);
faceDirection = normalI.Y;
break;
// case 2:
default:
faceExtentI.X = boxExtentI.X;
faceExtentI.Y = boxExtentI.Y;
absFaceOffsetI = boxExtentI.Z;
faceNormal = poseI.Orientation.GetColumn(2);
xAxisInc = poseI.Orientation.GetColumn(0);
yAxisInc = poseI.Orientation.GetColumn(1);
faceDirection = normalI.Z;
break;
}
// Compute center of incident face relative to the center of the reference box in world space.
float faceOffset = (faceDirection < 0) ? absFaceOffsetI : -absFaceOffsetI;
Vector3F centerOfFaceI = faceNormal * faceOffset + poseI.Position - poseR.Position;
// (Note: We will use the center of the incident face to compute the points of the incident
// face and transform the points into the reference-face frame. The center of the incident
// face is relative to the center of the reference box. We could also get center of the
// incident face relative to the center of the reference face. But since we are projecting
// the points from 3D to 2D this does not matter.)
Vector3F xAxisR, yAxisR; // The basis of the reference-face space.
float faceOffsetR; // The offset of the reference face to the center of the box.
Vector2F faceExtentR; // The half extent of the reference face.
switch (separatingAxisNumber)
{
case 1:
case 4:
faceExtentR.X = boxExtentR.Y;
faceExtentR.Y = boxExtentR.Z;
faceOffsetR = boxExtentR.X;
xAxisR = poseR.Orientation.GetColumn(1);
yAxisR = poseR.Orientation.GetColumn(2);
break;
case 2:
case 5:
faceExtentR.X = boxExtentR.X;
faceExtentR.Y = boxExtentR.Z;
faceOffsetR = boxExtentR.Y;
xAxisR = poseR.Orientation.GetColumn(0);
yAxisR = poseR.Orientation.GetColumn(2);
break;
// case 3:
// case 6:
default:
faceExtentR.X = boxExtentR.X;
faceExtentR.Y = boxExtentR.Y;
faceOffsetR = boxExtentR.Z;
xAxisR = poseR.Orientation.GetColumn(0);
yAxisR = poseR.Orientation.GetColumn(1);
break;
}
// Compute the center of the incident face in the reference-face frame.
// We can simply project centerOfFaceI onto the x- and y-axis of the reference
// face.
Vector2F centerOfFaceIInR;
//centerOfFaceIInR.X = Vector3F.Dot(centerOfFaceI, xAxisR);
// ----- Optimized version:
centerOfFaceIInR.X = centerOfFaceI.X * xAxisR.X + centerOfFaceI.Y * xAxisR.Y + centerOfFaceI.Z * xAxisR.Z;
//centerOfFaceIInR.Y = Vector3F.Dot(centerOfFaceI, yAxisR);
// ----- Optimized version:
centerOfFaceIInR.Y = centerOfFaceI.X * yAxisR.X + centerOfFaceI.Y * yAxisR.Y + centerOfFaceI.Z * yAxisR.Z;
// Now, we have the center of the incident face in reference-face coordinates.
// To compute the corners of the incident face in reference-face coordinates, we need
// transform faceExtentI (the half extent vector of the incident face) from the incident-
// face frame to the reference-face frame to compute the corners.
//
// The reference-face frame has the basis
// mR = (xAxisR, yAxisR, ?)
//
// The incident-face frame has the basis
// mI = (xAxisI, yAxisI, ?)
//
// Rotation from incident-face frame to reference-face frame is
// mIToR = mR^-1 * mI
//
// The corner offsets in incident-face space is are vectors (x, y, 0). To transform these
// vectors from incident-face space to reference-face space we need to calculate:
// mIToR * v
//
// Since the z-components are 0 and we are only interested in the resulting x, y coordinates
// in reference-space when can reduce the rotation to a 2 x 2 matrix. (The other components
// are not needed.)
// ----- Optimized version: (Original on the right)
Matrix22F mIToR;
mIToR.M00 = xAxisR.X * xAxisInc.X + xAxisR.Y * xAxisInc.Y + xAxisR.Z * xAxisInc.Z; // mIToR.M00 = Vector3F.Dot(xAxisR, xAxisInc);
mIToR.M01 = xAxisR.X * yAxisInc.X + xAxisR.Y * yAxisInc.Y + xAxisR.Z * yAxisInc.Z; // mIToR.M01 = Vector3F.Dot(xAxisR, yAxisInc);
mIToR.M10 = yAxisR.X * xAxisInc.X + yAxisR.Y * xAxisInc.Y + yAxisR.Z * xAxisInc.Z; // mIToR.M10 = Vector3F.Dot(yAxisR, xAxisInc);
mIToR.M11 = yAxisR.X * yAxisInc.X + yAxisR.Y * yAxisInc.Y + yAxisR.Z * yAxisInc.Z; // mIToR.M11 = Vector3F.Dot(yAxisR, yAxisInc);
// The corner offsets in incident-face space are:
// (-faceExtentI.X, -faceExtentI.Y) ... left, bottom corner
// ( faceExtentI.X, -faceExtentI.Y) ... right, bottom corner
// ( faceExtentI.X, faceExtentI.Y) ... right, top corner
// (-faceExtentI.X, faceExtentI.Y) ... left, top corner
//
// Instead of transforming each corner offset, we can optimize the computation: Do the
// matrix-vector multiplication once, keep the intermediate products, apply the sign
// of the components when adding the intermediate results.
float k1 = mIToR.M00 * faceExtentI.X; // Products of matrix-vector multiplication.
float k2 = mIToR.M01 * faceExtentI.Y;
float k3 = mIToR.M10 * faceExtentI.X;
float k4 = mIToR.M11 * faceExtentI.Y;
List<Vector2F> quad = DigitalRune.ResourcePools<Vector2F>.Lists.Obtain();
quad.Add(new Vector2F(centerOfFaceIInR.X - k1 - k2, centerOfFaceIInR.Y - k3 - k4));
quad.Add(new Vector2F(centerOfFaceIInR.X + k1 - k2, centerOfFaceIInR.Y + k3 - k4));
quad.Add(new Vector2F(centerOfFaceIInR.X + k1 + k2, centerOfFaceIInR.Y + k3 + k4));
quad.Add(new Vector2F(centerOfFaceIInR.X - k1 + k2, centerOfFaceIInR.Y - k3 + k4));
// Clip incident face (quadrilateral) against reference face (rectangle).
List<Vector2F> contacts2D = ClipQuadrilateralAgainstRectangle(faceExtentR, quad);
// Transform contact points back to world space and compute penetration depths.
int numberOfContacts = contacts2D.Count;
List<Vector3F> contacts3D = DigitalRune.ResourcePools<Vector3F>.Lists.Obtain();
List<float> penetrationDepths = DigitalRune.ResourcePools<float>.Lists.Obtain();
Matrix22F mRToI = mIToR.Inverse;
for (int i = numberOfContacts - 1; i >= 0; i--)
{
Vector2F contact2DR = contacts2D[i]; // Contact in reference-face space.
Vector2F contact2DI = mRToI * (contact2DR - centerOfFaceIInR); // Contact in incident-face space.
// Transform point in incident-face space to world (relative to center of reference box).
// contact3D = mI * (x, y, 0) + centerOfFaceI
Vector3F contact3D;
contact3D.X = xAxisInc.X * contact2DI.X + yAxisInc.X * contact2DI.Y + centerOfFaceI.X;
contact3D.Y = xAxisInc.Y * contact2DI.X + yAxisInc.Y * contact2DI.Y + centerOfFaceI.Y;
contact3D.Z = xAxisInc.Z * contact2DI.X + yAxisInc.Z * contact2DI.Y + centerOfFaceI.Z;
// Compute penetration depth.
//float penetrationDepth = faceOffsetR - Vector3F.Dot(normalWorld, contact3D);
// ----- Optimized version:
float penetrationDepth = faceOffsetR - (normalWorld.X * contact3D.X + normalWorld.Y * contact3D.Y + normalWorld.Z * contact3D.Z);
if (penetrationDepth >= 0)
{
// Valid contact.
contacts3D.Add(contact3D);
penetrationDepths.Add(penetrationDepth);
}
else
{
// Remove bad contacts from the 2D contacts.
// (We might still need the 2D contacts, if we need to reduce the contacts.)
contacts2D.RemoveAt(i);
}
}
numberOfContacts = contacts3D.Count;
if (numberOfContacts == 0)
return; // Should never happen.
// Revert normal back to original direction.
normal = (isNormalInverted) ? -normalWorld : normalWorld;
// Note: normal ........ contact normal pointing from box A to B.
// normalWorld ... contact normal pointing from reference box to incident box.
if (numberOfContacts <= MaxNumberOfContacts)
{
// Add all contacts to contact set.
for (int i = 0; i < numberOfContacts; i++)
{
float penetrationDepth = penetrationDepths[i];
// Position is between the positions of the box surfaces.
Vector3F position = contacts3D[i] + poseR.Position + normalWorld * (penetrationDepth / 2);
Contact contact = ContactHelper.CreateContact(contactSet, position, normal, penetrationDepth, false);
ContactHelper.Merge(contactSet, contact, type, CollisionDetection.ContactPositionTolerance);
}
}
else
{
// Reduce number of contacts, keep the contact with the max penetration depth.
int indexOfDeepest = 0;
float maxPenetrationDepth = penetrationDepths[0];
for (int i = 1; i < numberOfContacts; i++)
{
float penetrationDepth = penetrationDepths[i];
if (penetrationDepth > maxPenetrationDepth)
{
maxPenetrationDepth = penetrationDepth;
indexOfDeepest = i;
}
}
List<int> indicesOfContacts = ReduceContacts(contacts2D, indexOfDeepest, MaxNumberOfContacts);
// Add selected contacts to contact set.
numberOfContacts = indicesOfContacts.Count;
for (int i = 0; i < numberOfContacts; i++)
{
int index = indicesOfContacts[i];
float penetrationDepth = penetrationDepths[index];
// Position is between the positions of the box surfaces.
Vector3F position = contacts3D[index] + poseR.Position + normalWorld * (penetrationDepth / 2);
Contact contact = ContactHelper.CreateContact(contactSet, position, normal, penetrationDepths[index], false);
ContactHelper.Merge(contactSet, contact, type, CollisionDetection.ContactPositionTolerance);
}
DigitalRune.ResourcePools<int>.Lists.Recycle(indicesOfContacts);
}
DigitalRune.ResourcePools<Vector2F>.Lists.Recycle(contacts2D);
DigitalRune.ResourcePools<Vector3F>.Lists.Recycle(contacts3D);
DigitalRune.ResourcePools<float>.Lists.Recycle(penetrationDepths);
}
}
private void AddTriangleContacts(ContactSet contactSet,
bool swapped,
int triangleIndex,
CollisionQueryType type,
ContactSet testContactSet,
CollisionObject testCollisionObject,
TestGeometricObject testGeometricObject,
TriangleShape testTriangle)
{
// Object A should be the triangle mesh.
CollisionObject collisionObjectA = (swapped) ? contactSet.ObjectB : contactSet.ObjectA;
CollisionObject collisionObjectB = (swapped) ? contactSet.ObjectA : contactSet.ObjectB;
IGeometricObject geometricObjectA = collisionObjectA.GeometricObject;
var triangleMeshShape = ((TriangleMeshShape)geometricObjectA.Shape);
Triangle triangle = triangleMeshShape.Mesh.GetTriangle(triangleIndex);
Pose poseA = geometricObjectA.Pose;
Vector3F scaleA = geometricObjectA.Scale;
// Find collision algorithm.
CollisionAlgorithm collisionAlgorithm = CollisionDetection.AlgorithmMatrix[typeof(TriangleShape), collisionObjectB.GeometricObject.Shape.GetType()];
// Apply scaling.
testTriangle.Vertex0 = triangle.Vertex0 * scaleA;
testTriangle.Vertex1 = triangle.Vertex1 * scaleA;
testTriangle.Vertex2 = triangle.Vertex2 * scaleA;
// Set the shape temporarily to the current triangles.
testGeometricObject.Shape = testTriangle;
testGeometricObject.Scale = Vector3F.One;
testGeometricObject.Pose = poseA;
testCollisionObject.SetInternal(collisionObjectA, testGeometricObject);
// Make a temporary contact set.
// (Object A and object B should have the same order as in contactSet; otherwise we couldn't
// simply merge them.)
Debug.Assert(testContactSet.Count == 0, "testContactSet needs to be cleared.");
if (swapped)
{
testContactSet.Reset(collisionObjectB, testCollisionObject);
}
else
{
testContactSet.Reset(testCollisionObject, collisionObjectB);
}
if (type == CollisionQueryType.Boolean)
{
collisionAlgorithm.ComputeCollision(testContactSet, CollisionQueryType.Boolean);
contactSet.HaveContact = contactSet.HaveContact || testContactSet.HaveContact;
}
else
{
// No perturbation test. Most triangle mesh shapes are either complex and automatically
// have more contacts. Or they are complex and will not be used for stacking
// where full contact sets would be needed.
testContactSet.IsPerturbationTestAllowed = false;
// TODO: Copy separating axis info and similar things into triangleContactSet.
// But currently this info is not used in the queries.
// For closest points: If we know that we have a contact, then we can make a
// faster contact query instead of a closest-point query.
CollisionQueryType queryType = (contactSet.HaveContact) ? CollisionQueryType.Contacts : type;
collisionAlgorithm.ComputeCollision(testContactSet, queryType);
contactSet.HaveContact = contactSet.HaveContact || testContactSet.HaveContact;
if (testContactSet.HaveContact && testContactSet.Count > 0 && !triangleMeshShape.IsTwoSided)
{
// To compute the triangle normal in world space we take the normal of the unscaled
// triangle and transform the normal with: (M^-1)^T = 1 / scale
Vector3F triangleNormalLocal = Vector3F.Cross(triangle.Vertex1 - triangle.Vertex0, triangle.Vertex2 - triangle.Vertex0) / scaleA;
Vector3F triangleNormal = poseA.ToWorldDirection(triangleNormalLocal);
if (triangleNormal.TryNormalize())
{
var preferredNormal = swapped ? -triangleNormal : triangleNormal;
// ----- Remove bad normal.
// Triangles are double sided, but meshes are single sided.
// --> Remove contacts where the contact normal points into the wrong direction.
ContactHelper.RemoveBadContacts(testContactSet, preferredNormal, -Numeric.EpsilonF);
if (testContactSet.Count > 0 && triangleMeshShape.EnableContactWelding)
{
var contactDotTriangle = Vector3F.Dot(testContactSet[0].Normal, preferredNormal);
if (contactDotTriangle < WeldingLimit)
{
// Bad normal. Perform welding.
Vector3F contactPositionOnTriangle = swapped
? testContactSet[0].PositionBLocal / scaleA
: testContactSet[0].PositionALocal / scaleA;
Vector3F neighborNormal;
float triangleDotNeighbor;
GetNeighborNormal(triangleIndex, triangle, contactPositionOnTriangle, triangleNormal, triangleMeshShape, poseA, scaleA, out neighborNormal, out triangleDotNeighbor);
if (triangleDotNeighbor < float.MaxValue && Numeric.IsLess(contactDotTriangle, triangleDotNeighbor))
{
// Normal is not in allowed range.
// Test again in triangle normal direction.
Contact c0 = testContactSet[0];
testContactSet.RemoveAt(0);
testContactSet.Clear();
testContactSet.PreferredNormal = preferredNormal;
collisionAlgorithm.ComputeCollision(testContactSet, queryType);
testContactSet.PreferredNormal = Vector3F.Zero;
if (testContactSet.Count > 0)
{
Contact c1 = testContactSet[0];
float contact1DotTriangle = Vector3F.Dot(c1.Normal, preferredNormal);
// We use c1 instead of c0 if it has lower penetration depth (then it is simply
// better). Or we use c1 if the penetration depth increase is in an allowed range
// and c1 has a normal in the allowed range.
if (c1.PenetrationDepth < c0.PenetrationDepth ||
Numeric.IsGreaterOrEqual(contact1DotTriangle, triangleDotNeighbor) &&
c1.PenetrationDepth < c0.PenetrationDepth + CollisionDetection.ContactPositionTolerance)
{
c0.Recycle();
c0 = c1;
testContactSet.RemoveAt(0);
contactDotTriangle = contact1DotTriangle;
}
}
if (Numeric.IsLess(contactDotTriangle, triangleDotNeighbor))
{
// Clamp contact to allowed normal:
// We keep the contact position on the mesh and the penetration depth. We set
// a new normal and compute the other related values for this normal.
if (!swapped)
{
var positionAWorld = c0.PositionAWorld;
c0.Normal = neighborNormal;
var positionBWorld = positionAWorld - c0.Normal * c0.PenetrationDepth;
c0.Position = (positionAWorld + positionBWorld) / 2;
c0.PositionBLocal = testContactSet.ObjectB.GeometricObject.Pose.ToLocalPosition(positionBWorld);
}
else
{
var positionBWorld = c0.PositionBWorld;
c0.Normal = -neighborNormal;
var positionAWorld = positionBWorld + c0.Normal * c0.PenetrationDepth;
c0.Position = (positionAWorld + positionBWorld) / 2;
c0.PositionALocal = testContactSet.ObjectA.GeometricObject.Pose.ToLocalPosition(positionAWorld);
}
}
c0.Recycle();
}
}
}
}
}
#region ----- Merge testContactSet into contactSet -----
if (testContactSet.Count > 0)
{
// Set the shape feature of the new contacts.
int numberOfContacts = testContactSet.Count;
for (int i = 0; i < numberOfContacts; i++)
{
Contact contact = testContactSet[i];
//if (contact.Lifetime.Ticks == 0) // Currently, this check is not necessary because triangleSet does not contain old contacts.
//{
if (swapped)
{
contact.FeatureB = triangleIndex;
}
else
{
contact.FeatureA = triangleIndex;
}
//}
}
// Merge the contact info.
ContactHelper.Merge(contactSet, testContactSet, type, CollisionDetection.ContactPositionTolerance);
}
#endregion
}
}
public override void ComputeCollision(ContactSet contactSet, CollisionQueryType type)
{
contactSet.HaveContact = true;
}
public override void ComputeCollision(ContactSet contactSet, CollisionQueryType type)
{
CollisionObject collisionObjectA = contactSet.ObjectA;
CollisionObject collisionObjectB = contactSet.ObjectB;
IGeometricObject geometricObjectA = collisionObjectA.GeometricObject;
IGeometricObject geometricObjectB = collisionObjectB.GeometricObject;
// Object A should be the triangle mesh, swap objects if necessary.
// When testing TriangleMeshShape vs. TriangleMeshShape with BVH, object A should be the
// TriangleMeshShape with BVH - except when the TriangleMeshShape with BVH is a lot smaller
// than the other TriangleMeshShape. (See tests below.)
TriangleMeshShape triangleMeshShapeA = geometricObjectA.Shape as TriangleMeshShape;
TriangleMeshShape triangleMeshShapeB = geometricObjectB.Shape as TriangleMeshShape;
bool swapped = false;
// Check if collision objects shapes are correct.
if (triangleMeshShapeA == null && triangleMeshShapeB == null)
{
throw new ArgumentException("The contact set must contain a triangle mesh.", "contactSet");
}
Pose poseA = geometricObjectA.Pose;
Pose poseB = geometricObjectB.Pose;
Vector3F scaleA = geometricObjectA.Scale;
Vector3F scaleB = geometricObjectB.Scale;
// First we assume that we do not have a contact.
contactSet.HaveContact = false;
// ----- Use temporary test objects.
var testCollisionObjectA = ResourcePools.TestCollisionObjects.Obtain();
var testCollisionObjectB = ResourcePools.TestCollisionObjects.Obtain();
// Create a test contact set and initialize with dummy objects.
// (The actual collision objects are set below.)
var testContactSet = ContactSet.Create(testCollisionObjectA, testCollisionObjectB);
var testGeometricObjectA = TestGeometricObject.Create();
var testGeometricObjectB = TestGeometricObject.Create();
var testTriangleShapeA = ResourcePools.TriangleShapes.Obtain();
var testTriangleShapeB = ResourcePools.TriangleShapes.Obtain();
try
{
if (triangleMeshShapeA != null &&
triangleMeshShapeA.Partition != null &&
triangleMeshShapeB != null &&
triangleMeshShapeB.Partition != null &&
(type != CollisionQueryType.ClosestPoints ||
triangleMeshShapeA.Partition is ISupportClosestPointQueries <int>))
{
#region ----- TriangleMesh with BVH vs. TriangleMesh with BVH -----
Debug.Assert(swapped == false, "Why did we swap the objects? Order of objects is fine.");
// Find collision algorithm for triangle vs. triangle (used in AddTriangleTriangleContacts()).
if (_triangleTriangleAlgorithm == null)
{
_triangleTriangleAlgorithm = CollisionDetection.AlgorithmMatrix[typeof(TriangleShape), typeof(TriangleShape)];
}
if (type != CollisionQueryType.ClosestPoints)
{
// ----- Boolean or Contact Query
// Heuristic: Test large BVH vs. small BVH.
Aabb aabbOfA = geometricObjectA.Aabb;
Aabb aabbOfB = geometricObjectB.Aabb;
float largestExtentA = aabbOfA.Extent.LargestComponent;
float largestExtentB = aabbOfB.Extent.LargestComponent;
IEnumerable <Pair <int> > overlaps;
bool overlapsSwapped = largestExtentA < largestExtentB;
if (overlapsSwapped)
{
overlaps = triangleMeshShapeB.Partition.GetOverlaps(
scaleB,
geometricObjectB.Pose,
triangleMeshShapeA.Partition,
scaleA,
geometricObjectA.Pose);
}
else
{
overlaps = triangleMeshShapeA.Partition.GetOverlaps(
scaleA,
geometricObjectA.Pose,
triangleMeshShapeB.Partition,
scaleB,
geometricObjectB.Pose);
}
foreach (var overlap in overlaps)
{
if (type == CollisionQueryType.Boolean && contactSet.HaveContact)
{
break; // We can abort early.
}
AddTriangleTriangleContacts(
contactSet,
overlapsSwapped ? overlap.Second : overlap.First,
overlapsSwapped ? overlap.First : overlap.Second,
type,
testContactSet,
testCollisionObjectA,
testGeometricObjectA,
testTriangleShapeA,
testCollisionObjectB,
testGeometricObjectB,
testTriangleShapeB);
}
}
else
{
// ----- Closest-Point Query
var callback = ClosestPointCallbacks.Obtain();
callback.CollisionAlgorithm = this;
callback.Swapped = false;
callback.ContactSet = contactSet;
callback.TestContactSet = testContactSet;
callback.TestCollisionObjectA = testCollisionObjectA;
callback.TestCollisionObjectB = testCollisionObjectB;
callback.TestGeometricObjectA = testGeometricObjectA;
callback.TestGeometricObjectB = testGeometricObjectB;
callback.TestTriangleA = testTriangleShapeA;
callback.TestTriangleB = testTriangleShapeB;
((ISupportClosestPointQueries <int>)triangleMeshShapeA.Partition)
.GetClosestPointCandidates(
scaleA,
geometricObjectA.Pose,
triangleMeshShapeB.Partition,
scaleB,
geometricObjectB.Pose,
callback.HandlePair);
ClosestPointCallbacks.Recycle(callback);
}
#endregion
}
else
{
Aabb aabbOfA = geometricObjectA.Aabb;
Aabb aabbOfB = geometricObjectB.Aabb;
float largestExtentA = aabbOfA.Extent.LargestComponent;
float largestExtentB = aabbOfB.Extent.LargestComponent;
// Choose which object should be A.
if (triangleMeshShapeA == null)
{
// A is no TriangleMesh. B must be a TriangleMesh.
swapped = true;
}
else if (triangleMeshShapeB == null)
{
// A is a TriangleMesh and B is no TriangleMesh.
}
else if (triangleMeshShapeA.Partition != null)
{
// A is a TriangleMesh with BVH and B is a TriangleMesh.
// We want to test TriangleMesh with BVH vs. * - unless the TriangleMesh with BVH is a lot
// smaller than the TriangleMesh.
if (largestExtentA * 2 < largestExtentB)
{
swapped = true;
}
}
else if (triangleMeshShapeB.Partition != null)
{
// A is a TriangleMesh and B is a TriangleMesh with BVH.
// We want to test TriangleMesh with BVH vs. * - unless the TriangleMesh BVH is a lot
// smaller than the TriangleMesh.
if (largestExtentB * 2 >= largestExtentA)
{
swapped = true;
}
}
else
{
// A and B are normal triangle meshes. A should be the larger object.
if (largestExtentA < largestExtentB)
{
swapped = true;
}
}
if (swapped)
{
// Swap all variables.
MathHelper.Swap(ref collisionObjectA, ref collisionObjectB);
MathHelper.Swap(ref geometricObjectA, ref geometricObjectB);
MathHelper.Swap(ref aabbOfA, ref aabbOfB);
MathHelper.Swap(ref largestExtentA, ref largestExtentB);
MathHelper.Swap(ref triangleMeshShapeA, ref triangleMeshShapeB);
MathHelper.Swap(ref poseA, ref poseB);
MathHelper.Swap(ref scaleA, ref scaleB);
}
if (triangleMeshShapeB == null &&
type != CollisionQueryType.ClosestPoints &&
largestExtentA * 2 < largestExtentB)
{
// B is a very large object and no triangle mesh.
// Make a AABB vs. Shape of B test for quick rejection.
BoxShape testBoxShape = ResourcePools.BoxShapes.Obtain();
testBoxShape.Extent = aabbOfA.Extent;
testGeometricObjectA.Shape = testBoxShape;
testGeometricObjectA.Scale = Vector3F.One;
testGeometricObjectA.Pose = new Pose(aabbOfA.Center);
testCollisionObjectA.SetInternal(collisionObjectA, testGeometricObjectA);
Debug.Assert(testContactSet.Count == 0, "testContactSet needs to be cleared.");
testContactSet.Reset(testCollisionObjectA, collisionObjectB);
CollisionAlgorithm collisionAlgorithm = CollisionDetection.AlgorithmMatrix[testContactSet];
collisionAlgorithm.ComputeCollision(testContactSet, CollisionQueryType.Boolean);
ResourcePools.BoxShapes.Recycle(testBoxShape);
if (!testContactSet.HaveContact)
{
contactSet.HaveContact = false;
return;
}
}
if (triangleMeshShapeA.Partition != null &&
(type != CollisionQueryType.ClosestPoints ||
triangleMeshShapeA.Partition is ISupportClosestPointQueries <int>))
{
#region ----- TriangleMesh BVH vs. * -----
// Get AABB of B in local space of A.
var aabbBInA = geometricObjectB.Shape.GetAabb(scaleB, poseA.Inverse * poseB);
// Apply inverse scaling to do the AABB-tree checks in the unscaled local space of A.
aabbBInA.Scale(Vector3F.One / scaleA);
if (type != CollisionQueryType.ClosestPoints)
{
// Boolean or Contact Query
foreach (var triangleIndex in triangleMeshShapeA.Partition.GetOverlaps(aabbBInA))
{
if (type == CollisionQueryType.Boolean && contactSet.HaveContact)
{
break; // We can abort early.
}
AddTriangleContacts(
contactSet,
swapped,
triangleIndex,
type,
testContactSet,
testCollisionObjectA,
testGeometricObjectA,
testTriangleShapeA);
}
}
else if (type == CollisionQueryType.ClosestPoints)
{
// Closest-Point Query
var callback = ClosestPointCallbacks.Obtain();
callback.CollisionAlgorithm = this;
callback.Swapped = swapped;
callback.ContactSet = contactSet;
callback.TestContactSet = testContactSet;
callback.TestCollisionObjectA = testCollisionObjectA;
callback.TestCollisionObjectB = testCollisionObjectB;
callback.TestGeometricObjectA = testGeometricObjectA;
callback.TestGeometricObjectB = testGeometricObjectB;
callback.TestTriangleA = testTriangleShapeA;
callback.TestTriangleB = testTriangleShapeB;
((ISupportClosestPointQueries <int>)triangleMeshShapeA.Partition)
.GetClosestPointCandidates(
aabbBInA,
float.PositiveInfinity,
callback.HandleItem);
ClosestPointCallbacks.Recycle(callback);
}
#endregion
}
else
{
#region ----- TriangleMesh vs. * -----
// Find an upper bound for the distance we have to search.
// If object are in contact or we make contact/boolean query, then the distance is 0.
float closestPairDistance;
if (contactSet.HaveContact || type != CollisionQueryType.ClosestPoints)
{
closestPairDistance = 0;
}
else
{
// Make first guess for closest pair: inner point of B to inner point of mesh.
Vector3F innerPointA = poseA.ToWorldPosition(geometricObjectA.Shape.InnerPoint * scaleA);
Vector3F innerPointB = poseB.ToWorldPosition(geometricObjectB.Shape.InnerPoint * scaleB);
closestPairDistance = (innerPointB - innerPointA).Length + CollisionDetection.Epsilon;
}
// The search-space is a space where the closest points must lie in.
Vector3F minimum = aabbOfB.Minimum - new Vector3F(closestPairDistance);
Vector3F maximum = aabbOfB.Maximum + new Vector3F(closestPairDistance);
Aabb searchSpaceAabb = new Aabb(minimum, maximum);
// Test all triangles.
ITriangleMesh triangleMeshA = triangleMeshShapeA.Mesh;
int numberOfTriangles = triangleMeshA.NumberOfTriangles;
for (int i = 0; i < numberOfTriangles; i++)
{
// TODO: GetTriangle is performed twice! Here and in AddTriangleContacts() below!
Triangle triangle = triangleMeshA.GetTriangle(i);
testTriangleShapeA.Vertex0 = triangle.Vertex0 * scaleA;
testTriangleShapeA.Vertex1 = triangle.Vertex1 * scaleA;
testTriangleShapeA.Vertex2 = triangle.Vertex2 * scaleA;
// Make AABB test with search space.
if (GeometryHelper.HaveContact(searchSpaceAabb, testTriangleShapeA.GetAabb(poseA)))
{
// IMPORTANT: Info in triangleShape is destroyed in this method!
// Triangle is given to the method so that method does not allocate garbage.
AddTriangleContacts(
contactSet,
swapped,
i,
type,
testContactSet,
testCollisionObjectA,
testGeometricObjectA,
testTriangleShapeA);
// We have contact and stop for boolean queries.
if (contactSet.HaveContact && type == CollisionQueryType.Boolean)
{
break;
}
if (closestPairDistance > 0 && contactSet.HaveContact ||
contactSet.Count > 0 && -contactSet[contactSet.Count - 1].PenetrationDepth < closestPairDistance)
{
// Reduce search space
// Note: contactSet can contain several contacts. We assume that the last contact
// is the newest one and check only this.
if (contactSet.Count > 0)
{
closestPairDistance = Math.Max(0, -contactSet[contactSet.Count - 1].PenetrationDepth);
}
else
{
closestPairDistance = 0;
}
searchSpaceAabb.Minimum = aabbOfB.Minimum - new Vector3F(closestPairDistance);
searchSpaceAabb.Maximum = aabbOfB.Maximum + new Vector3F(closestPairDistance);
}
}
}
#endregion
}
}
}
finally
{
testContactSet.Recycle();
ResourcePools.TestCollisionObjects.Recycle(testCollisionObjectA);
ResourcePools.TestCollisionObjects.Recycle(testCollisionObjectB);
testGeometricObjectB.Recycle();
testGeometricObjectA.Recycle();
ResourcePools.TriangleShapes.Recycle(testTriangleShapeB);
ResourcePools.TriangleShapes.Recycle(testTriangleShapeA);
}
}
public static void Merge(ContactSet contactSet, Contact newContact, CollisionQueryType type, float contactPositionTolerance)
{
Debug.Assert(contactSet != null);
Debug.Assert(newContact != null);
Debug.Assert(contactPositionTolerance >= 0, "The contact position tolerance must be greater than or equal to 0");
Debug.Assert(type == CollisionQueryType.ClosestPoints || type == CollisionQueryType.Contacts);
if (type == CollisionQueryType.Contacts && newContact.PenetrationDepth < 0)
return; // Do not merge separated contacts.
// ----- Simplest case: Contact set is empty.
if (contactSet.Count == 0)
{
// Simply add the new contact.
contactSet.Add(newContact);
return;
}
// ----- Try to merge with nearest old contact.
bool merged = TryMergeWithNearestContact(contactSet, newContact, contactPositionTolerance, true);
if (merged)
{
newContact.Recycle();
return;
}
// ----- Default: Add the new contact.
contactSet.Add(newContact);
}
public override void ComputeCollision(ContactSet contactSet, CollisionQueryType type)
{
// Object B should be the transformed shape.
CollisionObject objectA = contactSet.ObjectA;
CollisionObject objectB = contactSet.ObjectB;
// Swap objects if necessary.
bool swapped = !(objectB.GeometricObject.Shape is TransformedShape);
if (swapped)
MathHelper.Swap(ref objectA, ref objectB);
IGeometricObject geometricObjectB = objectB.GeometricObject;
TransformedShape transformedShape = geometricObjectB.Shape as TransformedShape;
// Check if collision objects shapes are correct.
if (transformedShape == null)
throw new ArgumentException("The contact set must contain a transformed shape.", "contactSet");
// Assume no contact.
contactSet.HaveContact = false;
// Currently object B has the following structure:
//
// CollisionObject objectB
// GeometricObject geometricObjectB
// Pose poseB
// Scale scaleB
// TransformedShape transformedShape
// GeometricObject childGeometricObject
// Pose childPose
// Scale childScale
// Shape childShape
//
// To compute the collisions we temporarily remove the TransformedShape:
// We replace the original geometric object with a test geometric object and combine the
// transformations:
//
// CollisionObject testObjectB
// GeometricObject testGeometricObjectB
// Pose poseB * childPose
// Shape childShape
// Scale scaleB * childScale
Pose poseB = geometricObjectB.Pose;
Vector3F scaleB = geometricObjectB.Scale;
// Apply scale to pose and test geometric object.
// (Note: The scaling is either uniform or the transformed object has no local rotation.
// Therefore, we only need to apply the scale of the parent to the scale and translation of
// the child. We can ignore the rotation.)
IGeometricObject childGeometricObject = transformedShape.Child;
Pose childPose = childGeometricObject.Pose;
// Non-uniform scaling is not supported for rotated child objects.
if ((scaleB.X != scaleB.Y || scaleB.Y != scaleB.Z) && childPose.HasRotation)
throw new NotSupportedException("Computing collisions for transformed shapes with local rotations and non-uniform scaling is not supported.");
childPose.Position *= scaleB; // Apply scaling to local translation.
var testGeometricObjectB = TestGeometricObject.Create();
testGeometricObjectB.Shape = childGeometricObject.Shape;
testGeometricObjectB.Scale = scaleB * childGeometricObject.Scale; // Apply scaling to local scale.
testGeometricObjectB.Pose = poseB * childPose;
var testCollisionObjectB = ResourcePools.TestCollisionObjects.Obtain();
testCollisionObjectB.SetInternal(objectB, testGeometricObjectB);
var testContactSet = swapped ? ContactSet.Create(testCollisionObjectB, objectA)
: ContactSet.Create(objectA, testCollisionObjectB);
testContactSet.IsPerturbationTestAllowed = contactSet.IsPerturbationTestAllowed;
// Transform contacts into space of child and copy them into the testContactSet.
//int numberOfOldContacts = contactSet.Count;
//for (int i = 0; i < numberOfOldContacts; i++)
//{
// Contact contact = contactSet[i];
// if (swapped)
// contact.PositionALocal = childPose.ToLocalPosition(contact.PositionALocal);
// else
// contact.PositionBLocal = childPose.ToLocalPosition(contact.PositionBLocal);
//
// testContactSet.Add(contact);
//}
// Compute collision.
var collisionAlgorithm = CollisionDetection.AlgorithmMatrix[objectA, testCollisionObjectB];
collisionAlgorithm.ComputeCollision(testContactSet, type);
if (testContactSet.HaveContact)
contactSet.HaveContact = true;
// Transform contacts into space of parent TransformShape.
int numberOfNewContacts = testContactSet.Count;
for (int i = 0; i < numberOfNewContacts; i++)
{
Contact contact = testContactSet[i];
if (swapped)
contact.PositionALocal = childPose.ToWorldPosition(contact.PositionALocal);
else
contact.PositionBLocal = childPose.ToWorldPosition(contact.PositionBLocal);
}
// Merge new contacts to contactSet.
// (If testContactSet contains all original contacts (see commented out part above), we can
// simply clear contactSet and copy all contacts of testContactSet.)
//contactSet.Clear();
//foreach (Contact contact in testContactSet)
// contactSet.Add(contact);
ContactHelper.Merge(contactSet, testContactSet, type, CollisionDetection.ContactPositionTolerance);
// Recycle temporary objects.
testContactSet.Recycle();
ResourcePools.TestCollisionObjects.Recycle(testCollisionObjectB);
testGeometricObjectB.Recycle();
}
// Compute contacts between a shape and the child shapes of a <see cref="CompositeShape"/>.
// testXxx are initialized objects which are re-used to avoid a lot of GC garbage.
private void AddChildContacts(ContactSet contactSet,
bool swapped,
int childIndex,
CollisionQueryType type,
ContactSet testContactSet,
CollisionObject testCollisionObject,
TestGeometricObject testGeometricObject)
{
// This method is also used in RayCompositeAlgorithm.cs. Keep changes in sync!
// Object A should be the CompositeShape.
CollisionObject collisionObjectA = (swapped) ? contactSet.ObjectB : contactSet.ObjectA;
CollisionObject collisionObjectB = (swapped) ? contactSet.ObjectA : contactSet.ObjectB;
IGeometricObject geometricObjectA = collisionObjectA.GeometricObject;
IGeometricObject geometricObjectB = collisionObjectB.GeometricObject;
Vector3F scaleA = geometricObjectA.Scale;
IGeometricObject childA = ((CompositeShape)geometricObjectA.Shape).Children[childIndex];
// Find collision algorithm.
CollisionAlgorithm collisionAlgorithm = CollisionDetection.AlgorithmMatrix[childA, geometricObjectB];
// ----- Set the shape temporarily to the current child.
// (Note: The scaling is either uniform or the child has no local rotation. Therefore, we only
// need to apply the scale of the parent to the scale and translation of the child. We can
// ignore the rotation.)
Debug.Assert(
(scaleA.X == scaleA.Y && scaleA.Y == scaleA.Z) || !childA.Pose.HasRotation,
"CompositeShapeAlgorithm should have thrown an exception. Non-uniform scaling is not supported for rotated children.");
var childPose = childA.Pose;
childPose.Position *= scaleA; // Apply scaling to local translation.
testGeometricObject.Pose = geometricObjectA.Pose * childPose;
testGeometricObject.Shape = childA.Shape;
testGeometricObject.Scale = scaleA * childA.Scale; // Apply scaling to local scale.
testCollisionObject.SetInternal(collisionObjectA, testGeometricObject);
// Create a temporary contact set.
// (ObjectA and ObjectB should have the same order as in contactSet; otherwise we couldn't
// simply merge them.)
Debug.Assert(testContactSet.Count == 0, "testContactSet needs to be cleared.");
if (swapped)
testContactSet.Reset(collisionObjectB, testCollisionObject);
else
testContactSet.Reset(testCollisionObject, collisionObjectB);
if (type == CollisionQueryType.Boolean)
{
// Boolean queries.
collisionAlgorithm.ComputeCollision(testContactSet, CollisionQueryType.Boolean);
contactSet.HaveContact = (contactSet.HaveContact || testContactSet.HaveContact);
}
else
{
// No perturbation test. Most composite shapes are either complex and automatically
// have more contacts. Or they are complex and will not be used for stacking
// where full contact sets would be needed.
testContactSet.IsPerturbationTestAllowed = false;
// TODO: We could add existing contacts with the same child shape to childContactSet.
// Collision algorithms could take advantage of existing contact information to speed up
// calculations. However, at the moment the collision algorithms ignore existing contacts.
// If we add the exiting contacts to childContactSet we need to uncomment the comment
// code lines below.
// Transform contacts into space of child shape.
//foreach (Contact c in childContactSet)
//{
// if (childContactSet.ObjectA == childCollisionObject)
// c.PositionALocal = childPose.ToLocalPosition(c.PositionALocal);
// else
// c.PositionBLocal = childPose.ToLocalPosition(c.PositionBLocal);
//}
// Make collision check. As soon as we have found contact, we can make faster
// contact queries instead of closest-point queries.
CollisionQueryType queryType = (contactSet.HaveContact) ? CollisionQueryType.Contacts : type;
collisionAlgorithm.ComputeCollision(testContactSet, queryType);
contactSet.HaveContact = (contactSet.HaveContact || testContactSet.HaveContact);
// Transform contacts into space of composite shape.
// And set the shape feature of the contact.
int numberOfContacts = testContactSet.Count;
for (int i = 0; i < numberOfContacts; i++)
{
Contact contact = testContactSet[i];
if (swapped)
{
contact.PositionBLocal = childPose.ToWorldPosition(contact.PositionBLocal);
//if (contact.Lifetime.Ticks == 0) // Currently, all contacts are new, so this check is not necessary.
//{
contact.FeatureB = childIndex;
//}
}
else
{
contact.PositionALocal = childPose.ToWorldPosition(contact.PositionALocal);
//if (contact.Lifetime.Ticks == 0) // Currently, all contacts are new, so this check is not necessary.
//{
contact.FeatureA = childIndex;
//}
}
}
// Merge child contacts.
ContactHelper.Merge(contactSet, testContactSet, type, CollisionDetection.ContactPositionTolerance);
}
}
public override void ComputeCollision(ContactSet contactSet, CollisionQueryType type)
{
// Object A should be the ray.
// Object B should be the triangle.
IGeometricObject rayObject = contactSet.ObjectA.GeometricObject;
IGeometricObject triangleObject = contactSet.ObjectB.GeometricObject;
// Swap if necessary.
bool swapped = (triangleObject.Shape is RayShape);
if (swapped)
MathHelper.Swap(ref rayObject, ref triangleObject);
RayShape rayShape = rayObject.Shape as RayShape;
TriangleShape triangleShape = triangleObject.Shape as TriangleShape;
// Check if shapes are correct.
if (rayShape == null || triangleShape == null)
throw new ArgumentException("The contact set must contain a ray and a triangle.", "contactSet");
// See SOLID and Bergen: "Collision Detection in Interactive 3D Environments", pp. 84.
// Note: All computations are done in triangle local space.
// Get transformations.
Vector3F rayScale = rayObject.Scale;
Vector3F triangleScale = triangleObject.Scale;
Pose rayPose = rayObject.Pose;
Pose trianglePose = triangleObject.Pose;
// Scale triangle.
Vector3F v0 = triangleShape.Vertex0 * triangleScale;
Vector3F v1 = triangleShape.Vertex1 * triangleScale;
Vector3F v2 = triangleShape.Vertex2 * triangleScale;
// Scale ray and transform ray to local space of triangle.
Ray rayWorld = new Ray(rayShape);
rayWorld.Scale(ref rayScale); // Scale ray.
rayWorld.ToWorld(ref rayPose); // Transform to world space.
Ray ray = rayWorld;
ray.ToLocal(ref trianglePose); // Transform to local space of triangle.
Vector3F d1 = (v1 - v0);
Vector3F d2 = (v2 - v0);
Vector3F n = Vector3F.Cross(d1, d2);
// Tolerance value, see SOLID, Bergen: "Collision Detection in Interactive 3D Environments".
float ε = n.Length * Numeric.EpsilonFSquared;
Vector3F r = ray.Direction * ray.Length;
float δ = -Vector3F.Dot(r, n);
if (ε == 0.0f || Numeric.IsZero(δ, ε))
{
// The triangle is degenerate or the ray is parallel to triangle.
if (type == CollisionQueryType.Contacts || type == CollisionQueryType.Boolean)
contactSet.HaveContact = false;
else if (type == CollisionQueryType.ClosestPoints)
GetClosestPoints(contactSet, rayWorld.Origin);
return;
}
Vector3F triangleToRayOrigin = ray.Origin - v0;
float λ = Vector3F.Dot(triangleToRayOrigin, n) / δ;
// Assume no contact.
contactSet.HaveContact = false;
if (λ < 0 || λ > 1)
{
// The ray does not hit.
if (type == CollisionQueryType.ClosestPoints)
GetClosestPoints(contactSet, rayWorld.Origin);
}
else
{
Vector3F u = Vector3F.Cross(triangleToRayOrigin, r);
float μ1 = Vector3F.Dot(d2, u) / δ;
float μ2 = Vector3F.Dot(-d1, u) / δ;
if (μ1 + μ2 <= 1 + ε && μ1 >= -ε && μ2 >= -ε)
{
// Hit!
contactSet.HaveContact = true;
if (type == CollisionQueryType.Boolean)
return;
float penetrationDepth = λ * ray.Length;
// Create contact info.
Vector3F position = rayWorld.Origin + rayWorld.Direction * penetrationDepth;
n = trianglePose.ToWorldDirection(n);
Debug.Assert(!n.IsNumericallyZero, "Degenerate cases of ray vs. triangle should be treated above.");
n.Normalize();
if (δ > 0)
n = -n;
if (swapped)
n = -n;
Contact contact = ContactHelper.CreateContact(contactSet, position, n, penetrationDepth, true);
ContactHelper.Merge(contactSet, contact, type, CollisionDetection.ContactPositionTolerance);
}
else
{
// No Hit!
if (type == CollisionQueryType.ClosestPoints)
GetClosestPoints(contactSet, rayWorld.Origin);
}
}
}
public override void ComputeCollision(ContactSet contactSet, CollisionQueryType type)
{
CollisionObject collisionObjectA = contactSet.ObjectA;
CollisionObject collisionObjectB = contactSet.ObjectB;
IGeometricObject geometricObjectA = collisionObjectA.GeometricObject;
IGeometricObject geometricObjectB = collisionObjectB.GeometricObject;
// Object A should be the composite, swap objects if necessary.
// When testing CompositeShape vs. CompositeShape with BVH, object A should be the
// CompositeShape with BVH.
CompositeShape compositeShapeA = geometricObjectA.Shape as CompositeShape;
CompositeShape compositeShapeB = geometricObjectB.Shape as CompositeShape;
bool swapped = false;
if (compositeShapeA == null)
{
// Object A is something else. Object B must be a composite shape.
swapped = true;
}
else if (compositeShapeA.Partition == null
&& compositeShapeB != null
&& compositeShapeB.Partition != null)
{
// Object A has no BVH, object B is CompositeShape with BVH.
swapped = true;
}
if (swapped)
{
MathHelper.Swap(ref collisionObjectA, ref collisionObjectB);
MathHelper.Swap(ref geometricObjectA, ref geometricObjectB);
MathHelper.Swap(ref compositeShapeA, ref compositeShapeB);
}
// Check if collision objects shapes are correct.
if (compositeShapeA == null)
throw new ArgumentException("The contact set must contain a composite shape.", "contactSet");
// Assume no contact.
contactSet.HaveContact = false;
Vector3F scaleA = geometricObjectA.Scale;
Vector3F scaleB = geometricObjectB.Scale;
// Check if transforms are supported.
if (compositeShapeA != null // When object A is a CompositeShape
&& (scaleA.X != scaleA.Y || scaleA.Y != scaleA.Z) // non-uniform scaling is not supported
&& compositeShapeA.Children.Any(child => child.Pose.HasRotation)) // when a child has a local rotation.
{ // Note: Any() creates garbage, but non-uniform scalings should not be used anyway.
throw new NotSupportedException("Computing collisions for composite shapes with non-uniform scaling and rotated children is not supported.");
}
// Same check for object B.
if (compositeShapeB != null
&& (scaleB.X != scaleB.Y || scaleB.Y != scaleB.Z)
&& compositeShapeB.Children.Any(child => child.Pose.HasRotation)) // Note: Any() creates garbage, but non-uniform scalings should not be used anyway.
{
throw new NotSupportedException("Computing collisions for composite shapes with non-uniform scaling and rotated children is not supported.");
}
// ----- A few fixed objects which are reused to avoid GC garbage.
var testCollisionObjectA = ResourcePools.TestCollisionObjects.Obtain();
var testCollisionObjectB = ResourcePools.TestCollisionObjects.Obtain();
// Create a test contact set and initialize with dummy objects.
// (The actual collision objects are set below.)
var testContactSet = ContactSet.Create(testCollisionObjectA, testCollisionObjectB);
var testGeometricObjectA = TestGeometricObject.Create();
var testGeometricObjectB = TestGeometricObject.Create();
try
{
if (compositeShapeA.Partition != null
&& (type != CollisionQueryType.ClosestPoints
|| compositeShapeA.Partition is ISupportClosestPointQueries<int>))
{
if (compositeShapeB != null && compositeShapeB.Partition != null)
{
#region ----- Composite with BVH vs. Composite with BVH -----
Debug.Assert(swapped == false, "Why did we swap the objects? Order of objects is fine.");
if (type != CollisionQueryType.ClosestPoints)
{
// ----- Boolean or Contact Query
// Heuristic: Test large BVH vs. small BVH.
Aabb aabbOfA = geometricObjectA.Aabb;
Aabb aabbOfB = geometricObjectB.Aabb;
float largestExtentA = aabbOfA.Extent.LargestComponent;
float largestExtentB = aabbOfB.Extent.LargestComponent;
IEnumerable<Pair<int>> overlaps;
bool overlapsSwapped = largestExtentA < largestExtentB;
if (overlapsSwapped)
{
overlaps = compositeShapeB.Partition.GetOverlaps(
scaleB,
geometricObjectB.Pose,
compositeShapeA.Partition,
scaleA,
geometricObjectA.Pose);
}
else
{
overlaps = compositeShapeA.Partition.GetOverlaps(
scaleA,
geometricObjectA.Pose,
compositeShapeB.Partition,
scaleB,
geometricObjectB.Pose);
}
foreach (var overlap in overlaps)
{
if (type == CollisionQueryType.Boolean && contactSet.HaveContact)
break; // We can abort early.
AddChildChildContacts(
contactSet,
overlapsSwapped ? overlap.Second : overlap.First,
overlapsSwapped ? overlap.First : overlap.Second,
type,
testContactSet,
testCollisionObjectA,
testGeometricObjectA,
testCollisionObjectB,
testGeometricObjectB);
}
}
else
{
// Closest-Point Query
var callback = ClosestPointsCallbacks.Obtain();
callback.CollisionAlgorithm = this;
callback.Swapped = false;
callback.ContactSet = contactSet;
callback.TestCollisionObjectA = testCollisionObjectA;
callback.TestCollisionObjectB = testCollisionObjectB;
callback.TestGeometricObjectA = testGeometricObjectA;
callback.TestGeometricObjectB = testGeometricObjectB;
callback.TestContactSet = testContactSet;
((ISupportClosestPointQueries<int>)compositeShapeA.Partition)
.GetClosestPointCandidates(
scaleA,
geometricObjectA.Pose,
compositeShapeB.Partition,
scaleB,
geometricObjectB.Pose,
callback.HandlePair);
ClosestPointsCallbacks.Recycle(callback);
}
#endregion
}
else
{
#region ----- Composite with BVH vs. * -----
// Compute AABB of B in local space of the CompositeShape.
Aabb aabbBInA = geometricObjectB.Shape.GetAabb(
scaleB, geometricObjectA.Pose.Inverse * geometricObjectB.Pose);
// Apply inverse scaling to do the AABB checks in the unscaled local space of A.
aabbBInA.Scale(Vector3F.One / scaleA);
if (type != CollisionQueryType.ClosestPoints)
{
// Boolean or Contact Query
foreach (var childIndex in compositeShapeA.Partition.GetOverlaps(aabbBInA))
{
if (type == CollisionQueryType.Boolean && contactSet.HaveContact)
break; // We can abort early.
AddChildContacts(
contactSet,
swapped,
childIndex,
type,
testContactSet,
testCollisionObjectA,
testGeometricObjectA);
}
}
else if (type == CollisionQueryType.ClosestPoints)
{
// Closest-Point Query
var callback = ClosestPointsCallbacks.Obtain();
callback.CollisionAlgorithm = this;
callback.Swapped = swapped;
callback.ContactSet = contactSet;
callback.TestCollisionObjectA = testCollisionObjectA;
callback.TestCollisionObjectB = testCollisionObjectB;
callback.TestGeometricObjectA = testGeometricObjectA;
callback.TestGeometricObjectB = testGeometricObjectB;
callback.TestContactSet = testContactSet;
((ISupportClosestPointQueries<int>)compositeShapeA.Partition)
.GetClosestPointCandidates(
aabbBInA,
float.PositiveInfinity,
callback.HandleItem);
ClosestPointsCallbacks.Recycle(callback);
}
#endregion
}
}
else
{
#region ----- Composite vs. *-----
// Compute AABB of B in local space of the composite.
Aabb aabbBInA = geometricObjectB.Shape.GetAabb(scaleB, geometricObjectA.Pose.Inverse * geometricObjectB.Pose);
// Apply inverse scaling to do the AABB checks in the unscaled local space of A.
aabbBInA.Scale(Vector3F.One / scaleA);
// Go through list of children and find contacts.
int numberOfChildGeometries = compositeShapeA.Children.Count;
for (int i = 0; i < numberOfChildGeometries; i++)
{
IGeometricObject child = compositeShapeA.Children[i];
// NOTE: For closest-point queries we could be faster estimating a search space.
// See TriangleMeshAlgorithm or BVH queries.
// But the current implementation is sufficient. If the CompositeShape is more complex
// the user should be using spatial partitions anyway.
// For boolean or contact queries, we make an AABB test first.
// For closest points where we have not found a contact yet, we have to search
// all children.
if ((type == CollisionQueryType.ClosestPoints && !contactSet.HaveContact)
|| GeometryHelper.HaveContact(aabbBInA, child.Shape.GetAabb(child.Scale, child.Pose)))
{
// TODO: We could compute the minDistance of the child AABB and the AABB of the
// other shape. If the minDistance is greater than the current closestPairDistance
// we can ignore this pair. - This could be a performance boost.
// Get contacts/closest pairs of this child.
AddChildContacts(
contactSet,
swapped,
i,
type,
testContactSet,
testCollisionObjectA,
testGeometricObjectA);
// We have contact and stop for boolean queries.
if (contactSet.HaveContact && type == CollisionQueryType.Boolean)
break;
}
}
#endregion
}
}
finally
{
Debug.Assert(collisionObjectA.GeometricObject.Shape == compositeShapeA, "Shape was altered and not restored.");
testContactSet.Recycle();
ResourcePools.TestCollisionObjects.Recycle(testCollisionObjectA);
ResourcePools.TestCollisionObjects.Recycle(testCollisionObjectB);
testGeometricObjectB.Recycle();
testGeometricObjectA.Recycle();
}
}
/// <summary> /// Computes the collision. - This method should only be used by /// <see cref="CollisionAlgorithm"/> instances! /// </summary> /// <param name="contactSet">The contact set.</param> /// <param name="type">The type of collision query.</param> /// <remarks> /// <para> /// This method does the real work. It is called from the other public methods of a /// <see cref="CollisionAlgorithm"/>. Also, if one <see cref="CollisionAlgorithm"/> uses another /// <see cref="CollisionAlgorithm"/> internally, this method should be called directly instead /// of <see cref="CollisionAlgorithm.UpdateClosestPoints"/> or /// <see cref="CollisionAlgorithm.UpdateContacts"/>. /// </para> /// <para> /// <strong>Notes to Inheritors:</strong> This is the central method which has to be implemented /// in derived classes. <paramref name="contactSet"/> is never <see langword="null"/>. This /// method has to add new contact/closest-point info with /// <see cref="ContactHelper.Merge(ContactSet,Contact,CollisionQueryType,float)"/>. It is not /// necessary to remove old contacts. At the beginning of the method /// <see cref="ContactSet.HaveContact"/> in <paramref name="contactSet"/> indicates the result /// of the last narrow phase algorithm that was run on <paramref name="contactSet"/>. This /// method must set <see cref="ContactSet.HaveContact"/> to <see langword="false"/> if it /// doesn't find a contact or to <see langword="true"/> if it finds a contact. /// </para> /// </remarks> public abstract void ComputeCollision(ContactSet contactSet, CollisionQueryType type);
public override void ComputeCollision(ContactSet contactSet, CollisionQueryType type)
{
if (type == CollisionQueryType.ClosestPoints)
{
// Just use normal height field shape algorithm.
_heightFieldAlgorithm.ComputeCollision(contactSet, type);
return;
}
Debug.Assert(type != CollisionQueryType.ClosestPoints, "Closest point queries should have already been handled!");
// HeightField = A, Ray = B
IGeometricObject heightFieldObject = contactSet.ObjectA.GeometricObject;
IGeometricObject rayObject = contactSet.ObjectB.GeometricObject;
// Object A should be the height field, swap objects if necessary.
bool swapped = (heightFieldObject.Shape is RayShape);
if (swapped)
MathHelper.Swap(ref rayObject, ref heightFieldObject);
RayShape rayShape = rayObject.Shape as RayShape;
HeightField heightField = heightFieldObject.Shape as HeightField;
// Check if shapes are correct.
if (rayShape == null || heightField == null)
throw new ArgumentException("The contact set must contain a ray and a height field.", "contactSet");
// Assume no contact.
contactSet.HaveContact = false;
// Get transformations.
Vector3F rayScale = rayObject.Scale;
Pose rayPose = rayObject.Pose;
Vector3F heightFieldScale = heightFieldObject.Scale;
Pose heightFieldPose = heightFieldObject.Pose;
// We do not support negative scaling. It is not clear what should happen when y is
// scaled with a negative factor and triangle orders would be wrong... Not worth the trouble.
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.");
// Ray in world space.
Ray rayWorld = new Ray(rayShape);
rayWorld.Scale(ref rayScale);
rayWorld.ToWorld(ref rayPose);
// Ray in local scaled space of the height field.
Ray rayScaled = rayWorld;
rayScaled.ToLocal(ref heightFieldPose);
// Ray in local unscaled space of the mesh.
Ray rayUnscaled = rayScaled;
var inverseCompositeScale = Vector3F.One / heightFieldScale;
rayUnscaled.Scale(ref inverseCompositeScale);
// Get height field and basic info.
int arrayLengthX = heightField.NumberOfSamplesX;
int arrayLengthZ = heightField.NumberOfSamplesZ;
int numberOfCellsX = arrayLengthX - 1;
int numberOfCellsZ = arrayLengthZ - 1;
Debug.Assert(arrayLengthX > 1 && arrayLengthZ > 1, "A height field should contain at least 2 x 2 elements (= 1 cell).");
float cellWidthX = heightField.WidthX / numberOfCellsX; // Unscaled!
float cellWidthZ = heightField.WidthZ / numberOfCellsZ; // Unscaled!
// We use a 2D-DDA traversal of the height field cells. In other words: Look at it from
// above. The height field is our screen and we will select the cells as if we draw
// a pixel line. This could be made more efficient when we do not recompute values and
// reuse values and make incremental steps Bresenham-style.
// See GeometryHelper_Casts.cs method HaveContact(Aabb, ray) for explanation of the
// ray parameter formula.
var rayUnscaledDirectionInverse = new Vector3F(
1 / rayUnscaled.Direction.X,
1 / rayUnscaled.Direction.Y,
1 / rayUnscaled.Direction.Z);
// The position where the ray enters the current cell.
var cellEnter = rayUnscaled.Origin; // Unscaled!!!
var originX = heightField.OriginX;
var originZ = heightField.OriginZ;
// ----- Find first cell.
int indexX = (cellEnter.X >= originX) ? (int)((cellEnter.X - originX) / cellWidthX) : -1; // (int)(...) does not return the desired result for negative values!
if (indexX < 0)
{
if (rayUnscaled.Direction.X <= 0)
return;
float parameter = (originX - rayUnscaled.Origin.X) * rayUnscaledDirectionInverse.X;
if (parameter > rayUnscaled.Length)
return; // The ray does not reach the height field.
cellEnter = rayUnscaled.Origin + parameter * rayUnscaled.Direction;
indexX = 0;
}
else if (indexX >= numberOfCellsX)
{
if (rayUnscaled.Direction.X >= 0)
return;
float parameter = (originX + heightField.WidthX - rayUnscaled.Origin.X) * rayUnscaledDirectionInverse.X;
if (parameter > rayUnscaled.Length)
return; // The ray does not reach the height field.
cellEnter = rayUnscaled.Origin + parameter * rayUnscaled.Direction;
indexX = numberOfCellsX - 1;
}
int indexZ = (cellEnter.Z >= originZ) ? (int)((cellEnter.Z - originZ) / cellWidthZ) : -1;
if (indexZ < 0)
{
if (rayUnscaled.Direction.Z <= 0)
return;
float parameter = (originZ - rayUnscaled.Origin.Z) * rayUnscaledDirectionInverse.Z;
if (parameter > rayUnscaled.Length)
return; // The ray does not reach the next height field.
cellEnter = rayUnscaled.Origin + parameter * rayUnscaled.Direction;
// We also have to correct the indexX!
indexX = (cellEnter.X >= originX) ? (int)((cellEnter.X - originX) / cellWidthX) : -1;
indexZ = 0;
}
else if (indexZ >= numberOfCellsZ)
{
if (rayUnscaled.Direction.Z >= 0)
return;
float parameter = (originZ + heightField.WidthZ - rayUnscaled.Origin.Z) * rayUnscaledDirectionInverse.Z;
if (parameter > rayUnscaled.Length)
return; // The ray does not reach the next height field.
cellEnter = rayUnscaled.Origin + parameter * rayUnscaled.Direction;
indexX = (cellEnter.X >= originX) ? (int)((cellEnter.X - originX) / cellWidthX) : -1;
indexZ = numberOfCellsZ - 1;
}
if (indexX < 0 || indexX >= numberOfCellsX || indexZ < 0 || indexZ >= numberOfCellsZ)
return;
while (true)
{
// ----- Get triangles of current cell.
var triangle0 = heightField.GetTriangle(indexX, indexZ, false);
var triangle1 = heightField.GetTriangle(indexX, indexZ, true);
// Index of first triangle.
var triangleIndex = (indexZ * numberOfCellsX + indexX) * 2;
float xRelative = (cellEnter.X - originX) / cellWidthX - indexX;
float zRelative = (cellEnter.Z - originZ) / cellWidthZ - indexZ;
bool enterSecondTriangle = (xRelative + zRelative) > 1; // The diagonal is where xRel + zRel == 1.
// ----- Find cell exit and move indices to next cell.
// The position where the ray leaves the current cell.
Vector3F cellExit;
float nextXParameter = float.PositiveInfinity;
if (rayUnscaled.Direction.X > 0)
nextXParameter = (originX + (indexX + 1) * cellWidthX - rayUnscaled.Origin.X) * rayUnscaledDirectionInverse.X;
else if (rayUnscaled.Direction.X < 0)
nextXParameter = (originX + indexX * cellWidthX - rayUnscaled.Origin.X) * rayUnscaledDirectionInverse.X;
float nextZParameter = float.PositiveInfinity;
if (rayUnscaled.Direction.Z > 0)
nextZParameter = (originZ + (indexZ + 1) * cellWidthZ - rayUnscaled.Origin.Z) * rayUnscaledDirectionInverse.Z;
else if (rayUnscaled.Direction.Z < 0)
nextZParameter = (originZ + indexZ * cellWidthZ - rayUnscaled.Origin.Z) * rayUnscaledDirectionInverse.Z;
bool isLastCell = false;
if (nextXParameter < nextZParameter)
{
if (rayUnscaled.Direction.X > 0)
{
indexX++;
if (indexX >= numberOfCellsX) // Abort if we have left the height field.
isLastCell = true;
}
else
{
indexX--;
if (indexX < 0)
isLastCell = true;
}
if (nextXParameter > rayUnscaled.Length)
{
isLastCell = true; // The ray does not reach the next cell.
nextXParameter = rayUnscaled.Length;
}
cellExit = rayUnscaled.Origin + nextXParameter * rayUnscaled.Direction;
}
else
{
if (rayUnscaled.Direction.Z > 0)
{
indexZ++;
if (indexZ >= numberOfCellsZ)
isLastCell = true;
}
else
{
indexZ--;
if (indexZ < 0)
isLastCell = true;
}
if (nextZParameter > rayUnscaled.Length)
{
isLastCell = true;
nextZParameter = rayUnscaled.Length;
}
cellExit = rayUnscaled.Origin + nextZParameter * rayUnscaled.Direction;
}
// ----- We can skip cell if cell AABB is below the ray.
var rayMinY = Math.Min(cellEnter.Y, cellExit.Y) - CollisionDetection.Epsilon; // Apply to avoid missing collisions when ray hits a cell border.
// The ray is above if no height field height is higher the ray height.
// (This check handles NaN height values (holes) correctly.)
bool rayIsAbove = !(triangle0.Vertex0.Y >= rayMinY
|| triangle0.Vertex1.Y >= rayMinY
|| triangle0.Vertex2.Y >= rayMinY
|| triangle1.Vertex1.Y >= rayMinY); // Vertex1 of triangle1 is the fourth quad vertex!
// ----- Test ray against the 2 triangles of the cell.
bool triangle0IsHole = false;
bool triangle1IsHole = false;
if (!rayIsAbove)
{
// Abort if a height value is NaN (hole).
triangle0IsHole = Numeric.IsNaN(triangle0.Vertex0.Y * triangle0.Vertex1.Y * triangle0.Vertex2.Y);
triangle1IsHole = Numeric.IsNaN(triangle1.Vertex0.Y * triangle1.Vertex1.Y * triangle1.Vertex2.Y);
bool contactAdded = false;
if (enterSecondTriangle)
{
// Test second triangle first.
if (!triangle1IsHole)
contactAdded = AddContact(contactSet, swapped, type, ref rayWorld, ref rayScaled, ref triangle1, triangleIndex + 1, ref heightFieldPose, ref heightFieldScale);
if (!contactAdded && !triangle0IsHole)
contactAdded = AddContact(contactSet, swapped, type, ref rayWorld, ref rayScaled, ref triangle0, triangleIndex, ref heightFieldPose, ref heightFieldScale);
}
else
{
// Test first triangle first.
if (!triangle0IsHole)
contactAdded = AddContact(contactSet, swapped, type, ref rayWorld, ref rayScaled, ref triangle0, triangleIndex, ref heightFieldPose, ref heightFieldScale);
if (!contactAdded && !triangle1IsHole)
contactAdded = AddContact(contactSet, swapped, type, ref rayWorld, ref rayScaled, ref triangle1, triangleIndex + 1, ref heightFieldPose, ref heightFieldScale);
}
if (contactAdded)
return;
// We have contact and stop for boolean queries.
if (contactSet.HaveContact && type == CollisionQueryType.Boolean)
return;
}
// ----- Return simplified contact if cellEnter is below the cell.
if (!rayIsAbove)
{
if (!enterSecondTriangle && !triangle0IsHole && GeometryHelper.IsInFront(triangle0, cellEnter) < 0
|| enterSecondTriangle && !triangle1IsHole && GeometryHelper.IsInFront(triangle1, cellEnter) < 0)
{
contactSet.HaveContact = true;
if (type == CollisionQueryType.Boolean)
return;
var position = heightFieldPose.ToWorldPosition(cellEnter * heightFieldScale);
var normal = heightFieldPose.ToWorldDirection(Vector3F.UnitY);
if (swapped)
normal = -normal;
float penetrationDepth = (position - rayWorld.Origin).Length;
Contact contact = ContactHelper.CreateContact(contactSet, position, normal, penetrationDepth, true);
ContactHelper.Merge(contactSet, contact, type, CollisionDetection.ContactPositionTolerance);
return;
}
}
// ----- Move to next cell.
if (isLastCell)
return;
cellEnter = cellExit;
}
}
public override void ComputeCollision(ContactSet contactSet, CollisionQueryType type)
{
// Object A should be the plane.
// Object B should be the sphere.
IGeometricObject planeObject = contactSet.ObjectA.GeometricObject;
IGeometricObject sphereObject = contactSet.ObjectB.GeometricObject;
// A should be the plane, swap objects if necessary.
bool swapped = (sphereObject.Shape is PlaneShape);
if (swapped)
MathHelper.Swap(ref planeObject, ref sphereObject);
PlaneShape planeShape = planeObject.Shape as PlaneShape;
SphereShape sphereShape = sphereObject.Shape as SphereShape;
// Check if collision object shapes are correct.
if (planeShape == null || sphereShape == null)
throw new ArgumentException("The contact set must contain a plane and a sphere.", "contactSet");
// Get scalings.
Vector3F planeScale = planeObject.Scale;
Vector3F sphereScale = Vector3F.Absolute(sphereObject.Scale);
// Call other algorithm for non-uniformly scaled spheres.
if (sphereScale.X != sphereScale.Y || sphereScale.Y != sphereScale.Z)
{
if (_fallbackAlgorithm == null)
_fallbackAlgorithm = CollisionDetection.AlgorithmMatrix[typeof(PlaneShape), typeof(ConvexShape)];
_fallbackAlgorithm.ComputeCollision(contactSet, type);
return;
}
// Get poses.
Pose planePose = planeObject.Pose;
Pose spherePose = sphereObject.Pose;
// Apply scaling to plane and transform plane to world space.
Plane plane = new Plane(planeShape);
plane.Scale(ref planeScale); // Scale plane.
plane.ToWorld(ref planePose); // Transform plane to world space.
// Calculate distance from plane to sphere surface.
float sphereRadius = sphereShape.Radius * sphereScale.X;
Vector3F sphereCenter = spherePose.Position;
float planeToSphereDistance = Vector3F.Dot(sphereCenter, plane.Normal) - sphereRadius - plane.DistanceFromOrigin;
float penetrationDepth = -planeToSphereDistance;
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;
}
// Compute contact details.
Vector3F position = sphereCenter - plane.Normal * (sphereRadius - penetrationDepth / 2);
Vector3F normal = (swapped) ? -plane.Normal : plane.Normal;
// Update contact set.
Contact contact = ContactHelper.CreateContact(contactSet, position, normal, penetrationDepth, false);
ContactHelper.Merge(contactSet, contact, type, CollisionDetection.ContactPositionTolerance);
}
private void AddContact(ContactSet contactSet,
bool swapped,
CollisionQueryType type,
ref Ray rayWorld, // The ray in world space.
ref Ray rayInMesh, // The ray in the scaled triangle mesh space.
ref Triangle triangle, // The unscaled triangle in the mesh space.
int triangleIndex,
ref Pose trianglePose,
ref Vector3F triangleScale,
bool isTwoSided)
{
// This code is from GeometryHelper_Triangles.cs. Sync changes!
Vector3F v0 = triangle.Vertex0 * triangleScale;
Vector3F v1 = triangle.Vertex1 * triangleScale;
Vector3F v2 = triangle.Vertex2 * triangleScale;
Vector3F d1 = (v1 - v0);
Vector3F d2 = (v2 - v0);
Vector3F n = Vector3F.Cross(d1, d2);
// Tolerance value, see SOLID, Bergen: "Collision Detection in Interactive 3D Environments".
float ε = n.Length * Numeric.EpsilonFSquared;
Vector3F r = rayInMesh.Direction * rayInMesh.Length;
float δ = -Vector3F.Dot(r, n);
// Degenerate triangle --> No hit.
if (ε == 0.0f || Numeric.IsZero(δ, ε))
return;
Vector3F triangleToRayOrigin = rayInMesh.Origin - v0;
float λ = Vector3F.Dot(triangleToRayOrigin, n) / δ;
if (λ < 0 || λ > 1)
return;
// The ray hit the triangle plane.
Vector3F u = Vector3F.Cross(triangleToRayOrigin, r);
float μ1 = Vector3F.Dot(d2, u) / δ;
float μ2 = Vector3F.Dot(-d1, u) / δ;
if (μ1 + μ2 <= 1 + ε && μ1 >= -ε && μ2 >= -ε)
{
// Hit!
contactSet.HaveContact = true;
if (type == CollisionQueryType.Boolean)
return;
if (δ < 0 && !isTwoSided)
return; // Shooting into the back of a one-sided triangle - no contact.
float penetrationDepth = λ * rayInMesh.Length;
// Create contact info.
Vector3F position = rayWorld.Origin + rayWorld.Direction * penetrationDepth;
n = trianglePose.ToWorldDirection(n);
Debug.Assert(!n.IsNumericallyZero, "Degenerate cases of ray vs. triangle should be treated above.");
n.Normalize();
if (δ < 0)
n = -n;
if (swapped)
n = -n;
Contact contact = ContactHelper.CreateContact(contactSet, position, n, penetrationDepth, true);
if (swapped)
contact.FeatureB = triangleIndex;
else
contact.FeatureA = triangleIndex;
Debug.Assert(
contactSet.ObjectA.GeometricObject.Shape is RayShape && contact.FeatureA == -1 ||
contactSet.ObjectB.GeometricObject.Shape is RayShape && contact.FeatureB == -1,
"RayTriangleMeshAlgorithm has set the wrong feature property.");
ContactHelper.Merge(contactSet, contact, type, CollisionDetection.ContactPositionTolerance);
}
}
public override void ComputeCollision(ContactSet contactSet, CollisionQueryType type)
{
if (type == CollisionQueryType.Contacts)
{
throw new GeometryException("GJK cannot handle contact queries. Use MPR instead.");
}
IGeometricObject objectA = contactSet.ObjectA.GeometricObject;
ConvexShape shapeA = objectA.Shape as ConvexShape;
Vector3 scaleA = objectA.Scale;
Pose poseA = objectA.Pose;
IGeometricObject objectB = contactSet.ObjectB.GeometricObject;
ConvexShape shapeB = objectB.Shape as ConvexShape;
Vector3 scaleB = objectB.Scale;
Pose poseB = objectB.Pose;
if (shapeA == null || shapeB == null)
{
throw new ArgumentException("The contact set must contain two convex shapes.", "contactSet");
}
// GJK builds a simplex of the CSO (A-B). This simplex is managed in a GjkSimplexSolver.
var simplex = GjkSimplexSolver.Create();
bool foundSeparatingAxis = false;
try
{
// v is the separating axis or the CSO point nearest to the origin.
// We start with last known separating axis or with an arbitrary CSO point.
Vector3 v;
if (contactSet.Count > 0)
{
// Use last separating axis.
// The contact normal points from A to B. This is the direction we want to sample first.
// If the frame-to-frame coherence is high we should get a point close to the origin.
// Note: To sample in the normal direction, we need to initialize the CSO point v with
// -normal.
v = -contactSet[0].Normal;
}
else
{
// Use difference of inner points.
Vector3 vA = poseA.ToWorldPosition(shapeA.InnerPoint * scaleA);
Vector3 vB = poseB.ToWorldPosition(shapeB.InnerPoint * scaleB);
v = vA - vB;
}
// If the inner points overlap, then we have already found a contact.
// We don't expect this case to happen often, so we simply choose an arbitrary separating
// axis to continue with the normal GJK code.
if (v.IsNumericallyZero)
{
v = Vector3.UnitZ;
}
// Cache inverted rotations.
var orientationAInverse = poseA.Orientation.Transposed;
var orientationBInverse = poseB.Orientation.Transposed;
int iterationCount = 0;
float distanceSquared = float.MaxValue;
float distanceEpsilon;
// Assume we have no contact.
contactSet.HaveContact = false;
do
{
// TODO: Translate A and B close to the origin to avoid numerical problems.
// This optimization is done in Bullet: The offset (a.Pose.Position + b.Pose.Position) / 2
// is subtracted from a.Pose and b.Pose. This offset is added when the Contact info is
// computed (also in EPA if the poses are still translated).
// Compute a new point w on the simplex. We seek for the point that is closest to the origin.
// Therefore, we get the support points on the current separating axis v.
Vector3 p = poseA.ToWorldPosition(shapeA.GetSupportPoint(orientationAInverse * -v, scaleA));
Vector3 q = poseB.ToWorldPosition(shapeB.GetSupportPoint(orientationBInverse * v, scaleB));
Vector3 w = p - q;
// w projected onto the separating axis.
float delta = Vector3.Dot(w, v);
// If v∙w > 0 then the objects do not overlap.
if (delta > 0)
{
// We have found a separating axis.
foundSeparatingAxis = true;
// Early exit for boolean and contact queries.
if (type == CollisionQueryType.Boolean || type == CollisionQueryType.Contacts)
{
// TODO: We could cache the separating axis n in ContactSet for future collision checks.
return;
}
// We continue for closest point queries because we don't know if there are other
// points closer than p and q.
}
// If the new w is already part of the simplex. We cannot improve any further.
if (simplex.Contains(w))
{
break;
}
// If the new w is not closer to the origin (within numerical tolerance), we stop.
if (distanceSquared - delta <= distanceSquared * Numeric.EpsilonF) // SOLID uses Epsilon = 10^-6
{
break;
}
// Add the new point to the simplex.
simplex.Add(w, p, q);
// Update the simplex. (Unneeded simplex points are removed).
simplex.Update();
// Get new point of simplex closest to the origin.
v = simplex.ClosestPoint;
float previousDistanceSquared = distanceSquared;
distanceSquared = v.LengthSquared();
if (previousDistanceSquared < distanceSquared)
{
// If the result got worse, we use the previous result. This happens for
// degenerate cases for example when the simplex is a tetrahedron with all
// 4 vertices in a plane.
distanceSquared = previousDistanceSquared;
simplex.Backup();
break;
}
// If the new simplex is invalid, we stop.
// Example: A simplex gets invalid if a fourth vertex is added to create a tetrahedron
// simplex but all vertices are in a plane. This can happen if a box corner nearly touches a
// face of another box.
if (!simplex.IsValid)
{
break;
}
// Compare the distance of v to the origin with the distance of the last iteration.
// We stop if the improvement is less than the numerical tolerance.
if (previousDistanceSquared - distanceSquared <= previousDistanceSquared * Numeric.EpsilonF)
{
break;
}
// If we reach the iteration limit, we stop.
iterationCount++;
if (iterationCount > MaxNumberOfIterations)
{
Debug.Assert(false, "GJK reached the iteration limit.");
break;
}
// Compute a scaled epsilon.
distanceEpsilon = Numeric.EpsilonFSquared * Math.Max(1, simplex.MaxVertexDistanceSquared);
// Loop until the simplex is full (i.e. it contains the origin) or we have come
// sufficiently close to the origin.
} while (!simplex.IsFull && distanceSquared > distanceEpsilon);
Debug.Assert(simplex.IsEmpty == false, "The GJK simplex must contain at least 1 point.");
// Compute contact normal and separation.
Vector3 normal = -simplex.ClosestPoint; // simplex.ClosestPoint = ClosestPointA-ClosestPointB
float distance;
distanceEpsilon = Numeric.EpsilonFSquared * Math.Max(1, simplex.MaxVertexDistanceSquared);
if (distanceSquared <= distanceEpsilon)
{
// Distance is approximately 0.
// --> Objects are in contact.
if (simplex.IsValid && normal.TryNormalize())
{
// Normal can be used but we have to invert it because for contact we
// have to compute normal as pointOnA - pointOnB.
normal = -normal;
}
else
{
// No useful normal. Use direction between inner points as a fallback.
Vector3 innerA = poseA.ToWorldPosition(shapeA.InnerPoint * scaleA);
normal = simplex.ClosestPointOnA - innerA;
if (!normal.TryNormalize())
{
Vector3 innerB = poseB.ToWorldPosition(shapeB.InnerPoint * scaleB);
normal = innerB - innerA;
if (!normal.TryNormalize())
{
normal = Vector3.UnitY;
// TODO: We could use better normal: e.g. normal of old contact or PreferredNormal?
}
}
}
distance = 0;
contactSet.HaveContact = true;
}
else
{
// Distance is greater than 0.
distance = (float)Math.Sqrt(distanceSquared);
normal /= distance;
// If the simplex is valid and full, then we have a contact.
if (simplex.IsFull && simplex.IsValid)
{
// Let's use the current result as an estimated contact info for
// shallow contacts.
// TODO: The following IF was added because this can occur for valid
// triangle vs. triangle separation. Check this.
if (!foundSeparatingAxis)
{
contactSet.HaveContact = true;
// Distance is a penetration depth
distance = -distance;
// Since the simplex tetrahedron can have any position in the Minkowsky difference,
// we do not know the real normal. Let's use the current normal and make
// sure that it points away from A. - This is only a heuristic...
Vector3 innerA = poseA.ToWorldPosition(shapeA.InnerPoint * scaleA);
if (Vector3.Dot(simplex.ClosestPointOnA - innerA, normal) < 0)
{
normal = -normal;
}
}
}
}
Debug.Assert(normal.IsNumericallyZero == false);
if (type != CollisionQueryType.Boolean)
{
Vector3 position = (simplex.ClosestPointOnA + simplex.ClosestPointOnB) / 2;
Contact contact = ContactHelper.CreateContact(contactSet, position, normal, -distance, false);
ContactHelper.Merge(contactSet, contact, type, CollisionDetection.ContactPositionTolerance);
}
}
finally
{
simplex.Recycle();
}
}
public override void ComputeCollision(ContactSet contactSet, CollisionQueryType type)
{
// From Coutinho: "Dynamic Simulations of Multibody Systems" and
// Bergen: "Collision Detection in Interactive 3D Environments".
// Object A should be the box.
// Object B should be the sphere.
IGeometricObject boxObject = contactSet.ObjectA.GeometricObject;
IGeometricObject sphereObject = contactSet.ObjectB.GeometricObject;
// Swap objects if necessary.
bool swapped = (sphereObject.Shape is BoxShape);
if (swapped)
MathHelper.Swap(ref boxObject, ref sphereObject);
BoxShape boxShape = boxObject.Shape as BoxShape;
SphereShape sphereShape = sphereObject.Shape as SphereShape;
// Check if collision objects shapes are correct.
if (boxShape == null || sphereShape == null)
throw new ArgumentException("The contact set must contain a box and a sphere.", "contactSet");
Vector3F scaleBox = Vector3F.Absolute(boxObject.Scale);
Vector3F scaleSphere = Vector3F.Absolute(sphereObject.Scale);
// Call other algorithm for non-uniformly scaled spheres.
if (scaleSphere.X != scaleSphere.Y || scaleSphere.Y != scaleSphere.Z)
{
if (_fallbackAlgorithm == null)
_fallbackAlgorithm = CollisionDetection.AlgorithmMatrix[typeof(BoxShape), typeof(ConvexShape)];
_fallbackAlgorithm.ComputeCollision(contactSet, type);
return;
}
// Apply scale.
Vector3F boxExtent = boxShape.Extent * scaleBox;
float sphereRadius = sphereShape.Radius * scaleSphere.X;
// ----- First transform sphere center into the local space of the box.
Pose boxPose = boxObject.Pose;
Vector3F sphereCenterWorld = sphereObject.Pose.Position;
Vector3F sphereCenter = boxPose.ToLocalPosition(sphereCenterWorld);
Vector3F p = Vector3F.Zero;
bool sphereCenterIsContainedInBox = true;
// When sphere center is on a box surface we have to choose a suitable normal.
// otherwise the normal will be computed later.
Vector3F normal = Vector3F.Zero;
#region ----- Look for the point p of the box that is closest to center of the sphere. -----
Vector3F boxHalfExtent = 0.5f * boxExtent;
// x component
if (sphereCenter.X < -boxHalfExtent.X)
{
p.X = -boxHalfExtent.X;
sphereCenterIsContainedInBox = false;
}
else if (sphereCenter.X > boxHalfExtent.X)
{
p.X = boxHalfExtent.X;
sphereCenterIsContainedInBox = false;
}
else
{
p.X = sphereCenter.X;
}
// y component
if (sphereCenter.Y < -boxHalfExtent.Y)
{
p.Y = -boxHalfExtent.Y;
sphereCenterIsContainedInBox = false;
}
else if (sphereCenter.Y > boxHalfExtent.Y)
{
p.Y = boxHalfExtent.Y;
sphereCenterIsContainedInBox = false;
}
else
{
p.Y = sphereCenter.Y;
}
// z component
if (sphereCenter.Z < -boxHalfExtent.Z)
{
p.Z = -boxHalfExtent.Z;
sphereCenterIsContainedInBox = false;
}
else if (sphereCenter.Z > boxHalfExtent.Z)
{
p.Z = boxHalfExtent.Z;
sphereCenterIsContainedInBox = false;
}
else
{
p.Z = sphereCenter.Z;
}
if (sphereCenterIsContainedInBox || (sphereCenter - p).IsNumericallyZero)
{
// Special case: Sphere center is within box. In this case p == center.
// Lets return a point on the surface of the box.
// Lets find the axis with the smallest way out (penetration depth).
Vector3F diff = boxHalfExtent - Vector3F.Absolute(sphereCenter);
if (diff.X <= diff.Y && diff.X <= diff.Z)
{
// Point on one of the x surfaces is nearest.
// Check whether positive or negative surface.
bool positive = (sphereCenter.X > 0);
p.X = positive ? boxHalfExtent.X : -boxHalfExtent.X;
if (Numeric.IsZero(diff.X))
{
// Sphere center is on box surface.
normal = positive ? Vector3F.UnitX : -Vector3F.UnitX;
}
}
else if (diff.Y <= diff.X && diff.Y <= diff.Z)
{
// Point on one of the y surfaces is nearest.
// Check whether positive or negative surface.
bool positive = (sphereCenter.Y > 0);
p.Y = positive ? boxHalfExtent.Y : -boxHalfExtent.Y;
if (Numeric.IsZero(diff.Y))
{
// Sphere center is on box surface.
normal = positive ? Vector3F.UnitY : -Vector3F.UnitY;
}
}
else
{
// Point on one of the z surfaces is nearest.
// Check whether positive or negative surface.
bool positive = (sphereCenter.Z > 0);
p.Z = positive ? boxHalfExtent.Z : -boxHalfExtent.Z;
if (Numeric.IsZero(diff.Z))
{
// Sphere center is on box surface.
normal = positive ? Vector3F.UnitZ : -Vector3F.UnitZ;
}
}
}
#endregion
// ----- Convert back to world space
p = boxPose.ToWorldPosition(p);
Vector3F sphereCenterToP = p - sphereCenterWorld;
// Compute penetration depth.
float penetrationDepth = sphereCenterIsContainedInBox
? sphereRadius + sphereCenterToP.Length
: sphereRadius - sphereCenterToP.Length;
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;
}
// ----- Create collision info.
// Compute normal if we haven't set one yet.
if (normal == Vector3F.Zero)
{
Debug.Assert(!sphereCenterToP.IsNumericallyZero, "When the center of the sphere lies on the box surface a normal should be have been set explicitly.");
normal = sphereCenterIsContainedInBox ? sphereCenterToP : -sphereCenterToP;
normal.Normalize();
}
else
{
normal = boxPose.ToWorldDirection(normal);
}
// Position = point between sphere and box surface.
Vector3F position = p - normal * (penetrationDepth / 2);
if (swapped)
normal = -normal;
// Update contact set.
Contact contact = ContactHelper.CreateContact(contactSet, position, normal, penetrationDepth, false);
ContactHelper.Merge(contactSet, contact, type, CollisionDetection.ContactPositionTolerance);
}
public override void ComputeCollision(ContactSet contactSet, CollisionQueryType type)
{
// Invoke GJK for closest points.
if (type == CollisionQueryType.ClosestPoints)
{
throw new GeometryException("This collision algorithm cannot handle closest-point queries. Use GJK instead.");
}
//if (UseMpr)
//{
// if (_mpr == null)
// _mpr = new MinkowskiPortalRefinement(CollisionDetection);
// _mpr.ComputeCollision(contactSet, type);
// return;
//}
CollisionObject collisionObjectA = contactSet.ObjectA;
CollisionObject collisionObjectB = contactSet.ObjectB;
IGeometricObject geometricObjectA = collisionObjectA.GeometricObject;
IGeometricObject geometricObjectB = collisionObjectB.GeometricObject;
TriangleShape triangleShapeA = geometricObjectA.Shape as TriangleShape;
TriangleShape triangleShapeB = geometricObjectB.Shape as TriangleShape;
// Check if collision objects shapes are correct.
if (triangleShapeA == null || triangleShapeB == null)
{
throw new ArgumentException("The contact set must contain triangle shapes.", "contactSet");
}
Vector3 scaleA = Vector3.Absolute(geometricObjectA.Scale);
Vector3 scaleB = Vector3.Absolute(geometricObjectB.Scale);
Pose poseA = geometricObjectA.Pose;
Pose poseB = geometricObjectB.Pose;
// Get triangles in world space.
Triangle triangleA;
triangleA.Vertex0 = poseA.ToWorldPosition(triangleShapeA.Vertex0 * scaleA);
triangleA.Vertex1 = poseA.ToWorldPosition(triangleShapeA.Vertex1 * scaleA);
triangleA.Vertex2 = poseA.ToWorldPosition(triangleShapeA.Vertex2 * scaleA);
Triangle triangleB;
triangleB.Vertex0 = poseB.ToWorldPosition(triangleShapeB.Vertex0 * scaleB);
triangleB.Vertex1 = poseB.ToWorldPosition(triangleShapeB.Vertex1 * scaleB);
triangleB.Vertex2 = poseB.ToWorldPosition(triangleShapeB.Vertex2 * scaleB);
if (type == CollisionQueryType.Boolean)
{
contactSet.HaveContact = GeometryHelper.HaveContact(ref triangleA, ref triangleB);
return;
}
Debug.Assert(type == CollisionQueryType.Contacts, "TriangleTriangleAlgorithm cannot handle closest point queries.");
// Assume no contact.
contactSet.HaveContact = false;
Vector3 position, normal;
float penetrationDepth;
if (!GetContact(ref triangleA, ref triangleB, false, false, out position, out normal, out penetrationDepth))
{
return;
}
contactSet.HaveContact = true;
Contact contact = ContactHelper.CreateContact(contactSet, position, normal, penetrationDepth, false);
ContactHelper.Merge(contactSet, contact, type, CollisionDetection.ContactPositionTolerance);
}
