Missing/disabled sphere-box collision algorithm.

Post Reply
kermado
Posts: 20
Joined: Tue Jan 12, 2016 11:20 am

Missing/disabled sphere-box collision algorithm.

Post by kermado »

I have been having some issues where spheres would fall through large boxes (one side > 1000 units). After a little investigation I found that the sphere-box algorithm is disabled (buggy?) and so I think that the GJK/EPA implementation is currently being used. So, in case anyone is interested, here is Arvo's algorithm extended to support cases where the sphere's centroid may be enclosed by the box. This seems to be faster and more robust for my use cases.

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;
		}
	}
Now the Sphere-OBB (or rather, just Sphere-Box) version:

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.
	}
Finally, the processCollision function:

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();
			}
		}
	}
Post Reply