public override void ComputeCollision(ContactSet contactSet, CollisionQueryType type)
{
// Object A should be the plane.
// Object B should be the other object.
IGeometricObject planeObject = contactSet.ObjectA.GeometricObject;
IGeometricObject boxObject = contactSet.ObjectB.GeometricObject;
// Swap objects if necessary.
bool swapped = (boxObject.Shape is PlaneShape);
if (swapped)
{
MathHelper.Swap(ref planeObject, ref boxObject);
}
PlaneShape planeShape = planeObject.Shape as PlaneShape;
BoxShape boxShape = boxObject.Shape as BoxShape;
// Check if shapes are correct.
if (planeShape == null || boxShape == null)
{
throw new ArgumentException("The contact set must contain a plane and a box shape.", "contactSet");
}
// Get transformations.
Vector3 scalePlane = planeObject.Scale;
Vector3 scaleBox = boxObject.Scale;
Pose posePlane = planeObject.Pose;
Pose poseBox = boxObject.Pose;
// Apply scale to plane and transform plane into world space.
Plane planeWorld = new Plane(planeShape);
planeWorld.Scale(ref scalePlane); // Scale plane.
planeWorld.ToWorld(ref posePlane); // Transform plane to world space.
// Transform plane normal to local space of box.
Vector3 planeNormalLocalB = poseBox.ToLocalDirection(planeWorld.Normal);
// Get support vertex nearest to the plane.
Vector3 supportVertexBLocal = boxShape.GetSupportPoint(-planeNormalLocalB, scaleBox);
// Transform support vertex into world space.
Vector3 supportVertexBWorld = poseBox.ToWorldPosition(supportVertexBLocal);
// Project vertex onto separating axis (given by plane normal).
float distance = Vector3.Dot(supportVertexBWorld, planeWorld.Normal);
// Check for collision.
float penetrationDepth = planeWorld.DistanceFromOrigin - distance;
contactSet.HaveContact = (penetrationDepth >= 0);
if (type == CollisionQueryType.Boolean || (type == CollisionQueryType.Contacts && !contactSet.HaveContact))
{
// HaveContact queries can exit here.
// GetContacts queries can exit here if we don't have a contact.
return;
}
// We have contact.
if (!CollisionDetection.FullContactSetPerFrame || type == CollisionQueryType.ClosestPoints)
{
// Use only the already known contact.
AddContact(ref supportVertexBWorld, ref planeWorld, penetrationDepth, swapped, contactSet, type);
}
else
{
// Apply scale to box extent.
Vector3 boxHalfExtent = boxShape.Extent * scaleBox * 0.5f;
// Check all box vertices against plane.
CheckContact(ref planeWorld, new Vector3(-boxHalfExtent.X, -boxHalfExtent.Y, -boxHalfExtent.Z), ref poseBox, swapped, contactSet);
CheckContact(ref planeWorld, new Vector3(-boxHalfExtent.X, -boxHalfExtent.Y, boxHalfExtent.Z), ref poseBox, swapped, contactSet);
CheckContact(ref planeWorld, new Vector3(-boxHalfExtent.X, boxHalfExtent.Y, -boxHalfExtent.Z), ref poseBox, swapped, contactSet);
CheckContact(ref planeWorld, new Vector3(-boxHalfExtent.X, boxHalfExtent.Y, boxHalfExtent.Z), ref poseBox, swapped, contactSet);
CheckContact(ref planeWorld, new Vector3(boxHalfExtent.X, -boxHalfExtent.Y, -boxHalfExtent.Z), ref poseBox, swapped, contactSet);
CheckContact(ref planeWorld, new Vector3(boxHalfExtent.X, -boxHalfExtent.Y, boxHalfExtent.Z), ref poseBox, swapped, contactSet);
CheckContact(ref planeWorld, new Vector3(boxHalfExtent.X, boxHalfExtent.Y, -boxHalfExtent.Z), ref poseBox, swapped, contactSet);
CheckContact(ref planeWorld, new Vector3(boxHalfExtent.X, boxHalfExtent.Y, boxHalfExtent.Z), ref poseBox, swapped, contactSet);
}
}
