A*寻路算法c语言实现(2012.3.7更新，修正bug)
来源：互联网 发布：查网站域名是否被注册 编辑：程序博客网 时间：2024/06/15 09:32
一天写了千行代码。。。
现在基本可以做成一个库了，有较强的灵活性。
估算函数预提供了三个。
生成路径也有在方法里提供out参数了。
感兴趣的同学可以用C++封装起来使用，c/c++都可以使用，我当时用c而没用c++写就是因为这一点。
main.c
#include <stdio.h>#include <stdlib.h>#include "astar.h"int main(){ char * mem = "\0\0\0\0\0\0\0\0\0\0\1\1\0\0\0\1\1\0\0\0\0\0\1\0\0\0\0\0\1\1\0\0\0\1\1\0\0\0\0\0\0\0\0\0\0"; ASTAR * astar = astar_main_initAStar(mem,9,5); //the callback function is used to calculate the cost. //and it has a callback function by default., three callback can be used in this lib astar_main_setCalculateCallBack(astar,astar_calc_manhatton); //astar_main_setCalculateCallBack(astar,astar_calc_euclidian); AStarPoint_ start; start.x = 0; start.y = 0; AStarPoint_ end; end.x = 8; end.y = 4; AStarPathList_ * path = astar_pathList_init(); astar_map_printBytesMatrix(astar->pMap); if(astar_findPath(astar,start,end,path)) { printf("found the path successfully!"); astar_path_print(path); astar_path_cleanup(path); } else { printf("can not find the path!"); } astar_main_cleanup(astar); return 0;}astar.h
/************************## author:Dante Lee## email:bhlzlx@gmail.com## qq:583035284************************/#ifndef ASTAR_H_INCLUDED#define ASTAR_H_INCLUDED#include <stdio.h>#include <stdlib.h>#define MAP_INITED 1// cell types#define WALKABLE 0#define UNWALKABLE 1#define YES 1#define NO 0// the struct contains all the basic information for a AStar Cell.typedef struct AStarCell{ // the position int x; int y; // attribute short int cellType;} *pAStarCell,AStarCell_;///contains the enssential information for the Node,/// for example 1.parent node 2.the node's cost and so on.typedef struct AStarCellNode{ //AStarCell pAStarCell pCell; //the cost float staticCost; float predictCost; float totalCost; //neighbor cells struct AStarCellNode * lpUp; struct AStarCellNode * lpRightUp; struct AStarCellNode * lpRight; struct AStarCellNode * lpRightDown; struct AStarCellNode * lpDown; struct AStarCellNode * lpLeftDown; struct AStarCellNode * lpLeft; struct AStarCellNode * lpLeftUp; //neighbor nodes array struct AStarCellNode * neighborArray[8]; //parent node struct AStarCellNode * lpParent;} *pAStarCellNode,AStarCellNode_;//AStarMap for manager the cells.typedef struct AStarMap{ // 1 means inited. char initState; int width; int height; char ** lpSourceMatrix; AStarCellNode_ *** lpCellMatrix;}*pAStarMap,AStarMap_;/*********************************** *Openlist & Closelist Structures * ***********************************/typedef struct AStarCellListNode{ struct AStarCellListNode * prevNode; struct AStarCellListNode * nextNode; pAStarCellNode currentCellNode;}*pAStarCellListNode,AStarCellListNode_;typedef struct AStarOpenList{ int nodeCount; pAStarMap pMap; char ** assertMap; pAStarCellListNode headNode; pAStarCellListNode tailNode;}*pAStarOpenList,AStarOpenList_;typedef struct AStarCloseList{ pAStarMap pMap; // choose char to instead of boolean type char ** assertMap;}*pAStarCloseList,AStarCloseList_;/*************************** *other structure * **************************/typedef struct AStarPoint{ int x; int y;}*pAStarPoint,AStarPoint_;typedef struct AStarPathNode{ struct AStarPathNode * prevNode; struct AStarPathNode * nextNode; pAStarCellNode currentNode;}*pAStarPathNode,AStarPathNode_;typedef struct AStarPathList{ pAStarPathNode headNode; pAStarPathNode tailNode; int nodesCount;}*pAStarPathList,AStarPathList_;typedef float (* CalculateCallBack)(pAStarCellNode,pAStarCellNode,pAStarCellNode);typedef struct AStarMain{ CalculateCallBack cal_fun; pAStarMap pMap; pAStarCloseList pCloseList; pAStarOpenList pOpenlist;}*pAStar,ASTAR,AStar;///////////////////////////////////////////////////////////////////#include <math.h>// astar cell functionspAStarCell astar_cell_init(float x_,float y_,short int cellType);//astar node functionsstruct AStarCellNode * astar_cellNode_init(struct AStarCell * pCell_);void astar_cellNode_assignNeighborNodes( struct AStarCellNode * pNode, pAStarCellNode down_, pAStarCellNode left_, pAStarCellNode leftDown_, pAStarCellNode leftUp_, pAStarCellNode right_, pAStarCellNode rightDown_, pAStarCellNode rightUp_, pAStarCellNode up_);void astar_cellNode_cleanup(struct AStarCellNode * pNode);void astar_cellNode_reset(struct AStarCellNode * pNode);// astar map functions//pAStarMap astar_map_initFromMemery(void * bytesBuffer,int width,int height);void astar_map_printBytesMatrix(pAStarMap pMap);void astar_map_cleanup(pAStarMap pMap);/**************************** *Openlist & Closelist utilities * ****************************/pAStarCellListNode astar_cellListNode_init(pAStarCellNode pCellNode);void astar_cellListNode_cleanup(pAStarCellListNode pCellListNode);pAStarOpenList astar_openList_init(pAStarMap pMap);void astar_openList_insertCellNode(pAStarOpenList pOpenList,pAStarCellNode pCellNode);pAStarCellNode astar_openList_removeTheLeaseCostNode(pAStarOpenList pOpenList);pAStarCellNode astar_openList_retriveTheLeaseCostNode(pAStarOpenList pOpenList);void astar_openList_cleanup(pAStarOpenList pOpenList);void astar_openList_printList(pAStarOpenList pOpenList);pAStarCloseList astar_closeList_init(pAStarMap pMap);void astar_closeList_addCellNode(pAStarCloseList pCloseList,pAStarCellNode pCellNode);char astar_closeList_assertHasCellNode(pAStarCloseList pCloseList,pAStarCellNode pCellNode);void astar_closeList_cleanup(pAStarCloseList pCloseList);/**************************** *main structures function* ****************************/ASTAR * astar_main_initAStar(void * ptr,int width,int height);void astar_main_cleanup(pAStar astar);void astar_main_setCalculateCallBack(pAStar astar,CalculateCallBack callbackfun);/**************************** *path finding function* ****************************/char astar_findPath(pAStar astar,AStarPoint_ start,AStarPoint_ end,pAStarPathList pPathOuter);/**************************** *path creating function* ****************************/pAStarPathList astar_pathList_init(void);void astar_path_insertFront(pAStarPathList pathList,pAStarCellNode pCellNode);void astar_path_print(pAStarPathList pPathList);void astar_path_cleanup(pAStarPathList pPathList);/**************************** *the cost calculating function* ****************************/float astar_calc_diagonal(pAStarCellNode parentNode,pAStarCellNode currentNode,pAStarCellNode destinationNode);float astar_calc_manhatton(pAStarCellNode parentNode,pAStarCellNode currentNode,pAStarCellNode destinationNode);float astar_calc_euclidian(pAStarCellNode parentNode,pAStarCellNode currentNode,pAStarCellNode destinationNode);#endif // ASTAR_H_INCLUDEDastar.c
/************************## author:Dante Lee## email:bhlzlx@gmail.com## qq:583035284************************/#include "astar.h"#include "memory.h"struct AStarCell * astar_cell_init(float x_,float y_,short int cellType_){ struct AStarCell * pCell = malloc(sizeof(struct AStarCell)); if(!pCell) { return NULL; } pCell->cellType = cellType_; pCell->x = x_; pCell->y = y_; return pCell;}struct AStarCellNode * astar_cellNode_init(struct AStarCell * pCell_){ if(pCell_ == NULL) { return NULL; } struct AStarCellNode * pNode = malloc(sizeof(struct AStarCellNode)); if(!pNode) { return pNode; } pNode->pCell = pCell_; pNode->lpDown = NULL; pNode->lpLeft = NULL; pNode->lpLeftDown = NULL; pNode->lpLeftUp = NULL; pNode->lpRight = NULL; pNode->lpRightDown = NULL; pNode->lpRightUp = NULL; pNode->lpUp = NULL; pNode->lpParent = NULL; //set the cost to maxmium pNode->predictCost = 0xffffff; pNode->staticCost = 0xffffff; pNode->totalCost = 0xffffff; return pNode;}void astar_cellNode_assignNeighborNodes( struct AStarCellNode * pNode, pAStarCellNode down_, pAStarCellNode left_, pAStarCellNode leftDown_, pAStarCellNode leftUp_, pAStarCellNode right_, pAStarCellNode rightDown_, pAStarCellNode rightUp_, pAStarCellNode up_){ pNode->lpDown = down_; pNode->lpLeft = left_; pNode->lpLeftDown = leftDown_; pNode->lpLeftUp = leftUp_; pNode->lpRight = right_; pNode->lpRightDown = rightDown_; pNode->lpRightUp = rightUp_; pNode->lpUp = up_; pNode->neighborArray[0] = pNode->lpDown; pNode->neighborArray[1] = pNode->lpLeft; pNode->neighborArray[2] = pNode->lpLeftDown; pNode->neighborArray[3] = pNode->lpLeftUp; pNode->neighborArray[4] = pNode->lpRight; pNode->neighborArray[5] = pNode->lpRightDown; pNode->neighborArray[6] = pNode->lpRightUp; pNode->neighborArray[7] = pNode->lpUp;}void astar_cellNode_cleanup(struct AStarCellNode * pNode){ if(!pNode) { return; } free(pNode->pCell); free(pNode);}void astar_cellNode_reset(struct AStarCellNode * pNode){ pNode->staticCost = 0x0; pNode->predictCost = 0xffffff; pNode->totalCost = 0xffffff; pNode->lpParent = NULL;}/*if the pMap param is not null,the pMap will be dealloc*/pAStarMap astar_map_initFromMemery(void * bytesBuffer,int width,int height){ // initialize the basic information(nodes & width & height) pAStarMap pMap; pMap = malloc(sizeof(AStarMap_)); if(!pMap) { return NULL; } pMap->lpSourceMatrix = malloc(sizeof(int *)*width); pMap->lpCellMatrix = malloc(sizeof(AStarCellNode_**)*width); int x = 0,y=0; for(;x<width;x++) { pMap->lpCellMatrix[x] = malloc(sizeof(AStarCellNode_ *)*height); pMap->lpSourceMatrix[x] = malloc(sizeof(int)*height); } for(y = 0;y<height;y++) { for(x = 0;x<width;x++) { pMap->lpSourceMatrix[x][y] = *((char*)(bytesBuffer+x+y*width)); pMap->lpCellMatrix[x][y] = astar_cellNode_init(astar_cell_init(x,y,pMap->lpSourceMatrix[x][y])); } } pMap->initState = MAP_INITED; pMap->height = height; pMap->width = width; //rebuild the neighborhoodship between the nodes for (x = 0; x<pMap->width; x++) { pAStarCellNode down_ = NULL; pAStarCellNode left_ = NULL; pAStarCellNode leftDown_ = NULL; pAStarCellNode leftUp_ = NULL; pAStarCellNode right_ = NULL; pAStarCellNode rightDown_ = NULL; pAStarCellNode rightUp_ = NULL; pAStarCellNode up_ = NULL; for (y = 0; y<pMap->height; y++) { if (x==0) { left_ = NULL; leftUp_ = NULL; leftDown_ = NULL; right_ = pMap->lpCellMatrix[x+1][y]; if (y == 0) { up_ = NULL; rightUp_ = NULL; rightDown_ = pMap->lpCellMatrix[x+1][y+1]; down_ = pMap->lpCellMatrix[x][y+1]; } else if (y == pMap->height -1) { down_ = NULL; rightDown_ = NULL; up_ = pMap->lpCellMatrix[x][y-1]; rightUp_ = pMap->lpCellMatrix[x+1][y-1]; } else { rightDown_ = pMap->lpCellMatrix[x+1][y+1]; down_ = pMap->lpCellMatrix[x][y+1]; up_ = pMap->lpCellMatrix[x][y-1]; rightUp_ = pMap->lpCellMatrix[x+1][y-1]; } } else if(x == pMap->width-1) { right_ = NULL; rightUp_ = NULL; rightDown_ = NULL; left_ = pMap->lpCellMatrix[x-1][y]; if (y == 0) { up_ = NULL; leftUp_ = NULL; leftDown_ = pMap->lpCellMatrix[x-1][y+1]; down_ = pMap->lpCellMatrix[x][y+1]; } else if(y == pMap->height-1) { down_ = NULL; leftDown_ = NULL; leftUp_ = pMap->lpCellMatrix[x-1][y-1]; } else { up_=pMap->lpCellMatrix[x][y-1]; leftDown_ = pMap->lpCellMatrix[x-1][y+1]; down_ = pMap->lpCellMatrix[x][y+1]; } } else if(y == 0) { up_ = NULL; leftUp_ = NULL; rightUp_ = NULL; left_ = pMap->lpCellMatrix[x-1][y]; right_ = pMap->lpCellMatrix[x+1][y]; leftDown_ = pMap->lpCellMatrix[x-1][y+1]; down_ = pMap->lpCellMatrix[x][y+1]; rightDown_ = pMap->lpCellMatrix[x+1][y+1]; } else if(y == pMap->height -1) { down_ = NULL; leftDown_ = NULL; rightDown_ = NULL; up_=pMap->lpCellMatrix[x][y-1]; leftUp_ = pMap->lpCellMatrix[x-1][y-1]; rightUp_ = pMap->lpCellMatrix[x+1][y-1]; left_ = pMap->lpCellMatrix[x-1][y]; right_ = pMap->lpCellMatrix[x+1][y]; } else { leftDown_ = pMap->lpCellMatrix[x-1][y+1]; down_ = pMap->lpCellMatrix[x][y+1]; rightDown_ = pMap->lpCellMatrix[x+1][y+1]; up_=pMap->lpCellMatrix[x][y-1]; leftUp_ = pMap->lpCellMatrix[x-1][y-1]; rightUp_ = pMap->lpCellMatrix[x+1][y-1]; left_ = pMap->lpCellMatrix[x-1][y]; right_ = pMap->lpCellMatrix[x+1][y]; } astar_cellNode_assignNeighborNodes(pMap->lpCellMatrix[x][y], down_, left_, leftDown_, leftUp_, right_, rightDown_, rightUp_, up_); } } return pMap;}void astar_map_cleanup(pAStarMap pMap){ if(pMap && pMap->initState == MAP_INITED) { //free all the data int x =0,y=0; for(;x<pMap->width;x++) { for(y = 0;y<pMap->height ;y++) { astar_cellNode_cleanup(pMap->lpCellMatrix[x][y]); } //free the pointer array //pAStarCellNode Array free(pMap->lpCellMatrix[x]); // int array free(pMap->lpSourceMatrix[x]); } free(pMap->lpCellMatrix); free(pMap->lpSourceMatrix); } free(pMap);}void astar_map_printBytesMatrix(pAStarMap pMap){ if(!pMap) { printf("the map has not been inited yet!"); return; } printf("\n"); int x,y; printf("with index print:\n"); for(y = 0;y<pMap->height;y++) { for(x = 0;x<pMap->width;x++) { printf("%d:[%d,%d] ",pMap->lpSourceMatrix[x][y],x,y); } printf("\n"); } printf("without index print:\n"); printf("**********************\n"); for(y = 0;y<pMap->height;y++) { printf("* "); for(x = 0;x<pMap->width;x++) { printf("%d",pMap->lpSourceMatrix[x][y]); } printf("\n"); } printf("**********************"); printf("\n");}/**************************** *Openlist & Closelist utilities * ****************************/pAStarCellListNode astar_cellListNode_init(pAStarCellNode pCellNode){ pAStarCellListNode pCellListNode = malloc(sizeof(AStarCellListNode_)); pCellListNode->nextNode = NULL; pCellListNode->prevNode = NULL; pCellListNode->currentCellNode = pCellNode; return pCellListNode;}void astar_cellListNode_cleanup(pAStarCellListNode pCellListNode){ free(pCellListNode);}pAStarOpenList astar_openList_init(pAStarMap pMap){ pAStarOpenList pOpenList = malloc(sizeof(AStarOpenList_)); pOpenList->pMap = pMap; pOpenList->assertMap = malloc(sizeof(char*)*pMap->width); int x; for (x = 0; x < pMap->width; x++) { pOpenList->assertMap[x] = malloc(sizeof(char)*pMap->height); memset(pOpenList->assertMap[x], 0, pMap->height); } pOpenList->headNode = astar_cellListNode_init(NULL); pOpenList->tailNode = astar_cellListNode_init(NULL); pOpenList->headNode->nextNode = pOpenList->tailNode; pOpenList->tailNode->prevNode = pOpenList->headNode; pOpenList->nodeCount = 0; return pOpenList;}void astar_openList_insertCellNode(pAStarOpenList pOpenList,pAStarCellNode pCellNode){ if (!pCellNode) { return; } // if this node has been added.skip it. if(pOpenList->assertMap[pCellNode->pCell->x][pCellNode->pCell->y] == 1) { return; } pAStarCellListNode nodeIter = pOpenList->headNode->nextNode; while (nodeIter!=pOpenList->tailNode && (pCellNode->totalCost > nodeIter->currentCellNode->totalCost) ) { nodeIter= nodeIter->nextNode; } pAStarCellListNode newListNode = astar_cellListNode_init(pCellNode); newListNode->nextNode = nodeIter; newListNode->prevNode = nodeIter->prevNode; nodeIter->prevNode->nextNode = newListNode; nodeIter->prevNode = newListNode; pOpenList->assertMap[pCellNode->pCell->x][pCellNode->pCell->y] = 1; pOpenList->nodeCount++;}// if not exist any valid node,return NULL;pAStarCellNode astar_openList_removeTheLeaseCostNode(pAStarOpenList pOpenList){ if (pOpenList->nodeCount>0) { pAStarCellListNode pCellListNode = pOpenList->headNode->nextNode; pOpenList->headNode->nextNode = pCellListNode->nextNode; pCellListNode->nextNode->prevNode = pOpenList->headNode; pAStarCellNode pCellNode = pCellListNode->currentCellNode; astar_cellListNode_cleanup(pCellListNode); pOpenList->nodeCount--; return pCellNode; } else { return NULL; }}pAStarCellNode astar_openList_retriveTheLeaseCostNode(pAStarOpenList pOpenList){ return pOpenList->headNode->nextNode->currentCellNode;}void astar_openList_printList(pAStarOpenList pOpenList){ pAStarCellListNode iter = pOpenList->headNode->nextNode; printf("\n"); while(iter != pOpenList->tailNode) { printf("[x:%d,y:%d]cost:%f\n",iter->currentCellNode->pCell->x,iter->currentCellNode->pCell->y,iter->currentCellNode->totalCost); iter = iter->nextNode; } printf("\n");}void astar_openList_cleanup(pAStarOpenList pOpenList){ //clean assert map int x; for (x = 0; x < pOpenList->pMap->width; x++) { free(pOpenList->assertMap[x]); } // clean nodes pAStarCellListNode cellListNodeIter = pOpenList->headNode; while (cellListNodeIter!=pOpenList->tailNode) { cellListNodeIter = cellListNodeIter->nextNode; astar_cellListNode_cleanup(cellListNodeIter->prevNode); } astar_cellListNode_cleanup(cellListNodeIter); free(pOpenList);}pAStarCloseList astar_closeList_init(pAStarMap pMap){ pAStarCloseList pCloseList = malloc(sizeof(AStarCloseList_)); pCloseList->pMap = pMap; pCloseList->assertMap = malloc(sizeof(char*)*pMap->width); int x; for (x = 0; x < pMap->width; x++) { pCloseList->assertMap[x] = malloc(sizeof(char)*pMap->height); memset(pCloseList->assertMap[x], 0, pMap->height); } return pCloseList;}void astar_closeList_addCellNode(pAStarCloseList pCloseList,pAStarCellNode pCellNode){ pCloseList->assertMap[pCellNode->pCell->x][pCellNode->pCell->y] = 1;}char astar_closeList_assertHasCellNode(pAStarCloseList pCloseList,pAStarCellNode pCellNode){ return pCloseList->assertMap[pCellNode->pCell->x][pCellNode->pCell->y];}void astar_closeList_cleanup(pAStarCloseList pCloseList){ int x; for (x = 0; x < pCloseList->pMap->width; x++) { free(pCloseList->assertMap[x]); }}/**/ASTAR * astar_main_initAStar(void * ptr,int width,int height){ ASTAR * astar = malloc(sizeof(ASTAR)); astar->cal_fun = &astar_calc_diagonal; astar->pMap = astar_map_initFromMemery(ptr,width,height); astar->pCloseList = astar_closeList_init(astar->pMap); astar->pOpenlist = astar_openList_init(astar->pMap); return astar;}void astar_main_cleanup(pAStar astar){ // closelist & openlist should be cleaned before the map was cleaned. astar_closeList_cleanup(astar->pCloseList); astar_openList_cleanup(astar->pOpenlist); astar_map_cleanup(astar->pMap);}void astar_main_setCalculateCallBack(pAStar astar,CalculateCallBack callbackfun){ astar->cal_fun = callbackfun;}/////////////////////////////////////////**************************** *path creating function* ****************************/ pAStarPathList astar_pathList_init(void) { pAStarPathList pPath = malloc(sizeof(AStarPathList_)); pPath->headNode = malloc(sizeof(AStarPathNode_)); pPath->tailNode = malloc(sizeof(AStarPathNode_)); pPath->headNode->currentNode = NULL; pPath->headNode->nextNode = pPath->tailNode; pPath->headNode->prevNode = NULL; pPath->tailNode->currentNode = NULL; pPath->tailNode->nextNode = NULL; pPath->tailNode->prevNode = pPath->headNode; pPath->nodesCount = 0; return pPath; } void astar_path_insertFront(pAStarPathList pathList,pAStarCellNode pCellNode) { if(!pCellNode) { return; } pathList->nodesCount++; pAStarPathNode newPathNode = malloc(sizeof(AStarCellNode_)); newPathNode->currentNode = pCellNode; newPathNode->nextNode = pathList->headNode->nextNode; newPathNode->prevNode = pathList->headNode; pathList->headNode->nextNode->prevNode = newPathNode; pathList->headNode->nextNode = newPathNode; }void astar_path_print(pAStarPathList pPathList){ if(!pPathList) { return; } pAStarPathNode iter = pPathList->headNode->nextNode; while(iter!=pPathList->tailNode) { printf("(x:%d,y:%d) , ",iter->currentNode->pCell->x,iter->currentNode->pCell->y); iter = iter->nextNode; } return;} void astar_path_cleanup(pAStarPathList pPathList) { pAStarPathNode iter = pPathList->headNode->nextNode; while(iter!=pPathList->tailNode) { free(iter->prevNode); iter = iter->nextNode; } free(iter->prevNode); free(iter); }///////////////////////////////////////////////finding path !!!!!/////!!!!!!!!!!!!!!!!!//////////////////////////////////////////char astar_findPath(pAStar astar,AStarPoint_ start,AStarPoint_ end,pAStarPathList pPathOuter){ if (!(start.x >=0 && start.x<astar->pMap->width)) { return 0; } if (!(start.y >=0 && start.y<astar->pMap->height)) { return 0; } if (!(end.x >=0 && end.x<astar->pMap->width)) { return 0; } if (!(end.y >=0 && end.y<astar->pMap->height)) { return 0; } // the begin & the end node is valid. pAStarCellNode endNode = astar->pMap->lpCellMatrix[end.x][end.y]; pAStarCellNode startNode = astar->pMap->lpCellMatrix[start.x][start.y]; pAStarCellNode currentNode = astar->pMap->lpCellMatrix[start.x][start.y]; currentNode->lpParent = currentNode; currentNode->staticCost = 0; currentNode->totalCost = 0; astar_openList_insertCellNode(astar->pOpenlist,currentNode); while(1) { currentNode = astar_openList_retriveTheLeaseCostNode(astar->pOpenlist); if(astar_closeList_assertHasCellNode(astar->pCloseList,currentNode)) { astar_openList_removeTheLeaseCostNode(astar->pOpenlist); if(astar->pOpenlist->nodeCount == 0) { printf("can not find the path!"); return 0;//return false. } continue; } if(currentNode == endNode) { //has got the path. break; } int i = 0; for (;i<8 ;i++ ) { if(currentNode->neighborArray[i] == NULL) { continue; } if(currentNode->neighborArray[i]->pCell->cellType == UNWALKABLE) { continue; } if(astar_closeList_assertHasCellNode(astar->pCloseList,currentNode->neighborArray[i]) == YES) { continue; } // not in the same row or column int cx = currentNode->pCell->x; int cy = currentNode->pCell->y; int nx = currentNode->neighborArray[i]->pCell->x; int ny = currentNode->neighborArray[i]->pCell->y; if(cx != nx && cy != ny) { if(astar->pMap->lpCellMatrix[cx][ny]->pCell->cellType == UNWALKABLE || astar->pMap->lpCellMatrix[nx][cy]->pCell->cellType == UNWALKABLE) { continue; } } astar->cal_fun(currentNode,currentNode->neighborArray[i],endNode); currentNode->neighborArray[i]->lpParent = currentNode; astar_openList_insertCellNode(astar->pOpenlist,currentNode->neighborArray[i]); } astar_closeList_addCellNode(astar->pCloseList,currentNode); //astar_openList_printList(astar->pOpenlist); } //save the result to the path list. pAStarCellNode cellNodeIter = endNode; while(cellNodeIter!=startNode) { // printf("[x:%d,y:%d]\n",cellNodeIter->pCell->x,cellNodeIter->pCell->y); if(pPathOuter) { astar_path_insertFront(pPathOuter,cellNodeIter); } cellNodeIter = cellNodeIter->lpParent; } astar_path_insertFront(pPathOuter,cellNodeIter); //printf("[x:%d,y:%d]\n",cellNodeIter->pCell->x,cellNodeIter->pCell->y); //restore all cell nodes int x =0,y=0; for(;x<astar->pMap->width;x++) { for(y = 0;y<astar->pMap->height ;y++) { astar_cellNode_reset(astar->pMap->lpCellMatrix[x][y]); } } return 1;}float astar_calc_diagonal(pAStarCellNode parentNode,pAStarCellNode currentNode,pAStarCellNode destinationNode){ float staticCost = sqrt(pow(currentNode->pCell->x-parentNode->pCell->x,2)+pow(currentNode->pCell->y-parentNode->pCell->y,2)) + parentNode->staticCost; currentNode->staticCost = staticCost; float dx,dy; dx = abs(currentNode->pCell->x - destinationNode->pCell->x); dy = abs(currentNode->pCell->y - destinationNode->pCell->y); float diag = dx>dy?dy:dx; float straight = dx+dy; float predictCost = diag*1.414 + straight - 2*diag; currentNode->totalCost = staticCost + predictCost; return currentNode->totalCost;}float astar_calc_manhatton(pAStarCellNode parentNode,pAStarCellNode currentNode,pAStarCellNode destinationNode){ float staticCost = sqrtf(pow(currentNode->pCell->x-parentNode->pCell->x,2)+pow(currentNode->pCell->y-parentNode->pCell->y,2)) + parentNode->staticCost; currentNode->staticCost = staticCost; float dx,dy; dx = abs(currentNode->pCell->x - destinationNode->pCell->x); dy = abs(currentNode->pCell->y - destinationNode->pCell->y); float predictCost = dx+dy; currentNode->totalCost = staticCost + predictCost; return currentNode->totalCost;}float astar_calc_euclidian(pAStarCellNode parentNode,pAStarCellNode currentNode,pAStarCellNode destinationNode){ float dx1,dx2,dy1,dy2; dx1 = currentNode->pCell->x-parentNode->pCell->x; dx2 = currentNode->pCell->x-destinationNode->pCell->x; dy1 = currentNode->pCell->y-parentNode->pCell->y; dy2 = currentNode->pCell->y-destinationNode->pCell->y; float staticCost = sqrtf(pow(dx1,2)+pow(dy1,2)) + parentNode->staticCost; float predictCost = sqrtf(pow(dx2,2)+pow(dy2,2)); currentNode->staticCost = staticCost; currentNode->totalCost = staticCost + predictCost; return currentNode->totalCost;}- A*寻路算法c语言实现(2012.3.7更新，修正bug)
- 【修正BUG】SHA-256算法 C语言实现
- 用C语言实现的A*算法
- A*算法的C语言实现
- 常见排序算法（冒泡，选择，快速）的C语言实现【修正版】
- 快速排序算法bug修正
- A* A星 算法 C语言 实现代码
- A* A星 算法 C语言 实现代码
- A* A星 算法 C语言 实现代码
- A* A星 算法 C语言 实现代码
- A*算法的一份C语言实现品！
- A*(Astar)搜索算法的实现(C语言)
- A*算法C实现
- 更新进度条C语言实现
- C语言的A*算法
- 网上流传的一个Canny边缘检测算法C语言实现的bug
- 常见排序算法的C语言实现以及算法复杂度分析（持续更新）
- ScrollView反弹效果实现[bug修正版]
- C语言宏与强制类型转换的妙用
- 算法问题《Card Game》的数学模型和Java实现
- 编写时间的类函数
- 别放弃每一次的进步
- 第三周实验报告1
- A*寻路算法c语言实现(2012.3.7更新，修正bug)
- Win7以管理员方式运行cmd.exe
- c++与c#的比较
- 从 SQL Server 到 My SQL
- .net下作业调度实战[quartz]
- DirectDraw GDI的区别之二———DirectDraw知识入门
- JAVA UTF-8 GB2312 编码互转
- dhcp容灾方案（主备）所遇到的问题以及解决方案
- POJ-1511 Invitation Cards【最短路】
