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; }