A*寻路算法的C++实现,共两个文件 astar.h astar.cpp
代码如下
// astar.h BEGIN
#ifndef ASTAR_H
#define ASTAR_H
#include <stdio.h>
#include <vector>
#include <set>
// 地图格子数据结构
struct grid_t
{
int id; // grid id {1,100}
int x; // x location {0,9}
int y; // y location {0,9}
int g; // g value 10{front, back, left, right}
//14{east_north, east_south, west_east, west_south}
int h; // h value ( diffx + diffy ) * 10
int block; // if available
int total; // total = g + h*10
void show(){
printf("{%d,%d,%d,%d,%d,%d,%d}, ", id, x, y, g, h, block, total);
}
};
// 地图格子信息 规格 10*10
extern std::vector<grid_t> map;
// 地图初始化 大小: 10*10 以左下角(0,0)作为坐标起点
void init_map();
//经过的格子编号 因有先后顺序 因此需要用vector存储
extern std::vector<int> path_grid_id;
grid_t* find(int x, int y);
#endif
// astar.h END
// astar.cpp BEGIN
#include <sys/time.h>
#include <stdlib.h>
#include <time.h>
#include "astar.h"
std::vector<grid_t> map;
std::vector<int> path_grid_id;
int start = 12;
int end = 78;
int abs(int val)
{
if( val >= 0 ){
return val;
}
return -val;
}
//grid_t* end_grid = &map[end-1];
grid_t* end_grid = NULL;
void init_map()
{
map.clear();
for( int i=1; i<=100; i++ ){
grid_t grid;
grid.id = i;
grid.x = (i-1) % 10;
grid.y = (i - 1) / 10;
grid.h = 0;
grid.g = 0;
grid.block = 0;
grid.total = 0;
// 设置障碍
if( grid.x==4 && grid.y >= 3 && grid.y <= 6 ){
grid.block = 1;
}
map.push_back(grid);
}
}
struct id_val_t
{
int id;
int val;
};
grid_t* find(int x, int y)
{
if( x < 0 || x > 9 ){
return NULL;
}
if( y < 0 || y > 9 ){
return NULL;
}
int id = y*10 + (x+1);
if( id <= 0 || id > map.size() ){
return NULL;
}
return &map[id-1];
}
grid_t* find_next_grid(int id)
{
if( id <= 0 || id > 100 ){
return NULL;
}
grid_t* grid = &map[id-1];
// 8 grid around
//path_grid_id.push_back(id);
// all ready find the path
for( int i=0; i<path_grid_id.size(); i++ ){
if( path_grid_id[i] != end ){
continue;
}
return NULL;
}
std::vector<id_val_t> vec_id_val;
int loc[][2] = {
{grid->x+1, grid->y},
{grid->x-1, grid->y},
{grid->x, grid->y+1},
{grid->x, grid->y-1},
{grid->x+1, grid->y+1},
{grid->x+1, grid->y-1},
{grid->x-1, grid->y+1},
{grid->x-1, grid->y-1}
};
for( int i=0; i<8; i++ ){
int loc_x = loc[i][0];
int loc_y = loc[i][1];
grid_t* find_grid = find(loc_x, loc_y);
if( NULL == find_grid ){
continue;
}
if( find_grid->block == 1 ){
continue;
}
}
//front grid
grid_t* find_grid = find(grid->x+1, grid->y);
if( NULL != find_grid && find_grid->block == 0 ){
find_grid->g = 10;
find_grid->h = abs(end_grid->y - find_grid->y) + abs(end_grid->x - find_grid->x);
find_grid->total = find_grid->h * 10 + find_grid->g;
id_val_t id_val;
id_val.id = find_grid->id;
id_val.val = find_grid->total;
vec_id_val.push_back(id_val);
}
//back grid
find_grid = find(grid->x-1, grid->y);
if( NULL != find_grid && find_grid->block == 0 ){
find_grid->g = 10;
find_grid->h = abs(end_grid->y - find_grid->y) + abs(end_grid->x - find_grid->x);
find_grid->total = find_grid->h * 10 + find_grid->g;
id_val_t id_val;
id_val.id = find_grid->id;
id_val.val = find_grid->total;
vec_id_val.push_back(id_val);
}
//left grid
find_grid = find(grid->x, grid->y+1);
if( NULL != find_grid && find_grid->block == 0 ){
find_grid->g = 10;
find_grid->h = abs(end_grid->y - find_grid->y) + abs(end_grid->x - find_grid->x);
find_grid->total = find_grid->h * 10 + find_grid->g;
id_val_t id_val;
id_val.id = find_grid->id;
id_val.val = find_grid->total;
vec_id_val.push_back(id_val);
}
//right grid
find_grid = find(grid->x, grid->y-1);
if( NULL != find_grid && find_grid->block == 0 ){
find_grid->g = 10;
find_grid->h = abs(end_grid->y - find_grid->y) + abs(end_grid->x - find_grid->x);
find_grid->total = find_grid->h * 10 + find_grid->g;
id_val_t id_val;
id_val.id = find_grid->id;
id_val.val = find_grid->total;
vec_id_val.push_back(id_val);
}
//east-north
find_grid = find(grid->x+1, grid->y+1);
if( NULL != find_grid && find_grid->block == 0 ){
find_grid->g = 14;
find_grid->h = abs(end_grid->y - find_grid->y) + abs(end_grid->x - find_grid->x);
find_grid->total = find_grid->h * 10 + find_grid->g;
id_val_t id_val;
id_val.id = find_grid->id;
id_val.val = find_grid->total;
vec_id_val.push_back(id_val);
}
//east-south
find_grid = find(grid->x+1, grid->y+1);
if( NULL != find_grid && find_grid->block == 0 ){
find_grid->g = 14;
find_grid->h = abs(end_grid->y - find_grid->y) + abs(end_grid->x - find_grid->x);
find_grid->total = find_grid->h * 10 + find_grid->g;
id_val_t id_val;
id_val.id = find_grid->id;
id_val.val = find_grid->total;
vec_id_val.push_back(id_val);
}
//west-north
find_grid = find(grid->x-1, grid->y+1);
if( NULL != find_grid && find_grid->block == 0 ){
find_grid->g = 14;
find_grid->h = abs(end_grid->y - find_grid->y) + abs(end_grid->x - find_grid->x);
find_grid->total = find_grid->h * 10 + find_grid->g;
id_val_t id_val;
id_val.id = find_grid->id;
id_val.val = find_grid->total;
vec_id_val.push_back(id_val);
}
//west-south
find_grid = find(grid->x-1, grid->y-1);
if( NULL != find_grid && find_grid->block == 0 ){
find_grid->g = 14;
find_grid->h = abs(end_grid->y - find_grid->y) + abs(end_grid->x - find_grid->x);
find_grid->total = find_grid->h * 10 + find_grid->g;
id_val_t id_val;
id_val.id = find_grid->id;
id_val.val = find_grid->total;
vec_id_val.push_back(id_val);
}
int tid = vec_id_val[0].id;
int tval = vec_id_val[0].val;
for( int i=0; i<vec_id_val.size(); i++ ){
if( vec_id_val[i].val >= tval ){
continue;
}
tid = vec_id_val[i].id;
tval = vec_id_val[i].val;
}
return &map[tid-1];
//
}
int main(int argc, char* argv[])
{
init_map();
for( int i=0; i<100; i++ ){
if( i % 5 == 0 ){
printf("\n");
}
map[i].show();
// set grid 74 block
if( i>= 73 && i<= 75 ){
map[i].block = 1;
}
}
end_grid = &map[end-1];
int round = 1;
if( argc >= 2 ){
round = atoi(argv[1]);
}
struct timeval tv1;
struct timeval tv2;
gettimeofday(&tv1, NULL);
int time1 = time(NULL);
for( int round_num = 0; round_num < round; round_num++ ){
path_grid_id.clear();
path_grid_id.push_back(start);
int grid_id = start;
while(true){
grid_t* grid = find_next_grid(grid_id);
if( NULL == grid ){
break;
}
path_grid_id.push_back(grid->id);
bool is_over = false;
for( int i=0; i<path_grid_id.size(); i++ ){
if(path_grid_id[i] != end_grid->id){
continue;
}
is_over = true;
break;
}
if( is_over ){
break;
}
grid_id = grid->id;
}
}
int time2 = time(NULL);
gettimeofday(&tv2, NULL);
printf("path_grid_id size: %d\n", path_grid_id.size());
for( int i=0; i<path_grid_id.size(); i++ ){
printf("grid_id:%d\n", path_grid_id[i]);
}
printf("round:%d, use:%d\n", round, time2-time1);
printf("tv1_sec:%d, tv1_usec:%d\n", tv1.tv_sec, tv1.tv_usec);
printf("tv2_sec:%d, tv2_usec:%d\n", tv2.tv_sec, tv2.tv_usec);
return 0;
}
// astar.cpp END
编译
$ g++ -g -o hello astar.cpp
测试
$ ./hello [round]
精确到微秒, 测试结果:
执行1次
round:1 use:0
tv1_sec:1506153329, tv1_usec:920483
tv2_sec:1506153329, tv2_usec:920516
时间为920516-920483=33微秒
执行1W次
round:10000, use:0
tv1_sec:1506154327, tv1_usec:283682
tv2_sec:1506154327, tv2_usec:770319
执行10W次
round:100000, use:2
tv1_sec:1506154390, tv1_usec:71109
tv2_sec:1506154392, tv2_usec:666823
round:1000000, use:25
tv1_sec:1506154417, tv1_usec:960088
tv2_sec:1506154442, tv2_usec:562360