[ODE] RE: Terrain<->Box collision without tri-collider

[McEvoy, Nick]

Stephan Heigl wrote:
>Hi,
>due to data duplication i'd like to do the collision tests against the terrain 
>without the tricollider and OPCODE. So i created a custom dTerrainGeom. I 
>found some info on how to do terrain<->sphere intersection but nothing on box 
>intersection.
>Any idea how to handle that?
>Thank you very much in advance.

I've managed to get tri-box collision working without using tricollider or OPCODE.

My solution was to use Tomas Akenine-Möller Fast 3D Triangle-Box Overlap Testing - tribox.cpp at http://www.acm.org/jgt/papers/AkenineMoller01/tribox.html.

Read how it works: http://www.ce.chalmers.se/staff/tomasm/pubs/tribox.pdf

Steps involved:
1. do a simple bounding sphere test to determine potentially 'hit' triangles near your object
2. check each potentially 'hit' triangle using Tomas Akenine-Möller tribox test
3. if a tribox test returns true then use ODE box-plane collide method (dCollideBP) to generate the contact geom
4. generate contacts as usual (ie. using ODE's method dJointCreateContact)

Sorry, my code below is a little bit application specific (I use PLIB as my scene graph) ... but it shows my words above as real code.  My code below starts at step 2 ... PLIB has already determined potentially 'hit' triangles (pHitList) for me (ie. step 1).

bool
reTriangleBoxContact(int iNumHits,
					 ssgHit* pHitList,
					 dBodyID BoxBody,
					 dGeomID BoxGeom)
{
	reList<dContact> ContactList;

	// Check for hits
	if (iNumHits > 0 && pHitList)
	{
		//printf("** hits <%d>\n", iNumHits);

		for (int i = 0; i < iNumHits; i++)
		{
			ssgHit* h = &pHitList[i];

			sgVec3 vTriangle[3];
			sgVec3 vTriangleNormal;

			// Get 'hit' triangle in leaf
			short v1, v2, v3;
			h->leaf->getTriangle(h->triangle, &v1, &v2, &v3);
			sgCopyVec3(vTriangle[0], h->leaf->getVertex(v1));
			sgCopyVec3(vTriangle[1], h->leaf->getVertex(v2));
			sgCopyVec3(vTriangle[2], h->leaf->getVertex(v3));
			sgSetVec3(vTriangleNormal, h->plane[0], h->plane[1], h->plane[2]);

			// Apply current matrix to 'hit' triangle
			sgXformPnt3(vTriangle[0], vTriangle[0], h->matrix);
			sgXformPnt3(vTriangle[1], vTriangle[1], h->matrix);
			sgXformPnt3(vTriangle[2], vTriangle[2], h->matrix);

			// Get axis aligned bounding box (AABB) info
			dVector3 BoxSides;
			sgVec3 BoxExtents;
			sgVec3 BoxCenter = {0,0,0};
			dGeomBoxGetLengths(BoxGeom, BoxSides);
			sgSetVec3(BoxExtents, BoxSides[0]*0.5f, BoxSides[1]*0.5f, BoxSides[2]*0.5f);

			// To test against oriented bounding box (OBB) need to
			// translate triangle verticies by inverse box transform
			sgMat4 Xform;
			const dReal* pPos = dBodyGetPosition(BoxBody);
			const dReal* pRot = dBodyGetRotation(BoxBody);
			sgSetVec4(Xform[0], pRot[0], pRot[4], pRot[8],  0);
			sgSetVec4(Xform[1], pRot[1], pRot[5], pRot[9],  0);
			sgSetVec4(Xform[2], pRot[2], pRot[6], pRot[10], 0);
			sgSetVec4(Xform[3], pPos[0], pPos[1], pPos[2],  1);
			sgTransposeNegateMat4(Xform);
			sgXformPnt3(vTriangle[0], Xform);
			sgXformPnt3(vTriangle[1], Xform);
			sgXformPnt3(vTriangle[2], Xform);

			// Triangle-box collision test
			if (triBoxOverlap(BoxCenter, BoxExtents, vTriangle))
			{
				// Use ODE box-plane collide to generate contact geom
				dContactGeom BoxContacts[3];
				dGeomID TriPlaneGeom = dCreatePlane(reGetODESpaceId(),
					h->plane[0], h->plane[1], h->plane[2], -h->plane[3]);
				int n = dCollideBP(BoxGeom, TriPlaneGeom, 3, BoxContacts, sizeof(dContactGeom));
				for (int j = 0; j < n; j++)
				{
					// Get contact surface properties depending on material hit
					ssgState* pState = h->leaf->getState();
					if (pState->isA(ssgTypeSimpleState()))
					{
						dSurfaceParameters Surface;
						if (reGetMaterialManager()->GetSurfaceParams(
							pState->getExternalPropertyIndex(),
							Surface))
						{
							dContact* pContact = new dContact;
							pContact->surface = Surface;
							pContact->geom = BoxContacts[j];
							pContact->geom.g1 = BoxGeom;
							pContact->geom.g2 = NULL;
							ContactList.AddTail(pContact);
						}
					}
				}
				dGeomDestroy(TriPlaneGeom);
			}
		}
	}

	// If no hits then do a height over terrain check
	// to stop objects falling thru terrain
	if (ContactList.GetCount() == 0)
	{
		// Quick hack: just approximate box as a sphere !? :)
		sgVec3 vCenter;
		dVector3 BoxSides;
		reGetBodyPosition(vCenter, BoxBody);
		dGeomBoxGetLengths(BoxGeom, BoxSides);

		float fRadius = BoxSides[0];
		if (fRadius < BoxSides[1])
			fRadius = BoxSides[1];
		if (fRadius < BoxSides[2])
			fRadius = BoxSides[2];
		fRadius *= 0.5f;

		ssgHit* h;
		sgVec3 vTriangleNormal;
		float fHOT = reGetHeightAndNormal(reGetWorld(), vCenter, vTriangleNormal, &h);
		float fDepth = fHOT-vCenter[SG_Z];

		if (fDepth > fRadius)
		{
			//printf("** depth <%.2f>\n", fDepth);

			// todo: improve reGetHeightAndNormal() to return position
			// same as reTriangleSphereIntersectionPoint() above.
			sgVec3 vOffset;
			sgVec3 vPosition;
			sgScaleVec3(vOffset, vTriangleNormal, -fDepth);
			sgSubVec3(vPosition, vCenter, vOffset);

			ssgState* pState = h->leaf->getState();
			if (pState->isA(ssgTypeSimpleState()))
			{
				dSurfaceParameters Surface;
				if (reGetMaterialManager()->GetSurfaceParams(
					pState->getExternalPropertyIndex(),
					Surface))
				{
					dContact* pContact = new dContact;
					pContact->surface = Surface;
					pContact->geom.pos[0] = vPosition[0];
					pContact->geom.pos[1] = vPosition[1];
					pContact->geom.pos[2] = vPosition[2];
					pContact->geom.normal[0] = vTriangleNormal[0];
					pContact->geom.normal[1] = vTriangleNormal[1];
					pContact->geom.normal[2] = vTriangleNormal[2];
					pContact->geom.depth = fDepth;
					pContact->geom.g1 = BoxGeom;
					pContact->geom.g2 = NULL;
					ContactList.AddTail(pContact);
				}
			}
		}
	}

	return reCreateContacts(ContactList, BoxBody);
}

bool
reCreateContacts(reList<dContact>& ContactList, dBodyID Body)
{
	bool bContact = false;

	reList<dContact> OptimisedList;

	// Go thru contact list and filter out duplicate contacts
	LISTPOSITION pos = ContactList.GetHeadPosition();
	while (pos)
	{
		dContact* pContact = ContactList.GetNext(pos);

		// Check if contact is already in optimised list
		bool bContactExists = false;
		LISTPOSITION pos_opt = OptimisedList.GetHeadPosition();
		while (pos_opt)
		{
			dContact* pContactOpt = OptimisedList.GetNext(pos_opt);
			if (sgCompareVec3(pContact->geom.pos, pContactOpt->geom.pos, 0.001f) &&
				sgCompareVec3(pContact->geom.normal, pContactOpt->geom.normal, 0.001f))
			{
				bContactExists = true;
				break;
			}
		}

		// If not then add it to optimised list
		if (!bContactExists)
		{
			dContact* pContactOpt = new dContact;
			*pContactOpt = *pContact;
			OptimisedList.AddTail(pContactOpt);
		}
	}

	// Go thru optimised list and genarate contacts
	pos = OptimisedList.GetHeadPosition();
	while (pos)
	{
		dContact* pContact = OptimisedList.GetNext(pos);

		/*printf("pos <%.1f:%.1f:%.1f> nrm <%.1f:%.1f:%.1f>\n",
			pContact->geom.pos[0],
			pContact->geom.pos[1],
			pContact->geom.pos[2],
			pContact->geom.normal[0],
			pContact->geom.normal[1],
			pContact->geom.normal[2]);*/

		bContact = true;

		dJointID c = dJointCreateContact(
			reGetODEWorldId(),
			reGetODEJointGroupId(),
			pContact);
		dJointAttach(c, Body, NULL);
	}

	return bContact;
}

Nick