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