astar.cpp

类AStarPathFinder实现方块状网格上的A*算法

//------------------------------------------------------------------------------
// PROJECT: The A* Pathfinder base Class
// FILE:   ASTAR.CPP
// OVERVIEW:
//    -Pretty good working Astar Pathfinder Algorithm Class.
//    -Based on SPATH.C Author: unknown (thanks)
//          	 http://www-cs-students.stanford.edu/~amitp/Articles/spath.zip
//    -Easy to implement, also with Your
//     favorite tiles based map but nevertheless a
//     little bit tricky to handle.
//    -Needs to be optimized or modified, perhaps a pointer to
//     a spritelist to reinitialisize the A* tilemap
//     and avoid sprite collosion.
//	   Let me hear about it.
//    Have fun!
// REVISION: 1.1
// Last Update: 31.08.97
// AUTHOR: [e-mail] Germany 25.07.97
//------------------------------------------------------------------------------

#include "astar.h"

////////////////////////////////////////////////////////////////////////////////
//                           Constructor Destructor                           //
////////////////////////////////////////////////////////////////////////////////

AstarPathfinder::AstarPathfinder(CDXMap* pMap, int forbiddenTiles)
{
   Stack = ( STACK* )calloc(1,sizeof( STACK ));
   isPath = FALSE;
   OPEN = NULL;
   CLOSED = NULL;
   PATH = NULL;
                      // Sets the A* Tilemap append on CDXMap
   InitAstarTileMap(pMap, forbiddenTiles);
}

////////////////////////////////////////////////////////////////////////////////

AstarPathfinder::~AstarPathfinder()
{
   FreeNodes();
   free(Stack);
   if ( TileMap )
		delete [] TileMap;
}

////////////////////////////////////////////////////////////////////////////////
//                             Public Member Functions                        //
////////////////////////////////////////////////////////////////////////////////

void AstarPathfinder::InitAstarTileMap(CDXMap* pMap, int forbiddenTiles)
{
   COLS = pMap->m_Width+2;    // note the cols should be 2 more than the map size
   ROWS = pMap->m_Height+2;  // because if we have a point at the extremes and we check
   TOTAL_TILES = ROWS*COLS; // to see if we can go in all directions, we don't need a
                           // special case to check these. (same for rows)
   TileMap = new int[TOTAL_TILES];
   int index = COLS+1; // A* map index
   int CDXMapIndex = 0;

   BoundaryTiles(); // sets 1's arround the map area

	for (int i = 0; i < ROWS-2; i++)       // fill up the area with current map data
   {                                      // 1 = obstacle
	   for (int j = 0; j < COLS-2; j++)    // 0 = free
		{
         if ( pMap->DATA[CDXMapIndex] < forbiddenTiles ) // the first "forbidden" tiles
				TileMap[index] = 1;                         // in the CDX-tiles-bitmap are not
         CDXMapIndex++;                                // reachable
         index++;
      }
      index += 2; // jump boundary
   }
}

////////////////////////////////////////////////////////////////////////////////

BOOL AstarPathfinder::NewPath(int sx,int sy, int dx,int dy)
{
   if ( FreeTile(dx,dy)&&FreeTile(sx,sy) && (TileNum(sx,sy)!=TileNum(dx,dy)) )
   {
      FreeNodes(); // clear node lists
   	FindPath(sx,sy,dx,dy);
   	return (isPath=TRUE);
   }
   else
  	   return (isPath=FALSE);
}

////////////////////////////////////////////////////////////////////////////////

BOOL AstarPathfinder::ReachedGoal(void) // check it's return value before getting
{                                      // the node entries
   if ( !isPath ) return TRUE;  // this looks a little bit strange
   if ( PATH->Parent != NULL )  // but prevents illegal calls of ::PathNextNode()
   	return FALSE;             // or ::PathGet*()
   else
   	return TRUE;
}

////////////////////////////////////////////////////////////////////////////////

