public override void ComputeCollision(ContactSet contactSet, CollisionQueryType type)
{
// Invoke GJK for closest points.
if (type == CollisionQueryType.ClosestPoints)
{
throw new GeometryException("This collision algorithm cannot handle closest-point queries. Use GJK instead.");
}
//if (UseMpr)
//{
// if (_mpr == null)
// _mpr = new MinkowskiPortalRefinement(CollisionDetection);
// _mpr.ComputeCollision(contactSet, type);
// return;
//}
CollisionObject collisionObjectA = contactSet.ObjectA;
CollisionObject collisionObjectB = contactSet.ObjectB;
IGeometricObject geometricObjectA = collisionObjectA.GeometricObject;
IGeometricObject geometricObjectB = collisionObjectB.GeometricObject;
TriangleShape triangleShapeA = geometricObjectA.Shape as TriangleShape;
TriangleShape triangleShapeB = geometricObjectB.Shape as TriangleShape;
// Check if collision objects shapes are correct.
if (triangleShapeA == null || triangleShapeB == null)
{
throw new ArgumentException("The contact set must contain triangle shapes.", "contactSet");
}
Vector3 scaleA = Vector3.Absolute(geometricObjectA.Scale);
Vector3 scaleB = Vector3.Absolute(geometricObjectB.Scale);
Pose poseA = geometricObjectA.Pose;
Pose poseB = geometricObjectB.Pose;
// Get triangles in world space.
Triangle triangleA;
triangleA.Vertex0 = poseA.ToWorldPosition(triangleShapeA.Vertex0 * scaleA);
triangleA.Vertex1 = poseA.ToWorldPosition(triangleShapeA.Vertex1 * scaleA);
triangleA.Vertex2 = poseA.ToWorldPosition(triangleShapeA.Vertex2 * scaleA);
Triangle triangleB;
triangleB.Vertex0 = poseB.ToWorldPosition(triangleShapeB.Vertex0 * scaleB);
triangleB.Vertex1 = poseB.ToWorldPosition(triangleShapeB.Vertex1 * scaleB);
triangleB.Vertex2 = poseB.ToWorldPosition(triangleShapeB.Vertex2 * scaleB);
if (type == CollisionQueryType.Boolean)
{
contactSet.HaveContact = GeometryHelper.HaveContact(ref triangleA, ref triangleB);
return;
}
Debug.Assert(type == CollisionQueryType.Contacts, "TriangleTriangleAlgorithm cannot handle closest point queries.");
// Assume no contact.
contactSet.HaveContact = false;
Vector3 position, normal;
float penetrationDepth;
if (!GetContact(ref triangleA, ref triangleB, false, false, out position, out normal, out penetrationDepth))
{
return;
}
contactSet.HaveContact = true;
Contact contact = ContactHelper.CreateContact(contactSet, position, normal, penetrationDepth, false);
ContactHelper.Merge(contactSet, contact, type, CollisionDetection.ContactPositionTolerance);
}