A*寻路算法c语言实现(2012.3.7更新，修正bug)

最新推荐文章于 2024-02-11 09:56:35 发布
最新推荐文章于 2024-02-11 09:56:35 发布
阅读量4.2k 收藏
点赞数 2
分类专栏： C++ 算法 文章标签： 算法 语言 c struct null float
版权声明：本文为博主原创文章，遵循 CC 4.0 BY-SA 版权协议，转载请附上原文出处链接和本声明。
本文链接：https://blog.csdn.net/bhlzlx/article/details/7322495
版权

一天写了千行代码。。。

现在基本可以做成一个库了，有较强的灵活性。

估算函数预提供了三个。

生成路径也有在方法里提供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;
}






icefox
关注
  • 2
    点赞
  • 踩
  • 0
    收藏
    觉得还不错? 一键收藏
  • 1
    评论
专栏目录
A*算法(C语言版本)
02-27
A*算法c语言版本,可以在Windows和linux下应用,linux下使用命令"gcc -o astar astar.cpp -lm -lstdc++"编译
c语言实现a*算法
weixin_45137677的博客
12-22 8471
最近要实训需要实现迷宫，所以就先实现了之前一直总是了解算法思路的a算法。最开始我是想凭借自己对于这个寻路算法的理解，完全的按照自己的思路实现，最后结果感人，在实现函数的传递时候，才发现自己对于c语言函数参数传递的概念有些理解错误，导致一直有错误。再我测试了多次，终于发现是由于函数参数传递使用有误，在这里我分享以下自己所遇见的错误吧，并且把自己实现的a算法，所讲一下。 函数参数传递 c语言中的函数传...
C语言经典算法之A*搜索算法
最新发布
weixin_56154577的博客
02-11 2091
A*搜索算法（A-Star Search Algorithm）是一种启发式搜索算法，主要用于在图形或网络结构中寻找从起始节点到目标节点的最短路径。它结合了Dijkstra算法确定性的最优性（保证找到的是实际最短路径）和Best-First Search（最佳优先搜索）的高效性（通过启发式信息指导搜索方向），在保证找到最优解的同时尽可能减少搜索范围。
C语言实现A*算法
weixin_33726313的博客
06-17 808
最近搞MTK斯凯冒泡平台的游戏开发，碰到了自动寻路的问题，很多程序员都知道A*算法，既简单有使用！ 所以我也选择了A*算法，由于时间比较紧，就在网上百度此算法的C实现，确实有很多！ 但经测试都有不同的问题，并不能用在商业游戏中，所以最后决定还是自己写吧！ A*原理 比较简单，网上有很多介绍的！我也是在网上看的，这里就不重复了！ 由于我是J...
A*算法简单实现（C语言）
weixin_42352795的博客
04-10 3269
简单的A*算法，后续考虑更复杂情况，一个C文件即可，VS，linux等都可以直接编译运行。后面首先考虑加上图形化界面（地图可以自己输入确定也考虑下） A*算法原理 这个很简单就不多说了，根据f = g + h。像是欧几里得距离，绝对距离等等，还有周围8节点以及4节点（16节点）对算法的影响也都是同样的道理，简单修改即可 算法流程 //算法流程 //构建地图 //把起点放入open list //w...
配置文件分析函数 基于C++ STL
bhlzlx的专栏
01-21 993
给一个配置文件:比如http服务器发来的header信息 HTTP/1.1 200 OK Date: Sat, 21 Jan 2012 04:49:14 GMT Server: Apache/2.2.20 (Ubuntu) Last-Modified: Sat, 03 Sep 2011 11:05:41 GMT ETag: "28f96cd-4b500a-4ac07756b656f" Accep
Python3 A*寻路算法实现方式
09-18
今天小编就为大家分享一篇Python3 A*寻路算法实现方式，具有很好的参考价值，希望对大家有所帮助。一起跟随小编过来看看吧
A*自动寻路算法实现（java）
03-08
下载本程序仅可演示A*自动寻路算法实现（java），该程序是基于我写的网络版贪吃蛇基础上编写的（网络版贪吃蛇下载地址：https://download.csdn.net/download/w110713121/15657669）。wasd键控制太阳的方向，鼠标左击...
二叉堆实现A*寻路算法c语言实例
08-02
此代码包含多个文件，AStar.c、 AStar.h、main.c以及makefile相关文件，例子默认是在linux下编译实现，也可直接将三个代码文件移植到window开发平台下编译实现
three.js算法寻路示例
10-28
居于three.js+vue的算法避障寻路示例,A*算法，深度优先，广度优先搜索
unityAStar寻路 A星 A*寻路算法Demo
04-25
基于Unity5.4.4版本，随机障碍物，动态实现寻路，UnityA星寻路完整Demo
A星算法 c语言实现 a*算法
02-11
A星算法 用c语言实现 用到了队列 a*算法 A星算法 用c语言实现 用到了队列 a*算法
a*算法C实现
04-18
a*算法C语言具体实现
A*算法 A star算法 C语言版
04-12
非常好的A*算法自己写的，里面基本每条代码都有注释，方便基础差的同学观看，另外附带了调试好的exe程序，下载就可执行！！你值得拥有，学了他，等同于一起学了特斯拉算法。请尊重版权！！1
mfc vc c++ navmesh自动寻路（改）
09-18
平面构建网格实现自动寻路，navmesh （修复不能直接编译问题）
人工智能 八数码问题 A*算法 C语言
05-09
人工智能实验 A*算法 算法可能不完美但是自己一点点写出来的 希望能对你有所启发
A* A星 算法 C语言 实现代码
jishublog
05-26 1601
关于A*算法，很早就想写点什么，可是貌似天天在忙活着什么，可事实又没有做什么，真是浮躁啊！所以今晚还是来写一下总结吧！ A*算法是很经典的只能启发式搜索算法，关于只能搜索算法和一般的搜索算法(例如DFS，BFS之类)，在语言描述上的区别，我觉得用《代码大全》中的一句话描述的非常好：“驾驶汽车达到某人家，写成算法是：沿167号高速往南行至Puyallup，从XX出口后往山上开4.5英里，在...
A*算法C实现
weixin_44586502的博客
02-04 1151
A*算法的C语言实现： #include<stdio.h> #include<math.h> #define MaxLength 100 //用于优先队列（Open表）的数组 #define Height 15 //地图高度 #define Width 20 //地图宽度 #define Reachable 0 //可以到达...
C语言的A*算法
qian200801的博客
12-04 1614
毕业论文中需过用到路径优化和诱导的算法，网上看到很多资料，原理是简单的，代码实现颇为不易，不过站在巨人的肩膀上还是容易的，借他人之石攻玉，再次总结一下。 **************************************************************************************************************************...
A*寻路算法的实现
05-25
以下是一个基本的A*寻路算法的实现，其中使用了一个优先队列来存储待扩展的节点。 ``` class Node: def __init__(self, x, y): self.x = x self.y = y self.g = 0 self.h = 0 self.f = 0 self.parent = None...

“相关推荐”对你有帮助么？

  • 非常没帮助
  • 没帮助
  • 一般
  • 有帮助
  • 非常有帮助
提交
评论 1
添加红包

请填写红包祝福语或标题

个

红包个数最小为10个

元

红包金额最低5元

当前余额3.43元 前往充值 >
需支付：10.00元
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付元
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明：

1.余额是钱包充值的虚拟货币，按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载，可以购买VIP、付费专栏及课程。

余额充值