int AstarPathfinder::TileNum(int x, int y)
{
	return((y>>SHIFT)*COLS + (x>>SHIFT)+COLS+1); // The reason I add COLS+1 to
   // tile number is because I want the tile number to start at the 2nd
   // the column and the 2nd row. Because if we do this we don't have to have
   // a special case to test if at a boundary. In the function BoundaryTiles
   // I place 1's around the boundary, in this way when I call the function
   // FreeTile() it returns false because we can't go there.
}

////////////////////////////////////////////////////////////////////////////////

int AstarPathfinder::FreeTile(int x, int y)
{         //  returns 1 if tile is free(nothing on it).
	      //  Note: if TileMap[equation]==0 it means free so just !(not) it.
   return(!TileMap[(y>>SHIFT)*COLS + (x>>SHIFT)+COLS+1]);
}

////////////////////////////////////////////////////////////////////////////////
//								      Private Member Functions                        //
////////////////////////////////////////////////////////////////////////////////

void AstarPathfinder::FreeNodes(void)
{
  	NODE *Node, *OldNode;

   if ( OPEN != NULL )
   {
   	Node = OPEN->NextNode;
   	while ( Node != NULL )
	   {
      	OldNode = Node;
      	Node = Node->NextNode;
         free(OldNode);
	   }
   }

   if ( CLOSED != NULL )
   {
   	Node = CLOSED->NextNode;
   	while ( Node != NULL )
      {
         OldNode = Node;
      	Node = Node->NextNode;
     	   free(OldNode);
	   }
   }
}

////////////////////////////////////////////////////////////////////////////////

void AstarPathfinder::BoundaryTiles(void) // This function places 1 around the boundary
{                             // so that we don't go there. Also we don't
   int c, index = 0;         // have to treat the boundaries as a special case

   for (c = 0; c < TOTAL_TILES; c++)      // zero out tiles.
       TileMap[c] = 0;

   for (c = 0; c < COLS; c++)         // place 1's at boundaries.
   	 TileMap[c] = 1;

   for (c = 1;c < ROWS-1; c++)
   {
      index += COLS;
      TileMap[index] = 1;
      TileMap[index+COLS-1] = 1;
   }
   index += COLS;

   for (c = index; c < TOTAL_TILES; c++)
       TileMap[c]=1;
}

////////////////////////////////////////////////////////////////////////////////
//                               A* Algorithm                                 //
////////////////////////////////////////////////////////////////////////////////

void AstarPathfinder::FindPath(int sx, int sy, int dx, int dy)
{
	NODE *Node, *BestNode;
   int TileNumDest;

   TileNumDest = TileNum(sx, sy);

   OPEN=( NODE* )calloc(1,sizeof( NODE ));
   CLOSED=( NODE* )calloc(1,sizeof( NODE ));

   Node=( NODE* )calloc(1,sizeof( NODE ));
   Node->g = 0;
   Node->h = (dx-sx)*(dx-sx) + (dy-sy)*(dy-sy);  // should really use sqrt().
   Node->f = Node->g+Node->h;
   Node->NodeNum = TileNum(dx, dy);
   Node->x = dx;
   Node->y = dy;

   OPEN->NextNode=Node;        // make Open List point to first node
   for (;;)
   {
		BestNode=ReturnBestNode();
		if (BestNode->NodeNum == TileNumDest)    // if we've found the end, break and finish
	   	break;
		GenerateSuccessors(BestNode,sx,sy);
   }
   PATH = BestNode;
}

////////////////////////////////////////////////////////////////////////////////

AstarPathfinder::NODE
*AstarPathfinder::ReturnBestNode(void)
{
   NODE *tmp;
   char message[128];

   if ( OPEN->NextNode == NULL )
   {
		sprintf(message,"No more nodes on OPEN: Perhaps tilenum destination not reachable!");
      MessageBox(0, message, "Error A* Pathfinder", MB_OK | MB_ICONERROR);
      PostQuitMessage(0);
   }

// Pick Node with lowest f, in this case it's the first node in list
// because we sort the OPEN list wrt lowest f. Call it BESTNODE.

   tmp = OPEN->NextNode;   // point to first node on OPEN
   OPEN->NextNode = tmp->NextNode;    // Make OPEN point to nextnode or NULL.

// Next take BESTNODE (or temp in this case) and put it on CLOSED

   tmp->NextNode = CLOSED->NextNode;
   CLOSED->NextNode = tmp;

   return(tmp);
}

