A*寻路算法c语言实现(2012.3.7更新,修正bug)
2012-03-05 21:09
267 查看
一天写了千行代码。。。
现在基本可以做成一个库了,有较强的灵活性。
估算函数预提供了三个。
生成路径也有在方法里提供out参数了。
感兴趣的同学可以用C++封装起来使用,c/c++都可以使用,我当时用c而没用c++写就是因为这一点。
main.c
astar.h
astar.c
现在基本可以做成一个库了,有较强的灵活性。
估算函数预提供了三个。
生成路径也有在方法里提供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 functions pAStarCell astar_cell_init(float x_,float y_,short int cellType); //astar node functions struct 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_INCLUDED
astar.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; }
相关文章推荐
- C语言实现的计算一个文件下的所有文件的总大小(还有BUG,如果文件夹下面还有文件家就有问题,等待更新)
- ScrollView反弹效果实现[bug修正版]
- 话单采集测试(第一次更新修正用例部分,更新了程序Bug追加了一些功能)
- 话单采集测试(第一次更新修正用例部分,更新了程序Bug追加了一些功能)
- Android实现带下划线的EditText(BUG修正)
- 话单采集测试(第一次更新修正用例部分,更新了程序Bug追加了一些功能)
- MM图收最新更新 1.2.0.1 修正了无法启动的Bug
- 使用svnkit api,纯java操作svn,实现svn提交,更新等操作(修正版)
- 修正了版本管理实现中的一个bug
- MM图收最新更新 1.2.0.1 修正了无法启动的Bug
- 为MyOS实现了一个内存分配函数,并修正了GUI分配窗口的一个Bug
- 话单采集测试(第一次更新修正用例部分,更新了程序Bug追加了一些功能)
- MongoDB——更新操作(Update)c#实现
- JTable的使用示例,实现动态更新数据
- 实现.NET应用程序的自动更新
- Android 如何实现带滚动条的TextView,在更新文字时自动滚动到最后一行?
- jquery treeview插件在ie6上的bug修正
- C#实现程序的版本升级更新
- 【版本更新】Excel控件Spire.XLS for .NET V7.12发布 | 修复多个重大bug
- 钉钉环境下H5开发微应用遇到的问题和BUG(持续更新)