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