.cpp檔案代碼
++++++++++++++++++++++++++++++++++++++
#include "stdafx.h"
#include "astar.h"
extern char Block[sz][SX];
AstarPathfinder::AstarPathfinder(/*CMapManager * pMapManager*/)
{
//m_pStack = ( STACK* )calloc(1,sizeof( STACK ));
//m_bisPath = FALSE;
m_pOPEN = NULL;
m_pCLOSED = NULL;
m_pPATH = NULL;
//m_pMapManager = pMapManager;
m_tCount = 0;
BoundaryTiles();
}
////////////////////////////////////////////////////////////////////////////////
AstarPathfinder::~AstarPathfinder()
{
Freenodes();
/* free(m_pStack);*/
if(m_pOPEN)
free(m_pOPEN);
if(m_pCLOSED)
free(m_pCLOSED);
}
////////////////////////////////////////////////////////////////////////////////
BOOL AstarPathfinder::NewPath(float sx,float sz, float dx,float dz)
{
DWORD t1,t2;
t1 = TileNum(sx,sz);
t2 = TileNum(dx,dz);
if (t1 != t2)
{
m_tCount = 0;
FreeNodes();
if(FindPath(sx,sz,dx,dz))
return (m_bisPath=TRUE);
}
return (m_bisPath=FALSE);
}
////////////////////////////////////////////////////////////////////////////////
//BOOL AstarPathfinder::ReachedGoal(void)
//{
// if ( !m_bisPath ) return TRUE;
// if ( m_pPATH->s_pParent == NULL )
// {
// m_bisPath = FALSE;
// return TRUE;
// }
// return FALSE;
//}
DWORD AstarPathfinder::TileNum(float x,float z)
{
DWORD dx = DWORD(x);
DWORD dz = DWORD(z);
/*DWORD dWRT = dx + dz * m_pMapManager->GetXTileNum();*/
DWORD dwrt = dx + dz * SX;
return dwrt;
}
BOOL AstarPathfinder::FreeTile(float x,float z)
{
WORD wx = WORD(x);
WORD wz = WORD(z);
/*st_Tile * pt = m_pMapManager->GetTile(wx,wz);
if(pt && pt->s_on_obj_type & TILE_NOWAY_BIT)
return FALSE;
*/
if(Block[wz][wx]) return FALSE;
return TRUE;
}
////////////////////////////////////////////////////////////////////////////////
// Private Member Functions //
////////////////////////////////////////////////////////////////////////////////
void AstarPathfinder::FreeNodes(void)
{
NODE *Node, *OldNode;
if ( m_pOPEN != NULL )
{
Node = m_pOPEN->s_pNextNode;
while ( Node != NULL )
{
OldNode = Node;
Node = Node->s_pNextNode;
free(OldNode);
}
}
if ( m_pCLOSED != NULL )
{
Node = m_pCLOSED->s_pNextNode;
while ( Node != NULL )
{
OldNode = Node;
Node = Node->s_pNextNode;
free(OldNode);
}
}
}
////////////////////////////////////////////////////////////////////////////////
void AstarPathfinder::BoundaryTiles(void)
{
WORD xindex, zindex;
/*DWORD xnum = m_pMapManager->GetXTileNum();
DWORD znum = m_pMapManager->GetZTileNum();*/
DWORD xnum = SX;
DWORD znum = SZ;
for(xindex = 0; xindex SetTileObject(xindex,0,0,TILE_STR_BIT);
m_pMapManager->SetTileObject(xindex,WORD(znum - 1),0,TILE_STR_BIT);*/
Block[xindex][0] = 1;
Block[xindex][WORD(znum - 1)] = 1;
}
for(zindex = 0; zindex SetTileObject(0,zindex,0,TILE_STR_BIT);
m_pMapManager->SetTileObject(WORD(xnum - 1),zindex,0,TILE_STR_BIT);*/
Block[0][zindex] = 1;
Block[WORD(xnum-1)][zindex] = 1;
}
}
////////////////////////////////////////////////////////////////////////////////
// A* Algorithm //
////////////////////////////////////////////////////////////////////////////////
BOOL AstarPathfinder::FindPath(float sx, float sz, float dx, float dz)
{
NODE *Node, *BestNode;
DWORD TileNumDest;
TileNumDest = TileNum(sx, sz);
m_pOPEN=( NODE* )calloc(1,sizeof( NODE ));
m_pCLOSED=( NODE* )calloc(1,sizeof( NODE ));
Node=( NODE* )calloc(1,sizeof( NODE ));
Node->s_dw_g = 0;
Node->s_h = long((dx-sx)*(dx-sx) + (dz-sz)*(dz-sz)); // should really use sqrt().
Node->s_f = Node->s_dw_g+Node->s_h;
Node->s_dw_NodeNum = TileNum(dx,dz);
Node->s_fx = dx;
Node->s_fz = dz;
m_pOPEN->s_pNextNode=Node; // make Open List point to first nod
DWORD count = 0;
for (;;count++)
{
BestNode=ReturnBestNode();
if(BestNode == NULL)
break;
if(BestNode->s_dw_NodeNum == TileNumDest)
{
m_pPATH = BestNode;
return TRUE;
}
Generatesuccessors(BestNode,sx,sz);
m_tCount = count;
}
return FALSE;
}
////////////////////////////////////////////////////////////////////////////////
AstarPathfinder::NODE
*AstarPathfinder::ReturnBestNode(void)
{
NODE *tmp;
if ( m_pOPEN->s_pNextNode == NULL )
{
return NULL;
}
tmp = m_pOPEN->s_pNextNode;// point to first node on m_pOPEN
m_pOPEN->s_pNextNode = tmp->s_pNextNode;// Make m_pOPEN point to nextnode or NULL.
tmp->s_pNextNode = m_pCLOSED->s_pNextNode;// Next take BESTNODE (or temp in this case)
m_pCLOSED->s_pNextNode = tmp;// and put it on m_pCLOSED
return(tmp);
}
////////////////////////////////////////////////////////////////////////////////
void AstarPathfinder::GenerateSuccessors(NODE *BestNode,float dx, float dz)
{
float x, z;
float w,h;
w = 1;
h = 1;
// Upper-Left
if ( FreeTile(x = BestNode->s_fx - w, z = BestNode->s_fz - h) )
GenerateSucc(BestNode,x,z,dx,dz);
// Upper
if ( FreeTile(x = BestNode->s_fx, z = BestNode->s_fz - h) )
GenerateSucc(BestNode,x,z,dx,dz);
// Upper-Right
if ( FreeTile(x = BestNode->s_fx + w, z = BestNode->s_fz - h) )
GenerateSucc(BestNode,x,z,dx,dz);
// Right
if ( FreeTile(x = BestNode->s_fx + w, z = BestNode->s_fz) )
GenerateSucc(BestNode,x,z,dx,dz);
// Lower-Right
if ( FreeTile(x = BestNode->s_fx + w, z = BestNode->s_fz + h) )
GenerateSucc(BestNode,x,z,dx,dz);
// Lower
if ( FreeTile(x = BestNode->s_fx, z = BestNode->s_fz + h) )
GenerateSucc(BestNode,x,z,dx,dz);
// Lower-Left
if ( FreeTile(x = BestNode->s_fx - w, z = BestNode->s_fz + h) )
GenerateSucc(BestNode,x,z,dx,dz);
// Left
if ( FreeTile(x = BestNode->s_fx - w, z = BestNode->s_fz) )
GenerateSucc(BestNode,x,z,dx,dz);
}
////////////////////////////////////////////////////////////////////////////////
void AstarPathfinder::GenerateSucc(NODE *BestNode,float x,float z,float dx,float dz)
{
DWORD g, TileNumS, c = 0;
NODE *Old, *Successor;
g = BestNode->s_dw_g+1;
TileNumS = TileNum(x,z); // identification purposes
if ( (Old=CheckOPEN(TileNumS)) != NULL ) // if equal to NULL then not in m_pOPEN list, else it returns the Node in Old
{
for( c = 0; c s_Child[c] == NULL ) // Add Old to the list of BestNode's Children (or Successors).
break;
BestNode->s_Child[c] = Old;
if ( g s_dw_g ) // if our new g value is s_pParent = BestNode;
Old->s_dw_g = g;
Old->s_f = g + Old->s_h;
}
}
else if ( (Old=CheckCLOSED(TileNumS)) != NULL ) // if equal to NULL then not in m_pOPEN list, else it returns the Node in Old
{
// for( c = 0; cs_Child[c] == NULL ) // Add Old to the list of BestNode's Children (or Successors).
// break;
//BestNode->s_Child[c] = Old;
//if ( g s_dw_g ) // if our new g value is s_pParent = BestNode;
// Old->s_dw_g = g;
// Old->s_f = g + Old->s_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->s_pParent = BestNode;
Successor->s_dw_g = g;
Successor->s_h = long((x-dx)*(x-dx) + (z-dz)*(z-dz)); // should do sqrt(), but since we don't really
Successor->s_f = g+Successor->s_h; // care about the distance but just which branch looks
Successor->s_fx = x; // better this should suffice. Anyayz it's faster.
Successor->s_fz = z;
Successor->s_dw_NodeNum = TileNumS;
Insert(Successor); // Insert Successor on m_pOPEN list wrt f
for( c =0; c s_Child[c] == NULL ) // Add Old to the list of BestNode's Children (or Successors).
break;
BestNode->s_Child[c] = Successor;
}
}
////////////////////////////////////////////////////////////////////////////////
AstarPathfinder::NODE
*AstarPathfinder::CheckOPEN(DWORD tilenum)
{
NODE *tmp;
tmp = m_pOPEN->s_pNextNode;
while ( tmp != NULL )
{
if ( tmp->s_dw_NodeNum == tilenum )
return (tmp);
else
tmp = tmp->s_pNextNode;
}
return(NULL);
}
////////////////////////////////////////////////////////////////////////////////
AstarPathfinder::NODE
*AstarPathfinder::CheckCLOSED(DWORD tilenum)
{
NODE *tmp;
tmp = m_pCLOSED->s_pNextNode;
while ( tmp != NULL )
{
if ( tmp->s_dw_NodeNum == tilenum )
return(tmp);
else
tmp = tmp->s_pNextNode;
}
return(NULL);
}
////////////////////////////////////////////////////////////////////////////////
void AstarPathfinder::Insert(NODE *Successor)
{
NODE *tmp1, *tmp2;
int f;
if ( m_pOPEN->s_pNextNode == NULL )
{
m_pOPEN->s_pNextNode = Successor;
return;
}
// insert into m_pOPEN successor wrt f
f = Successor->s_f;
tmp1 = m_pOPEN;
tmp2 = m_pOPEN->s_pNextNode;
while ( (tmp2 != NULL) && (tmp2->s_f s_pNextNode;
}
Successor->s_pNextNode = tmp2;
tmp1->s_pNextNode = Successor;
}
////////////////////////////////////////////////////////////////////////////////
//void AstarPathfinder::PropagateDown(NODE *Old)
//{
// DWORD c, g;
// NODE *Child, *Father;
//
// g = Old->s_dw_g; // alias.
// for ( c = 0; c s_Child[c]) == NULL ) // create alias for faster access.
// break;
//if ( g+1 s_dw_g)
//{
//Child->s_dw_g = g+1;
//Child->s_f = Child->s_dw_g + Child->s_h;
//Child->s_pParent = 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 ( m_pStack->s_pNextStackPtr != NULL )
// {
//Father = Pop();
//for (c = 0; cs_Child[c]) == NULL ) // we may stop the propagation 2 ways: either
// break;
// if ( Father->s_dw_g+1 s_dw_g ) // there are no children, or that the g value of
// { // the child is equal or better than the cost we're propagating
// Child->s_dw_g = Father->s_dw_g+1;
// Child->s_f = Child->s_dw_g+Child->s_h;
// Child->s_pParent = Father;
// Push(Child);
// }
// }
// }
//}
//
////////////////////////////////////////////////////////////////////////////////
// STACK FUNCTIONS //
////////////////////////////////////////////////////////////////////////////////
//void AstarPathfinder::Push(NODE *Node)
//{
// STACK *tmp;
//
// tmp =( STACK* )calloc(1,sizeof( STACK ));
// tmp->s_pNodePtr = Node;
// tmp->s_pNextStackPtr = m_pStack->s_pNextStackPtr;
// m_pStack->s_pNextStackPtr = tmp;
//}
//
//////////////////////////////////////////////////////////////////////////////////
//
//AstarPathfinder::NODE
//*AstarPathfinder::Pop(void)
//{
// NODE *tmp;
// STACK *tmpSTK;
//
// tmpSTK = m_pStack->s_pNextStackPtr;
// tmp = tmpSTK->s_pNodePtr;
//
// m_pStack->s_pNextStackPtr = tmpSTK->s_pNextStackPtr;
// free(tmpSTK);
// return(tmp);
//}
+++++++++++++++++++++++++++
.h檔案代碼
+++++++++++++++++++++++++++
#ifndef ASTAR_H
#define ASTAR_H
#include "stdafx.h"
#include
#include
//#include "MapManager.h"
#define SX 10
#define SZ 10
class AstarPathfinder
{
private:
struct NODE
{ // node structure
longs_f,s_h;
DWORDs_dw_g,s_dw_tmpg;
floats_fx,s_fz;
DWORDs_dw_NodeNum;
NODE*s_pParent;
NODE*s_Child[8];
NODE*s_pNextNode;
};
NODE*m_pOPEN;
NODE*m_pCLOSED;
NODE*m_pPATH;
//struct STACK
//{ // the stack structure
//NODE*s_pNodePtr;
//STACK*s_pNextStackPtr;
//};
//
//STACK *m_pStack;
DWORDm_tCount;
BOOLm_bisPath;
//CMapManager * m_pMapManager;
public:
AstarPathfinder(/*CMapManager * pMapManager*/);
~AstarPathfinder();
BOOL NewPath(float sx, float sz, float dx, float dz);
BOOL ReachedGoal(void);
NODE * PathNextNode(void) { return m_pPATH=m_pPATH->s_pParent; }
float NodeGetX(void) { return m_pPATH->s_fx; }
float NodeGetZ(void) { return m_pPATH->s_fz; }
DWORD GetNodeCount() { return m_tCount; }
//=============================================hun 7.24 begin
DWORDTileNum(float x,float z);
BOOLFreeTile(float x,float z);
//=============================================hun 7.24 end
private:
void BoundaryTiles(void);
void FreeNodes(void);
// The A* Algorithm and it's stuff
BOOL FindPath(float sx, float sz, float dx, float dz);
NODE *ReturnBestNode(void);
void GenerateSuccessors(NODE *BestNode, float dx, float dz);
void GenerateSucc(NODE *BestNode,float x, float z, float dx, float dz);
NODE *CheckOPEN(DWORD dw_tilenum);
NODE *CheckCLOSED(DWORD dw_tilenum);
void Insert(NODE *Successor);
/*void PropagateDown(NODE *Old);*/
//void Push(NODE *Node); // stack functions
//NODE *Pop(void);
};
extern AstarPathfinder * g_pPathFinder;
#endif // ASTAR_H
+++++++++++++++++++++++++++
