unsigned CollisionDetector::boxAndSphere(
const CollisionBox &box,
const CollisionSphere &sphere,
CollisionData *data
)
{
// Transform the centre of the sphere into box coordinates
Vector3 centre = sphere.getAxis(3);
Vector3 relCentre = box.transform.transformInverse(centre);
// Early out check to see if we can exclude the contact
if (real_abs(relCentre.x) - sphere.radius > box.halfSize.x ||
real_abs(relCentre.y) - sphere.radius > box.halfSize.y ||
real_abs(relCentre.z) - sphere.radius > box.halfSize.z)
{
return 0;
}
Vector3 closestPt(0,0,0);
real dist;
// Clamp each coordinate to the box.
dist = relCentre.x;
if (dist > box.halfSize.x) dist = box.halfSize.x;
if (dist < -box.halfSize.x) dist = -box.halfSize.x;
closestPt.x = dist;
dist = relCentre.y;
if (dist > box.halfSize.y) dist = box.halfSize.y;
if (dist < -box.halfSize.y) dist = -box.halfSize.y;
closestPt.y = dist;
dist = relCentre.z;
if (dist > box.halfSize.z) dist = box.halfSize.z;
if (dist < -box.halfSize.z) dist = -box.halfSize.z;
closestPt.z = dist;
// Check we're in contact
dist = (closestPt - relCentre).squareMagnitude();
if (dist > sphere.radius * sphere.radius) return 0;
// Compile the contact
Vector3 closestPtWorld = box.transform.transform(closestPt);
Contact* contact = data->contacts;
contact->contactNormal = (closestPtWorld - centre);
contact->contactNormal.normalise();
contact->contactPoint = closestPtWorld;
contact->penetration = sphere.radius - real_sqrt(dist);
contact->setBodyData(box.body, sphere.body,
data->friction, data->restitution);
data->addContacts(1);
return 1;
}
unsigned jfCollisionDetector_x86::boxAndSphere(const jfCollisionBox& box,
const jfCollisionSphere& sphere,
jfCollisionData* data
) const
{
// Transform the centre of the sphere into box coordinates
jfMatrix4_x86 boxTransformMatrix;
jfVector3_x86 relCentre;
jfVector3_x86 centre;
jfVector3_x86 boxHalfSize;
sphere.getAxisVector(3, ¢re);
// cout<<"Printing centre"<<centre<< endl;
box.getTransformMatrix(&boxTransformMatrix);
//cout <<"Printing boxTransformMatrix" << boxTransformMatrix << endl;
boxTransformMatrix.transformInverse(centre, &relCentre);
box.getHalfSize(&boxHalfSize);
cout<<"boxTransformMatrix is :"<<boxTransformMatrix<<endl;
cout<<"boxHalfSize is :"<<boxHalfSize<<endl;
cout<<"sphere.getRadius() :"<<sphere.getRadius()<<endl;
jfVector3_x86 sphereCentre, boxPos;
box.getBody()->getPos(&boxPos);
sphere.getBody()->getPos(&sphereCentre);
cout<<"Printing boxPos"<<boxPos<<endl;
cout<<"Printing centre"<<sphereCentre<<endl;
cout<<"Printing relCentre"<<relCentre<<endl;
// Early out check to see if we can exclude the contact
// Principle of Seperating Axis
// @ref Millington p.283
if (((jfRealAbs(relCentre.getX()) - sphere.getRadius()) > boxHalfSize.getX()) ||
((jfRealAbs(relCentre.getY()) - sphere.getRadius()) > boxHalfSize.getY()) ||
((jfRealAbs(relCentre.getZ()) - sphere.getRadius()) > boxHalfSize.getZ()))
{
cout<<"DEBUG: Early out"<<endl;
return 0;
}
jfVector3_x86 closestPt(0,0,0);
jfReal dist;
// Clamp each coordinate to the box.
dist = relCentre.getX();
if (dist > boxHalfSize.getX())
{
dist = boxHalfSize.getX();
}
if (dist < -boxHalfSize.getX())
{
dist = -boxHalfSize.getX();
}
closestPt.setX(dist);
dist = relCentre.getY();
if (dist > boxHalfSize.getY())
{
dist = boxHalfSize.getY();
}
if (dist < -boxHalfSize.getY())
{
dist = -boxHalfSize.getY();
}
closestPt.setY(dist);
dist = relCentre.getZ();
if (dist > boxHalfSize.getZ())
{
dist = boxHalfSize.getZ();
}
if (dist < -boxHalfSize.getZ())
{
dist = -boxHalfSize.getZ();
}
closestPt.setZ(dist);
cout <<"closestPt is :"<<closestPt<<endl;
// Check we're in contact
jfVector3_x86 closestPtMinusRelCentre;
closestPt.subtract(relCentre, &closestPtMinusRelCentre);
dist = closestPtMinusRelCentre.squareMagnitude();
if (dist > (sphere.getRadius() * sphere.getRadius()))
{
return 0;
}
//cout <<"Sphere radius : "<<sphere.getRadius()<<endl;
//cout <<"dist : "<<dist<<endl;
// Compile the contact
jfVector3_x86 closestPtWorld;
jfContact_x86 contact;
jfVector3_x86 contactNormal;
boxTransformMatrix.transform(closestPt, &closestPtWorld);
cout <<"closestPtWorld is :"<<closestPtWorld<<endl;
closestPtWorld.subtract(centre, &contactNormal);
contactNormal.normalize();
contact.setContactNormal(contactNormal);
contact.setContactPoint(closestPtWorld);
contact.setPenetration(sphere.getRadius() - jfRealSqrt(dist));
contact.setBodyData(box.getBody(), sphere.getBody(), data->getFriction(), data->getRestitution());
data->addContact(contact);
return 1;
}
