AStar寻路算法 c语言实现

 关于AStar的算法请参见网络.

在些大体介绍一下.

AStar算法的核心是估价函数,不同的估价函数可能会表现出不同的行为,因此效率也会有所不同.是一种启发式搜索.有一张open和close表,使用这两张表来确定哪些遍历过,确定下一个节点.

 

astar.h

 

/************************
## author:Dante Lee
## email:[email protected]
## qq:583035284
************************/
#ifndef ASTAR_H_INCLUDED
#define ASTAR_H_INCLUDED

#include
#include
#include

#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



// 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);
void astar_openList_reset(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_reset(pAStarCloseList pCloseList);
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

 

main.c

 

#include
#include
#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\0\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!");
    }

    end.x = 2;

    astar_path_cleanup(path);
    path= astar_pathList_init();

    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算法较为简单,对算法熟悉者一两个小时就可以弄懂并实现.

而且基本的astar算法掌握好之后,可以对此进行扩展比如蜂窝状寻路, 三维寻路等等.都是轻而易举的事情.