bool jfCollisionDetector_x86::tryAxis( const jfCollisionBox& one, const jfCollisionBox& two, jfVector3& axis, const jfVector3& toCentre, unsigned index, // These values may be updated jfReal& smallestPenetration, unsigned& smallestCase ) const { // Make sure we have a normalized axis, and don't check almost parallel axes if (axis.squareMagnitude() < 0.0001) { return true; } axis.normalize(); jfReal penetration = penetrationOnAxis(one, two, axis, toCentre); if (penetration < 0) { return false; } if (penetration < smallestPenetration) { smallestPenetration = penetration; smallestCase = index; } return true; }
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); } }