Path collision check

This bit of code will help solve a common problem involved in path finding: Determining if a path is available to a destination. Instead of a single ray check between the start and end points, a series of links is created between them along the ground to test the clearance.

The most common way to do this is to simply cast a ray from the start to end and test for collisions. This will work for some situations, but can be flawed. With this method, a series of links are created between the start and the end point and a ray is cast between each link. It will determine the ground point of each link automatically and will also test for ground slope.

Installation

This code can go in any file, for this resource I will use SceneObject.

Modify SceneObject.h to include the function definitons:

struct RayInfo: public Collision 
{
   // The collision struct has object, point, normal & material.
   F32 t;
};

[b]bool PathCollisionCheck(Point3F startPos, Point3F endPos, F32 linkDistance = 2.0f, F32 pathHeight = 1.5f);
F32 FindGroundPoint(Point3F point, VectorF &normal);
[/b]

Place this code at the end of SceneObject.cc:

/////////////////////////////////////////////////////////
/////  Path Collision Check
/////////////////////////////////////////////////////////

// Determine if a path from startPos to endPos can be created without colliding with an object
// linkDistance determines the distance between each ray check; smaller numbers will provide
// more accurate results at the cost of speed
bool PathCollisionCheck(Point3F startPos, Point3F endPos, F32 linkDistance, F32 pathHeight)
{
	// Early out if it the same point
	if( startPos == endPos )
		return true;

	// Distance between the two points
	F32 pTotalDistance = (startPos - endPos).len();

	// Calculate the number of links to test for in the path, always rounded up
	U32 pLinkCount = mCeil(pTotalDistance / linkDistance);

	// Find the vector of the path from the start to end points
    F32 xDiff = startPos.x - endPos.x;
    F32 yDiff = startPos.y - endPos.y;
    F32 newYaw = mAtan( xDiff, yDiff );
	Point3F pPathVector;
	F32 ang = 0.0;
	MathUtils::getVectorFromAngles(pPathVector, newYaw, ang);
	
	// Offset the link point by pathHeight above the ground
	Point3F pTestEnd = startPos;
	VectorF normal;
	F32 groundPoint = FindGroundPoint(pTestEnd, normal);
	pTestEnd.z = ( groundPoint + pathHeight );
	Point3F pTestStart = pTestEnd;
		

	// Loop through the links of the path looking for collisions
	for( U32 x=0; x < pLinkCount; x++ )
	{
		// The amount of distance we have already traveled in the path
		F32 currentDistTested = linkDistance * x;
		
		// How long is the current link going to be. If the link will be longer than the total length
		U32 pLinkDistance = ( (currentDistTested + linkDistance) > pTotalDistance ) ? (pTotalDistance - currentDistTested) : linkDistance;
		
		// Find the two link coordinates
		pTestStart = pTestEnd;
		pTestEnd = pTestStart - (pPathVector * pLinkDistance);

		// Offset the link point by pathHeight above the ground
		F32 groundPoint = FindGroundPoint(pTestEnd, normal);
		pTestEnd.z = ( groundPoint + pathHeight );

                // The max slope of the ground below the current link
                // 0.0 = Vertical, 1.0 = Horizontal
		if(normal.z < 0.7)
		{
			return false;
		}

		// Check the links height delta
		F32 pLinkHeightDelta = mFabs( pTestStart.z - pTestEnd.z );
		if( pLinkHeightDelta > linkDistance ) // > 45 degree slope
		{
			return false;
		}
		
		// Perform the collision check
		RayInfo pDummy;
		if (gServerContainer.castRay(pTestStart, pTestEnd, AI_COLLISION_MASK, &pDummy))
		{
			return false;
		}

	}

	F32 pEndHeightDelta = mFabs( endPos.z - pTestEnd.z );
	if( pEndHeightDelta > 6 ) // End path point needs to be withing 6 units of destination end point
	{
		return false;
	}
	return true;
}

F32 FindGroundPoint(Point3F point, VectorF &normal)
{
	Point3F pStartPoint = point; pStartPoint.z += 4;
	Point3F pEndPoint = point; pEndPoint.z -= 20;

	RayInfo pCol;
	gServerContainer.castRay(pStartPoint, pEndPoint, AI_COLLISION_MASK, &pCol);

	normal = pCol.normal;
	return pCol.point.z;
}


ConsoleFunction( PathCollisionCheck, bool, 3, 3, "")
{
	Point3F start;
	Point3F end;
	dSscanf(argv[1], "%f %f %f", &start.x, &start.y, &start.z);
	dSscanf(argv[2], "%f %f %f", &end.x, &end.y, &end.z);

	return PathCollisionCheck(start, end);
}

Usage

This resource was originally created for use in C++ but you can use the console function to access it from script:

PathCollisionCheck(%startPos, %endPos);

There are a few variables preset in the function declaration:

linkDistance: Default = 2.0. The distance between each link. Smaller links will produce more accurate results
pathHeight: Default = 1.5. The height of the links off of the ground, 1.5 should work good for most situations