A*寻路算法

//astar.h

#ifndef __ASTAR_H
#define __ASTAR_H

#include
#include
#include
#include
#include

using std::vector;
using std::make_heap;
using std::sort_heap;

/* for debug. */
#ifdef DEBUG
#define PRINTF(info) do {/
        printf(info);/
    }while(0)
#else
#define PRINTF(info)
#endif

typedef struct _position_t {
    int x;
    int y;

    _position_t() : x(0), y(0) {}
    _position_t(int a, int b) : x(a), y(b) {}
} position_t;

typedef struct _node_t {
    position_t pos;
    int f;    /* function of evaluate for desion. */
    int g;    /* cost had expended. */
    int h;    /* cost be evaluate. */
    struct _node_t* prev;
    struct _node_t* next;

    _node_t() : f(0), g(0), h(0), prev(NULL),next(NULL) {}

    _node_t(int x, int y) : f(0), g(0), h(0), prev(NULL), next(NULL) {
        pos.x = x;
        pos.y = y;
    }

    _node_t(position_t position) :  f(0), g(0), h(0), prev(NULL),next(NULL) {
        pos.x = position.x;
        pos.y = position.y;
    }

    void evaluate(_node_t *dest) {
        dest->h = (pos.x - dest->pos.x) * (pos.x - dest->pos.x) + (pos.y - dest->pos.y) * (pos.y - dest->pos.y);
        dest->f = dest->h + dest->g;
    }

    bool is_equal(_node_t * another) {
        assert(NULL != another);
        return ( (pos.x == another->pos.x) && (pos.y == another->pos.y) );
    }
} node_t;

typedef struct _cmp_t {
    bool operator() (node_t *a, node_t *b) {
        return a->f < b->f;
    }
}cmp_t;   

typedef struct _priority_queue_t {
    vector list;
    cmp_t cmp;

    void update() {
        sort_heap( list.begin(), list.end(), cmp );
    }

    void push(node_t * node) {
        list.push_back(node);
        push_heap( list.begin(), list.end(), cmp );
    }

    node_t *top() {
         return list.front();
    }

    void pop(){
         pop_heap( list.begin(), list.end(), cmp );
         list.pop_back();
    }

    bool empty() {
        return list.empty();
    }

    bool find(node_t * node) {
        assert(NULL != node);
        for(vector::iterator it = list.begin(); it != list.end(); ++it) {
            if( (*it)->is_equal(node) ) {
                return true;
            }
        }
       
        return false;
    }

    _priority_queue_t() {
        make_heap( list.begin(), list.end(), cmp );
    }
     
} priority_queue_t;

typedef struct _map_size_t {
    int max_row;
    int max_col;
    _map_size_t() : max_row(0), max_col(0) {}
    _map_size_t(int x, int y) : max_row(x), max_col(y) {}
} map_size_t;

typedef priority_queue_t open_t;
typedef vector closed_t;
typedef bool (*is_availiable_f)(int x, int y);

typedef enum {
      DRICTION_FOUR = 4
    , DRICTION_EIGHT = 8
}direction_type_t;

#define ERROR_UNKNOWN_DRICTION  (0x0001)
#define ERROR_OUT_OF_MEMORY        (0x0002)

int astar_init_sys(map_size_t max_map_size, direction_type_t direction_type);
int astar_search(position_t start, position_t end, is_availiable_f is_availiable);
void astar_deinit_sys();
#endif

 

//astar.cpp

#include "astar.h"

#define X_DRICTION 0
#define Y_DRICTION 1

typedef struct _map_info_t {
    node_t node;
    int in_open;
    int in_closed;
} map_info_t;

#define SET_INOPEN(x) ((((map_info_t *)x)->in_open) = s_search_id)
#define SET_INCLOSED(x) ((((map_info_t *)x)->in_closed) = s_search_id)

static map_info_t *s_map_info;

/*left, top, right, bottom*/
static const int s_xy4_delta[2][DRICTION_FOUR] = { {-1, 0, 1, 0}
                                                                                  , {0, 1, 0, -1}
};

/*left, left-top, top, top-right, right, right-bottom, bottom, bottom-left*/
static const int s_xy8_delta[2][DRICTION_EIGHT] = { {-1, -1, 0, 1, 1, 1, 0, -1 }
                                                                                    ,{0, 1, 1, 1, 0, -1, -1, -1}
};

static int s_xy_delta[2][8];

/*seach id */
static int s_search_id;

static direction_type_t s_direction_type;

static map_size_t s_map_size ;
/**
*
*/
int astar_init(open_t &open, position_t &start, position_t &end) {
    node_t *start_node = &(s_map_info[start.x * s_map_size.max_col + start.y].node);
    start_node->pos.x = start.x;
    start_node->pos.y = start.y;
    start_node->g = 0;
    start_node->evaluate( &(s_map_info[end.x * s_map_size.max_col + end.y].node) );
    start_node->prev = NULL;
    start_node->next = NULL;
    open.push(start_node);
    SET_INOPEN(start_node);
    return 0;
}

node_t *get_nearby_node(node_t *c_node, int delta_x, int delta_y) {
    if(c_node->pos.x + delta_x < 0 || c_node->pos.y + delta_y < 0) {
        return NULL;
    }

    return &s_map_info[(c_node->pos.x + delta_x) * s_map_size.max_col +(c_node->pos.y + delta_y) ].node;   
}

bool is_node_in_open(node_t *node) {
    return (s_map_info[node->pos.x * s_map_size.max_col + node->pos.y].in_open == s_search_id);
}

