unsigned jfCollisionDetector_x86::boxAndBox(const jfCollisionBox& one,
const jfCollisionBox& two,
jfCollisionData* data
) const
{
//TODO:Ideal candidate for CUDA
jfIntersectionTester_x86 intersectionTester;
// Check for intersection
if (! intersectionTester.boxAndBox(one, two))
{
return 0;
}
jfVector3_x86 oneAxis0, oneAxis1, oneAxis2, oneAxis3;
jfVector3_x86 twoAxis0, twoAxis1, twoAxis2, twoAxis3;
jfVector3_x86 toCentre;
//Find vector between the two centres
one.getAxisVector(3, &oneAxis3);
two.getAxisVector(3, &twoAxis3);
twoAxis3.subtract(oneAxis3, &toCentre);
//Assume no contact at start
jfReal pen = JF_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.
one.getAxisVector(0, &oneAxis0);
CHECK_OVERLAP(oneAxis0, 0);
one.getAxisVector(1, &oneAxis1);
CHECK_OVERLAP(oneAxis1, 1);
one.getAxisVector(2, &oneAxis2);
CHECK_OVERLAP(oneAxis2, 2);
two.getAxisVector(0,&twoAxis0);
CHECK_OVERLAP(twoAxis0, 3);
two.getAxisVector(1,&twoAxis1);
CHECK_OVERLAP(twoAxis1, 4);
two.getAxisVector(2,&twoAxis2);
CHECK_OVERLAP(twoAxis2, 5);
// Store the best axis-major, in case we run into almost
// parallel edge collisions later
unsigned bestSingleAxis = best;
jfVector3_x86 crossProduct;
oneAxis0.crossProduct(twoAxis0, &crossProduct);
CHECK_OVERLAP(crossProduct, 6);
oneAxis0.crossProduct(twoAxis1, &crossProduct);
CHECK_OVERLAP(crossProduct, 7);
oneAxis0.crossProduct(twoAxis2, &crossProduct);
CHECK_OVERLAP(crossProduct, 8);
oneAxis1.crossProduct(twoAxis0, &crossProduct);
CHECK_OVERLAP(crossProduct, 9);
oneAxis1.crossProduct(twoAxis1, &crossProduct);
CHECK_OVERLAP(crossProduct, 10);
oneAxis1.crossProduct(twoAxis2, &crossProduct);
CHECK_OVERLAP(crossProduct, 11);
oneAxis2.crossProduct(twoAxis0, &crossProduct);
CHECK_OVERLAP(crossProduct, 12);
oneAxis2.crossProduct(twoAxis1, &crossProduct);
CHECK_OVERLAP(crossProduct, 13);
oneAxis2.crossProduct(twoAxis2, &crossProduct);
CHECK_OVERLAP(crossProduct, 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).
toCentre *= -1;
fillPointFaceBoxBox(two, one, toCentre, 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;
jfVector3_x86 oneAxis;
one.getAxisVector(oneAxisIndex, &oneAxis);
jfVector3_x86 twoAxis;
two.getAxisVector(twoAxisIndex, &twoAxis);
jfVector3_x86 axis;
oneAxis.crossProduct(twoAxis, &axis);
axis.normalize();
// The axis should point from box one to box two.
if (axis.dotProduct(toCentre) > 0)
{
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.
jfVector3_x86 ptOnOneEdge;
one.getHalfSize(&ptOnOneEdge);
jfVector3_x86 ptOnTwoEdge;
two.getHalfSize(&ptOnTwoEdge);
for (unsigned i = 0; i < 3; i++)
{
jfVector3_x86 oneAxisCurrent;
jfVector3_x86 twoAxisCurrent;
one.getAxisVector(i, &oneAxisCurrent);
two.getAxisVector(i, &twoAxisCurrent);
if (i == oneAxisIndex)
{
ptOnOneEdge.setElem(i,0);
}
else if (oneAxisCurrent.dotProduct(axis) > 0)
{
ptOnOneEdge.setElem(i, -ptOnOneEdge.getElem(i));
}
if (i == twoAxisIndex)
{
ptOnTwoEdge.setElem(i, 0);
}
else if (twoAxisCurrent.dotProduct(axis) < 0)
{
ptOnTwoEdge.setElem(i, -ptOnTwoEdge.getElem(i));
}
}
// Move them into world coordinates (they are already oriented
// correctly, since they have been derived from the axes).
jfMatrix4_x86 oneTransform, twoTransform;
one.getTransformMatrix(&oneTransform);
oneTransform.multiply(ptOnOneEdge, &ptOnOneEdge);
two.getTransformMatrix(&twoTransform);
twoTransform.multiply(ptOnTwoEdge, &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.
jfVector3_x86 vertex;
jfVector3_x86 oneHalfSize, twoHalfSize;
one.getHalfSize(&oneHalfSize);
two.getHalfSize(&twoHalfSize);
contactPoint(
ptOnOneEdge,
oneAxis,
oneHalfSize.getElem(oneAxisIndex),
ptOnTwoEdge,
twoAxis,
twoHalfSize.getElem(twoAxisIndex),
(bestSingleAxis > 2),
&vertex
);
// We can fill the contact.
//jfContact* contact = data->contacts;
jfContact_x86 contact;
contact.setPenetration(pen);
contact.setContactNormal(axis);
contact.setContactPoint(vertex);
contact.setBodyData(one.getBody(),
two.getBody(),
data->getFriction(),
data->getRestitution());
data->addContact(contact);
return 1;
}
return 0;
}
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;
}
