devbox
小舞很执着
这个屌丝很懒，什么也没留下！
关注作者
热门标签
热门文章
当前位置:   article > 正文

A星寻路算法C++实现_a星 路径算法,可编辑路径 源码c++

作者：小舞很执着 | 2024-07-19 02:01:19

赞

踩

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



声明：本文内容由网友自发贡献，不代表【wpsshop博客】立场，版权归原作者所有，本站不承担相应法律责任。如您发现有侵权的内容，请联系我们。转载请注明出处：https://www.wpsshop.cn/w/小舞很执着/article/detail/848869
推荐阅读
相关标签

Copyright © 2003-2013 www.wpsshop.cn 版权所有，并保留所有权利。

  

闽ICP备14008679号