private static double TransformToAxis(CollisionBox box, Vector3d axis)
{
return
(box.HalfSize.x * Vector3d.AbsDot(axis, box.GetAxis(0)) +
box.HalfSize.y * Vector3d.AbsDot(axis, box.GetAxis(1)) +
box.HalfSize.z * Vector3d.AbsDot(axis, box.GetAxis(2)));
}
public static void BoxAndPoint(CollisionBox box, Vector3d point, CollisionData data)
{
// Transform the point into box coordinates
Vector3d relPt = box.Transform.TransformInverse(point);
Vector3d normal;
// Check each axis, looking for the axis on which the
// penetration is least deep.
double min_depth = box.HalfSize.x - Math.Abs(relPt.x);
if (min_depth < 0)
{
return;
}
normal = box.GetAxis(0) * ((relPt.x < 0) ? -1 : 1);
double depth = box.HalfSize.y - Math.Abs(relPt.y);
if (depth < 0)
{
return;
}
else if (depth < min_depth)
{
min_depth = depth;
normal = box.GetAxis(1) * ((relPt.y < 0) ? -1 : 1);
}
depth = box.HalfSize.z - Math.Abs(relPt.z);
if (depth < 0)
{
return;
}
else if (depth < min_depth)
{
min_depth = depth;
normal = box.GetAxis(2) * ((relPt.z < 0) ? -1 : 1);
}
// Compile the contact
var contact = data.GetContact();
contact.ContactNormal = normal;
contact.ContactPoint = point;
contact.Penetration = min_depth;
// Note that we don't know what rigid body the point
// belongs to, so we just use NULL. Where this is called
// this value can be left, or filled in.
contact.SetBodyData(box.Body, null, data.Friction, data.Restitution);
}
public static bool BoxAndHalfSpace(CollisionBox box, CollisionPlane plane)
{
// Work out the projected radius of the box onto the plane direction
double projectedRadius = TransformToAxis(box, plane.Direction);
// Work out how far the box is from the origin
double boxDistance = Vector3d.Dot(plane.Direction, box.GetAxis(3)) - projectedRadius;
// Check for the intersection
return(boxDistance <= plane.Offset);
}
private static void FillPointFaceBoxBox(CollisionBox one, CollisionBox two, Vector3d toCentre, CollisionData data, int best, double pen)
{
// This method is called when we know that a vertex from
// box two is in contact with box one.
var contact = data.GetContact();
// We know which axis the collision is on (i.e. best),
// but we need to work out which of the two faces on
// this axis.
Vector3d normal = one.GetAxis(best);
if (Vector3d.Dot(one.GetAxis(best), toCentre) > 0)
{
normal = normal * -1.0f;
}
// Work out which vertex of box two we're colliding with.
// Using toCentre doesn't work!
Vector3d vertex = two.HalfSize;
if (Vector3d.Dot(two.GetAxis(0), normal) < 0)
{
vertex.x = -vertex.x;
}
if (Vector3d.Dot(two.GetAxis(1), normal) < 0)
{
vertex.y = -vertex.y;
}
if (Vector3d.Dot(two.GetAxis(2), normal) < 0)
{
vertex.z = -vertex.z;
}
// Create the contact data
contact.ContactNormal = normal;
contact.Penetration = pen;
contact.ContactPoint = two.Transform * vertex;
contact.SetBodyData(one.Body, two.Body, data.Friction, data.Restitution);
}
public static void BoxAndBox(CollisionBox one, CollisionBox two, CollisionData data)
{
// Make sure we have contacts
if (data.NoMoreContacts())
{
return;
}
//if (!IntersectionTests.BoxAndBox(one, two)) return;
// Find the vector between the two centres
Vector3d toCentre = two.GetAxis(3) - one.GetAxis(3);
// We start assuming there is no contact
double pen = double.MaxValue;
int best = 0xffffff;
// Now we check each axes, returning if it gives us
// a separating axis, and keeping track of the axis with
// the smallest penetration otherwise.
if (!TryAxis(one, two, one.GetAxis(0), toCentre, 0, ref pen, ref best))
{
return;
}
if (!TryAxis(one, two, one.GetAxis(1), toCentre, 1, ref pen, ref best))
{
return;
}
if (!TryAxis(one, two, one.GetAxis(2), toCentre, 2, ref pen, ref best))
{
return;
}
if (!TryAxis(one, two, two.GetAxis(0), toCentre, 3, ref pen, ref best))
{
return;
}
if (!TryAxis(one, two, two.GetAxis(1), toCentre, 4, ref pen, ref best))
{
return;
}
if (!TryAxis(one, two, two.GetAxis(2), toCentre, 5, ref pen, ref best))
{
return;
}
// Store the best axis-major, in case we run into almost
// parallel edge collisions later
int bestSingleAxis = best;
if (!TryAxis(one, two, Vector3d.Cross(one.GetAxis(0), two.GetAxis(0)), toCentre, 6, ref pen, ref best))
{
return;
}
if (!TryAxis(one, two, Vector3d.Cross(one.GetAxis(0), two.GetAxis(1)), toCentre, 7, ref pen, ref best))
{
return;
}
if (!TryAxis(one, two, Vector3d.Cross(one.GetAxis(0), two.GetAxis(2)), toCentre, 8, ref pen, ref best))
{
return;
}
if (!TryAxis(one, two, Vector3d.Cross(one.GetAxis(1), two.GetAxis(0)), toCentre, 9, ref pen, ref best))
{
return;
}
if (!TryAxis(one, two, Vector3d.Cross(one.GetAxis(1), two.GetAxis(1)), toCentre, 10, ref pen, ref best))
{
return;
}
if (!TryAxis(one, two, Vector3d.Cross(one.GetAxis(1), two.GetAxis(2)), toCentre, 11, ref pen, ref best))
{
return;
}
if (!TryAxis(one, two, Vector3d.Cross(one.GetAxis(2), two.GetAxis(0)), toCentre, 12, ref pen, ref best))
{
return;
}
if (!TryAxis(one, two, Vector3d.Cross(one.GetAxis(2), two.GetAxis(1)), toCentre, 13, ref pen, ref best))
{
return;
}
if (!TryAxis(one, two, Vector3d.Cross(one.GetAxis(2), two.GetAxis(2)), toCentre, 14, ref pen, ref best))
{
return;
}
// Make sure we've got a result.
if (best == 0xffffff)
{
throw new Exception("best == 0xffffff");
}
// We now know there's a collision, and we know which
// of the axes gave the smallest penetration. We now
// can deal with it in different ways depending on
// the case.
if (best < 3)
{
// We've got a vertex of box two on a face of box one.
FillPointFaceBoxBox(one, two, toCentre, data, best, pen);
}
else if (best < 6)
{
// We've got a vertex of box one on a face of box two.
// We use the same algorithm as above, but swap around
// one and two (and therefore also the vector between their
// centres).
FillPointFaceBoxBox(two, one, toCentre * -1.0, data, best - 3, pen);
}
else
{
// We've got an edge-edge contact. Find out which axes
best -= 6;
int oneAxisIndex = best / 3;
int twoAxisIndex = best % 3;
Vector3d oneAxis = one.GetAxis(oneAxisIndex);
Vector3d twoAxis = two.GetAxis(twoAxisIndex);
Vector3d axis = Vector3d.Cross(oneAxis, twoAxis);
axis.Normalize();
// The axis should point from box one to box two.
if (Vector3d.Dot(axis, toCentre) > 0)
{
axis *= -1.0;
}
// We have the axes, but not the edges: each axis has 4 edges parallel
// to it, we need to find which of the 4 for each object. We do
// that by finding the point in the centre of the edge. We know
// its component in the direction of the box's collision axis is zero
// (its a mid-point) and we determine which of the extremes in each
// of the other axes is closest.
Vector3d ptOnOneEdge = one.HalfSize;
Vector3d ptOnTwoEdge = two.HalfSize;
for (int i = 0; i < 3; i++)
{
if (i == oneAxisIndex)
{
ptOnOneEdge[i] = 0;
}
else if (Vector3d.Dot(one.GetAxis(i), axis) > 0)
{
ptOnOneEdge[i] = -ptOnOneEdge[i];
}
if (i == twoAxisIndex)
{
ptOnTwoEdge[i] = 0;
}
else if (Vector3d.Dot(two.GetAxis(i), axis) < 0)
{
ptOnTwoEdge[i] = -ptOnTwoEdge[i];
}
}
// Move them into world coordinates (they are already oriented
// correctly, since they have been derived from the axes).
ptOnOneEdge = one.Transform * ptOnOneEdge;
ptOnTwoEdge = two.Transform * ptOnTwoEdge;
// So we have a point and a direction for the colliding edges.
// We need to find out point of closest approach of the two
// line-segments.
Vector3d vertex = ContactPoint(
ptOnOneEdge, oneAxis, one.HalfSize[oneAxisIndex],
ptOnTwoEdge, twoAxis, two.HalfSize[twoAxisIndex],
bestSingleAxis > 2
);
// We can fill the contact.
var contact = data.GetContact();
contact.Penetration = pen;
contact.ContactNormal = axis;
contact.ContactPoint = vertex;
contact.SetBodyData(one.Body, two.Body, data.Friction, data.Restitution);
}
}
public static bool BoxAndBox(CollisionBox one, CollisionBox two)
{
// Find the vector between the two centres
Vector3d toCentre = two.GetAxis(3) - one.GetAxis(3);
if (!OverlapOnAxis(one, two, one.GetAxis(0), toCentre))
{
return(false);
}
if (!OverlapOnAxis(one, two, one.GetAxis(1), toCentre))
{
return(false);
}
if (!OverlapOnAxis(one, two, one.GetAxis(2), toCentre))
{
return(false);
}
if (!OverlapOnAxis(one, two, two.GetAxis(0), toCentre))
{
return(false);
}
if (!OverlapOnAxis(one, two, two.GetAxis(1), toCentre))
{
return(false);
}
if (!OverlapOnAxis(one, two, two.GetAxis(2), toCentre))
{
return(false);
}
if (!OverlapOnAxis(one, two, Vector3d.Cross(one.GetAxis(0), two.GetAxis(0)), toCentre))
{
return(false);
}
if (!OverlapOnAxis(one, two, Vector3d.Cross(one.GetAxis(0), two.GetAxis(1)), toCentre))
{
return(false);
}
if (!OverlapOnAxis(one, two, Vector3d.Cross(one.GetAxis(0), two.GetAxis(2)), toCentre))
{
return(false);
}
if (!OverlapOnAxis(one, two, Vector3d.Cross(one.GetAxis(1), two.GetAxis(0)), toCentre))
{
return(false);
}
if (!OverlapOnAxis(one, two, Vector3d.Cross(one.GetAxis(1), two.GetAxis(1)), toCentre))
{
return(false);
}
if (!OverlapOnAxis(one, two, Vector3d.Cross(one.GetAxis(1), two.GetAxis(2)), toCentre))
{
return(false);
}
if (!OverlapOnAxis(one, two, Vector3d.Cross(one.GetAxis(2), two.GetAxis(0)), toCentre))
{
return(false);
}
if (!OverlapOnAxis(one, two, Vector3d.Cross(one.GetAxis(2), two.GetAxis(1)), toCentre))
{
return(false);
}
if (!OverlapOnAxis(one, two, Vector3d.Cross(one.GetAxis(2), two.GetAxis(2)), toCentre))
{
return(false);
}
return(true);
}