c语言启发式搜索,启发式搜索算法（A*算法）的实现

void AstarPathfinder::FindPath(int sx, int sy, int dx, int dy)

{

NODE *Node, *BestNode;

int TileNumDest;

//得到目标位置，作判断用

TileNumDest = TileNum(sx, sy);

//生成Open和Closed表

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

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

//生成起始节点，并放入Open表中

Node=( NODE* )calloc(1,sizeof( NODE ));

Node->g = 0;

//这是计算h值

// should really use sqrt().

Node->h = (dx-sx)*(dx-sx) + (dy-sy)*(dy-sy);

//这是计算f值，即估价值

Node->f = Node->g+Node->h;

Node->NodeNum = TileNum(dx, dy);

Node->x = dx; Node->y = dy;

// make Open List point to first node

OPEN->NextNode=Node;

for (;;)

{

//从Open表中取得一个估价值最好的节点

BestNode=ReturnBestNode();

//如果该节点是目标节点就退出

// if we've found the end, break and finish break;

if (BestNode->NodeNum == TileNumDest)

//否则生成子节点

GenerateSuccessors(BestNode,sx,sy);

}

PATH = BestNode;

}

//生成子节点函数：

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 值

// g(Successor)=g(BestNode)+cost of getting from BestNode to Successor

g = BestNode->g+1;

// identification purposes

TileNumS = TileNum(x,y);

//子节点再Open表中吗？

// if equal to NULL then not in OPEN list, else it returns the Node in Old

if ( (Old=CheckOPEN(TileNumS)) != NULL )

{

//若在

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;

//比较Open表中的估价值和当前的估价值(只要比较g值就可以了)

// if our new g value is < Old's then reset Old's parent to point to BestNode

if ( g < Old->g )

{

//当前的估价值小就更新Open表中的估价值

Old->Parent = BestNode;

Old->g = g;

Old->f = g + Old->h;

}

}

else

//在Closed表中吗？

// if equal to NULL then not in OPEN list, else it returns the Node in Old

if ( (Old=CheckCLOSED(TileNumS)) != NULL )

{

//若在

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;

//比较Closed表中的估价值和当前的估价值(只要比较g值就可以了)

// if our new g value is < Old's then reset Old's parent to point to BestNode

if ( g < Old->g )

{

//当前的估价值小就更新Closed表中的估价值

Old->Parent = BestNode;

Old->g = g;

Old->f = g + Old->h;

//再依次更新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!

PropagateDown(Old);

}

}

//不在Open表中也不在Close表中

else

{

//生成新的节点

Successor = ( NODE* )calloc(1,sizeof( NODE ));

Successor->Parent = BestNode;

Successor->g = g;

// should do sqrt(), but since we don't really

Successor->h = (x-dx)*(x-dx) + (y-dy)*(y-dy);

// care about the distance but just which branch looks

Successor->f = g+Successor->h;

// better this should suffice. Anyayz it's faster.

Successor->x = x;

Successor->y = y;

Successor->NodeNum = TileNumS;

//再插入Open表中，同时排序。

// Insert Successor on OPEN list wrt f

Insert(Successor);

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] = Successor;

}

}