AI Obstacle Avoidance / Path Finding

For the weekend, I have been completely dedicated to trying to figure out ways to implement simple Path Finding for avoiding obstacles for my AI (Nothing in depth for complex paths, no A* or Pre-Node Generation).

Of course, I know I could just use the common Precompiled Nav Node generation etc... but I wanted to try something different. Besides, my game is a top-down RPG, and the combat will not involve complex mazes for the AI :).

I read much about the subject. What interested me the most was articles/papers about mimicking insect behavior in robotics. I'm pretty sure I can make my AIPlayer as smart as a fly! (I hope).

I've been trying many different implementations, but so far they have been unsatisfactory.

I Tried:
1. Simple Collision Avoidance by steering away from an obstacle's center. It breaks when the bot is fixed on a final destination, he ends up just slaming against the center edge of the object repeatedly until stuck lol. So then I tried to suspend the destination, and rotate the bot and move forward then try to go back to the destination. Didn't get good results here either.

2. Calculate normal of surface collision to obstacle and aim the bot at an offset of the normal. I had a lot of faith in this one, but either it just didn't work right or I had some math errors :(.

3. My current implementation (and I'm quite proud of it :) ).
a. Take the obstacle's bounding box, and flatten it to make it a 2d rect (take out z)
b. Calculate the outward normals of each edge
c. Calculate 8 navigation nodes, 2 for each corner, which is (corner pos + normal + offset) where normal is a surface normal that touches this corner and offset is the distance away from the corner we should place the node.
d. Find the closest node, and make that the move destination
e. Determine which direction to go around the object (some complex math, hitting my head here right now)
f. Check if bot can see the player, if not go to the next node, or else exit collision avoidance code and follow player.


Once I figure out the best solution here, hopefully 3 will work, I will post a resource. There will definitely be math helper functions with this that may be beneficial just to have around, this is all 2D math. I'm not sure yet about optimizing. I don't know if doing this in 3D would be better to take advantage of graphics hardware.

More to come soon...

======================================================================
EDIT < 12.30.08 >

I just finished the code for this. It is compiling now... as long as that resolves without errors, I just need to test it. In the mean time, I'll post the pseudo-code.


IF LOS TO INTENDED DESTINATION IS BLOCKED BY OBJECT:
STEP 1: CALCULATE 2D RECT AND CALCULATE CORNER POINTS OF OBJECT

STEP 2: CALCULATE NORMALS FOR EACH EDGE

STEP 3: CALCULATE THE 8 PATH NODES, 2 FOR EACH CORNER

STEPS 4 & 5: CALULATE THE AVOIDANCE DIRECTION WITH THE CLOSEST NAV NODE

STEP 6: CANCEL CURRENT MOVE DESTINATION AND SET MOVE DESTINATION TO THE CLOSEST NAV NODE

STEP 7: ON ARRIVED TO NODE, IF LOS TO PLAYER, THEN BREAK OUT OF PATH FINDING. ELSE MOVE TO NEXT NODE AND REPEAT STEP 7.

STEP 8: RESUME MOVING TOWARD INTENDED DESTINATION

======================================================
Edit: 1/24/09

I'm posting the code I have so far, and taking a break from this plan for now. Maybe I'll run across a way to fix it in the future, or someone may chime in.

Here is the Math2d Namespace that I developed to aid this. The error is in the calculateBoxPoints method, in that the points are not placed correctly in world space relative to the object's scale. All CAPITOL letter variables belong to the Player class, which is cast as a sceneObject for these calculations.

Here is the Liang Barsky Algorithm for checking if a line is in a rect:

// see Foley/vanDam, pp. 122 for the Liang-Barsky line
// clipping algorithm
inline bool liangBarskyClipT( F32  nDenom, 
                              F32  nNumerator,
                              F32& io_rTE,
                              F32& io_rTL )
{
    F32 t;
    if( nDenom > 0 )
    {
        t = nNumerator / nDenom;
        if( t > io_rTL )
            return false;
        else if( t > io_rTE )
            io_rTE = t;
    } 
    else if( nDenom < 0 )
    {
        t = nNumerator / nDenom;
        if( t < io_rTE )
            return false;
        else
            io_rTL = t;
    }
    else if( nNumerator > 0 )
    {
        return false;
    }

    return true;
}


// see Foley/vanDam, pp. 122 for the Liang-Barsky line
// clipping algorithm
inline bool isLineIntersectRect(	Point2F& 		io_rStart, 
									Point2F& 		io_rEnd,
									const RectF&	rClipRect )
{
	const F32 nDX( io_rEnd.x - io_rStart.x );
	const F32 nDY( io_rEnd.y - io_rStart.y );

	if(nDX == 0 &&  nDY == 0)
	{
		 return rClipRect.contains( io_rStart );
	}
	else
	{
		 F32 nTE( 0.0 );
		 F32 nTL( 1.0 );
		 if( liangBarskyClipT(nDX, rClipRect.point.x - io_rStart.x,
									 nTE, nTL ) ) 					// inside wrt. left edge
		 {
			  if( liangBarskyClipT(-nDX, io_rStart.x - (rClipRect.point.x + rClipRect.extent.x),
										  nTE, nTL ) ) 				// inside wrt. right edge
			  {
					if( liangBarskyClipT(nDY, rClipRect.point.y - io_rStart.y,
												nTE, nTL ) ) 			// inside wrt. bottom edge
					{
						 if( liangBarskyClipT(-nDY, io_rStart.y - (rClipRect.point.y + rClipRect.extent.y),
													 nTE, nTL ) ) 		// inside wrt. top edge
						 {
							  // compute actual intersection points,
							  // if nTL has changed
							 /*
							  if( nTL < 1.0 )
							  {
									io_rEnd.x = io_rStart.x + nTL*nDX;
									io_rEnd.y = io_rStart.y + nTL*nDY;
							  }

							  // compute actual intersection points,
							  // if nTE has changed
							  if( nTE > 0.0 )
							  {
									io_rStart.x = io_rStart.x + nTE*nDX;
									io_rStart.y = io_rStart.y + nTE*nDY;
							  }

							  */

							  // line is (at least partially) visible
							  return true;
						 }
					}
			  }
		 }
	}

	return false;
}

Here is code for finding the inner facing and outer facing normals of an edge:

inline Point2F getLineNormalRight(const Point2F& p1, const Point2F& p2)
{
	Point2F rNorm( -(p2.y - p1.y), p2.x - p1.x ); // -VecY, VecX == RightNormal(x,y)
	rNorm.normalize();
	return rNorm;
}

inline Point2F getLineNormalLeft(const Point2F& p1, const Point2F& p2)
{
	Point2F lNorm( p2.y - p1.y, -(p2.x - p1.x) ); // VecY, -VecX == LeftNormal(x,y)
	lNorm.normalize();
	return lNorm;
}

inline Point2F getLineNormal(const Point2F& p1, const Point2F& p2, const Point2F& pos)
{
	Point2F rNormal( -(p2.y - p1.y), p2.x - p1.x );
	Point2F lNormal( -rNormal );

	F32 rDist = mSqrt( (rNormal.x - pos.x) * (rNormal.x - pos.x) + (rNormal.y - pos.y) * (rNormal.y - pos.y) );
	F32 lDist = mSqrt( (lNormal.x - pos.x) * (lNormal.x - pos.x) + (lNormal.y - pos.y) * (lNormal.y - pos.y) );

	if ( rDist < lDist )
	{
		rNormal.normalize();
		return rNormal;
	}
	
	lNormal.normalize();
	return lNormal;
}

Here is code for attempting to figure out the bounding box corners and calculate an offset of their normals for pathfinding. The scale error is here.

inline void calculateRectPoints(SceneObject* obj, const RectF& rect) //STEP 1
{
	
	obj->RECT_POINTS[0] = rect.point;
	obj->RECT_POINTS[1] = Point2F(rect.point.x + rect.extent.x	, rect.point.y);
	obj->RECT_POINTS[2] = Point2F(rect.point.x + rect.extent.x	, rect.point.y + rect.extent.y);
	obj->RECT_POINTS[3] = Point2F(rect.point.x						, rect.point.y + rect.extent.y);
}

inline void calculateBoxPoints(SceneObject* obj, const Box3F box, MatrixF worldTransform, Point3F worldScale)
{	
	//CALC DIST TO EDGES FROM CENTER, FIGURE OUT LOCAL CORNER POINTS
	Point3F p0(box.min.x, box.min.y, 0);
	Point3F p1(box.max.x, box.min.y, 0);
	Point3F p2(box.max.x, box.max.y, 0);
	Point3F p3(box.min.x, box.max.y, 0);

	//MULTIPLE BY THE TRANSFORM MATRIX TO GET WORLD SPACE POINTS
	
	MatrixF m0(EulerF(0,0,0), p0);
	MatrixF m1(EulerF(0,0,0), p1);
	MatrixF m2(EulerF(0,0,0), p2);
	MatrixF m3(EulerF(0,0,0), p3);

	m0.mul(worldTransform);
	m1.mul(worldTransform);
	m2.mul(worldTransform);
	m3.mul(worldTransform);

	m0.scale(worldScale);
	m1.scale(worldScale);
	m2.scale(worldScale);
	m3.scale(worldScale);
	
	m0.getColumn(3, &p0);
	m1.getColumn(3, &p1);
	m2.getColumn(3, &p2);
	m3.getColumn(3, &p3);	

	obj->RECT_POINTS[0] = Point2F(p0.x, p0.y);
	obj->RECT_POINTS[1] = Point2F(p1.x, p1.y);
	obj->RECT_POINTS[2] = Point2F(p2.x, p2.y);
	obj->RECT_POINTS[3] = Point2F(p3.x, p3.y);
}

inline void calculateRectCornerNormals(SceneObject* obj) //STEP 2
{
	obj->RECT_NORMALS[0] = getLineNormalLeft(obj->RECT_POINTS[0], obj->RECT_POINTS[1]);
	obj->RECT_NORMALS[1] = getLineNormalLeft(obj->RECT_POINTS[1], obj->RECT_POINTS[2]);
	obj->RECT_NORMALS[2] = getLineNormalLeft(obj->RECT_POINTS[2], obj->RECT_POINTS[3]);
	obj->RECT_NORMALS[3] = getLineNormalLeft(obj->RECT_POINTS[3], obj->RECT_POINTS[0]);
}

inline void calculateRectPathNodes(SceneObject* obj, const F32 offset) //STEP 3
{
	obj->RECT_PATHNODES[0] = obj->RECT_POINTS[0] + obj->RECT_NORMALS[0] * offset;		// _| (UP)
	
	obj->RECT_PATHNODES[1] = obj->RECT_POINTS[1] + obj->RECT_NORMALS[0] * offset;		// |_ (UP)
	obj->RECT_PATHNODES[2] = obj->RECT_POINTS[1] + obj->RECT_NORMALS[1] * offset;		// |_ (RIGHT)

	obj->RECT_PATHNODES[3] = obj->RECT_POINTS[2] + obj->RECT_NORMALS[1] * offset;		// |- (RIGHT)
	obj->RECT_PATHNODES[4] = obj->RECT_POINTS[2] + obj->RECT_NORMALS[2] * offset;		// |- (DOWN)

	obj->RECT_PATHNODES[5] = obj->RECT_POINTS[3] + obj->RECT_NORMALS[2] * offset;		// -| (DOWN)
	obj->RECT_PATHNODES[6] = obj->RECT_POINTS[3] + obj->RECT_NORMALS[3] * offset;		// -| (LEFT)

	obj->RECT_PATHNODES[7] = obj->RECT_POINTS[0] + obj->RECT_NORMALS[3] * offset;		// _| (LEFT)
}

This code attempts to calculate the closest path point to the player:

inline Point2F getRectClosestNavNode(SceneObject* obj, const Point2F& pos) //STEP 4, MOVE TO THIS NODE FIRST. THIS IS PARAMETER FOR STEP 5
{
	F32 dist[4];
	dist[0] = mSqrt( (obj->RECT_PATHNODES[0].x - pos.x) * (obj->RECT_PATHNODES[0].x - pos.x) + (obj->RECT_PATHNODES[0].y - pos.y) * (obj->RECT_PATHNODES[0].y - pos.y) );
	dist[1] = mSqrt( (obj->RECT_PATHNODES[1].x - pos.x) * (obj->RECT_PATHNODES[1].x - pos.x) + (obj->RECT_PATHNODES[1].y - pos.y) * (obj->RECT_PATHNODES[1].y - pos.y) );
	dist[2] = mSqrt( (obj->RECT_PATHNODES[2].x - pos.x) * (obj->RECT_PATHNODES[2].x - pos.x) + (obj->RECT_PATHNODES[2].y - pos.y) * (obj->RECT_PATHNODES[2].y - pos.y) );
	dist[3] = mSqrt( (obj->RECT_PATHNODES[3].x - pos.x) * (obj->RECT_PATHNODES[3].x - pos.x) + (obj->RECT_PATHNODES[3].y - pos.y) * (obj->RECT_PATHNODES[3].y - pos.y) );

	F32 distSorted[4];
	distSorted[0] = dist[0];
	distSorted[1] = dist[1];
	distSorted[2] = dist[2];
	distSorted[3] = dist[3];

	MathUtils::shellSort(distSorted, 4);

	for( U32 i = 0; i < 4; i++ )
	{
		if ( distSorted[0] == dist[i] )
		{
			return obj->RECT_PATHNODES[i];
		}
	}
	return Point2F(-100000, -100000); //IF THIS FAILS, WE WILL KNOW CAUSE THE AI WILL RUN OFF THE GRID
}

And this code attempts to figure out which direction the AI should go around the obstacle.

inline void calculateRectAvoidanceDirection(SceneObject* obj, const Point2F& closestNavNode) //STEP 5
{
	//FIND NEXT CLOSEST NODE FROM CLOSEST NODE
	Point2F nextClosest = getRectClosestNavNode(obj, closestNavNode);

	//FIND THE INDEX OF THE CLOSEST NODE
	U32 closestNavNodeIndex = -1;
	for (U32 i = 0; i < 8; i++)
	{
		if (closestNavNode == obj->RECT_PATHNODES[i])
		{
			closestNavNodeIndex = i;
			break;
		}
	}

	//RETURN THE DIRECTION OF THE CLOSEST NODE TO THE CLOSEST NODE
	if ( closestNavNodeIndex != 7 ) //DONT EXCEED ARRAY BOUNDS
	{
		if ( nextClosest == obj->RECT_PATHNODES[closestNavNodeIndex +1 ] )
		{
			obj->RECT_PATHDIRECTION_RIGHT = false; //DIRECTION IS PREVIOUS NODES
			obj->LAST_NAV_NODE_INDEX = closestNavNodeIndex + 1;
		}
		else
		{
			obj->RECT_PATHDIRECTION_RIGHT = true; //DIRECTION IS NEXT NODES
			if ( closestNavNodeIndex != 0 )
				obj->LAST_NAV_NODE_INDEX = closestNavNodeIndex - 1; //1 to the left
			else
				obj->LAST_NAV_NODE_INDEX = 7; //1 to the right
		}
	}
	else
	{
		if ( nextClosest == obj->RECT_PATHNODES[0] ) //WRAP IT BACK TO 0
		{
			obj->RECT_PATHDIRECTION_RIGHT = false; //DIRECTION IS PREVIOUS NODES
			obj->LAST_NAV_NODE_INDEX = 0; //1 to the right
		}
		else
		{
			obj->RECT_PATHDIRECTION_RIGHT = true; //DIRECTION IS NEXT NODES
			obj->LAST_NAV_NODE_INDEX = 6; //1 to the left
		}
	}

}

inline Point2F getRectNextNavNode(SceneObject* obj) //STEP 6
{
	if (obj->RECT_PATHDIRECTION_RIGHT)
	{
		if (obj->LAST_NAV_NODE_INDEX != 7 )
			return obj->RECT_PATHNODES[obj->LAST_NAV_NODE_INDEX++]; //ENSURE THE ADDITION HAPPENS BEFORE RETURN
		else
		{
			obj->LAST_NAV_NODE_INDEX = 0;
			return obj->RECT_PATHNODES[0];
		}
	}
	else
	{
		if (obj->LAST_NAV_NODE_INDEX != 0 )
			return obj->RECT_PATHNODES[obj->LAST_NAV_NODE_INDEX--];
		else
		{
			obj->LAST_NAV_NODE_INDEX = 7;
			return obj->RECT_PATHNODES[7];
		}
	}	
}