private bool HaveContact(IGeometricObject clipGeometry, Vector3F position)
{
// Use a shared collision detection instance.
var collisionDetection = SceneHelper.CollisionDetection;
if (_sphereCollisionObject == null)
{
// First time initializations.
var sphereGeometricObject = TestGeometricObject.Create();
sphereGeometricObject.Shape = new SphereShape(0);
_sphereCollisionObject = new CollisionObject(sphereGeometricObject);
_clipCollisionObject = new CollisionObject(TestGeometricObject.Create());
}
var _sphereGeometricObject = (TestGeometricObject)_sphereCollisionObject.GeometricObject;
_sphereGeometricObject.Pose = new Pose(position);
_clipCollisionObject.GeometricObject = clipGeometry;
var result = collisionDetection.HaveContact(_sphereCollisionObject, _clipCollisionObject);
_clipCollisionObject.GeometricObject = null;
return(result);
}
public void Events()
{
// The events are "empty". This is only for full code coverage:
var g = TestGeometricObject.Create();
g.PoseChanged += OnChanged;
g.ShapeChanged += OnChanged;
g.Pose = new Pose(new Vector3(1, 2, 3));
g.Shape = new BoxShape(1, 2, 3);
g.PoseChanged -= OnChanged;
g.ShapeChanged -= OnChanged;
}
/// <inheritdoc/>
/// <exception cref="ArgumentException">
/// Neither <paramref name="objectA"/> nor <paramref name="objectB"/> contains a
/// <see cref="HeightField"/>.
/// </exception>
/// <exception cref="ArgumentNullException">
/// <paramref name="objectA"/> or <paramref name="objectB"/> is <see langword="null"/>.
/// </exception>
/// <exception cref="NotSupportedException">
/// <paramref name="objectA"/> or <paramref name="objectB"/> contains a
/// <see cref="HeightField"/> with a negative scaling. Computing collisions for height fields
/// with a negative scaling is not supported.
/// </exception>
public override float GetTimeOfImpact(CollisionObject objectA, Pose targetPoseA, CollisionObject objectB, Pose targetPoseB, float allowedPenetration)
{
if (objectA == null)
{
throw new ArgumentNullException("objectA");
}
if (objectB == null)
{
throw new ArgumentNullException("objectB");
}
// Object A should be the height field, swap objects if necessary.
if (!(objectA.GeometricObject.Shape is HeightField))
{
MathHelper.Swap(ref objectA, ref objectB);
MathHelper.Swap(ref targetPoseA, ref targetPoseB);
}
IGeometricObject geometricObjectA = objectA.GeometricObject;
IGeometricObject geometricObjectB = objectB.GeometricObject;
HeightField heightFieldA = geometricObjectA.Shape as HeightField;
// Check if collision object shapes are correct.
if (heightFieldA == null)
{
throw new ArgumentException("One object must be a height field.");
}
// Height field vs height field makes no sense.
if (objectB.GeometricObject.Shape is HeightField)
{
return(1);
}
Pose startPoseA = geometricObjectA.Pose;
Pose startPoseB = geometricObjectB.Pose;
Vector3 scaleA = geometricObjectA.Scale;
Vector3 scaleB = geometricObjectB.Scale;
// We do not support negative scaling (see comments in ComputeCollision).
if (scaleA.X < 0 || scaleA.Y < 0 || scaleA.Z < 0)
{
throw new NotSupportedException("Computing collisions for height fields with a negative scaling is not supported.");
}
// Get an AABB of the swept B in the space of A.
// This simplified AABB can miss some rotational movement.
// To simplify, we assume that A is static and B is moving relative to A.
// In general, this is not correct! But for CCD we make this simplification.
// We convert everything to the space of A.
var aabbSweptBInA = geometricObjectB.Shape.GetAabb(scaleB, startPoseA.Inverse * startPoseB);
aabbSweptBInA.Grow(geometricObjectB.Shape.GetAabb(scaleB, targetPoseA.Inverse * targetPoseB));
// Use temporary object.
var triangleShape = ResourcePools.TriangleShapes.Obtain();
// (Vertices will be set in the loop below.)
var testGeometricObject = TestGeometricObject.Create();
testGeometricObject.Shape = triangleShape;
testGeometricObject.Scale = Vector3.One;
testGeometricObject.Pose = startPoseA;
var testCollisionObject = ResourcePools.TestCollisionObjects.Obtain();
testCollisionObject.SetInternal(objectA, testGeometricObject);
var collisionAlgorithm = CollisionDetection.AlgorithmMatrix[typeof(TriangleShape), geometricObjectB.Shape.GetType()];
// Get height field and basic info.
int arrayLengthX = heightFieldA.NumberOfSamplesX;
int arrayLengthZ = heightFieldA.NumberOfSamplesZ;
Debug.Assert(arrayLengthX > 1 && arrayLengthZ > 1, "A height field should contain at least 2 x 2 elements (= 1 cell).");
float cellWidthX = heightFieldA.WidthX * scaleA.X / (arrayLengthX - 1);
float cellWidthZ = heightFieldA.WidthZ * scaleA.Z / (arrayLengthZ - 1);
float originX = heightFieldA.OriginX;
float originZ = heightFieldA.OriginZ;
// ----- Compute the cell indices for the AABB.
// Estimate start and end indices from our search distance.
int xIndexStartEstimated = (int)((aabbSweptBInA.Minimum.X - originX) / cellWidthX);
int xIndexEndEstimated = (int)((aabbSweptBInA.Maximum.X - originX) / cellWidthX);
int zIndexStartEstimated = (int)((aabbSweptBInA.Minimum.Z - originZ) / cellWidthZ);
int zIndexEndEstimated = (int)((aabbSweptBInA.Maximum.Z - 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);
float timeOfImpact = 1;
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 = heightFieldA.GetTriangle(xIndex, zIndex, triangleIndex != 0);
// Apply scale.
triangle.Vertex0 = triangle.Vertex0 * scaleA;
triangle.Vertex1 = triangle.Vertex1 * scaleA;
triangle.Vertex2 = triangle.Vertex2 * scaleA;
// Make AABB test of triangle vs. sweep of B.
if (!GeometryHelper.HaveContact(aabbSweptBInA, triangle.Aabb))
{
continue;
}
triangleShape.Vertex0 = triangle.Vertex0;
triangleShape.Vertex1 = triangle.Vertex1;
triangleShape.Vertex2 = triangle.Vertex2;
float triangleTimeOfImpact = collisionAlgorithm.GetTimeOfImpact(
testCollisionObject,
targetPoseA,
objectB,
targetPoseB,
allowedPenetration);
timeOfImpact = Math.Min(timeOfImpact, triangleTimeOfImpact);
}
}
}
// Recycle temporary objects.
ResourcePools.TestCollisionObjects.Recycle(testCollisionObject);
testGeometricObject.Recycle();
ResourcePools.TriangleShapes.Recycle(triangleShape);
return(timeOfImpact);
}
// Compute contacts between child shapes of two <see cref="CompositeShape"/>s.
// testXxx are initialized objects which are re-used to avoid a lot of GC garbage.
private void AddChildChildContacts(ContactSet contactSet,
int childIndexA,
int childIndexB,
CollisionQueryType type,
ContactSet testContactSet,
CollisionObject testCollisionObjectA,
TestGeometricObject testGeometricObjectA,
CollisionObject testCollisionObjectB,
TestGeometricObject testGeometricObjectB)
{
CollisionObject collisionObjectA = contactSet.ObjectA;
CollisionObject collisionObjectB = contactSet.ObjectB;
IGeometricObject geometricObjectA = collisionObjectA.GeometricObject;
IGeometricObject geometricObjectB = collisionObjectB.GeometricObject;
CompositeShape shapeA = (CompositeShape)geometricObjectA.Shape;
CompositeShape shapeB = (CompositeShape)geometricObjectB.Shape;
Vector3F scaleA = geometricObjectA.Scale;
Vector3F scaleB = geometricObjectB.Scale;
IGeometricObject childA = shapeA.Children[childIndexA];
IGeometricObject childB = shapeB.Children[childIndexB];
// Find collision algorithm.
CollisionAlgorithm collisionAlgorithm = CollisionDetection.AlgorithmMatrix[childA, childB];
// ----- Set the shape temporarily to the current children.
// (Note: The scaling is either uniform or the child has no local rotation. Therefore, we only
// need to apply the scale of the parent to the scale and translation of the child. We can
// ignore the rotation.)
Debug.Assert(
(scaleA.X == scaleA.Y && scaleA.Y == scaleA.Z) || !childA.Pose.HasRotation,
"CompositeShapeAlgorithm should have thrown an exception. Non-uniform scaling is not supported for rotated children.");
Debug.Assert(
(scaleB.X == scaleB.Y && scaleB.Y == scaleB.Z) || !childB.Pose.HasRotation,
"CompositeShapeAlgorithm should have thrown an exception. Non-uniform scaling is not supported for rotated children.");
var childAPose = childA.Pose;
childAPose.Position *= scaleA; // Apply scaling to local translation.
testGeometricObjectA.Pose = geometricObjectA.Pose * childAPose;
testGeometricObjectA.Shape = childA.Shape;
testGeometricObjectA.Scale = scaleA * childA.Scale; // Apply scaling to local scale.
testCollisionObjectA.SetInternal(collisionObjectA, testGeometricObjectA);
var childBPose = childB.Pose;
childBPose.Position *= scaleB; // Apply scaling to local translation.
testGeometricObjectB.Pose = geometricObjectB.Pose * childBPose;
testGeometricObjectB.Shape = childB.Shape;
testGeometricObjectB.Scale = scaleB * childB.Scale; // Apply scaling to local scale.
testCollisionObjectB.SetInternal(collisionObjectB, testGeometricObjectB);
Debug.Assert(testContactSet.Count == 0, "testContactSet needs to be cleared.");
testContactSet.Reset(testCollisionObjectA, testCollisionObjectB);
if (type == CollisionQueryType.Boolean)
{
// Boolean queries.
collisionAlgorithm.ComputeCollision(testContactSet, CollisionQueryType.Boolean);
contactSet.HaveContact = (contactSet.HaveContact || testContactSet.HaveContact);
}
else
{
// TODO: We could add existing contacts with the same child shape to childContactSet.
// No perturbation test. Most composite shapes are either complex and automatically
// have more contacts. Or they are complex and will not be used for stacking
// where full contact sets would be needed.
testContactSet.IsPerturbationTestAllowed = false;
// Make collision check. As soon as we have found contact, we can make faster
// contact queries instead of closest-point queries.
CollisionQueryType queryType = (contactSet.HaveContact) ? CollisionQueryType.Contacts : type;
collisionAlgorithm.ComputeCollision(testContactSet, queryType);
contactSet.HaveContact = (contactSet.HaveContact || testContactSet.HaveContact);
// Transform contacts into space of composite shape.
// And set the shape feature of the contact.
int numberOfContacts = testContactSet.Count;
for (int i = 0; i < numberOfContacts; i++)
{
Contact contact = testContactSet[i];
contact.PositionALocal = childAPose.ToWorldPosition(contact.PositionALocal);
//if (contact.Lifetime.Ticks == 0) // Currently, all contacts are new, so this check is not necessary.
//{
contact.FeatureA = childIndexA;
//}
contact.PositionBLocal = childBPose.ToWorldPosition(contact.PositionBLocal);
//if (contact.Lifetime.Ticks == 0) // Currently, all contacts are new, so this check is not necessary.
//{
contact.FeatureB = childIndexB;
//}
}
// Merge child contacts.
ContactHelper.Merge(contactSet, testContactSet, type, CollisionDetection.ContactPositionTolerance);
}
}
public void Recycle()
{
ObjectA = null;
ObjectB = null;
Pool.Recycle(this);
}
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);
}
// 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);
}
}
// The parameters 'testXxx' are initialized objects which are re-used to avoid a lot of GC garbage.
private void AddTriangleTriangleContacts(
ContactSet contactSet, int triangleIndexA, int triangleIndexB, CollisionQueryType type,
ContactSet testContactSet, CollisionObject testCollisionObjectA, TestGeometricObject testGeometricObjectA,
TriangleShape testTriangleA, CollisionObject testCollisionObjectB, TestGeometricObject testGeometricObjectB,
TriangleShape testTriangleB)
{
CollisionObject collisionObjectA = contactSet.ObjectA;
CollisionObject collisionObjectB = contactSet.ObjectB;
IGeometricObject geometricObjectA = collisionObjectA.GeometricObject;
IGeometricObject geometricObjectB = collisionObjectB.GeometricObject;
TriangleMeshShape triangleMeshShapeA = (TriangleMeshShape)geometricObjectA.Shape;
Triangle triangleA = triangleMeshShapeA.Mesh.GetTriangle(triangleIndexA);
TriangleMeshShape triangleMeshShapeB = (TriangleMeshShape)geometricObjectB.Shape;
Triangle triangleB = triangleMeshShapeB.Mesh.GetTriangle(triangleIndexB);
Pose poseA = geometricObjectA.Pose;
Pose poseB = geometricObjectB.Pose;
Vector3F scaleA = geometricObjectA.Scale;
Vector3F scaleB = geometricObjectB.Scale;
// Apply SRT.
Triangle transformedTriangleA;
transformedTriangleA.Vertex0 = poseA.ToWorldPosition(triangleA.Vertex0 * scaleA);
transformedTriangleA.Vertex1 = poseA.ToWorldPosition(triangleA.Vertex1 * scaleA);
transformedTriangleA.Vertex2 = poseA.ToWorldPosition(triangleA.Vertex2 * scaleA);
Triangle transformedTriangleB;
transformedTriangleB.Vertex0 = poseB.ToWorldPosition(triangleB.Vertex0 * scaleB);
transformedTriangleB.Vertex1 = poseB.ToWorldPosition(triangleB.Vertex1 * scaleB);
transformedTriangleB.Vertex2 = poseB.ToWorldPosition(triangleB.Vertex2 * scaleB);
// Make super-fast boolean check first. This is redundant if we have to compute
// a contact with SAT below. But in stochastic benchmarks it seems to be 10% faster.
bool haveContact = GeometryHelper.HaveContact(ref transformedTriangleA, ref transformedTriangleB);
if (type == CollisionQueryType.Boolean)
{
contactSet.HaveContact = (contactSet.HaveContact || haveContact);
return;
}
if (haveContact)
{
// Make sure the scaled triangles have the correct normal.
// (A negative scale changes the normal/winding order. See unit test in TriangleTest.cs.)
if (scaleA.X * scaleA.Y * scaleA.Z < 0)
MathHelper.Swap(ref transformedTriangleA.Vertex0, ref transformedTriangleA.Vertex1);
if (scaleB.X * scaleB.Y * scaleB.Z < 0)
MathHelper.Swap(ref transformedTriangleB.Vertex0, ref transformedTriangleB.Vertex1);
// Compute contact.
Vector3F position, normal;
float penetrationDepth;
haveContact = TriangleTriangleAlgorithm.GetContact(
ref transformedTriangleA, ref transformedTriangleB,
!triangleMeshShapeA.IsTwoSided, !triangleMeshShapeB.IsTwoSided,
out position, out normal, out penetrationDepth);
if (haveContact)
{
contactSet.HaveContact = true;
// In deep interpenetrations we might get no contact (penDepth = NaN).
if (!Numeric.IsNaN(penetrationDepth))
{
Contact contact = ContactHelper.CreateContact(contactSet, position, normal, penetrationDepth, false);
contact.FeatureA = triangleIndexA;
contact.FeatureB = triangleIndexB;
ContactHelper.Merge(contactSet, contact, type, CollisionDetection.ContactPositionTolerance);
}
return;
}
// We might come here if the boolean test reports contact but the SAT test
// does not because of numerical errors.
}
Debug.Assert(!haveContact);
if (type == CollisionQueryType.Contacts)
return;
Debug.Assert(type == CollisionQueryType.ClosestPoints);
if (contactSet.HaveContact)
{
// These triangles are separated but other parts of the meshes touches.
// --> Abort.
return;
}
// We do not have a specialized triangle-triangle closest points algorithm.
// Fall back to the default algorithm (GJK).
// Initialize temporary test contact set and test objects.
// Note: We assume the triangle-triangle does not care about front/back faces.
testTriangleA.Vertex0 = transformedTriangleA.Vertex0;
testTriangleA.Vertex1 = transformedTriangleA.Vertex1;
testTriangleA.Vertex2 = transformedTriangleA.Vertex2;
testGeometricObjectA.Shape = testTriangleA;
Debug.Assert(testGeometricObjectA.Scale == Vector3F.One);
Debug.Assert(testGeometricObjectA.Pose == Pose.Identity);
testCollisionObjectA.SetInternal(collisionObjectA, testGeometricObjectA);
testTriangleB.Vertex0 = transformedTriangleB.Vertex0;
testTriangleB.Vertex1 = transformedTriangleB.Vertex1;
testTriangleB.Vertex2 = transformedTriangleB.Vertex2;
testGeometricObjectB.Shape = testTriangleB;
Debug.Assert(testGeometricObjectB.Scale == Vector3F.One);
Debug.Assert(testGeometricObjectB.Pose == Pose.Identity);
testCollisionObjectB.SetInternal(collisionObjectB, testGeometricObjectB);
Debug.Assert(testContactSet.Count == 0, "testContactSet needs to be cleared.");
testContactSet.Reset(testCollisionObjectA, testCollisionObjectB);
testContactSet.IsPerturbationTestAllowed = false;
_triangleTriangleAlgorithm.ComputeCollision(testContactSet, type);
// Note: We expect no contact but because of numerical differences the triangle-triangle
// algorithm could find a shallow surface contact.
contactSet.HaveContact = (contactSet.HaveContact || testContactSet.HaveContact);
#region ----- Merge testContactSet into contactSet -----
if (testContactSet.Count > 0)
{
// Set the shape feature of the new contacts.
int numberOfContacts = testContactSet.Count;
for (int i = 0; i < numberOfContacts; i++)
{
Contact contact = testContactSet[i];
//if (contact.Lifetime.Ticks == 0) // Currently, this check is not necessary because triangleSet does not contain old contacts.
//{
contact.FeatureA = triangleIndexA;
contact.FeatureB = triangleIndexB;
//}
}
// Merge the contact info.
ContactHelper.Merge(contactSet, testContactSet, type, CollisionDetection.ContactPositionTolerance);
}
#endregion
}
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();
}
// 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);
}
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);
}
}
/// <inheritdoc/>
/// <exception cref="ArgumentNullException">
/// <paramref name="objectA"/> or <paramref name="objectB"/> is <see langword="null"/>.
/// </exception>
/// <exception cref="ArgumentException">
/// Neither <paramref name="objectA"/> nor <paramref name="objectB"/> is a
/// <see cref="TriangleMeshShape"/>.
/// </exception>
public override float GetTimeOfImpact(CollisionObject objectA, Pose targetPoseA, CollisionObject objectB, Pose targetPoseB, float allowedPenetration)
{
if (objectA == null)
{
throw new ArgumentNullException("objectA");
}
if (objectB == null)
{
throw new ArgumentNullException("objectB");
}
// Object A should be the triangle mesh, swap objects if necessary.
if (!(objectA.GeometricObject.Shape is TriangleMeshShape))
{
MathHelper.Swap(ref objectA, ref objectB);
MathHelper.Swap(ref targetPoseA, ref targetPoseB);
}
IGeometricObject geometricObjectA = objectA.GeometricObject;
IGeometricObject geometricObjectB = objectB.GeometricObject;
TriangleMeshShape triangleMeshShapeA = geometricObjectA.Shape as TriangleMeshShape;
// Check if collision objects shapes are correct.
if (triangleMeshShapeA == null)
{
throw new ArgumentException("One object must be a triangle mesh.");
}
// Currently mesh vs. mesh CCD is not supported.
if (objectB.GeometricObject.Shape is TriangleMeshShape)
{
return(1);
}
ITriangleMesh triangleMeshA = triangleMeshShapeA.Mesh;
Pose startPoseA = geometricObjectA.Pose;
Pose startPoseB = geometricObjectB.Pose;
Vector3F scaleA = geometricObjectA.Scale;
Vector3F scaleB = geometricObjectB.Scale;
// Get an AABB of the swept B in the space of A.
// This simplified AABB can miss some rotational movement.
// To simplify, we assume that A is static and B is moving relative to A.
// In general, this is not correct! But for CCD we make this simplification.
// We convert everything to the space of A.
var aabbSweptBInA = geometricObjectB.Shape.GetAabb(scaleB, startPoseA.Inverse * startPoseB);
aabbSweptBInA.Grow(geometricObjectB.Shape.GetAabb(scaleB, targetPoseA.Inverse * targetPoseB));
// Use temporary object.
var triangleShape = ResourcePools.TriangleShapes.Obtain();
// (Vertices will be set in the loop below.)
var testGeometricObject = TestGeometricObject.Create();
testGeometricObject.Shape = triangleShape;
testGeometricObject.Scale = Vector3F.One;
testGeometricObject.Pose = startPoseA;
var testCollisionObject = ResourcePools.TestCollisionObjects.Obtain();
testCollisionObject.SetInternal(objectA, testGeometricObject);
var collisionAlgorithm = CollisionDetection.AlgorithmMatrix[typeof(TriangleShape), geometricObjectB.Shape.GetType()];
float timeOfImpact = 1;
if (triangleMeshShapeA.Partition != null)
{
// Apply inverse scaling to do the AABB-tree checks in the unscaled local space of A.
aabbSweptBInA.Scale(Vector3F.One / scaleA);
foreach (var triangleIndex in triangleMeshShapeA.Partition.GetOverlaps(aabbSweptBInA))
{
Triangle triangle = triangleMeshA.GetTriangle(triangleIndex);
// Apply scale.
triangle.Vertex0 = triangle.Vertex0 * scaleA;
triangle.Vertex1 = triangle.Vertex1 * scaleA;
triangle.Vertex2 = triangle.Vertex2 * scaleA;
triangleShape.Vertex0 = triangle.Vertex0;
triangleShape.Vertex1 = triangle.Vertex1;
triangleShape.Vertex2 = triangle.Vertex2;
float triangleTimeOfImpact = collisionAlgorithm.GetTimeOfImpact(
testCollisionObject, targetPoseA,
objectB, targetPoseB,
allowedPenetration);
timeOfImpact = Math.Min(timeOfImpact, triangleTimeOfImpact);
}
}
else
{
// Test all triangles.
int numberOfTriangles = triangleMeshA.NumberOfTriangles;
for (int triangleIndex = 0; triangleIndex < numberOfTriangles; triangleIndex++)
{
Triangle triangle = triangleMeshA.GetTriangle(triangleIndex);
// Apply scale.
triangle.Vertex0 = triangle.Vertex0 * scaleA;
triangle.Vertex1 = triangle.Vertex1 * scaleA;
triangle.Vertex2 = triangle.Vertex2 * scaleA;
// Make AABB test of triangle vs. sweep of B.
if (!GeometryHelper.HaveContact(aabbSweptBInA, triangle.Aabb))
{
continue;
}
triangleShape.Vertex0 = triangle.Vertex0;
triangleShape.Vertex1 = triangle.Vertex1;
triangleShape.Vertex2 = triangle.Vertex2;
float triangleTimeOfImpact = collisionAlgorithm.GetTimeOfImpact(
testCollisionObject, targetPoseA,
objectB, targetPoseB,
allowedPenetration);
timeOfImpact = Math.Min(timeOfImpact, triangleTimeOfImpact);
}
}
// Recycle temporary objects.
ResourcePools.TestCollisionObjects.Recycle(testCollisionObject);
testGeometricObject.Recycle();
ResourcePools.TriangleShapes.Recycle(triangleShape);
return(timeOfImpact);
}
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
}
}
/// <summary>
/// Determines whether the specified world space position is underwater.
/// </summary>
/// <param name="position">The position in world space.</param>
/// <returns>
/// <see langword="true"/> if the position is underwater; otherwise, <see langword="false"/>
/// </returns>
/// <remarks>
/// A position is underwater if it is inside the <see cref="Shape"/> of this node.
/// </remarks>
public bool IsUnderwater(Vector3F position)
{
//if (!EnableUnderwaterEffect)
// return false;
// Oceans are treated like a horizontal plane through the node origin.
if (Volume == null)
{
return(position.Y < PoseWorld.Position.Y);
}
// Thread-safety: We lock this operation because all tests use the same cache
// and test objects.
lock (_underwaterTestLock)
{
// Return cached result if the point and the water pose/shape are still the same.
if (!IsDirty)
{
if (Vector3F.AreNumericallyEqual(position, _lastTestPosition))
{
return(_lastTestResult);
}
}
else
{
// Clear flag. We will cache a new result.
IsDirty = false;
}
_lastTestPosition = position;
_lastTestResult = false;
// Use a shared collision detection instance.
var collisionDetection = SceneHelper.CollisionDetection;
if (_sphereShape == null)
{
// First time initializations.
_sphereShape = new SphereShape(0);
_rayShape = new RayShape();
_testCollisionObject = new CollisionObject(TestGeometricObject.Create());
_waterCollisionObject = new CollisionObject(TestGeometricObject.Create());
}
var testGeometricObject = (TestGeometricObject)_testCollisionObject.GeometricObject;
var waterGeometricObject = (TestGeometricObject)_waterCollisionObject.GeometricObject;
try
{
// Initialize water collision object.
waterGeometricObject.Shape = Volume;
waterGeometricObject.Scale = ScaleWorld;
waterGeometricObject.Pose = PoseWorld;
// Test if point touches underwater volume. (Skip this test for triangle mesh shapes.)
if (!(Shape is TriangleMeshShape))
{
testGeometricObject.Pose = new Pose(position);
testGeometricObject.Shape = _sphereShape;
if (collisionDetection.HaveContact(_testCollisionObject, _waterCollisionObject))
{
_lastTestResult = true;
return(true);
}
// For convex shapes, the above test is sufficient.
if (Shape is ConvexShape)
{
return(false);
}
}
// For triangle meshes - which are hollow - we have to make a more complex test.
// We shoot vertical rays and check if we hit the underwater volume surface.
// Make explicit point vs. AABB test first.
if (!collisionDetection.HaveAabbContact(_testCollisionObject, _waterCollisionObject))
{
return(false);
}
// Switch to ray shape.
testGeometricObject.Shape = _rayShape;
// Shoot down. Start 1 unit above the surface.
Vector3F origin = position;
origin.Y = Math.Max(Aabb.Maximum.Y, origin.Y) + 1;
_rayShape.Origin = origin;
_rayShape.Length = (origin - position).Length;
_rayShape.Direction = Vector3F.Down;
if (!collisionDetection.HaveContact(_testCollisionObject, _waterCollisionObject))
{
return(false); // Camera is above water.
}
// Shoot up. Start 1 m under the water volume.
origin = position;
origin.Y = Math.Min(Aabb.Minimum.Y, origin.Y) - 1;
_rayShape.Origin = origin;
_rayShape.Length = (origin - position).Length;
_rayShape.Direction = Vector3F.Up;
_lastTestResult = collisionDetection.HaveContact(_testCollisionObject, _waterCollisionObject);
return(_lastTestResult);
}
finally
{
// Remove references to avoid "memory leaks".
waterGeometricObject.Shape = Volume;
}
}
}
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);
}
}
// 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;
Vector3 scaleA = geometricObjectA.Scale;
Vector3 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);
}
}
/// <inheritdoc/>
/// <exception cref="ArgumentException">
/// Neither <paramref name="objectA"/> nor <paramref name="objectB"/> is a
/// <see cref="TransformedShape"/>.
/// <exception cref="ArgumentNullException">
/// <paramref name="objectA"/> or <paramref name="objectB"/> is <see langword="null"/>.
/// </exception>
/// </exception>
public override float GetTimeOfImpact(CollisionObject objectA, Pose targetPoseA, CollisionObject objectB, Pose targetPoseB, float allowedPenetration)
{
// Most of this code is copied from ComputeCollision() above.
// We get the child and compute the TOI for the child movement. The child movement is
// linearly interpolated from start to end pose. - This is not correct if the real center
// of rotation and the center of the child shape are not equal! But for small rotational
// movement and small offsets this is acceptable.
if (objectA == null)
{
throw new ArgumentNullException("objectA");
}
if (objectB == null)
{
throw new ArgumentNullException("objectB");
}
// B should be the transformed shape, swap objects if necessary.
bool swapped = !(objectB.GeometricObject.Shape is TransformedShape);
if (swapped)
{
MathHelper.Swap(ref objectA, ref objectB);
MathHelper.Swap(ref targetPoseA, ref targetPoseB);
}
IGeometricObject geometricObjectB = objectB.GeometricObject;
TransformedShape transformedShape = geometricObjectB.Shape as TransformedShape;
// Check if collision objects shapes are correct.
if (transformedShape == null)
{
throw new ArgumentException("objectA or objectB must be a TransformedShape.");
}
Pose poseB = geometricObjectB.Pose;
Vector3F scaleB = geometricObjectB.Scale;
// Note: Non-uniform scaling for rotated child objects is not supported
// but we might still get a usable TOI query result.
// 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;
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);
// Compute TOI.
var collisionAlgorithm = CollisionDetection.AlgorithmMatrix[objectA, testCollisionObjectB];
float timeOfImpact = collisionAlgorithm.GetTimeOfImpact(
objectA, targetPoseA,
testCollisionObjectB, targetPoseB * childPose,
allowedPenetration);
// Recycle temporary objects.
ResourcePools.TestCollisionObjects.Recycle(testCollisionObjectB);
testGeometricObjectB.Recycle();
return(timeOfImpact);
}
/// <inheritdoc/>
/// <exception cref="ArgumentException">
/// Neither <paramref name="objectA"/> nor <paramref name="objectB"/> is a
/// <see cref="CompositeShape"/>.
/// </exception>
/// <exception cref="ArgumentNullException">
/// <paramref name="objectA"/> or <paramref name="objectB"/> is <see langword="null"/>.
/// </exception>
public override float GetTimeOfImpact(CollisionObject objectA, Pose targetPoseA, CollisionObject objectB, Pose targetPoseB, float allowedPenetration)
{
// We get the children and compute the minimal TOI for all children. The child movement is
// linearly interpolated from start to end pose. - This is not correct if the real center
// of rotation and the center of the child shape are not equal! But for small rotational
// movement and small offsets this is acceptable.
// The correct solution:
// Conservative Advancement must not use linear motion interpolation. Instead the correct
// intermediate motion must be computed relative to the parent space.
// The faster solution:
// Use Hierarchical Conservative Advancement as described in the papers by Kim Young et al:
// FAST, C2A, ...
if (objectA == null)
{
throw new ArgumentNullException("objectA");
}
if (objectB == null)
{
throw new ArgumentNullException("objectB");
}
// B should be the composite, swap objects if necessary.
if (!(objectB.GeometricObject.Shape is CompositeShape))
{
MathHelper.Swap(ref objectA, ref objectB);
MathHelper.Swap(ref targetPoseA, ref targetPoseB);
}
CompositeShape compositeShapeB = objectB.GeometricObject.Shape as CompositeShape;
// Check if collision objects shapes are correct.
if (compositeShapeB == null)
{
throw new ArgumentException("One object must be a composite shape.");
}
IGeometricObject geometricObjectB = objectB.GeometricObject;
Pose poseB = geometricObjectB.Pose;
Vector3 scaleB = geometricObjectB.Scale;
// Note: Non-uniform scaling for rotated child objects is not supported
// but we might still get a usable TOI query result.
float timeOfImpact = 1;
// Use temporary object.
var testGeometricObjectB = TestGeometricObject.Create();
var testCollisionObjectB = ResourcePools.TestCollisionObjects.Obtain();
// Go through list of children and find minimal TOI.
int numberOfChildren = compositeShapeB.Children.Count;
for (int i = 0; i < numberOfChildren; i++)
{
IGeometricObject childGeometricObject = compositeShapeB.Children[i];
// Following code is taken from the TransformedShapeAlgorithm:
Pose childPose = childGeometricObject.Pose;
childPose.Position *= scaleB;
testGeometricObjectB.Shape = childGeometricObject.Shape;
testGeometricObjectB.Scale = scaleB * childGeometricObject.Scale;
testGeometricObjectB.Pose = poseB * childPose;
testCollisionObjectB.SetInternal(objectB, testGeometricObjectB);
var collisionAlgorithm = CollisionDetection.AlgorithmMatrix[objectA, testCollisionObjectB];
float childTimeOfImpact = collisionAlgorithm.GetTimeOfImpact(
objectA, targetPoseA,
testCollisionObjectB, targetPoseB * childPose,
allowedPenetration);
timeOfImpact = Math.Min(timeOfImpact, childTimeOfImpact);
}
// Recycle temporary objects.
ResourcePools.TestCollisionObjects.Recycle(testCollisionObjectB);
testGeometricObjectB.Recycle();
return(timeOfImpact);
}
/// <summary>
/// Performs more collision tests while slightly rotating one collision object.
/// </summary>
/// <param name="collisionDetection">The collision detection.</param>
/// <param name="contactSet">
/// The contact set; must contain at least 1 <see cref="Contact"/>.
/// </param>
/// <param name="perturbB">
/// if set to <see langword="true"/> collision object B will be rotated; otherwise collision
/// object A will be rotated.
/// </param>
/// <param name="testMethod">The test method that is called to compute contacts.</param>
/// <remarks>
/// This method rotates one object 3 times and calls contact computation for the new
/// orientations. It is recommended to call this method only when the contact set has 1 new
/// contact.
/// </remarks>
internal static void TestWithPerturbations(CollisionDetection collisionDetection, ContactSet contactSet, bool perturbB, Action <ContactSet> testMethod)
{
Debug.Assert(contactSet != null);
Debug.Assert(contactSet.Count > 0 && contactSet.HaveContact || !contactSet.IsPerturbationTestAllowed);
Debug.Assert(testMethod != null);
// Make this test only if there is 1 contact.
// If there are 0 contacts, we assume that the contact pair is separated.
// If there are more than 3 contacts, then we already have a lot of contacts to work with, no
// need to search for more.
if (!contactSet.HaveContact || contactSet.Count == 0 || contactSet.Count >= 4 || !contactSet.IsPerturbationTestAllowed)
{
return;
}
// Get data of object that will be rotated.
var collisionObject = (perturbB) ? contactSet.ObjectB : contactSet.ObjectA;
var geometricObject = collisionObject.GeometricObject;
var pose = geometricObject.Pose;
// Get normal, pointing to the test object.
var normal = contactSet[0].Normal;
if (!perturbB)
{
normal = -normal;
}
var contactPosition = contactSet[0].Position;
var centerToContact = contactPosition - pose.Position;
// Compute a perturbation angle proportional to the dimension of the object.
var radius = geometricObject.Aabb.Extent.Length;
var angle = collisionDetection.ContactPositionTolerance / radius;
// axis1 is in the contact tangent plane, orthogonal to normal.
var axis1 = Vector3.Cross(normal, centerToContact);
// If axis1 is zero then normal and centerToContact are collinear. This happens
// for example for spheres or cone tips against flat faces. In these cases we assume
// that there will be max. 1 contact.
if (axis1.IsNumericallyZero)
{
return;
}
var axis1Local = pose.ToLocalDirection(axis1);
var rotation = Matrix.CreateRotation(axis1Local, -angle);
// Use temporary test objects.
var testGeometricObject = TestGeometricObject.Create();
testGeometricObject.Shape = geometricObject.Shape;
testGeometricObject.Scale = geometricObject.Scale;
testGeometricObject.Pose = new Pose(pose.Position, pose.Orientation * rotation);
var testCollisionObject = ResourcePools.TestCollisionObjects.Obtain();
testCollisionObject.SetInternal(collisionObject, testGeometricObject);
var testContactSet = perturbB ? ContactSet.Create(contactSet.ObjectA, testCollisionObject)
: ContactSet.Create(testCollisionObject, contactSet.ObjectB);
testContactSet.IsPerturbationTestAllowed = false; // Avoid recursive perturbation tests!
testContactSet.PreferredNormal = contactSet.PreferredNormal;
// Compute next contacts.
testMethod(testContactSet);
if (testContactSet.Count > 0)
{
// axis2 is in the contact tangent plane, orthogonal to axis1.
var axis2 = Vector3.Cross(axis1, normal);
var axis2Local = pose.ToLocalDirection(axis2);
var rotation2 = Matrix.CreateRotation(axis2Local, -angle);
testGeometricObject.Pose = new Pose(pose.Position, pose.Orientation * rotation2);
// Compute next contacts.
testMethod(testContactSet);
// Invert rotation2.
rotation2 = Matrix.Transpose(rotation2);
testGeometricObject.Pose = new Pose(pose.Position, pose.Orientation * rotation2);
// Compute next contacts.
testMethod(testContactSet);
}
// Set HaveContact. It is reset when a perturbation separates the objects.
testContactSet.HaveContact = true;
// TODO: Test if we need this:
// The contact world positions are not really correct because one object was rotated.
// UpdateContacts recomputes the world positions from the local positions.
UpdateContacts(testContactSet, 0, collisionDetection.ContactPositionTolerance);
// Merge contacts of testContactSet into contact set, but do not change existing contacts.
foreach (var contact in testContactSet)
{
// We call TryMerge() to see if the contact is similar to an existing contact.
bool exists = TryMergeWithNearestContact(
contactSet,
contact,
collisionDetection.ContactPositionTolerance,
false); // The existing contact must no be changed!
if (exists)
{
// We can throw away the new contact because a similar is already in the contact set.
contact.Recycle();
}
else
{
// Add new contact.
contactSet.Add(contact);
}
}
// Recycle temporary objects.
testContactSet.Recycle();
ResourcePools.TestCollisionObjects.Recycle(testCollisionObject);
testGeometricObject.Recycle();
}
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();
}
}
// The parameters 'testXxx' are initialized objects which are re-used to avoid a lot of GC garbage.
private void AddTriangleTriangleContacts(
ContactSet contactSet, int triangleIndexA, int triangleIndexB, CollisionQueryType type,
ContactSet testContactSet, CollisionObject testCollisionObjectA, TestGeometricObject testGeometricObjectA,
TriangleShape testTriangleA, CollisionObject testCollisionObjectB, TestGeometricObject testGeometricObjectB,
TriangleShape testTriangleB)
{
CollisionObject collisionObjectA = contactSet.ObjectA;
CollisionObject collisionObjectB = contactSet.ObjectB;
IGeometricObject geometricObjectA = collisionObjectA.GeometricObject;
IGeometricObject geometricObjectB = collisionObjectB.GeometricObject;
TriangleMeshShape triangleMeshShapeA = (TriangleMeshShape)geometricObjectA.Shape;
Triangle triangleA = triangleMeshShapeA.Mesh.GetTriangle(triangleIndexA);
TriangleMeshShape triangleMeshShapeB = (TriangleMeshShape)geometricObjectB.Shape;
Triangle triangleB = triangleMeshShapeB.Mesh.GetTriangle(triangleIndexB);
Pose poseA = geometricObjectA.Pose;
Pose poseB = geometricObjectB.Pose;
Vector3F scaleA = geometricObjectA.Scale;
Vector3F scaleB = geometricObjectB.Scale;
// Apply SRT.
Triangle transformedTriangleA;
transformedTriangleA.Vertex0 = poseA.ToWorldPosition(triangleA.Vertex0 * scaleA);
transformedTriangleA.Vertex1 = poseA.ToWorldPosition(triangleA.Vertex1 * scaleA);
transformedTriangleA.Vertex2 = poseA.ToWorldPosition(triangleA.Vertex2 * scaleA);
Triangle transformedTriangleB;
transformedTriangleB.Vertex0 = poseB.ToWorldPosition(triangleB.Vertex0 * scaleB);
transformedTriangleB.Vertex1 = poseB.ToWorldPosition(triangleB.Vertex1 * scaleB);
transformedTriangleB.Vertex2 = poseB.ToWorldPosition(triangleB.Vertex2 * scaleB);
// Make super-fast boolean check first. This is redundant if we have to compute
// a contact with SAT below. But in stochastic benchmarks it seems to be 10% faster.
bool haveContact = GeometryHelper.HaveContact(ref transformedTriangleA, ref transformedTriangleB);
if (type == CollisionQueryType.Boolean)
{
contactSet.HaveContact = (contactSet.HaveContact || haveContact);
return;
}
if (haveContact)
{
// Make sure the scaled triangles have the correct normal.
// (A negative scale changes the normal/winding order. See unit test in TriangleTest.cs.)
if (scaleA.X * scaleA.Y * scaleA.Z < 0)
{
MathHelper.Swap(ref transformedTriangleA.Vertex0, ref transformedTriangleA.Vertex1);
}
if (scaleB.X * scaleB.Y * scaleB.Z < 0)
{
MathHelper.Swap(ref transformedTriangleB.Vertex0, ref transformedTriangleB.Vertex1);
}
// Compute contact.
Vector3F position, normal;
float penetrationDepth;
haveContact = TriangleTriangleAlgorithm.GetContact(
ref transformedTriangleA, ref transformedTriangleB,
!triangleMeshShapeA.IsTwoSided, !triangleMeshShapeB.IsTwoSided,
out position, out normal, out penetrationDepth);
if (haveContact)
{
contactSet.HaveContact = true;
// In deep interpenetrations we might get no contact (penDepth = NaN).
if (!Numeric.IsNaN(penetrationDepth))
{
Contact contact = ContactHelper.CreateContact(contactSet, position, normal, penetrationDepth, false);
contact.FeatureA = triangleIndexA;
contact.FeatureB = triangleIndexB;
ContactHelper.Merge(contactSet, contact, type, CollisionDetection.ContactPositionTolerance);
}
return;
}
// We might come here if the boolean test reports contact but the SAT test
// does not because of numerical errors.
}
Debug.Assert(!haveContact);
if (type == CollisionQueryType.Contacts)
{
return;
}
Debug.Assert(type == CollisionQueryType.ClosestPoints);
if (contactSet.HaveContact)
{
// These triangles are separated but other parts of the meshes touches.
// --> Abort.
return;
}
// We do not have a specialized triangle-triangle closest points algorithm.
// Fall back to the default algorithm (GJK).
// Initialize temporary test contact set and test objects.
// Note: We assume the triangle-triangle does not care about front/back faces.
testTriangleA.Vertex0 = transformedTriangleA.Vertex0;
testTriangleA.Vertex1 = transformedTriangleA.Vertex1;
testTriangleA.Vertex2 = transformedTriangleA.Vertex2;
testGeometricObjectA.Shape = testTriangleA;
Debug.Assert(testGeometricObjectA.Scale == Vector3F.One);
Debug.Assert(testGeometricObjectA.Pose == Pose.Identity);
testCollisionObjectA.SetInternal(collisionObjectA, testGeometricObjectA);
testTriangleB.Vertex0 = transformedTriangleB.Vertex0;
testTriangleB.Vertex1 = transformedTriangleB.Vertex1;
testTriangleB.Vertex2 = transformedTriangleB.Vertex2;
testGeometricObjectB.Shape = testTriangleB;
Debug.Assert(testGeometricObjectB.Scale == Vector3F.One);
Debug.Assert(testGeometricObjectB.Pose == Pose.Identity);
testCollisionObjectB.SetInternal(collisionObjectB, testGeometricObjectB);
Debug.Assert(testContactSet.Count == 0, "testContactSet needs to be cleared.");
testContactSet.Reset(testCollisionObjectA, testCollisionObjectB);
testContactSet.IsPerturbationTestAllowed = false;
_triangleTriangleAlgorithm.ComputeCollision(testContactSet, type);
// Note: We expect no contact but because of numerical differences the triangle-triangle
// algorithm could find a shallow surface contact.
contactSet.HaveContact = (contactSet.HaveContact || testContactSet.HaveContact);
#region ----- Merge testContactSet into contactSet -----
if (testContactSet.Count > 0)
{
// Set the shape feature of the new contacts.
int numberOfContacts = testContactSet.Count;
for (int i = 0; i < numberOfContacts; i++)
{
Contact contact = testContactSet[i];
//if (contact.Lifetime.Ticks == 0) // Currently, this check is not necessary because triangleSet does not contain old contacts.
//{
contact.FeatureA = triangleIndexA;
contact.FeatureB = triangleIndexB;
//}
}
// Merge the contact info.
ContactHelper.Merge(contactSet, testContactSet, type, CollisionDetection.ContactPositionTolerance);
}
#endregion
}
public void Recycle()
{
ObjectA = null;
ObjectB = null;
Pool.Recycle(this);
}
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();
}
}
// Compute contacts between a shape and the child shapes of a <see cref="CompositeShape"/>.
// testXxx are initialized objects which are re-used to avoid a lot of GC garbage.
private void AddChildContacts(ContactSet contactSet,
bool swapped,
int childIndex,
CollisionQueryType type,
ContactSet testContactSet,
CollisionObject testCollisionObject,
TestGeometricObject testGeometricObject)
{
// This method is also used in RayCompositeAlgorithm.cs. Keep changes in sync!
// Object A should be the CompositeShape.
CollisionObject collisionObjectA = (swapped) ? contactSet.ObjectB : contactSet.ObjectA;
CollisionObject collisionObjectB = (swapped) ? contactSet.ObjectA : contactSet.ObjectB;
IGeometricObject geometricObjectA = collisionObjectA.GeometricObject;
IGeometricObject geometricObjectB = collisionObjectB.GeometricObject;
Vector3F scaleA = geometricObjectA.Scale;
IGeometricObject childA = ((CompositeShape)geometricObjectA.Shape).Children[childIndex];
// Find collision algorithm.
CollisionAlgorithm collisionAlgorithm = CollisionDetection.AlgorithmMatrix[childA, geometricObjectB];
// ----- Set the shape temporarily to the current child.
// (Note: The scaling is either uniform or the child has no local rotation. Therefore, we only
// need to apply the scale of the parent to the scale and translation of the child. We can
// ignore the rotation.)
Debug.Assert(
(scaleA.X == scaleA.Y && scaleA.Y == scaleA.Z) || !childA.Pose.HasRotation,
"CompositeShapeAlgorithm should have thrown an exception. Non-uniform scaling is not supported for rotated children.");
var childPose = childA.Pose;
childPose.Position *= scaleA; // Apply scaling to local translation.
testGeometricObject.Pose = geometricObjectA.Pose * childPose;
testGeometricObject.Shape = childA.Shape;
testGeometricObject.Scale = scaleA * childA.Scale; // Apply scaling to local scale.
testCollisionObject.SetInternal(collisionObjectA, testGeometricObject);
// Create a temporary contact set.
// (ObjectA and ObjectB should have the same order as in contactSet; otherwise we couldn't
// simply merge them.)
Debug.Assert(testContactSet.Count == 0, "testContactSet needs to be cleared.");
if (swapped)
testContactSet.Reset(collisionObjectB, testCollisionObject);
else
testContactSet.Reset(testCollisionObject, collisionObjectB);
if (type == CollisionQueryType.Boolean)
{
// Boolean queries.
collisionAlgorithm.ComputeCollision(testContactSet, CollisionQueryType.Boolean);
contactSet.HaveContact = (contactSet.HaveContact || testContactSet.HaveContact);
}
else
{
// No perturbation test. Most composite shapes are either complex and automatically
// have more contacts. Or they are complex and will not be used for stacking
// where full contact sets would be needed.
testContactSet.IsPerturbationTestAllowed = false;
// TODO: We could add existing contacts with the same child shape to childContactSet.
// Collision algorithms could take advantage of existing contact information to speed up
// calculations. However, at the moment the collision algorithms ignore existing contacts.
// If we add the exiting contacts to childContactSet we need to uncomment the comment
// code lines below.
// Transform contacts into space of child shape.
//foreach (Contact c in childContactSet)
//{
// if (childContactSet.ObjectA == childCollisionObject)
// c.PositionALocal = childPose.ToLocalPosition(c.PositionALocal);
// else
// c.PositionBLocal = childPose.ToLocalPosition(c.PositionBLocal);
//}
// Make collision check. As soon as we have found contact, we can make faster
// contact queries instead of closest-point queries.
CollisionQueryType queryType = (contactSet.HaveContact) ? CollisionQueryType.Contacts : type;
collisionAlgorithm.ComputeCollision(testContactSet, queryType);
contactSet.HaveContact = (contactSet.HaveContact || testContactSet.HaveContact);
// Transform contacts into space of composite shape.
// And set the shape feature of the contact.
int numberOfContacts = testContactSet.Count;
for (int i = 0; i < numberOfContacts; i++)
{
Contact contact = testContactSet[i];
if (swapped)
{
contact.PositionBLocal = childPose.ToWorldPosition(contact.PositionBLocal);
//if (contact.Lifetime.Ticks == 0) // Currently, all contacts are new, so this check is not necessary.
//{
contact.FeatureB = childIndex;
//}
}
else
{
contact.PositionALocal = childPose.ToWorldPosition(contact.PositionALocal);
//if (contact.Lifetime.Ticks == 0) // Currently, all contacts are new, so this check is not necessary.
//{
contact.FeatureA = childIndex;
//}
}
}
// Merge child contacts.
ContactHelper.Merge(contactSet, testContactSet, type, CollisionDetection.ContactPositionTolerance);
}
}
// 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.)
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))
{
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);
}
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 velocityProjected = Vector3F.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.
Vector3F positionA = startPoseA.Position + λ * (targetPoseA.Position - startPoseA.Position);
testGeometricObjectA.Pose = new Pose(positionA, startPoseA.Orientation);
Vector3F 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);
}
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
}
}