////////////////////////////////////////////////////////////////////////////////

void AstarPathfinder::GenerateSuccessors(NODE *BestNode, int dx, int dy)
{
   int x, y;

		    // Upper-Left
   if ( FreeTile(x=BestNode->x-TILESIZE, y=BestNode->y-TILESIZE) )
      GenerateSucc(BestNode,x,y,dx,dy);
		    // Upper
   if ( FreeTile(x=BestNode->x, y=BestNode->y-TILESIZE) )
      GenerateSucc(BestNode,x,y,dx,dy);
		    // Upper-Right
   if ( FreeTile(x=BestNode->x+TILESIZE, y=BestNode->y-TILESIZE) )
      GenerateSucc(BestNode,x,y,dx,dy);
		    // Right
   if ( FreeTile(x=BestNode->x+TILESIZE, y=BestNode->y) )
      GenerateSucc(BestNode,x,y,dx,dy);
		    // Lower-Right
   if ( FreeTile(x=BestNode->x+TILESIZE, y=BestNode->y+TILESIZE) )
      GenerateSucc(BestNode,x,y,dx,dy);
		    // Lower
   if ( FreeTile(x=BestNode->x, y=BestNode->y+TILESIZE) )
      GenerateSucc(BestNode,x,y,dx,dy);
		    // Lower-Left
   if ( FreeTile(x=BestNode->x-TILESIZE, y=BestNode->y+TILESIZE) )
      GenerateSucc(BestNode,x,y,dx,dy);
		    // Left
   if ( FreeTile(x=BestNode->x-TILESIZE, y=BestNode->y) )
      GenerateSucc(BestNode,x,y,dx,dy);
}

////////////////////////////////////////////////////////////////////////////////

void AstarPathfinder::GenerateSucc(NODE *BestNode,int x, int y, int dx, int dy)
{
   int g, TileNumS, c = 0;
   NODE *Old, *Successor;

   g = BestNode->g+1;	    // g(Successor)=g(BestNode)+cost of getting from BestNode to Successor
   TileNumS = TileNum(x,y);  // identification purposes

   if ( (Old=CheckOPEN(TileNumS)) != NULL ) // if equal to NULL then not in OPEN list, else it returns the Node in Old
   {
		for( c = 0; c < 8; c++)
	   	if( BestNode->Child[c] == NULL ) // Add Old to the list of BestNode's Children (or Successors).
	   		break;
		BestNode->Child[c] = Old;

		if ( g < Old->g )  // if our new g value is < Old's then reset Old's parent to point to BestNode
		{
			Old->Parent = BestNode;
	   	Old->g = g;
	   	Old->f = g + Old->h;
		}
   }
   else if ( (Old=CheckCLOSED(TileNumS)) != NULL ) // if equal to NULL then not in OPEN list, else it returns the Node in Old
   {
   for( c = 0; c< 8; c++)
		if ( BestNode->Child[c] == NULL ) // Add Old to the list of BestNode's Children (or Successors).
	   		break;
		BestNode->Child[c] = Old;

		if ( g < Old->g )  // if our new g value is < Old's then reset Old's parent to point to BestNode
		{
	   	    Old->Parent = BestNode;
	   	    Old->g = g;
	   	    Old->f = g + Old->h;
	   	    PropagateDown(Old);  // Since we changed the g value of Old, we need
					   	        // to propagate this new value downwards, i.e.
					 	          // do a Depth-First traversal of the tree!
		}
   }
   else
   {
		Successor = ( NODE* )calloc(1,sizeof( NODE ));
		Successor->Parent = BestNode;
		Successor->g = g;
		Successor->h = (x-dx)*(x-dx) + (y-dy)*(y-dy);  // should do sqrt(), but since we don't really
		Successor->f = g+Successor->h;     // care about the distance but just which branch looks
		Successor->x = x;                 // better this should suffice. Anyayz it's faster.
		Successor->y = y;
		Successor->NodeNum = TileNumS;
		Insert(Successor);     // Insert Successor on OPEN list wrt f
		for( c =0; c < 8; c++)
			if ( BestNode->Child[c] == NULL ) // Add Old to the list of BestNode's Children (or Successors).
	  		    break;
		BestNode->Child[c] = Successor;
   }
}