bool is_node_in_closed(node_t *node) {
    node_t *t_node = &s_map_info[node->pos.x * s_map_size.max_col + node->pos.y].node;
    return (s_map_info[node->pos.x * s_map_size.max_col + node->pos.y].in_closed == s_search_id);
}

void trace_back(node_t *node) {
    node_t * t_node = node->prev;
    if( NULL != t_node ) {
        t_node->next = node;
        char t[100];
        sprintf(t, "x: %d, y: %d/n", node->pos.x, node->pos.y);
        PRINTF(t);
        trace_back(t_node);
    }
}

bool is_node_unaviliable(node_t *node, is_availiable_f is_availiable) {
    if( is_availiable(node->pos.x, node->pos.y) ) {
        return false;
    }

    return true;
}

int astar_init_sys(map_size_t max_map_size, direction_type_t direction_type) {
    if( NULL != s_map_info) {
        astar_deinit_sys();
        PRINTF("WARNNING: astar_deinit_sys missed!/n");
    }

    int e_size =  max_map_size.max_row * max_map_size.max_col;
    s_map_info = (map_info_t *) malloc( sizeof(map_info_t) * e_size );
    if( NULL == s_map_info) {
        PRINTF("ERROR: out of memory!/n");
        return ERROR_OUT_OF_MEMORY;
    }

    s_map_size.max_row = max_map_size.max_row;
    s_map_size.max_col = max_map_size.max_col;

    for(int i = 0; i != e_size; ++i) {
        s_map_info[i].node.pos.x = i / max_map_size.max_col;
        s_map_info[i].node.pos.y = i % max_map_size.max_col;
        s_map_info[i].in_open = 0;
        s_map_info[i].in_closed = 0;
    }

    if( DRICTION_FOUR == direction_type ) {
        s_xy_delta[0][0] =  s_xy4_delta[0][0];
        s_xy_delta[0][1] =  s_xy4_delta[0][1];
        s_xy_delta[0][2] =  s_xy4_delta[0][2];
        s_xy_delta[0][3] =  s_xy4_delta[0][3];
        s_xy_delta[1][0] =  s_xy4_delta[1][0];
        s_xy_delta[1][1] =  s_xy4_delta[1][1];
        s_xy_delta[1][2] =  s_xy4_delta[1][2];
        s_xy_delta[1][3] =  s_xy4_delta[1][3];
    } else if ( DRICTION_EIGHT == direction_type ){
        memcpy( s_xy_delta, s_xy8_delta, sizeof(s_xy8_delta) );
    } else {
        return ERROR_UNKNOWN_DRICTION;
    }
    s_direction_type = direction_type;
    return 0;
}

void astar_deinit_sys() {
    if(NULL != s_map_info) {
        free(s_map_info);
        s_map_info = NULL;
    }
    s_search_id = 0;
}

int astar_search(position_t start, position_t end, is_availiable_f is_availiable) {
    open_t open;
    closed_t closed;
    node_t *c_node = NULL;
    node_t *e_node = &s_map_info[end.x * s_map_size.max_col + end.y].node;
    node_t *t_node = NULL;
   
    ++s_search_id;

    astar_init(open, start, end);

    PRINTF("astar start!/n");

    while( !open.empty() ) {
        c_node = open.top();
        if( c_node->is_equal(e_node) ) {
            trace_back(c_node);
            printf("search path success!/n");
                node_t *w = &s_map_info[start.x * s_map_size.max_col + start.y].node;
    while(NULL != w->next)
    {
        printf("%d, %d/n", w->pos.x, w->pos.y);
        w = w->next;
    }
    printf("%d, %d/n", w->pos.x, w->pos.y);
            return 0;
       }
      
        open.pop();

        closed.push_back(c_node);
        SET_INCLOSED(c_node);

        for(int i = 0; i < s_direction_type; ++i ) {
            t_node = get_nearby_node(c_node, s_xy_delta[X_DRICTION][i], s_xy_delta[Y_DRICTION][i]);
            if(NULL == t_node) {
                continue;
            }
         
            if( is_node_in_closed(t_node) || is_node_unaviliable(t_node, is_availiable) ) {
                continue;
            }

            t_node->evaluate(e_node);
            if( is_node_in_open(t_node) ) {
                if(c_node->g + 1 < t_node->g) {
                    t_node->g = c_node->g + 1;
                    t_node->f = t_node->g + t_node->h;
                    t_node->prev = c_node;
                    open.update();
                }
            } else {
                t_node->g = ++c_node->g;
                t_node->evaluate(e_node);
                t_node->prev = c_node;
                open.push(t_node);
                SET_INOPEN(t_node);
            }
        } /* end of for. */
    }
   
    printf("search path fail!/n");
    return -1;
}

 

//test.cpp

#include "astar.h"

int s_array[6][8] = {
     {1, 1, 0, 1, 1, 1, 1, 1}
    ,{0, 1, 1, 1, 1, 1, 1, 1}
    ,{1, 1, 0, 0, 1, 1, 1, 1}
    ,{1, 1, 0, 1, 1, 1, 1, 1}
    ,{1, 1, 1, 0, 1, 1, 0, 0}
    ,{1, 1, 0, 1, 1, 1, 1, 1}
};

bool is_availiable(int x, int y)
{
    if(x < 0 || x > 5 || y < 0 || y >7)
        return false;
    return s_array[x][y];
}

int main()
{
     position_t start(0, 0);
     position_t end(5, 7);
     map_size_t max_map_size(100, 100);
     direction_type_t type = DRICTION_FOUR;
     astar_init_sys(max_map_size, type);
     astar_search(start,  end,  is_availiable);
     astar_deinit_sys();

     return 0;
}