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