////////////////////////////////////////////////////////////////////////////////

AstarPathfinder::NODE
*AstarPathfinder::CheckOPEN(int tilenum)
{
   NODE *tmp;

   tmp = OPEN->NextNode;
   while ( tmp != NULL )
   {
		if ( tmp->NodeNum == tilenum )
	   	return (tmp);
		else
	  		tmp = tmp->NextNode;
   }
   return(NULL);
}

////////////////////////////////////////////////////////////////////////////////

AstarPathfinder::NODE
*AstarPathfinder::CheckCLOSED(int tilenum)
{
   NODE *tmp;

   tmp = CLOSED->NextNode;

   while ( tmp != NULL )
   {
		if ( tmp->NodeNum == tilenum )
      	return(tmp);
		else
	   	tmp = tmp->NextNode;
   }
   return(NULL);
}

////////////////////////////////////////////////////////////////////////////////

void AstarPathfinder::Insert(NODE *Successor)
{
   NODE *tmp1, *tmp2;
   int f;

   if ( OPEN->NextNode == NULL )
   {
		OPEN->NextNode = Successor;
		return;
   }

       // insert into OPEN successor wrt f
   f = Successor->f;
   tmp1 = OPEN;
   tmp2 = OPEN->NextNode;

   while ( (tmp2 != NULL) && (tmp2->f < f) )
   {
   	 tmp1 = tmp2;
       tmp2 = tmp2->NextNode;
   }

   Successor->NextNode = tmp2;
   tmp1->NextNode = Successor;
}

////////////////////////////////////////////////////////////////////////////////

void AstarPathfinder::PropagateDown(NODE *Old)
{
   int c, g;
   NODE *Child, *Father;

   g = Old->g;            // alias.
   for ( c = 0; c < 8; c++)
   {
   	if ( (Child=Old->Child[c]) == NULL )   // create alias for faster access.
          break;
		if ( g+1 < Child->g)
		{
	   	 Child->g = g+1;
	   	 Child->f = Child->g + Child->h;
	  	    Child->Parent = Old;           // reset parent to new path.
	       Push(Child);                 // Now the Child's branch need to be
		}     // checked out. Remember the new cost must be propagated down.
   }

   while ( Stack->NextStackPtr != NULL )
   {
		Father = Pop();
		for (c = 0; c<8; c++)
		{
	   	if ( (Child=Father->Child[c]) == NULL )  // we may stop the propagation 2 ways: either
	  			break;
	      if ( Father->g+1 < Child->g ) // there are no children, or that the g value of
	  		{                          // the child is equal or better than the cost we're propagating
	      	Child->g = Father->g+1;
	    		Child->f = Child->g+Child->h;
	    		Child->Parent = Father;
	    		Push(Child);
	      }
	   }
   }
}


////////////////////////////////////////////////////////////////////////////////
//                              STACK FUNCTIONS                               //
////////////////////////////////////////////////////////////////////////////////

void AstarPathfinder::Push(NODE *Node)
{
   STACK *tmp;

   tmp =( STACK* )calloc(1,sizeof( STACK ));
   tmp->NodePtr = Node;
   tmp->NextStackPtr = Stack->NextStackPtr;
   Stack->NextStackPtr = tmp;
}

////////////////////////////////////////////////////////////////////////////////

AstarPathfinder::NODE
*AstarPathfinder::Pop(void)
{
   NODE *tmp;
   STACK *tmpSTK;

   tmpSTK = Stack->NextStackPtr;
   tmp = tmpSTK->NodePtr;

   Stack->NextStackPtr = tmpSTK->NextStackPtr;
   free(tmpSTK);
   return(tmp);
}