First for Sphere-AABB.
Code: Select all
// Simple extension of Avro's method that works for cases when the sphere's centroid is enclosed
// by the AABB. This is important for sensors. In such cases, the shortest vector from the
// sphere's centroid to the surface of the box is always exclusively in the x, y or z direction.
// This follows trivially from Pythagorous.
void CollideSphereAABB(const btVector3& center, btScalar radius, const btVector3& min, const btVector3& max, btVector3& normal, btScalar& distance)
{
normal.setZero();
int numDimInsidePlanes = 0;
btScalar t[3]; // No need to pre-initialize (only used if all values set).
const btVector3 s1 = max - center;
const btVector3 s2 = center - min;
// Test yz-planes.
if (s1.x() < btScalar(0.))
{
// Center is right of max plane.
normal.setX(s1.x());
}
else if (s2.x() < btScalar(0.))
{
// Center is left of min plane.
normal.setX(-s2.x());
}
else
{
// Center is between planes.
++numDimInsidePlanes;
t[0] = (s1.x() <= s2.x()) ? s1.x() : -s2.x();
}
// Test xz-planes.
if (s1.y() < btScalar(0.))
{
// Center is above max plane.
normal.setY(s1.y());
}
else if (s2.y() < btScalar(0.))
{
// Center is below min plane.
normal.setY(-s2.y());
}
else
{
// Center is between planes.
++numDimInsidePlanes;
t[1] = (s1.y() <= s2.y()) ? s1.y() : -s2.y();
}
// Test xy-planes.
if (s1.z() < btScalar(0.))
{
// Center is forward of max plane.
normal.setZ(s1.z());
}
else if (s2.z() < btScalar(0.))
{
// Center is behind of min plane.
normal.setZ(-s2.z());
}
else
{
// Center is between planes.
++numDimInsidePlanes;
t[2] = (s1.z() <= s2.z()) ? s1.z() : -s2.z();
}
// Center is between all three planes?
if (numDimInsidePlanes >= 3)
{
// Choose the direction of the shortest distance.
int minIndex = 0;
if (btFabs(t[1]) < btFabs(t[minIndex]))
{
minIndex = 1;
}
if (btFabs(t[2]) < btFabs(t[minIndex]))
{
minIndex = 2;
}
const btScalar minValue = t[minIndex];
normal.m_floats[minIndex] = (minValue >= btScalar(0.)) ? btScalar(-1.) : btScalar(1.);
distance = -btFabs(minValue);
}
else
{
const btScalar length = normal.length();
normal *= btScalar(1.) / length;
distance = length;
}
}
Code: Select all
// Transform's the sphere's centroid into the box's local space. From there, we can use the
// AABB algorithm above.
void CollideSphereOBB(const btVector3& center, btScalar radius, const btVector3& halfExtents, const btTransform& worldTransform, btVector3& normal, btScalar& distance)
{
const btVector3 localCenter = worldTransform.invXform(center); // Transform the sphere's centre into the OBB's local space.
const btVector3 localMin = -halfExtents;
const btVector3 localMax = halfExtents;
CollideSphereAABB(localCenter, radius, localMin, localMax, normal, distance);
normal = worldTransform.getBasis() * normal; // Transform the normal back into world-space.
}
Code: Select all
void processCollision(const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut)
{
if (!m_manifoldPtr)
{
return;
}
const btCollisionObjectWrapper* sphereObjWrap = (m_isSwapped) ? body1Wrap : body0Wrap;
const btVector3 sphereCenter = sphereObjWrap->getWorldTransform().getOrigin();
const btSphereShape* sphere0 = static_cast<const btSphereShape*>(sphereObjWrap->getCollisionShape());
const btScalar radius = sphere0->getRadius();
const btCollisionObjectWrapper* boxObjWrap = (m_isSwapped) ? body0Wrap : body1Wrap;
const btTransform& boxWorldTransform = boxObjWrap->getWorldTransform();
const btBoxShape* boxShape = static_cast<const btBoxShape*>(boxObjWrap->getCollisionShape());
const btVector3& boxHalfExtents = boxShape->getHalfExtentsWithoutMargin();
const btScalar boxMargin = boxShape->getMargin();
const btScalar maxContactDistance = m_manifoldPtr->getContactBreakingThreshold();
resultOut->setPersistentManifold(m_manifoldPtr);
btVector3 normalOnSphere; // Contact normal vector on the sphere.
btScalar centerToBoxDistance; // Distance between the sphere's center and the box.
CollideSphereOBB(sphereCenter, radius, boxHalfExtents, boxWorldTransform, normalOnSphere, centerToBoxDistance);
const btScalar surfaceToBoxDistance = centerToBoxDistance - radius; // Distance between the sphere's surface and the box (will be negative if penetration).
const btScalar breakingDistance = boxMargin + maxContactDistance; // Distance beyond which there is no contact point.
if (surfaceToBoxDistance <= breakingDistance)
{
btVector3 normalOnBox = -normalOnSphere;
btVector3 closestPointOnBoxMargin = sphereCenter + (normalOnSphere * (centerToBoxDistance - boxMargin));
resultOut->addContactPoint(normalOnBox, closestPointOnBoxMargin, surfaceToBoxDistance);
}
if (m_ownManifold)
{
if (m_manifoldPtr->getNumContacts())
{
resultOut->refreshContactPoints();
}
}
}