void jfBoundingSphere_x86::recalculateBoundingVolume(const jfBoundingVolume& one, const jfBoundingVolume& two) { //@ref : Millington code "collide_coarse.cpp" jfVector3_x86 centreOffset; jfVector3_x86 oneCentre; jfVector3_x86 twoCentre; one.getCentre(&oneCentre); two.getCentre(&twoCentre); twoCentre.subtract(oneCentre, ¢reOffset); jfReal distance = centreOffset.squareMagnitude(); jfReal radiusDiff = two.getRadius() - one.getRadius(); // Check if the larger sphere encloses the small one if ((radiusDiff*radiusDiff) >= distance) { if (one.getRadius() > two.getRadius()) { (*m_Centre) = oneCentre; m_Radius = one.getRadius(); } else { (*m_Centre) = twoCentre; m_Radius = two.getRadius(); } } // Otherwise we need to work with partially // overlapping spheres else { distance = jfRealSqrt(distance); m_Radius = (distance + one.getRadius() + two.getRadius()) * ((jfReal)0.5); // The new centre is based on one's centre, moved towards // two's centre by an amount proportional to the spheres' // radii. (*m_Centre) = oneCentre; if (distance > 0) { jfVector3_x86 propCentre; centreOffset.multiply((m_Radius - one.getRadius())/distance, &propCentre); (*m_Centre) += propCentre; } } }
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; }
jfReal jfVector3_x86::magnitude() const { return jfRealSqrt((m_X*m_X) + (m_Y*m_Y) + (m_Z*m_Z)); }