precision CollisionBox::projectToAxis(CollisionBox b, const Vector& axis)
{
// Find all the component contributions in the direction of
// the given axis
return (b.halfLength.x * std::abs(b.getAxis(0).scalarProduct(axis))) +
(b.halfLength.y * std::abs(b.getAxis(1).scalarProduct(axis)))+
(b.halfLength.z * std::abs(b.getAxis(2).scalarProduct(axis)));
}
static inline real transformToAxis(
const CollisionBox &box,
const Vector3 &axis
)
{
return
box.halfSize.x * real_abs(axis * box.getAxis(0)) +
box.halfSize.y * real_abs(axis * box.getAxis(1)) +
box.halfSize.z * real_abs(axis * box.getAxis(2));
}
unsigned CollisionDetector::boxAndPoint(
const CollisionBox &box,
const Vector3 &point,
CollisionData *data
)
{
// Transform the point into box coordinates
Vector3 relPt = box.transform.transformInverse(point);
Vector3 normal;
// Check each axis, looking for the axis on which the
// penetration is least deep.
real min_depth = box.halfSize.x - real_abs(relPt.x);
if (min_depth < 0) return 0;
normal = box.getAxis(0) * ((relPt.x < 0)?-1:1);
real depth = box.halfSize.y - real_abs(relPt.y);
if (depth < 0) return 0;
else if (depth < min_depth)
{
min_depth = depth;
normal = box.getAxis(1) * ((relPt.y < 0)?-1:1);
}
depth = box.halfSize.z - real_abs(relPt.z);
if (depth < 0) return 0;
else if (depth < min_depth)
{
min_depth = depth;
normal = box.getAxis(2) * ((relPt.z < 0)?-1:1);
}
// Compile the contact
Contact* contact = data->contacts;
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);
data->addContacts(1);
return 1;
}
void fillPointFaceBoxBox(
const CollisionBox &one,
const CollisionBox &two,
const Vector3 &toCentre,
CollisionData *data,
unsigned best,
real pen
)
{
// This method is called when we know that a vertex from
// box two is in contact with box one.
Contact* contact = data->contacts;
// 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.
Vector3 normal = one.getAxis(best);
if (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!
Vector3 vertex = two.halfSize;
if (two.getAxis(0) * normal < 0) vertex.x = -vertex.x;
if (two.getAxis(1) * normal < 0) vertex.y = -vertex.y;
if (two.getAxis(2) * normal < 0) vertex.z = -vertex.z;
// Create the contact data
contact->contactNormal = normal;
contact->penetration = pen;
contact->contactPoint = two.getTransform() * vertex;
contact->setBodyData(one.body, two.body,
data->friction, data->restitution);
}
bool IntersectionTests::boxAndHalfSpace(
const CollisionBox &box,
const CollisionPlane &plane
)
{
// Work out the projected radius of the box onto the plane direction
real projectedRadius = transformToAxis(box, plane.direction);
// Work out how far the box is from the origin
real boxDistance =
plane.direction *
box.getAxis(3) -
projectedRadius;
// Check for the intersection
return boxDistance <= plane.offset;
}
bool IntersectionTests::boxAndBox(
const CollisionBox &one,
const CollisionBox &two
)
{
// Find the vector between the two centres
Vector3 toCentre = two.getAxis(3) - one.getAxis(3);
return (
// Check on box one's axes first
TEST_OVERLAP(one.getAxis(0)) &&
TEST_OVERLAP(one.getAxis(1)) &&
TEST_OVERLAP(one.getAxis(2)) &&
// And on two's
TEST_OVERLAP(two.getAxis(0)) &&
TEST_OVERLAP(two.getAxis(1)) &&
TEST_OVERLAP(two.getAxis(2)) &&
// Now on the cross products
TEST_OVERLAP(one.getAxis(0) % two.getAxis(0)) &&
TEST_OVERLAP(one.getAxis(0) % two.getAxis(1)) &&
TEST_OVERLAP(one.getAxis(0) % two.getAxis(2)) &&
TEST_OVERLAP(one.getAxis(1) % two.getAxis(0)) &&
TEST_OVERLAP(one.getAxis(1) % two.getAxis(1)) &&
TEST_OVERLAP(one.getAxis(1) % two.getAxis(2)) &&
TEST_OVERLAP(one.getAxis(2) % two.getAxis(0)) &&
TEST_OVERLAP(one.getAxis(2) % two.getAxis(1)) &&
TEST_OVERLAP(one.getAxis(2) % two.getAxis(2))
);
}
unsigned CollisionDetector::boxAndBox(
const CollisionBox &one,
const CollisionBox &two,
CollisionData *data
)
{
//if (!IntersectionTests::boxAndBox(one, two)) return 0;
// Find the vector between the two centres
Vector3 toCentre = two.getAxis(3) - one.getAxis(3);
// We start assuming there is no contact
real pen = REAL_MAX;
unsigned 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.
CHECK_OVERLAP(one.getAxis(0), 0);
CHECK_OVERLAP(one.getAxis(1), 1);
CHECK_OVERLAP(one.getAxis(2), 2);
CHECK_OVERLAP(two.getAxis(0), 3);
CHECK_OVERLAP(two.getAxis(1), 4);
CHECK_OVERLAP(two.getAxis(2), 5);
// Store the best axis-major, in case we run into almost
// parallel edge collisions later
unsigned bestSingleAxis = best;
CHECK_OVERLAP(one.getAxis(0) % two.getAxis(0), 6);
CHECK_OVERLAP(one.getAxis(0) % two.getAxis(1), 7);
CHECK_OVERLAP(one.getAxis(0) % two.getAxis(2), 8);
CHECK_OVERLAP(one.getAxis(1) % two.getAxis(0), 9);
CHECK_OVERLAP(one.getAxis(1) % two.getAxis(1), 10);
CHECK_OVERLAP(one.getAxis(1) % two.getAxis(2), 11);
CHECK_OVERLAP(one.getAxis(2) % two.getAxis(0), 12);
CHECK_OVERLAP(one.getAxis(2) % two.getAxis(1), 13);
CHECK_OVERLAP(one.getAxis(2) % two.getAxis(2), 14);
// Make sure we've got a result.
assert(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);
data->addContacts(1);
return 1;
}
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.0f, data, best-3, pen);
data->addContacts(1);
return 1;
}
else
{
// We've got an edge-edge contact. Find out which axes
best -= 6;
unsigned oneAxisIndex = best / 3;
unsigned twoAxisIndex = best % 3;
Vector3 oneAxis = one.getAxis(oneAxisIndex);
Vector3 twoAxis = two.getAxis(twoAxisIndex);
Vector3 axis = oneAxis % twoAxis;
axis.normalise();
// The axis should point from box one to box two.
if (axis * toCentre > 0) axis = axis * -1.0f;
// 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.
Vector3 ptOnOneEdge = one.halfSize;
Vector3 ptOnTwoEdge = two.halfSize;
for (unsigned i = 0; i < 3; i++)
{
if (i == oneAxisIndex) ptOnOneEdge[i] = 0;
else if (one.getAxis(i) * axis > 0) ptOnOneEdge[i] = -ptOnOneEdge[i];
if (i == twoAxisIndex) ptOnTwoEdge[i] = 0;
else if (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.
Vector3 vertex = contactPoint(
ptOnOneEdge, oneAxis, one.halfSize[oneAxisIndex],
ptOnTwoEdge, twoAxis, two.halfSize[twoAxisIndex],
bestSingleAxis > 2
);
// We can fill the contact.
Contact* contact = data->contacts;
contact->penetration = pen;
contact->contactNormal = axis;
contact->contactPoint = vertex;
contact->setBodyData(one.body, two.body,
data->friction, data->restitution);
data->addContacts(1);
return 1;
}
return 0;
}
bool CollisionBox::earlyOut(CollisionBox b1, CollisionBox b2)
{
Vector centre = b2.centre - b1.centre;
return !(
overlaps(b1, b2, b1.getAxis(0)) &&
overlaps(b1, b2, b1.getAxis(1)) &&
overlaps(b1, b2, b1.getAxis(2)) &&
overlaps(b1, b2, b2.getAxis(0)) &&
overlaps(b1, b2, b2.getAxis(1)) &&
overlaps(b1, b2, b2.getAxis(2)) &&
overlaps(b1, b2, b1.getAxis(0) % b2.getAxis(0)) &&
overlaps(b1, b2, b1.getAxis(0) % b2.getAxis(1)) &&
overlaps(b1, b2, b1.getAxis(0) % b2.getAxis(2)) &&
overlaps(b1, b2, b1.getAxis(1) % b2.getAxis(0)) &&
overlaps(b1, b2, b1.getAxis(1) % b2.getAxis(1)) &&
overlaps(b1, b2, b1.getAxis(1) % b2.getAxis(2)) &&
overlaps(b1, b2, b1.getAxis(2) % b2.getAxis(0)) &&
overlaps(b1, b2, b1.getAxis(2) % b2.getAxis(1)) &&
overlaps(b1, b2, b1.getAxis(2) % b2.getAxis(2))
);
}
