//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;
}