/// <summary>
/// Initializes a new instance of the <see cref="CombinedCollisionAlgorithm"/> class.
/// </summary>
/// <param name="collisionDetection">The collision detection service.</param>
/// <param name="closestPointsAlgorithm">The closest points algorithm.</param>
/// <param name="contactAlgorithm">The contact algorithm.</param>
/// <exception cref="ArgumentNullException">
/// <paramref name="closestPointsAlgorithm"/> is <see langword="null"/>.
/// </exception>
/// <exception cref="ArgumentNullException">
/// <paramref name="contactAlgorithm"/> is <see langword="null"/>.
/// </exception>
public CombinedCollisionAlgorithm(CollisionDetection collisionDetection, CollisionAlgorithm closestPointsAlgorithm, CollisionAlgorithm contactAlgorithm)
: base(collisionDetection)
{
if (closestPointsAlgorithm == null)
throw new ArgumentNullException("closestPointsAlgorithm");
if (contactAlgorithm == null)
throw new ArgumentNullException("contactAlgorithm");
_closestPointsAlgorithm = closestPointsAlgorithm;
_contactAlgorithm = contactAlgorithm;
}
/// <summary>
/// Initializes a new instance of the <see cref="CombinedCollisionAlgorithm"/> class.
/// </summary>
/// <param name="collisionDetection">The collision detection service.</param>
/// <param name="closestPointsAlgorithm">The closest points algorithm.</param>
/// <param name="contactAlgorithm">The contact algorithm.</param>
/// <exception cref="ArgumentNullException">
/// <paramref name="closestPointsAlgorithm"/> is <see langword="null"/>.
/// </exception>
/// <exception cref="ArgumentNullException">
/// <paramref name="contactAlgorithm"/> is <see langword="null"/>.
/// </exception>
public CombinedCollisionAlgorithm(CollisionDetection collisionDetection, CollisionAlgorithm closestPointsAlgorithm, CollisionAlgorithm contactAlgorithm)
: base(collisionDetection)
{
if (closestPointsAlgorithm == null)
{
throw new ArgumentNullException("closestPointsAlgorithm");
}
if (contactAlgorithm == null)
{
throw new ArgumentNullException("contactAlgorithm");
}
_closestPointsAlgorithm = closestPointsAlgorithm;
_contactAlgorithm = contactAlgorithm;
}
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);
Vector3F lineScale = lineGeometricObject.Scale;
line.Scale(ref lineScale);
// Step 1: Get any bounding sphere that encloses the other object.
Aabb aabb = otherGeometricObject.Aabb;
Vector3F 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.
Vector3F 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 = Vector3F.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.
Vector3F v0A = geometricObjectA.Pose.ToWorldPosition(shapeA.InnerPoint * geometricObjectA.Scale);
Vector3F v0B = geometricObjectB.Pose.ToWorldPosition(shapeB.InnerPoint * geometricObjectB.Scale);
Vector3F n = v0B - v0A; // This is the default MPR ray direction.
// Make n normal to the line.
n = n - Vector3F.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)
{
// 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)
{
// 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);
}
// 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;
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;
// 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)
{
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)
{
// 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)
{
// 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)
{
// 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);
}
}
}
}
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);
}
}
}
}
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);
}
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);
}
}
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)
{
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);
}
// 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);
}
}
