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