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

最新推荐文章于 2021-07-29 02:12:44 发布
最新推荐文章于 2021-07-29 02:12:44 发布
阅读量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
关注
专栏目录
A*算法(C语言版本)
02-27
A*算法c语言版本,可以在Windows和linux下应用,linux下使用命令"gcc -o astar astar.cpp -lm -lstdc++"编译
a*算法C实现
04-18
a*算法C语言具体实现
自动寻路算法C语言,C语言实现A*算法
weixin_33275503的博客
05-17 334
/********************************************************************************CopyRight(c)HYTCLtd.Allrightsreserved.*Filename:main.c*Creator:GaoLei*Version:0.0*Date:2011-06...
寻路算法c语言,A*寻路算法C 实现
weixin_39517199的博客
05-24 444
分为开列表和关列表，步骤如下：1.开始搜索，将起始节点压入开队列中，队列不为空时，从开队列中取出一个节点，寻找开始列表里代价最小的一个作为当前节点。判断该点是否是终点，如果是的话，结束，2.将当前点压入关闭列表，同时移出开队列，3.遍历当前节点的8领域内的节点，首先判断是否是墙或在关闭队列里面，如果不在，重新计算代价G和H，判断是否在开列表，如果在开队列中，且此次代价小于之前的，则更新代价值，并把...
A*算法C实现
nellson的专栏
10-04 4334
参考 http://www.cppblog.com/christanxw/archive/2006/04/07/5126.html 实现了A*算法，模拟了一下，大多数场景还是可以应对的，以传统的场景为例    以下，1为源，2为目的，-1为墙。   初始地图：  0
Python3 A*寻路算法实现方式
09-18
在Python3中实现A*寻路算法，我们需要以下几个关键组件： 1. **地图表示**：地图通常被表示为一个二维网格，每个单元格可以是可通行的（用0或空格表示）或障碍物（用非0字符表示）。在这个例子中，地图用字符串数组...
二叉堆实现A*寻路算法c语言实例
08-02
二叉堆实现A*寻路算法是计算机科学中一种经典的路径搜索方法，它结合了Dijkstra算法和优先级队列的特性，以高效的方式寻找从起点到目标点的最短路径。在这个C语言实例中，我们看到AStar.c、AStar.h、main.c和...
A*自动寻路算法实现（java）
03-08
下载本程序仅可演示A*自动寻路算法实现（java），该程序是基于我写的网络版贪吃蛇基础上编写的（网络版贪吃蛇下载地址：https://download.csdn.net/download/w110713121/15657669）。wasd键控制太阳的方向，鼠标左击...
JS实现的A*寻路算法详解
12-12
本文将详细解析使用JavaScript实现A*寻路算法的过程。 首先，我们需要理解A*算法的基本原理。A*算法的核心在于评估每个节点（在本例中是地图上的方块）的F值，该值由两部分组成：G值和H值。G值是从起点到当前节点的...
three.js算法寻路示例
10-28
总结来说，这个"three.js算法寻路示例"是一个综合项目，涉及到了3D图形编程、前端框架的应用以及路径规划算法的实现。通过学习和理解这个示例，开发者可以提升在WebGL、three.js、Vue和路径寻找算法方面的技能，这在...
A*算法源代码（C）
12-21
A*算法源代码
A*算法C/C++代码
07-30
的VS2013实现A*算法，包含2个工程，一个采用C++自带优先队函数实现OPEN表的插入删除及排序，一个用单链表实现。
A*和D*算法源代码(c文件)
03-24
实现A*和D*算法的源代码，可以直接编译运行，会在界面显示寻迹路径，用以学习深入了解算法原理。
A*算法 A star算法 C语言版
04-12
非常好的A*算法自己写的，里面基本每条代码都有注释，方便基础差的同学观看，另外附带了调试好的exe程序，下载就可执行！！你值得拥有，学了他，等同于一起学了特斯拉算法。请尊重版权！！1
astar实现代码(c)
12-19
Astar 最短路径寻优的代码实现，使用的是C语言
c语言实现a*算法
weixin_45137677的博客
12-22 8772
最近要实训需要实现迷宫，所以就先实现了之前一直总是了解算法思路的a算法。最开始我是想凭借自己对于这个寻路算法的理解，完全的按照自己的思路实现，最后结果感人，在实现函数的传递时候，才发现自己对于c语言函数参数传递的概念有些理解错误，导致一直有错误。再我测试了多次，终于发现是由于函数参数传递使用有误，在这里我分享以下自己所遇见的错误吧，并且把自己实现的a算法，所讲一下。 函数参数传递 c语言中的函数传...
❤️详解「 A*」算法原理及其算法实现(C/C++描述)
陈子青的博客
07-29 2938
定义和概念 A*算法，A*（A-Star)算法是一种静态路网中求解最短路径最有效的直接搜索方法，也是解决许多搜索问题的有效算法。算法中的距离估算值与实际值越接近，最终搜索速度越快（百度百科） 百科百科将A*算法定义为求解最短路径，我认为不够严谨。 A*算法较之传统的路径规划算法,实时性更高、灵活性更强,寻路结果更加接近人工选择的路径结果. A*寻路算法并不是找到最优路径，只是找到相对近的路径，因为找最优要把所有可行路径都找出来进行对比，消耗性能太大，寻路效果只要相对近路径就行了。 也因此在静
A星寻路算法简易版(代码实现)
Qinhaifu的博客
07-26 3611
绿色为起点，红色为终点，蓝色为墙不可走。启动只能上下左右不可以斜着走。 以下为A*寻路算法代码实现，核心思想就是只走F值(格子评估值)最小的格子，直至到达终点。 F值最小代表着从起点出发到达终点需要的最少的步数，只走最少的步数，那么路径就是最短的。 #include <iostream> #include <vector> #include <string>...
A*算法解决八数码问题（C++类改进版）
Type真是太帅了
05-05 8797
八数码问题：    在一个3*3的棋盘中，分别用1,2,3,...,8表示八个数码方格，用0表示空缺的方格，现给出一个初始状态和目标状态，寻找出在评估函数f(n)=g(n)+h(n)的限制下，以最少的步数到达目标状态（一次将一个数码方格移动到空缺的方格中）；其中，g(n)表示n节点与目标状态的“距离”，h(n)表示n节点与初始状态的距离。现在令g(n)为当前节点n与目标状态数码方格位置不同的个数，...
three.js实现A*算法寻路的Vue示例
本资源是一个基于three.js和vue框架的算法避障寻路示例，具体实现了三种常用的寻路算法：A*算法、深度优先搜索和广度优先搜索。Three.js是一个基于WebGL的JavaScript库，允许开发者在网页上使用3D图形；Vue是一个...
评论 1
添加红包

请填写红包祝福语或标题

个

红包个数最小为10个

元

红包金额最低5元

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

抵扣说明：

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

余额充值