/*
============
FreePathTree_r
============
*/
void FreePathTree_r( pathNode_t *node ) {
if ( node->children[0] ) {
FreePathTree_r( node->children[0] );
}
if ( node->children[1] ) {
FreePathTree_r( node->children[1] );
}
pathNodeAllocator.Free( node );
}
/*
============
PrunePathTree
============
*/
void PrunePathTree( pathNode_t *root, const idVec2 &seekPos )
{
int i;
float bestDist;
pathNode_t *node, *lastNode, *n, *bestNode;
node = root;
while( node )
{
node->dist = ( seekPos - node->pos ).LengthSqr();
if( node->children[0] )
{
node = node->children[0];
}
else if( node->children[1] )
{
node = node->children[1];
}
else
{
// find the node closest to the goal along this path
bestDist = idMath::INFINITY;
bestNode = node;
for( n = node; n; n = n->parent )
{
if( n->children[0] && n->children[1] )
{
break;
}
if( n->dist < bestDist )
{
bestDist = n->dist;
bestNode = n;
}
}
// free tree down from the best node
for( i = 0; i < 2; i++ )
{
if( bestNode->children[i] )
{
FreePathTree_r( bestNode->children[i] );
bestNode->children[i] = NULL;
}
}
for( lastNode = bestNode, node = bestNode->parent; node; lastNode = node, node = node->parent )
{
if( node->children[1] && ( node->children[1] != lastNode ) )
{
node = node->children[1];
break;
}
}
}
}
}
/*
============
idAI::FindPathAroundObstacles
Finds a path around dynamic obstacles using a path tree with clockwise and counter clockwise edge walks.
============
*/
bool idAI::FindPathAroundObstacles( const idPhysics *physics, const idAAS *aas, const idEntity *ignore, const idVec3 &startPos, const idVec3 &seekPos, obstaclePath_t &path ) {
int numObstacles, areaNum, insideObstacle;
obstacle_t obstacles[MAX_OBSTACLES];
idBounds clipBounds;
idBounds bounds;
pathNode_t *root;
bool pathToGoalExists;
path.seekPos = seekPos;
path.firstObstacle = NULL;
path.startPosOutsideObstacles = startPos;
path.startPosObstacle = NULL;
path.seekPosOutsideObstacles = seekPos;
path.seekPosObstacle = NULL;
if( !aas ) {
return true;
}
bounds[1] = aas->GetSettings()->boundingBoxes[0][1];
bounds[0] = -bounds[1];
bounds[1].z = 32.0f;
// get the AAS area number and a valid point inside that area
areaNum = aas->PointReachableAreaNum( path.startPosOutsideObstacles, bounds, ( AREA_REACHABLE_WALK | AREA_REACHABLE_FLY ) );
aas->PushPointIntoAreaNum( areaNum, path.startPosOutsideObstacles );
// get all the nearby obstacles
numObstacles = GetObstacles( physics, aas, ignore, areaNum, path.startPosOutsideObstacles, path.seekPosOutsideObstacles, obstacles, MAX_OBSTACLES, clipBounds );
// get a source position outside the obstacles
GetPointOutsideObstacles( obstacles, numObstacles, path.startPosOutsideObstacles.ToVec2(), &insideObstacle, NULL );
if( insideObstacle != -1 ) {
path.startPosObstacle = obstacles[insideObstacle].entity;
}
// get a goal position outside the obstacles
GetPointOutsideObstacles( obstacles, numObstacles, path.seekPosOutsideObstacles.ToVec2(), &insideObstacle, NULL );
if( insideObstacle != -1 ) {
path.seekPosObstacle = obstacles[insideObstacle].entity;
}
// if start and destination are pushed to the same point, we don't have a path around the obstacle
if( ( path.seekPosOutsideObstacles.ToVec2() - path.startPosOutsideObstacles.ToVec2() ).LengthSqr() < Square( 1.0f ) ) {
if( ( seekPos.ToVec2() - startPos.ToVec2() ).LengthSqr() > Square( 2.0f ) ) {
return false;
}
}
// build a path tree
root = BuildPathTree( obstacles, numObstacles, clipBounds, path.startPosOutsideObstacles.ToVec2(), path.seekPosOutsideObstacles.ToVec2(), path );
// draw the path tree
if( ai_showObstacleAvoidance.GetBool() ) {
DrawPathTree( root, physics->GetOrigin().z );
}
// prune the tree
PrunePathTree( root, path.seekPosOutsideObstacles.ToVec2() );
// find the optimal path
pathToGoalExists = FindOptimalPath( root, obstacles, numObstacles, physics->GetOrigin().z, physics->GetLinearVelocity(), path.seekPos );
// free the tree
FreePathTree_r( root );
return pathToGoalExists;
}
