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();
}
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)
{
// 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();
}
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.
Vector3F rayScale = rayObject.Scale;
Pose rayPose = rayObject.Pose;
Vector3F 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 = Vector3F.One / compositeScale;
ray.Scale(ref inverseCompositeScale);
try
{
if (compositeShape.Partition != null)
{
#region ----- Composite with BVH vs. * -----
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);
}
#endregion
}
else
{
#region ----- Composite vs. *-----
var rayDirectionInverse = new Vector3F(
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;
}
}
}
#endregion
}
}
finally
{
Debug.Assert(compositeObject.Shape == compositeShape, "Shape was altered and not restored.");
testContactSet.Recycle();
ResourcePools.TestCollisionObjects.Recycle(testCollisionObject);
testGeometricObject.Recycle();
}
}
public void ConstructorException3()
{
CollisionObject a = new CollisionObject();
ContactSet.Create(a, a);
}
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;
}
}
Vector3 scaleHeightField = heightFieldGeometricObject.Scale;
Vector3 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.
Vector3 heightFieldUpAxis = heightFieldPose.ToWorldDirection(Vector3.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;
// 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 = Vector3.Zero;
lineSegment.End = -heightField.Depth * Vector3.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.
Vector3 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 (Vector3.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;
// Get barycentric coordinates of contact position.
Vector3 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 Vector3(float.NaN);
}
}
else
{
if (xIndex > 0)
{
neighborNormal = heightFieldPose.ToWorldDirection(heightField.GetTriangle(xIndex - 1, zIndex, true).Normal / scaleHeightField);
neighborNormal.TryNormalize();
}
else
{
neighborNormal = new Vector3(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 Vector3(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 Vector3(float.NaN);
}
}
}
}
// Contact normals in the range triangleNormal - neighborNormal are allowed.
// Others, especially vertical contacts in slopes or horizontal normals are not
// allowed.
var cosMinAngle = Vector3.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 = Vector3.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) &&
Vector3.Dot(heightFieldUpAxis, triangleNormal) < WeldingLimit)
{
testContactSet.PreferredNormal = (swapped) ? -triangleNormal : triangleNormal;
collisionAlgorithm.ComputeCollision(testContactSet, CollisionQueryType.Contacts);
testContactSet.PreferredNormal = Vector3.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);
// 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);
}
}
}
}
}
}
// 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);
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);
}
}
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 void ConstructorException()
{
ContactSet.Create(null, new CollisionObject());
}
public void ConstructorException2()
{
ContactSet.Create(new CollisionObject(), null);
}
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;
Vector3 scaleA = geometricObjectA.Scale;
Vector3 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)
{
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);
}
}
else
{
// 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(Vector3.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);
}
}
}
else
{
// 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(Vector3.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;
}
}
}
}
}
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();
}
}
// See FAST paper: "Interactive Continuous Collision Detection for Non-Convex Polyhedra", Kim Young et al.
/// <summary>
/// Gets the time of impact using Conservative Advancement.
/// </summary>
/// <param name="objectA">The object A.</param>
/// <param name="targetPoseA">The target pose of A.</param>
/// <param name="objectB">The object B.</param>
/// <param name="targetPoseB">The target pose of B.</param>
/// <param name="allowedPenetrationDepth">The allowed penetration depth.</param>
/// <param name="collisionDetection">The collision detection.</param>
/// <returns>
/// The time of impact in the range [0, 1].
/// </returns>
/// <remarks>
/// This algorithm does not work for concave objects.
/// </remarks>
/// <exception cref="ArgumentNullException">
/// <paramref name="objectA"/>, <paramref name="objectB"/> or
/// <paramref name="collisionDetection"/> is <see langword="null"/>.
/// </exception>
internal static float GetTimeOfImpactCA(CollisionObject objectA, Pose targetPoseA,
CollisionObject objectB, Pose targetPoseB, float allowedPenetrationDepth,
CollisionDetection collisionDetection) // Required for collision algorithm matrix.
{
if (objectA == null)
{
throw new ArgumentNullException("objectA");
}
if (objectB == null)
{
throw new ArgumentNullException("objectB");
}
if (collisionDetection == null)
{
throw new ArgumentNullException("collisionDetection");
}
IGeometricObject geometricObjectA = objectA.GeometricObject;
IGeometricObject geometricObjectB = objectB.GeometricObject;
Pose startPoseA = geometricObjectA.Pose;
Pose startPoseB = geometricObjectB.Pose;
// Get angular velocity ω of object A (as magnitude + rotation axis).
// qEnd = ∆q * qStart
// => ∆q = qEnd * qStart.Inverse
QuaternionF qA = QuaternionF.CreateRotation(targetPoseA.Orientation * startPoseA.Orientation.Transposed);
// ω = ∆α / ∆t, ∆t = 1
// => ω = ∆α
float ωA = qA.Angle; // Magnitude |ω|
Vector3F ωAxisA = (!Numeric.AreEqual(qA.W, 1)) ? qA.Axis : Vector3F.UnitX; // Rotation axis of ω
// Get angular velocity ω of object B (as magnitude + rotation axis).
// (Same as above.)
QuaternionF qB = QuaternionF.CreateRotation(targetPoseB.Orientation * startPoseB.Orientation.Transposed);
float ωB = qB.Angle; // Magnitude |ω|
Vector3F ωAxisB = (!Numeric.AreEqual(qB.W, 1)) ? qB.Axis : Vector3F.UnitX; // Rotation axis of ω
// Bounding sphere radii.
float rMaxA = GetBoundingRadius(geometricObjectA);
float rMaxB = GetBoundingRadius(geometricObjectB);
// |ω| * rMax is the angular part of the projected velocity bound.
float angularVelocityProjected = ωA * rMaxA + ωB * rMaxB;
// Compute relative linear velocity.
// (linearRelVel ∙ normal > 0 if objects are getting closer.)
Vector3F linearVelocityA = targetPoseA.Position - startPoseA.Position;
Vector3F linearVelocityB = targetPoseB.Position - startPoseB.Position;
Vector3F linearVelocityRelative = linearVelocityA - linearVelocityB;
// Abort if relative movement is zero.
if (Numeric.IsZero(linearVelocityRelative.Length + angularVelocityProjected))
{
return(1);
}
var distanceAlgorithm = collisionDetection.AlgorithmMatrix[objectA, objectB];
// Use temporary test objects.
var testGeometricObjectA = TestGeometricObject.Create();
testGeometricObjectA.Shape = geometricObjectA.Shape;
testGeometricObjectA.Scale = geometricObjectA.Scale;
testGeometricObjectA.Pose = startPoseA;
var testGeometricObjectB = TestGeometricObject.Create();
testGeometricObjectB.Shape = geometricObjectB.Shape;
testGeometricObjectB.Scale = geometricObjectB.Scale;
testGeometricObjectB.Pose = startPoseB;
var testCollisionObjectA = ResourcePools.TestCollisionObjects.Obtain();
testCollisionObjectA.SetInternal(objectA, testGeometricObjectA);
var testCollisionObjectB = ResourcePools.TestCollisionObjects.Obtain();
testCollisionObjectB.SetInternal(objectB, testGeometricObjectB);
var testContactSet = ContactSet.Create(testCollisionObjectA, testCollisionObjectB);
try
{
distanceAlgorithm.UpdateClosestPoints(testContactSet, 0);
if (testContactSet.Count < 0)
{
// No distance result --> Abort.
return(1);
}
Vector3F normal = testContactSet[0].Normal;
float distance = -testContactSet[0].PenetrationDepth;
float λ = 0;
float λPrevious = 0;
for (int i = 0; i < MaxNumberOfIterations && distance > 0; i++)
{
// |v∙n|
float linearVelocityProject = Vector3F.Dot(linearVelocityRelative, normal);
// |n x ω| * rMax
angularVelocityProjected = Vector3F.Cross(normal, ωAxisA).Length *ωA *rMaxA
+ Vector3F.Cross(normal, ωAxisB).Length *ωB *rMaxB;
// Total projected velocity.
float velocityProjected = linearVelocityProject + angularVelocityProjected;
// Abort for separating objects.
if (Numeric.IsLess(velocityProjected, 0))
{
break;
}
// Increase TOI.
float μ = (distance + allowedPenetrationDepth) / velocityProjected;
λ = λ + μ;
if (λ < 0 || λ > 1)
{
break;
}
Debug.Assert(λPrevious < λ);
if (λ <= λPrevious)
{
break;
}
// Get new interpolated poses.
Vector3F positionA = startPoseA.Position + λ * (targetPoseA.Position - startPoseA.Position);
Matrix33F rotationA = Matrix33F.CreateRotation(ωAxisA, λ * ωA);
testGeometricObjectA.Pose = new Pose(positionA, rotationA * startPoseA.Orientation);
Vector3F positionB = startPoseB.Position + λ * (targetPoseB.Position - startPoseB.Position);
Matrix33F rotationB = Matrix33F.CreateRotation(ωAxisB, λ * ωB);
testGeometricObjectB.Pose = new Pose(positionB, rotationB * startPoseB.Orientation);
// Get new closest point distance.
distanceAlgorithm.UpdateClosestPoints(testContactSet, 0);
if (testContactSet.Count == 0)
{
break;
}
normal = testContactSet[0].Normal;
distance = -testContactSet[0].PenetrationDepth;
λPrevious = λ;
}
if (testContactSet.HaveContact && λ > 0 && λ < 1 && testContactSet.Count > 0)
{
return(λ);
// We already have a contact that we could use.
// result.Contact = testContactSet[0];
}
}
finally
{
// Recycle temporary objects.
testContactSet.Recycle(true);
ResourcePools.TestCollisionObjects.Recycle(testCollisionObjectA);
ResourcePools.TestCollisionObjects.Recycle(testCollisionObjectB);
testGeometricObjectA.Recycle();
testGeometricObjectB.Recycle();
}
return(1);
}
// See FAST paper: "Interactive Continuous Collision Detection for Non-Convex Polyhedra", Kim Young et al.
/// <summary>
/// Gets the time of impact using Conservative Advancement (ignoring rotational movement).
/// </summary>
/// <param name="objectA">The object A.</param>
/// <param name="targetPoseA">The target pose of A.</param>
/// <param name="objectB">The object B.</param>
/// <param name="targetPoseB">The target pose of B.</param>
/// <param name="allowedPenetration">The allowed penetration depth.</param>
/// <param name="collisionDetection">The collision detection.</param>
/// <returns>
/// The time of impact in the range [0, 1].
/// </returns>
/// <remarks>
/// This algorithm does not work for concave objects.
/// </remarks>
/// <exception cref="ArgumentNullException">
/// <paramref name="objectA"/>, <paramref name="objectB"/> or
/// <paramref name="collisionDetection"/> is <see langword="null"/>.
/// </exception>
internal static float GetTimeOfImpactLinearCA(CollisionObject objectA, Pose targetPoseA,
CollisionObject objectB, Pose targetPoseB, float allowedPenetration,
CollisionDetection collisionDetection) // Required for collision algorithm matrix.
{
if (objectA == null)
{
throw new ArgumentNullException("objectA");
}
if (objectB == null)
{
throw new ArgumentNullException("objectB");
}
if (collisionDetection == null)
{
throw new ArgumentNullException("collisionDetection");
}
IGeometricObject geometricObjectA = objectA.GeometricObject;
IGeometricObject geometricObjectB = objectB.GeometricObject;
Pose startPoseA = geometricObjectA.Pose;
Pose startPoseB = geometricObjectB.Pose;
// Compute relative linear velocity.
// (linearRelVel ∙ normal > 0 if objects are getting closer.)
Vector3 linearVelocityA = targetPoseA.Position - startPoseA.Position;
Vector3 linearVelocityB = targetPoseB.Position - startPoseB.Position;
Vector3 linearVelocityRelative = linearVelocityA - linearVelocityB;
// Abort if relative movement is zero.
if (Numeric.IsZero(linearVelocityRelative.Length))
{
return(1);
}
var distanceAlgorithm = collisionDetection.AlgorithmMatrix[objectA, objectB];
// Use temporary test objects.
var testGeometricObjectA = TestGeometricObject.Create();
testGeometricObjectA.Shape = geometricObjectA.Shape;
testGeometricObjectA.Scale = geometricObjectA.Scale;
testGeometricObjectA.Pose = startPoseA;
var testGeometricObjectB = TestGeometricObject.Create();
testGeometricObjectB.Shape = geometricObjectB.Shape;
testGeometricObjectB.Scale = geometricObjectB.Scale;
testGeometricObjectB.Pose = startPoseB;
var testCollisionObjectA = ResourcePools.TestCollisionObjects.Obtain();
testCollisionObjectA.SetInternal(objectA, testGeometricObjectA);
var testCollisionObjectB = ResourcePools.TestCollisionObjects.Obtain();
testCollisionObjectB.SetInternal(objectB, testGeometricObjectB);
var testContactSet = ContactSet.Create(testCollisionObjectA, testCollisionObjectB);
try
{
distanceAlgorithm.UpdateClosestPoints(testContactSet, 0);
if (testContactSet.Count < 0)
{
// No closest-distance result. --> Abort.
return(1);
}
Vector3 normal = testContactSet[0].Normal;
float distance = -testContactSet[0].PenetrationDepth;
float λ = 0;
float λPrevious = 0;
for (int i = 0; i < MaxNumberOfIterations && distance > 0; i++)
{
// |v∙n|
float velocityProjected = Vector3.Dot(linearVelocityRelative, normal);
// Abort for separating objects.
if (Numeric.IsLess(velocityProjected, 0))
{
break;
}
// Increase TOI.
float μ = (distance + allowedPenetration) / velocityProjected;
λ = λ + μ;
if (λ < 0 || λ > 1)
{
break;
}
Debug.Assert(λPrevious < λ);
if (λ <= λPrevious)
{
break;
}
// Get new interpolated poses - only positions are changed.
Vector3 positionA = startPoseA.Position + λ * (targetPoseA.Position - startPoseA.Position);
testGeometricObjectA.Pose = new Pose(positionA, startPoseA.Orientation);
Vector3 positionB = startPoseB.Position + λ * (targetPoseB.Position - startPoseB.Position);
testGeometricObjectB.Pose = new Pose(positionB, startPoseB.Orientation);
// Get new closest point distance.
distanceAlgorithm.UpdateClosestPoints(testContactSet, 0);
if (testContactSet.Count == 0)
{
break;
}
normal = testContactSet[0].Normal;
distance = -testContactSet[0].PenetrationDepth;
λPrevious = λ;
}
if (testContactSet.HaveContact && λ > 0 && λ < 1 && testContactSet.Count > 0)
{
return(λ);
// We already have a contact that we could use.
// result.Contact = testContactSet[0];
}
}
finally
{
// Recycle temporary objects.
testContactSet.Recycle(true);
ResourcePools.TestCollisionObjects.Recycle(testCollisionObjectA);
ResourcePools.TestCollisionObjects.Recycle(testCollisionObjectB);
testGeometricObjectA.Recycle();
testGeometricObjectB.Recycle();
}
return(1);
}
public void Merge1()
{
ContactSet set = ContactSet.Create(new CollisionObject(), new CollisionObject());
// Separated contact is not merged
Contact contact = Contact.Create();
contact.Position = new Vector3F();
contact.PenetrationDepth = -1;
ContactHelper.Merge(set, contact, CollisionQueryType.Contacts, 0.1f);
Assert.AreEqual(0, set.Count);
// Merge first contact.
contact = Contact.Create();
//contact.ApplicationData = "AppData";
contact.Lifetime = 10;
contact.Position = new Vector3F();
contact.PenetrationDepth = 1;
contact.UserData = "UserData";
ContactHelper.Merge(
set,
contact,
CollisionQueryType.Contacts,
0.1f);
Assert.AreEqual(1, set.Count);
// Merge next contact.
contact = Contact.Create();
contact.Position = new Vector3F();
contact.PenetrationDepth = 1;
ContactHelper.Merge(
set,
contact,
CollisionQueryType.Contacts,
0.1f);
Assert.AreEqual(1, set.Count);
//Assert.AreEqual("AppData", set[0].ApplicationData);
Assert.AreEqual(10, set[0].Lifetime);
Assert.AreEqual("UserData", set[0].UserData);
// TODO: This functionality was replaced. Write new tests.
//// Test ray casts.
//set.Clear();
//((DefaultGeometry)set.ObjectA.GeometricObject).Shape = new RayShape();
//Contact newContact = new Contact
// {
// Position = new Vector3F(),
// PenetrationDepth = 1,
// IsRayHit = true
// };
//ContactHelper.Merge(set, newContact, CollisionQueryType.ClosestPoints, 0.1f);
//Assert.AreEqual(1, set.Count);
//newContact = new Contact
// {
// Position = new Vector3F(1, 2, 3),
// PenetrationDepth = -1,
// IsRayHit = false
// };
//ContactHelper.Merge(set, newContact, CollisionQueryType.ClosestPoints, 0.1f);
//Assert.AreEqual(1, set.Count);
//Assert.AreEqual(1f, set[0].PenetrationDepth);
//Assert.AreEqual(new Vector3F(), set[0].Position);
//newContact = new Contact
// {
// Position = new Vector3F(1, 2, 3),
// PenetrationDepth = 0.5f,
// IsRayHit = true
// };
//ContactHelper.Merge(set, newContact, CollisionQueryType.ClosestPoints, 0.1f);
//Assert.AreEqual(1, set.Count);
//Assert.AreEqual(0.5f, set[0].PenetrationDepth);
//Assert.AreEqual(new Vector3F(1, 2, 3), set[0].Position);
//set.Clear();
//set.Add(
// new Contact
// {
// Position = new Vector3F(0, 0, 0),
// PenetrationDepth = -1,
// IsRayHit = false,
// });
//newContact = new Contact
// {
// Position = new Vector3F(1, 2, 3),
// PenetrationDepth = -2,
// IsRayHit = false
// };
//ContactHelper.Merge(set, newContact, CollisionQueryType.ClosestPoints, 0.1f);
//Assert.AreEqual(1, set.Count);
//Assert.AreEqual(-1f, set[0].PenetrationDepth);
//Assert.AreEqual(new Vector3F(0, 0, 0), set[0].Position);
//newContact = new Contact
// {
// Position = new Vector3F(1, 2, 3),
// PenetrationDepth = -0.5f,
// IsRayHit = false,
// };
//ContactHelper.Merge(set, newContact, CollisionQueryType.ClosestPoints, 0.1f);
//Assert.AreEqual(1, set.Count);
//Assert.AreEqual(-0.5f, set[0].PenetrationDepth);
//Assert.AreEqual(new Vector3F(1, 2, 3), set[0].Position);
//newContact = new Contact
// {
// Position = new Vector3F(3, 3, 3),
// PenetrationDepth = 1,
// IsRayHit = true
// };
//ContactHelper.Merge(set, newContact, CollisionQueryType.ClosestPoints, 0.1f);
//Assert.AreEqual(1, set.Count);
//Assert.AreEqual(1f, set[0].PenetrationDepth);
//Assert.AreEqual(new Vector3F(3, 3, 3), set[0].Position);
//((DefaultGeometry)set.ObjectA.GeometricObject).Shape = new Sphere();
//// Test closest points.
//set.Clear();
//ContactHelper.Merge(
// set,
// new Contact { Position = new Vector3F(), PenetrationDepth = 1 },
// CollisionQueryType.ClosestPoints,
// 0.1f);
//Assert.AreEqual(1, set.Count);
//ContactHelper.Merge(
// set,
// new Contact { Position = new Vector3F(1, 2, 3), PenetrationDepth = 0 },
// CollisionQueryType.ClosestPoints,
// 0.1f);
//Assert.AreEqual(1, set.Count);
//Assert.AreEqual(1, set[0].PenetrationDepth);
//ContactHelper.Merge(
// set,
// new Contact { Position = new Vector3F(1, 2, 3), PenetrationDepth = 1.1f },
// CollisionQueryType.ClosestPoints,
// 0.1f);
//Assert.AreEqual(1, set.Count);
//Assert.AreEqual(1.1f, set[0].PenetrationDepth);
//Assert.AreEqual(new Vector3F(1, 2, 3), set[0].Position);
//// Test default case with automatic reduction.
//set.Clear();
//ContactHelper.Merge(
// set,
// new Contact { Position = new Vector3F(), PenetrationDepth = 1 },
// CollisionQueryType.Contacts,
// 0.1f);
//ContactHelper.Merge(
// set,
// new Contact { Position = new Vector3F(1, 0, 0), PenetrationDepth = 1 },
// CollisionQueryType.Contacts,
// 0.1f);
//ContactHelper.Merge(
// set,
// new Contact { Position = new Vector3F(0, 1, 0), PenetrationDepth = 1 },
// CollisionQueryType.Contacts,
// 0.1f);
//ContactHelper.Merge(
// set,
// new Contact { Position = new Vector3F(0, 0, 1), PenetrationDepth = 1 },
// CollisionQueryType.Contacts,
// 0.1f);
//ContactHelper.Merge(
// set,
// new Contact { Position = new Vector3F(2, 0, 0), PenetrationDepth = 1 },
// CollisionQueryType.Contacts,
// 0.1f);
//Assert.AreEqual(4, set.Count); // Reduced to 4 instead of 5.
}
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);
}
}
