astarfindpath.cpp

用A形算法寻找迷宫的最短路径,VC实现,非常值得一看

// AStarFindPath.cpp: implementation of the AStarFindPath class.
//
//////////////////////////////////////////////////////////////////////

#include "stdafx.h"
#include "AStar.h"
#include "AStarFindPath.h"

#ifdef _DEBUG
#undef THIS_FILE
static char THIS_FILE[]=__FILE__;
#define new DEBUG_NEW
#endif

//////////////////////////////////////////////////////////////////////
// Construction/Destruction
//////////////////////////////////////////////////////////////////////

AStarFindPath::AStarFindPath()
{
	Stack   = ( STACK* )calloc(1,sizeof( STACK ));
	OPEN    = NULL;
	CLOSED  = NULL;
	PATH    = NULL;
	TileMap = NULL;	// Sets the A* Tilemap append on CDXMap
}

AStarFindPath::~AStarFindPath()
{
	FreeNodes();
	free(Stack);
	if (TileMap != NULL) 
	{	delete TileMap;
	TileMap = NULL;
	}
}

void AStarFindPath::InitAstarTileMap(BYTE *pMap, int w, int h)
{
	COLS = w;	// 障碍图的宽度
	ROWS = h;	// 障碍图的高度
	TOTAL_TILES = ROWS * COLS; // 障碍图的尺寸
	unsigned long BufSize;
	BufSize = (TOTAL_TILES + 7) >> 3;
	
	if(TileMap != NULL) 
	{	delete TileMap;
	TileMap = NULL;
	}
	TileMap = new BYTE[BufSize];
	memcpy(TileMap, pMap, BufSize);
}

BOOL AStarFindPath::NewPath(int sx, int sy, int dx, int dy)
{
	if ( FreeTile(dx,dy)&&FreeTile(sx,sy) && (TileNum(sx,sy)!=TileNum(dx,dy)) )
	{
		FreeNodes(); 
		FindPath(sx,sy,dx,dy);
		return TRUE;
	}
	else
		return FALSE;
}

BOOL AStarFindPath::ReachedGoal()
{
	if ( PATH->Parent == NULL )  
		return FALSE;            
	else
		return TRUE;
}

int AStarFindPath::TileNum(int x, int y)
{
	if (x<0 || x>=COLS || y<0 || y>=ROWS) return 0;
	return (y*COLS + x);
}

int AStarFindPath::FreeTile(int x, int y)
{
	if (x<0 || x>=COLS || y<0 || y>=ROWS) 
		return 0;

	else
	{	int bytes, bits, val, index;
	index = y * COLS + x;
	bytes = index >> 3;
	bits  = index & 0x07;
	val = TileMap[bytes] >> (7 - bits);
	if((val & 0x01) == 0)
		return 1;
	else
		return 0;

	}
}

void AStarFindPath::FreeNodes()
{
	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 AStarFindPath::FindPath(int sx, int sy, int dx, int dy)
{
	NODE *Node, *BestNode;
	int TileNumStart;
	
	TileNumStart = 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 = sqrt((dx-sx)*(dx-sx) + (dy-sy)*(dy-sy));  
	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 == TileNumStart)    
			break;
		GenerateSuccessors(BestNode,sx,sy);
	}
	PATH = BestNode;
}

NODE* AStarFindPath::ReturnBestNode()
{
	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 AStarFindPath", 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 AStarFindPath::GenerateSuccessors(NODE *BestNode, int sx, int sy)
{
	int x, y;
	
	// Upper-Left
	if ( FreeTile(x=BestNode->x-TILESIZE, y=BestNode->y-TILESIZE) )
		GenerateSucc(BestNode,x,y,sx,sy);
	// Upper
	if ( FreeTile(x=BestNode->x, y=BestNode->y-TILESIZE) )
		GenerateSucc(BestNode,x,y,sx,sy);
	// Upper-Right
	if ( FreeTile(x=BestNode->x+TILESIZE, y=BestNode->y-TILESIZE) )
		GenerateSucc(BestNode,x,y,sx,sy);
	// Right
	if ( FreeTile(x=BestNode->x+TILESIZE, y=BestNode->y) )
		GenerateSucc(BestNode,x,y,sx,sy);
	// Lower-Right
	if ( FreeTile(x=BestNode->x+TILESIZE, y=BestNode->y+TILESIZE) )
		GenerateSucc(BestNode,x,y,sx,sy);
	// Lower
	if ( FreeTile(x=BestNode->x, y=BestNode->y+TILESIZE) )
		GenerateSucc(BestNode,x,y,sx,sy);
	// Lower-Left
	if ( FreeTile(x=BestNode->x-TILESIZE, y=BestNode->y+TILESIZE) )
		GenerateSucc(BestNode,x,y,sx,sy);
	// Left
	if ( FreeTile(x=BestNode->x-TILESIZE, y=BestNode->y) )
		GenerateSucc(BestNode,x,y,sx,sy);
}

void AStarFindPath::GenerateSucc(NODE *BestNode, int x, int y, int sx, int sy)
{
	int g, TileNumS, c = 0;
	NODE *Old, *Successor,*Previous;
	
	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++)// Add Old to the list of BestNode's Children (or Successors).
			if( BestNode->Child[c] == NULL ) 
				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;
				Previous=OPEN;//Since we changed the g value of Old, we need to change the positon of Old in OPEN list
				while(Previous->NextNode!=Old)
					Previous=Previous->NextNode;
				Previous->NextNode=Old->NextNode;
				Insert(Old);	
				
			}
	}
	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++)// Add Old to the list of BestNode's Children (or Successors).
			if ( BestNode->Child[c] == NULL ) 
				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 = sqrt((x-sx)*(x-sx) + (y-sy)*(y-sy));  // 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;
        Successor->NextNode=NULL;
		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;
	}
}

NODE* AStarFindPath::CheckOPEN(int tilenum)
{
	NODE *tmp;
	
	tmp = OPEN->NextNode;
	while ( tmp != NULL )
	{
		if ( tmp->NodeNum == tilenum )
			return (tmp);
		else
			tmp = tmp->NextNode;
	}
	return(NULL);
}

NODE* AStarFindPath::CheckCLOSED(int tilenum)
{
	NODE *tmp;
	
	tmp = CLOSED->NextNode;
	
	while ( tmp != NULL )
	{
		if ( tmp->NodeNum == tilenum )
			return(tmp);
		else
			tmp = tmp->NextNode;
	}
	return(NULL);
}

void AStarFindPath::Insert(NODE *Successor)
{
	NODE *tmp1, *tmp2;
	double 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 AStarFindPath::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);
		}
		}
	}
}

void AStarFindPath::Push(NODE *Node)
{
	STACK *tmp;
	
	tmp =( STACK* )calloc(1,sizeof( STACK ));
	tmp->NodePtr = Node;
	tmp->NextStackPtr = Stack->NextStackPtr;
	Stack->NextStackPtr = tmp;
}

NODE* AStarFindPath::Pop()
{
	NODE *tmp;
	STACK *tmpSTK;
	
	tmpSTK = Stack->NextStackPtr;
	tmp = tmpSTK->NodePtr;
	
	Stack->NextStackPtr = tmpSTK->NextStackPtr;
	free(tmpSTK);
	return(tmp);
}

BOOL AStarFindPath::PathNextNode()
{
	if(PATH->Parent != NULL) 
	{	PATH = PATH->Parent; 
	return TRUE;
	}
	else
	return FALSE;
}

int AStarFindPath::NodeGetX()
{
	return PATH->x; 
}

int AStarFindPath::NodeGetY()
{
	return PATH->y;
}