jfReal jfCollisionDetector_x86::penetrationOnAxis(
const jfCollisionBox& one,
const jfCollisionBox& two,
jfVector3& axis,
const jfVector3& toCentre
) const
{
jfIntersectionTester_x86 intersectionTester;
// Project the half-size of one onto axis
jfReal oneProject = intersectionTester.transformToAxis(one, axis);
jfReal twoProject = intersectionTester.transformToAxis(two, axis);
// Project this onto the axis
jfReal distance = jfRealAbs(toCentre.dotProduct(axis));
// Return the overlap (i.e. positive indicates
// overlap, negative indicates separation).
return (oneProject + twoProject - distance);
}
void jfSpringForceGenerator_x86::updateForce(jfRigidBody* body, jfReal duration)
{
//Millington p.210
//2 ends
jfVector3_x86 end0;
jfVector3_x86 end1;
jfVector3_x86 force;
body->getPointInWorldSpace(*m_ConnectionPoint, &end0);
m_Other->getPointInWorldSpace(*m_OtherConnectionPoint, &end1);
end0.subtract(end1, &force);
jfReal magnitude = force.magnitude();
magnitude = jfRealAbs(magnitude - m_RestLength);
magnitude *= m_SpringConstant;
force.normalize();
force *= (- magnitude);
body->addForceAtPoint(force, end0);
}
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;
}
unsigned jfCollisionDetector_x86::boxAndPoint(const jfCollisionBox& box,
const jfVector3& point,
jfCollisionData* data
) const
{
// Transform the point into box coordinates
jfMatrix4_x86 boxTransform;
jfVector3_x86 relPt;
jfVector3_x86 normal;
jfVector3_x86 boxHalfSize;
jfVector3_x86 boxAxis;
box.getTransformMatrix(&boxTransform);
boxTransform.transformInverse(point, &relPt);
box.getHalfSize(&boxHalfSize);
// Check each axis, looking for the axis on which the
// penetration is least deep.
jfReal min_depth = boxHalfSize.getX() - jfRealAbs(relPt.getX());
if (min_depth < 0)
{
return 0;
}
box.getAxisVector(0, &boxAxis);
int multiplier = 1;
if(relPt.getX() < 0)
{
multiplier = -1;
}
boxAxis.multiply(multiplier, &normal);
jfReal depth = boxHalfSize.getY() - jfRealAbs(relPt.getY());
if (depth < 0)
{
return 0;
}
else if (depth < min_depth)
{
min_depth = depth;
box.getAxisVector(1, &boxAxis);
multiplier = 1;
//@opt put multiply in if statement
if(relPt.getY() < 0)
{
multiplier = -1;
}
boxAxis.multiply(multiplier, &normal);
}
depth = boxHalfSize.getZ() - jfRealAbs(relPt.getZ());
if (depth < 0)
{
return 0;
}
else if (depth < min_depth)
{
min_depth = depth;
box.getAxisVector(2, &boxAxis);
multiplier = 1;
//@opt put multiply in if statement
if(relPt.getZ() < 0)
{
multiplier = -1;
}
boxAxis.multiply(multiplier, &normal);
}
// Compile the contact
jfContact_x86 contact;
contact.setContactNormal(normal);
contact.setContactPoint(point);
contact.setPenetration(min_depth);
// Note that we don't know what rigid body the point
// belongs to, so we just use NULL. Where this is called
// this value can be left, or filled in.
contact.setBodyData(box.getBody(), NULL,
data->getFriction(), data->getRestitution());
data->addContact(contact);
return 1;
}
void jfCollisionDetector_x86::contactPoint(
const jfVector3& pOne,
const jfVector3& dOne,
jfReal oneSize,
const jfVector3& pTwo,
const jfVector3& dTwo,
jfReal twoSize,
// If this is true, and the contact point is outside
// the edge (in the case of an edge-face contact) then
// we use one's midpoint, otherwise we use two's.
bool useOne,
jfVector3* result) const
{
//TODO:Opportunity for CUDA?
jfVector3_x86 toSt, cOne, cTwo;
jfReal dpStaOne, dpStaTwo, dpOneTwo, smOne, smTwo;
jfReal denom, mua, mub;
smOne = dOne.squareMagnitude();
smTwo = dTwo.squareMagnitude();
dpOneTwo = dTwo.dotProduct(dOne);
pOne.subtract(pTwo, &toSt);
dpStaOne = dOne.dotProduct(toSt);
dpStaTwo = dTwo.dotProduct(toSt);
denom = (smOne * smTwo) - (dpOneTwo * dpOneTwo);
// Zero denominator indicates parrallel lines
if (jfRealAbs(denom) < 0.0001f) {
if(useOne)
{
(*result) = pOne;
return;
}
else
{
(*result) = pTwo;
return;
}
}
mua = ((dpOneTwo * dpStaTwo) - (smTwo * dpStaOne)) / denom;
mub = ((smOne * dpStaTwo) - (dpOneTwo * dpStaOne)) / denom;
// If either of the edges has the nearest point out
// of bounds, then the edges aren't crossed, we have
// an edge-face contact. Our point is on the edge, which
// we know from the useOne parameter.
if ((mua > oneSize) ||
(mua < -oneSize) ||
(mub > twoSize) ||
(mub < -twoSize))
{
if(useOne)
{
(*result) = pOne;
return;
}
else
{
(*result) = pTwo;
return;
}
}
else
{
//cOne = pOne + dOne * mua;
dOne.multiply(mua, &cOne);
cOne += pOne;
//cTwo = pTwo + dTwo * mub;
dTwo.multiply(mub, &cTwo);
cTwo += pTwo;
cOne *= 0.5;
cTwo *= 0.5;
cOne.add(cTwo, result);
}
}
