// 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
}
// The parameters 'testXxx' are initialized objects which are re-used to avoid a lot of GC garbage.
private void AddTriangleTriangleContacts(
ContactSet contactSet, int triangleIndexA, int triangleIndexB, CollisionQueryType type,
ContactSet testContactSet, CollisionObject testCollisionObjectA, TestGeometricObject testGeometricObjectA,
TriangleShape testTriangleA, CollisionObject testCollisionObjectB, TestGeometricObject testGeometricObjectB,
TriangleShape testTriangleB)
{
CollisionObject collisionObjectA = contactSet.ObjectA;
CollisionObject collisionObjectB = contactSet.ObjectB;
IGeometricObject geometricObjectA = collisionObjectA.GeometricObject;
IGeometricObject geometricObjectB = collisionObjectB.GeometricObject;
TriangleMeshShape triangleMeshShapeA = (TriangleMeshShape)geometricObjectA.Shape;
Triangle triangleA = triangleMeshShapeA.Mesh.GetTriangle(triangleIndexA);
TriangleMeshShape triangleMeshShapeB = (TriangleMeshShape)geometricObjectB.Shape;
Triangle triangleB = triangleMeshShapeB.Mesh.GetTriangle(triangleIndexB);
Pose poseA = geometricObjectA.Pose;
Pose poseB = geometricObjectB.Pose;
Vector3F scaleA = geometricObjectA.Scale;
Vector3F scaleB = geometricObjectB.Scale;
// Apply SRT.
Triangle transformedTriangleA;
transformedTriangleA.Vertex0 = poseA.ToWorldPosition(triangleA.Vertex0 * scaleA);
transformedTriangleA.Vertex1 = poseA.ToWorldPosition(triangleA.Vertex1 * scaleA);
transformedTriangleA.Vertex2 = poseA.ToWorldPosition(triangleA.Vertex2 * scaleA);
Triangle transformedTriangleB;
transformedTriangleB.Vertex0 = poseB.ToWorldPosition(triangleB.Vertex0 * scaleB);
transformedTriangleB.Vertex1 = poseB.ToWorldPosition(triangleB.Vertex1 * scaleB);
transformedTriangleB.Vertex2 = poseB.ToWorldPosition(triangleB.Vertex2 * scaleB);
// Make super-fast boolean check first. This is redundant if we have to compute
// a contact with SAT below. But in stochastic benchmarks it seems to be 10% faster.
bool haveContact = GeometryHelper.HaveContact(ref transformedTriangleA, ref transformedTriangleB);
if (type == CollisionQueryType.Boolean)
{
contactSet.HaveContact = (contactSet.HaveContact || haveContact);
return;
}
if (haveContact)
{
// Make sure the scaled triangles have the correct normal.
// (A negative scale changes the normal/winding order. See unit test in TriangleTest.cs.)
if (scaleA.X * scaleA.Y * scaleA.Z < 0)
{
MathHelper.Swap(ref transformedTriangleA.Vertex0, ref transformedTriangleA.Vertex1);
}
if (scaleB.X * scaleB.Y * scaleB.Z < 0)
{
MathHelper.Swap(ref transformedTriangleB.Vertex0, ref transformedTriangleB.Vertex1);
}
// Compute contact.
Vector3F position, normal;
float penetrationDepth;
haveContact = TriangleTriangleAlgorithm.GetContact(
ref transformedTriangleA, ref transformedTriangleB,
!triangleMeshShapeA.IsTwoSided, !triangleMeshShapeB.IsTwoSided,
out position, out normal, out penetrationDepth);
if (haveContact)
{
contactSet.HaveContact = true;
// In deep interpenetrations we might get no contact (penDepth = NaN).
if (!Numeric.IsNaN(penetrationDepth))
{
Contact contact = ContactHelper.CreateContact(contactSet, position, normal, penetrationDepth, false);
contact.FeatureA = triangleIndexA;
contact.FeatureB = triangleIndexB;
ContactHelper.Merge(contactSet, contact, type, CollisionDetection.ContactPositionTolerance);
}
return;
}
// We might come here if the boolean test reports contact but the SAT test
// does not because of numerical errors.
}
Debug.Assert(!haveContact);
if (type == CollisionQueryType.Contacts)
{
return;
}
Debug.Assert(type == CollisionQueryType.ClosestPoints);
if (contactSet.HaveContact)
{
// These triangles are separated but other parts of the meshes touches.
// --> Abort.
return;
}
// We do not have a specialized triangle-triangle closest points algorithm.
// Fall back to the default algorithm (GJK).
// Initialize temporary test contact set and test objects.
// Note: We assume the triangle-triangle does not care about front/back faces.
testTriangleA.Vertex0 = transformedTriangleA.Vertex0;
testTriangleA.Vertex1 = transformedTriangleA.Vertex1;
testTriangleA.Vertex2 = transformedTriangleA.Vertex2;
testGeometricObjectA.Shape = testTriangleA;
Debug.Assert(testGeometricObjectA.Scale == Vector3F.One);
Debug.Assert(testGeometricObjectA.Pose == Pose.Identity);
testCollisionObjectA.SetInternal(collisionObjectA, testGeometricObjectA);
testTriangleB.Vertex0 = transformedTriangleB.Vertex0;
testTriangleB.Vertex1 = transformedTriangleB.Vertex1;
testTriangleB.Vertex2 = transformedTriangleB.Vertex2;
testGeometricObjectB.Shape = testTriangleB;
Debug.Assert(testGeometricObjectB.Scale == Vector3F.One);
Debug.Assert(testGeometricObjectB.Pose == Pose.Identity);
testCollisionObjectB.SetInternal(collisionObjectB, testGeometricObjectB);
Debug.Assert(testContactSet.Count == 0, "testContactSet needs to be cleared.");
testContactSet.Reset(testCollisionObjectA, testCollisionObjectB);
testContactSet.IsPerturbationTestAllowed = false;
_triangleTriangleAlgorithm.ComputeCollision(testContactSet, type);
// Note: We expect no contact but because of numerical differences the triangle-triangle
// algorithm could find a shallow surface contact.
contactSet.HaveContact = (contactSet.HaveContact || testContactSet.HaveContact);
#region ----- Merge testContactSet into contactSet -----
if (testContactSet.Count > 0)
{
// Set the shape feature of the new contacts.
int numberOfContacts = testContactSet.Count;
for (int i = 0; i < numberOfContacts; i++)
{
Contact contact = testContactSet[i];
//if (contact.Lifetime.Ticks == 0) // Currently, this check is not necessary because triangleSet does not contain old contacts.
//{
contact.FeatureA = triangleIndexA;
contact.FeatureB = triangleIndexB;
//}
}
// Merge the contact info.
ContactHelper.Merge(contactSet, testContactSet, type, CollisionDetection.ContactPositionTolerance);
}
#endregion
}
// Compute contacts between 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);
}
}
// 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);
}
}
// 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);
}
}
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
}
}
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
}
}
