A星寻路算法C++实现

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