static inline real transformToAxis( const CollisionBox &box, const Vector3 &axis ) { return box.halfSize.x * real_abs(axis * box.getAxis(0)) + box.halfSize.y * real_abs(axis * box.getAxis(1)) + box.halfSize.z * real_abs(axis * box.getAxis(2)); }
unsigned CollisionDetector::boxAndSphere( const CollisionBox &box, const CollisionSphere &sphere, CollisionData *data ) { // Transform the centre of the sphere into box coordinates Vector3 centre = sphere.getAxis(3); Vector3 relCentre = box.transform.transformInverse(centre); // Early out check to see if we can exclude the contact if (real_abs(relCentre.x) - sphere.radius > box.halfSize.x || real_abs(relCentre.y) - sphere.radius > box.halfSize.y || real_abs(relCentre.z) - sphere.radius > box.halfSize.z) { return 0; } Vector3 closestPt(0,0,0); real dist; // Clamp each coordinate to the box. dist = relCentre.x; if (dist > box.halfSize.x) dist = box.halfSize.x; if (dist < -box.halfSize.x) dist = -box.halfSize.x; closestPt.x = dist; dist = relCentre.y; if (dist > box.halfSize.y) dist = box.halfSize.y; if (dist < -box.halfSize.y) dist = -box.halfSize.y; closestPt.y = dist; dist = relCentre.z; if (dist > box.halfSize.z) dist = box.halfSize.z; if (dist < -box.halfSize.z) dist = -box.halfSize.z; closestPt.z = dist; // Check we're in contact dist = (closestPt - relCentre).squareMagnitude(); if (dist > sphere.radius * sphere.radius) return 0; // Compile the contact Vector3 closestPtWorld = box.transform.transform(closestPt); Contact* contact = data->contacts; contact->contactNormal = (closestPtWorld - centre); contact->contactNormal.normalise(); contact->contactPoint = closestPtWorld; contact->penetration = sphere.radius - real_sqrt(dist); contact->setBodyData(box.body, sphere.body, data->friction, data->restitution); data->addContacts(1); return 1; }
bool Collider::Check(const BoundingBox &A, const BoundingBox &B) { const Vector3 T = B.position - A.position; return( real_abs(T.x) <= (A.extension.x + B.extension.x) && real_abs(T.y) <= (A.extension.y + B.extension.y) && real_abs(T.y) <= (A.extension.z + B.extension.z) ); }
unsigned CollisionDetector::boxAndPoint( const CollisionBox &box, const Vector3 &point, CollisionData *data ) { // Transform the point into box coordinates Vector3 relPt = box.transform.transformInverse(point); Vector3 normal; // Check each axis, looking for the axis on which the // penetration is least deep. real min_depth = box.halfSize.x - real_abs(relPt.x); if (min_depth < 0) return 0; normal = box.getAxis(0) * ((relPt.x < 0)?-1:1); real depth = box.halfSize.y - real_abs(relPt.y); if (depth < 0) return 0; else if (depth < min_depth) { min_depth = depth; normal = box.getAxis(1) * ((relPt.y < 0)?-1:1); } depth = box.halfSize.z - real_abs(relPt.z); if (depth < 0) return 0; else if (depth < min_depth) { min_depth = depth; normal = box.getAxis(2) * ((relPt.z < 0)?-1:1); } // Compile the contact Contact* contact = data->contacts; contact->contactNormal = normal; contact->contactPoint = point; contact->penetration = 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.body, NULL, data->friction, data->restitution); data->addContacts(1); return 1; }
static inline Vector3 contactPoint( const Vector3 &pOne, const Vector3 &dOne, real oneSize, const Vector3 &pTwo, const Vector3 &dTwo, real 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) { Vector3 toSt, cOne, cTwo; real dpStaOne, dpStaTwo, dpOneTwo, smOne, smTwo; real denom, mua, mub; smOne = dOne.squareMagnitude(); smTwo = dTwo.squareMagnitude(); dpOneTwo = dTwo * dOne; toSt = pOne - pTwo; dpStaOne = dOne * toSt; dpStaTwo = dTwo * toSt; denom = smOne * smTwo - dpOneTwo * dpOneTwo; // Zero denominator indicates parrallel lines if (real_abs(denom) < 0.0001f) { return useOne?pOne:pTwo; } 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) { return useOne?pOne:pTwo; } else { cOne = pOne + dOne * mua; cTwo = pTwo + dTwo * mub; return cOne * 0.5 + cTwo * 0.5; } }
/** * This function checks if the two boxes overlap * along the given axis. The final parameter toCentre * is used to pass in the vector between the boxes centre * points, to avoid having to recalculate it each time. */ static inline bool overlapOnAxis( const CollisionBox &one, const CollisionBox &two, const Vector3 &axis, const Vector3 &toCentre ) { // Project the half-size of one onto axis real oneProject = transformToAxis(one, axis); real twoProject = transformToAxis(two, axis); // Project this onto the axis real distance = real_abs(toCentre * axis); // Check for overlap return (distance < oneProject + twoProject); }
/* * This function checks if the two boxes overlap * along the given axis, returning the ammount of overlap. * The final parameter toCentre * is used to pass in the vector between the boxes centre * points, to avoid having to recalculate it each time. */ static inline real penetrationOnAxis( const CollisionBox &one, const CollisionBox &two, const Vector3 &axis, const Vector3 &toCentre ) { // Project the half-size of one onto axis real oneProject = transformToAxis(one, axis); real twoProject = transformToAxis(two, axis); // Project this onto the axis real distance = real_abs(toCentre * axis); // Return the overlap (i.e. positive indicates // overlap, negative indicates separation). return oneProject + twoProject - distance; }
bool Collider::Check(const OrientedBoundingBox &A, const OrientedBoundingBox &B) { real ra, rb; Matrix33 R, AbsR; for(int i= 0; i<3; ++i) { for(int j= 0; j<3; ++j) { R[i][j] = (A.u[i] * B.u[j]); } } Vector3 t = B.position - A.position; t = Vector3((t * A.u[0]),(t * A.u[1]),(t * A.u[2])); for(int i= 0; i<3; ++i) { for(int j= 0; j<3; ++j) { AbsR[i][j] = real_abs(R[i][j]) + REAL_EPSILON; } } for (int i = 0; i < 3; i++) { ra = A.extension[i]; rb = B.extension[0] * AbsR[i][0] + B.extension[1] * AbsR[i][1] + B.extension[2]* AbsR[i][2]; if(real_abs(t[i]) > ra + rb) { return false; } } for (int i = 0; i < 3; i++) { ra = A.extension[0] * AbsR[0][i] + A.extension[1] * AbsR[1][i] + A.extension[2] * AbsR[2][i] ; rb = B.extension[i]; if(real_abs(t[0]*R[0][i]+t[1]*R[1][i]+t[2]*R[2][i]) > ra + rb) { return false; } } ra = A.extension[1] * AbsR[2][0] + A.extension[2] * AbsR[1][0]; rb = B.extension[1] * AbsR[0][2] + B.extension[2] * AbsR[0][1]; if(real_abs(t[2] * R[1][0] - t[1] * R[2][0]) > ra + rb) { return false; } ra = A.extension[1] * AbsR[2][1] + A.extension[2] * AbsR[1][1]; rb = B.extension[0] * AbsR[0][2] + B.extension[2] * AbsR[0][0]; if(real_abs(t[2] * R[1][1] - t[1] * R[2][1]) > ra + rb) { return false; } ra = A.extension[1] * AbsR[2][2] + A.extension[2] * AbsR[1][2]; rb = B.extension[0] * AbsR[0][1] + B.extension[1] * AbsR[0][0]; if(real_abs(t[2] * R[1][2] - t[1] * R[2][2]) > ra + rb) { return false; } ra = A.extension[0] * AbsR[2][0] + A.extension[2] * AbsR[0][0]; rb = B.extension[1] * AbsR[1][2] + B.extension[2] * AbsR[1][1]; if (real_abs(t[0] * R[2][0] - t[2] * R[0][0]) > ra + rb) { return false; } ra = A.extension[0] * AbsR[2][1] + A.extension[2] * AbsR[0][1]; rb = B.extension[0] * AbsR[1][2] + B.extension[2] * AbsR[1][0]; if(real_abs(t[0] * R[2][1] - t[2] * R[0][1]) > ra + rb) { return false; } ra = A.extension[0] * AbsR[2][2] + A.extension[2] * AbsR[0][2]; rb = B.extension[0] * AbsR[1][1] + B.extension[1] * AbsR[1][0]; if(real_abs(t[0] * R[2][2] - t[2] * R[0][2]) > ra + rb) { return false; } ra = A.extension[0] * AbsR[1][0] + A.extension[1] * AbsR[0][0]; rb = B.extension[1] * AbsR[2][2] + B.extension[2] * AbsR[2][1]; if (real_abs(t[1] * R[0][0] - t[0] * R[1][0]) > ra + rb) { return false; } ra = A.extension[0] * AbsR[1][1] + A.extension[1] * AbsR[0][1]; rb = B.extension[0] * AbsR[2][2] + B.extension[2] * AbsR[2][0]; if(real_abs(t[1] * R[0][1] - t[0] * R[1][1]) > ra + rb) { return false; } ra = A.extension[0] * AbsR[1][2] + A.extension[1] * AbsR[0][2]; rb = B.extension[0] * AbsR[2][1] + B.extension[1] * AbsR[2][0]; if(real_abs(t[1] * R[0][2] - t[0] * R[1][2]) > ra + rb) { return false; } return true; }