void ContactGenerator::box_box(const CollisionShape &a, const RigidBody &rbA, const CollisionShape &b, const RigidBody &rbB, ContactManifold &contactManifold)
{
// Make sure we have contacts left and Check if they're boxes.
if ( a.getShapeType() != SHAPE_BOX || b.getShapeType() != SHAPE_BOX ) { return; }
// then typecast their appropriate shapes
const ShapeBox& boxA = (const ShapeBox&)a;
const ShapeBox& boxB = (const ShapeBox&)b;
const Vec3& boxAHalfExtents = boxA.getHalfExtents();
const Vec3& boxBHalfExtents = boxB.getHalfExtents();
Transform boxATransform = rbA.getTransform() * a.getOffset();
Transform boxBTransform = rbB.getTransform() * b.getOffset();
// Vector between box centres.
Vec3 separation = boxBTransform.getPosition() - boxATransform.getPosition();
Scalar smallestPen = SCALAR_MAX;
unsigned int bestPen = 0xffffffff;
// Check each axis, keeping track of the axis with the smallest penetration.
// Stops when if finds an axis without penetration.
if (!tryAxis(boxA, boxATransform, boxB, boxBTransform, boxATransform.getAxisVector(0), separation, 0, smallestPen, bestPen) ||
!tryAxis(boxA, boxATransform, boxB, boxBTransform, boxATransform.getAxisVector(1), separation, 1, smallestPen, bestPen) ||
!tryAxis(boxA, boxATransform, boxB, boxBTransform, boxATransform.getAxisVector(2), separation, 2, smallestPen, bestPen) ||
!tryAxis(boxA, boxATransform, boxB, boxBTransform, boxBTransform.getAxisVector(0), separation, 3, smallestPen, bestPen) ||
!tryAxis(boxA, boxATransform, boxB, boxBTransform, boxBTransform.getAxisVector(1), separation, 4, smallestPen, bestPen) ||
!tryAxis(boxA, boxATransform, boxB, boxBTransform, boxBTransform.getAxisVector(2), separation, 5, smallestPen, bestPen) )
{
return;
}
unsigned bestSingleAxis = bestPen;
if (!tryAxis(boxA, boxATransform, boxB, boxBTransform, boxATransform.getAxisVector(0).cross(boxBTransform.getAxisVector(0)), separation, 6, smallestPen, bestPen) ||
!tryAxis(boxA, boxATransform, boxB, boxBTransform, boxATransform.getAxisVector(0).cross(boxBTransform.getAxisVector(1)), separation, 7, smallestPen, bestPen) ||
!tryAxis(boxA, boxATransform, boxB, boxBTransform, boxATransform.getAxisVector(0).cross(boxBTransform.getAxisVector(2)), separation, 8, smallestPen, bestPen) ||
!tryAxis(boxA, boxATransform, boxB, boxBTransform, boxATransform.getAxisVector(1).cross(boxBTransform.getAxisVector(0)), separation, 9, smallestPen, bestPen) ||
!tryAxis(boxA, boxATransform, boxB, boxBTransform, boxATransform.getAxisVector(1).cross(boxBTransform.getAxisVector(1)), separation, 10, smallestPen, bestPen) ||
!tryAxis(boxA, boxATransform, boxB, boxBTransform, boxATransform.getAxisVector(1).cross(boxBTransform.getAxisVector(2)), separation, 11, smallestPen, bestPen) ||
!tryAxis(boxA, boxATransform, boxB, boxBTransform, boxATransform.getAxisVector(2).cross(boxBTransform.getAxisVector(0)), separation, 12, smallestPen, bestPen) ||
!tryAxis(boxA, boxATransform, boxB, boxBTransform, boxATransform.getAxisVector(2).cross(boxBTransform.getAxisVector(1)), separation, 13, smallestPen, bestPen) ||
!tryAxis(boxA, boxATransform, boxB, boxBTransform, boxATransform.getAxisVector(2).cross(boxBTransform.getAxisVector(2)), separation, 14, smallestPen, bestPen) )
{
return;
}
// We've found a collision, and we know which of the axes gave the smallest penetration.
if (bestPen < 3)
{
// Vertex of boxB in face of boxA
// TODO: Do stuff
//std::cout << bestPen << " Vertex of boxB in face of boxA\n";
fillPointFaceBoxBox(boxA, boxATransform, boxB, boxBTransform, separation, contactManifold, bestPen, smallestPen, false);
return;
}
else if (bestPen < 6)
{
// Vertex of boxA in face of boxB
// TODO: Do stuff
//std::cout << bestPen << " Vertex of boxA in face of boxB\n";
fillPointFaceBoxBox(boxA, boxATransform, boxB, boxBTransform, -separation, contactManifold, bestPen-3, smallestPen, true);
return;
}
else
{
// Edge Edge contact.
//std::cout << "Colliding >= 6";
//std::cout << bestPen << " Edge-Edge\n";
// Find which axis.
bestPen -= 6;
unsigned int axisIndexA = bestPen / 3;
unsigned int axisIndexB = bestPen % 3;
Vec3 axisA = boxATransform.getAxisVector(axisIndexA);
Vec3 axisB = boxBTransform.getAxisVector(axisIndexB);
Vec3 axis = axisA.cross(axisB);
axis.normalize();
// Axis should point from box one to box two.
if ( axis.dot(separation) > 0 )
{
axis = -axis;
}
Vec3 ptOnEdgeA = boxAHalfExtents;
Vec3 ptOnEdgeB = -boxBHalfExtents;
for (unsigned int i = 0; i < 3; i++)
{
if (i == axisIndexA)
{
ptOnEdgeA[i] = 0;
}
else if (boxATransform.getAxisVector(i).dot(axis) > 0)
{
ptOnEdgeA[i] = -ptOnEdgeA[i];
}
if (i == axisIndexB)
{
ptOnEdgeB[i] = 0;
}
else if (boxBTransform.getAxisVector(i).dot(axis) > 0)
{
ptOnEdgeB[i] = -ptOnEdgeB[i];
}
}
// Transform the points into world coordinates.
ptOnEdgeA = boxATransform * ptOnEdgeA;
ptOnEdgeB = boxBTransform * ptOnEdgeB;
// Find the point of closest approach of the two
// line-segments.
Vec3 vertex = contactPoint(ptOnEdgeA, axisA, boxAHalfExtents.get(axisIndexA), ptOnEdgeB, axisB, boxBHalfExtents.get(axisIndexB), bestSingleAxis > 2);
// Create contact data
ContactPoint newContact;
newContact.normal = axis;
newContact.penetration = smallestPen;
newContact.position = vertex;
contactManifold.addContactPoint(newContact);
}
return;
}
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;
}
