A* 演演算法

.cpp文件代碼

++++++++++++++++++++++++++++++++++++++

#include "stdafx.h"

#include "astar.h"

extern char Block[sz][SX];

AstarPathfinder::AstarPathfinder()

{

//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();

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 * SX;

return dwrt;

}

BOOL AstarPathfinder::FreeTile(float x,float z)

{

WORD wx = WORD(x);

WORD wz = WORD(z);

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 = 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();

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

DWORD TileNum(float x,float z);

BOOL FreeTile(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 Push(NODE *Node); // stack functions

//NODE *Pop(void);

};

extern AstarPathfinder * g_pPathFinder;

#endif // ASTAR_H

+++++++++++++++++++++++++++