A星(AStar)算法的实现

关于AStar的原理这里简述一下，

首先有一张地图，然后准备一个open list 和 close list，open list存放所有可能的路径，但是需要注意的是这个列表是动态怎加的，也就是每走一步就把当前可能的路径都加进去，然后每次从open list中去除一个最小代价的点，最为下一步的路径，并用该点计算之后可能的路径在加入到open list中去，然后把这个代价最小的点放到close list中，表示该点已经走过了，如此循环直到从open list取出的最小代价点为终点，此时通过回溯找到路径；代价的计算方法具有多样性，如何计算可以自行百度；

首先是python的实现：

import numpy as np
import heapqclass Node:def __init__(self, coord, parent=None, g=0, h=0):self.coord = coordself.parent: Node = parentself.g = gself.h = hself.f = g + hself.master = 0# self.iter = self.gen_data()# def gen_data(self):#     cur_node = self#     while 1:#         if cur_node is None:#             break#         yield cur_node#         cur_node = cur_node.parent## def __iter__(self):#     return self.iterdef __gt__(self, other):return self.f > other.fdef __lt__(self, other):return self.f < other.fdef __eq__(self, other):return self.f == other.fdef __str__(self):return "<(%s,%s) G: %s,H: %s,F: %s> " % (self.coord[0], self.coord[1], self.g, self.h, self.f)__repr__ = __str__class Walker:def __init__(self, node):self.node = nodeself.iter = self.gen_data()def gen_data(self):cur_node = self.nodewhile 1:if cur_node is None:breakyield cur_nodecur_node = cur_node.parentdef __iter__(self):return self.iterclass AStar:def __init__(self, world_map, bar_value, direct_cost, oblique_cost):self.world_map = world_mapself.bar_value = bar_valueself.direct_cost = direct_costself.oblique_cost = oblique_costself.map_cache = np.zeros_like(world_map)def new_node(self, prev_node: Node, end_node, x, y, weight):coord = x, yheight, width = self.world_map.shapeif x < 0 or x >= width or y < 0 or y >= height:returncolor = self.world_map[y, x]if color == self.bar_value:returnH = abs(end_node.coord[0] - x) * self.direct_cost + abs(end_node.coord[1] - y) * self.direct_costG = prev_node.g + weightt_node = Node(coord, prev_node, G, H)return t_nodedef get_neighbors(self, p_node, end_node):coord = p_node.coordup = self.new_node(p_node, end_node, coord[0], coord[1] - 1, self.direct_cost)down = self.new_node(p_node, end_node, coord[0], coord[1] + 1, self.direct_cost)left = self.new_node(p_node, end_node, coord[0] - 1, coord[1], self.direct_cost)right = self.new_node(p_node, end_node, coord[0] + 1, coord[1], self.direct_cost)# return up, down, left, rightleft_up = self.new_node(p_node, end_node, coord[0] - 1, coord[1] - 1, self.oblique_cost)right_up = self.new_node(p_node, end_node, coord[0] + 1, coord[1] - 1, self.oblique_cost)left_down = self.new_node(p_node, end_node, coord[0] - 1, coord[1] + 1, self.oblique_cost)right_down = self.new_node(p_node, end_node, coord[0] + 1, coord[1] + 1, self.oblique_cost)return up, down, left, right, left_up, right_up, left_down, right_downdef find_path(self, start_node, end_node):open_ls = []close_ls = []self.map_cache[:, :] = 0heapq.heappush(open_ls, start)self.map_cache[start_node.coord[1], start_node.coord[0]] = 1while 1:if len(open_ls) == 0:print("failed!")breakcur_node: Node = heapq.heappop(open_ls)if cur_node.coord == end_node.coord:print("success")return cur_nodefor node in self.get_neighbors(cur_node, end_node):if node is None:continueif self.map_cache[node.coord[1], node.coord[0]] != 0:continueheapq.heappush(open_ls, node)self.map_cache[node.coord[1], node.coord[0]] = 1close_ls.append(cur_node)self.map_cache[cur_node.coord[1], cur_node.coord[0]] = 2# #
if __name__ == '__main__':import timeimport cv2default_bar_value = 70default_set_path = 255DIRECT_WEIGHT = 10OBLIQUE_WEIGHT = 14# create a mapmaps = np.zeros((650, 750), np.intc) + 1maps[40:, 20:30] = default_bar_valuemaps[:400, 100:110] = default_bar_valuemaps[100:, 200:210] = default_bar_valuemaps[:200, 300:310] = default_bar_valuemaps[220:230, 210:710] = default_bar_valuemaps[220:600, 710:720] = default_bar_valuemaps[600:610, 300:720] = default_bar_valuemaps[300:610, 300:310] = default_bar_valuestart = Node((10, 10))  # start coordend = Node((600, 400))  # end coordfinder = AStar(maps, default_bar_value, DIRECT_WEIGHT, OBLIQUE_WEIGHT)t0 = time.time()node = finder.find_path(start, end)print("耗时:", time.time()-t0)for node in Walker(node):maps[node.coord[1], node.coord[0]] = default_set_pathmaps = maps.astype(np.uint8)maps = maps.reshape((*maps.shape, 1))maps = maps.repeat(3, 2)cv2.circle(maps, tuple(start.coord), 5, (0, 255, 0), 5)cv2.circle(maps, tuple(end.coord), 5, (255, 0, 0), 5)maps[maps[:, :, 0] == default_set_path] = 50, 255, 50maps[maps[:, :, 0] == default_bar_value] = 0, 0, 255cv2.imshow("result", maps)cv2.waitKey()cv2.destroyAllWindows()

需要注意的这里为了提高性能，省去了一些步骤，所以个标准的算法略微区别

一下是c++的实现：

// dllmain.cpp : 定义 DLL 应用程序的入口点。
#include "pch.h"
#include <iostream>
#include <algorithm>
#include <set>
#include <vector>using namespace std;struct Coord
{int x;int y;bool operator==(const Coord& c) {return x == c.x && y == c.y;}Coord() {x = 0;y = 0;}Coord(int x, int y) {this->x = x;this->y = y;}
};struct Point
{int x;int y;Point* next;Point(int x, int y) {this->x = x;this->y = y;next = nullptr;}
};struct Node
{Coord coord;Node* parent;int g;int h;int f;Node(Coord coord) {this->coord.x = coord.x;this->coord.y = coord.y;this->parent = 0;this->g = 0;this->h = 0;this->f = 0;}Node(Coord coord, Node* parent) {this->coord.x = coord.x;this->coord.y = coord.y;this->parent = parent;this->g = 0;this->h = 0;this->f = 0;}Node(Coord coord, Node* parent, int g, int h) {this->coord.x = coord.x;this->coord.y = coord.y;this->parent = parent;this->g = g;this->h = h;this->f = g + h;}
};class HeapCompare_f
{
public:bool operator() (const Node* x, const Node* y) const{return x->f > y->f;}
};class AStar
{
public:int* world_map;int width;int height;int bar_value;int direct_cost;int oblique_cost;int set_path_value;Node** neighbors;Node** map_cache;vector<Node*> open_ls;vector<Node* > close_ls;AStar(int* world_map, int width, int height, int bar_value, int dircect_cost, int oblique_cost, int set_path_value);~AStar();Node* new_node(Node* parent, Coord end, int x, int y, int cost);void get_neighbors(Node* parent, Coord coord);Point* find_path(Coord start, Coord end);void free_ls();
};AStar::AStar(int* world_map, int width, int height, int bar_value, int direct_cost, int oblique_cost, int set_path_value)
{this->world_map = world_map;this->width = width;this->height = height;this->bar_value = bar_value;this->direct_cost = direct_cost;this->oblique_cost = oblique_cost;this->map_cache = new Node * [(size_t)width * (size_t)height];this->neighbors = new Node * [8];this->set_path_value = set_path_value;
}AStar::~AStar()
{delete[] map_cache;delete[] neighbors;
}Node* AStar::new_node(Node* parent, Coord end, int x, int y, int cost)
{if (x < 0 || x >= width || y < 0 || y >= height){return nullptr;}if (world_map[y * width + x] >= bar_value) {return nullptr;}int H = abs(end.x - x) * direct_cost + abs(end.y - y) * direct_cost;int G = parent->g + cost;Node* node = new Node(Coord(x, y), parent, G, H);return node;
}void AStar::get_neighbors(Node* parent, Coord end)
{Coord cd = parent->coord;Node* up, * down, * left, * right, * left_up, * right_up, * left_down, * right_down;up = new_node(parent, end, cd.x, cd.y - 1, direct_cost);down = new_node(parent, end, cd.x, cd.y + 1, direct_cost);left = new_node(parent, end, cd.x - 1, cd.y, direct_cost);right = new_node(parent, end, cd.x + 1, cd.y, direct_cost);left_up = new_node(parent, end, cd.x - 1, cd.y - 1, oblique_cost);right_up = new_node(parent, end, cd.x + 1, cd.y - 1, oblique_cost);left_down = new_node(parent, end, cd.x - 1, cd.y + 1, oblique_cost);right_down = new_node(parent, end, cd.x + 1, cd.y + 1, oblique_cost);neighbors[0] = up;neighbors[1] = down;neighbors[2] = left;neighbors[3] = right;neighbors[4] = left_up;neighbors[5] = right_up;neighbors[6] = left_down;neighbors[7] = right_down;
}void AStar::free_ls() {typename vector< Node* >::iterator iter_node;for (iter_node = open_ls.begin(); iter_node != open_ls.end(); iter_node++){//if (*iter_node != nullptr)delete (*iter_node);}for (iter_node = close_ls.begin(); iter_node != close_ls.end(); iter_node++){//if (*iter_node != nullptr)delete (*iter_node);}open_ls.clear();close_ls.clear();
}Point* AStar::find_path(Coord start, Coord end)
{memset(map_cache, 0, (size_t)width * (size_t)height);Node* start_node = new Node(start);open_ls.push_back(start_node);make_heap(open_ls.begin(), open_ls.end(), HeapCompare_f());push_heap(open_ls.begin(), open_ls.end(), HeapCompare_f());map_cache[start.y * width + start.x] = start_node;Node* cur_node = nullptr;while (true){if (open_ls.empty()) {printf("failed.\n");break;}cur_node = open_ls.front();pop_heap(open_ls.begin(), open_ls.end(), HeapCompare_f());open_ls.pop_back();if (cur_node->coord == end) {printf("success.\n");break;}get_neighbors(cur_node, end);for (size_t i = 0; i < 8; i++){Node* node = neighbors[i];if (node == nullptr) continue;Node** cache_node = &map_cache[node->coord.y * width + node->coord.x];if (*cache_node != 0){delete node;continue;}open_ls.push_back(node);push_heap(open_ls.begin(), open_ls.end(), HeapCompare_f());*cache_node = node;  //map_cache[node->coord.y * width + node->coord.x] = node;}close_ls.push_back(cur_node);map_cache[cur_node->coord.y * width + cur_node->coord.x] = cur_node;}Point* last = nullptr;if (cur_node == nullptr) goto end;last = new Point(cur_node->coord.x, cur_node->coord.y); //printf("x:%d,y:%d\n", cur_node->coord.x, cur_node->coord.y);while (true){if (set_path_value > 0)world_map[cur_node->coord.y * width + cur_node->coord.x] = set_path_value;cur_node = cur_node->parent;if (!cur_node) { break; }Point* t = new Point(cur_node->coord.x, cur_node->coord.y);  //printf("x:%d,y:%d\n", cur_node->coord.x, cur_node->coord.y);t->next = last;last = t;}
end:free_ls();return last;
}extern "C" {__declspec(dllexport)int CreateAStar(int* world_map, int width, int height, int bar_value, int direct_cost, int oblique_cost, int set_path_value);__declspec(dllexport)Point* FindPath(int id, int start_x, int start_y, int end_x, int end_y);__declspec(dllexport)void AStarFree(int id);__declspec(dllexport)void free_node_ls(Point** node_var);
}
#define MAX_NUMS 64
AStar* FINDER_OBJ_LIST[MAX_NUMS];
int CUR_INDEX = 0;
int CreateAStar(int* world_map, int width, int height, int bar_value, int direct_cost, int oblique_cost, int set_path_value) {std::cout << "Hello CreateAStar!\n";if (CUR_INDEX >= MAX_NUMS)CUR_INDEX = 0;int id = CUR_INDEX;if ( FINDER_OBJ_LIST[CUR_INDEX]!=0 ){int i= MAX_NUMS;for (i = 0; i < MAX_NUMS; i++){if (FINDER_OBJ_LIST[i] == 0)break;}if (i != MAX_NUMS){CUR_INDEX = i;}else {delete FINDER_OBJ_LIST[CUR_INDEX];}  }FINDER_OBJ_LIST[CUR_INDEX] = new AStar(world_map, width, height, bar_value, direct_cost, oblique_cost, set_path_value);CUR_INDEX++;return id;
}Point* FindPath(int id,int start_x,int start_y,int end_x,int end_y) {if (id > MAX_NUMS || id < 0) return 0;auto a = FINDER_OBJ_LIST[id];if (a == 0) return 0;std::cout << "Hello FindPath!\n";return a->find_path(Coord(start_x, start_y), Coord(end_x, end_y));
}void AStarFree(int id) {if (id > MAX_NUMS||id<0) return;auto a = FINDER_OBJ_LIST[id];if (a == 0) return;std::cout << "Hello AStarFree!\n";delete a;FINDER_OBJ_LIST[id] = 0;
}void free_node_ls(Point** node_var) {Point* node = *node_var;Point* next;for (;;){if (!node)break;next = node->next;delete node;node = next;//printf("free\n");}//if (node) {*node_var = 0;//}
}int main()
{int MAP_WIDTH = 20;int MAP_HEIGHT = 20;int BarValue = 9;int AreaValue = 1;int direct_cost = 10;int oblique_cost = 14;int is_direction = 0;int map[] = {1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1,   // 001, 9, 9, 9, 9, 9, 9, 9, 9, 9, 9, 9, 9, 9, 9, 9, 9, 9, 9, 1,   // 011, 9, 9, 1, 1, 9, 9, 9, 1, 9, 1, 9, 1, 9, 1, 9, 9, 9, 1, 1,   // 021, 9, 9, 1, 1, 9, 9, 9, 1, 9, 1, 9, 1, 9, 1, 9, 9, 9, 1, 1,   // 031, 9, 1, 1, 1, 1, 9, 9, 1, 9, 1, 9, 1, 1, 1, 1, 9, 9, 1, 1,   // 041, 9, 1, 1, 9, 1, 1, 1, 1, 9, 1, 1, 1, 1, 9, 1, 1, 1, 1, 1,   // 051, 9, 9, 9, 9, 1, 1, 1, 1, 1, 1, 9, 9, 9, 9, 1, 1, 1, 1, 1,   // 061, 9, 9, 9, 9, 9, 9, 9, 9, 1, 1, 1, 9, 9, 9, 9, 9, 9, 9, 1,   // 071, 9, 1, 1, 1, 1, 1, 1, 1, 1, 1, 9, 1, 1, 1, 1, 1, 1, 1, 1,   // 081, 9, 1, 9, 9, 9, 9, 9, 9, 9, 1, 1, 9, 9, 9, 9, 9, 9, 9, 1,   // 091, 9, 1, 1, 1, 1, 9, 1, 1, 9, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1,   // 101, 9, 9, 9, 9, 9, 1, 9, 1, 9, 1, 9, 9, 9, 9, 9, 1, 1, 1, 1,   // 111, 9, 1, 9, 1, 9, 9, 9, 1, 9, 1, 9, 1, 9, 1, 9, 9, 9, 1, 1,   // 121, 9, 1, 9, 1, 9, 9, 9, 1, 9, 1, 9, 1, 9, 1, 9, 9, 9, 1, 1,   // 131, 9, 1, 1, 1, 1, 9, 9, 1, 9, 1, 9, 1, 1, 1, 1, 9, 9, 1, 1,   // 141, 9, 1, 1, 9, 1, 1, 1, 1, 9, 1, 1, 1, 1, 9, 1, 1, 1, 1, 1,   // 151, 9, 9, 9, 9, 1, 1, 1, 1, 1, 1, 9, 9, 9, 9, 1, 1, 1, 1, 1,   // 161, 1, 9, 9, 9, 9, 9, 9, 9, 1, 1, 1, 9, 9, 9, 1, 9, 9, 9, 9,   // 171, 9, 1, 1, 1, 1, 1, 1, 1, 1, 1, 9, 1, 1, 1, 1, 1, 1, 1, 1,   // 181, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1,   // 19};AStar a = AStar(map, MAP_WIDTH, MAP_HEIGHT, BarValue, direct_cost, oblique_cost, 7);a.find_path(Coord(0, 0), Coord(3, 3));for (int i = 0; i < MAP_HEIGHT; i++){for (int j = 0; j < MAP_WIDTH; j++){printf("%d  ", map[i * MAP_WIDTH + j]);}cout << endl;}
}BOOL APIENTRY DllMain( HMODULE hModule,DWORD  ul_reason_for_call,LPVOID lpReserved)
{switch (ul_reason_for_call){case DLL_PROCESS_ATTACH:case DLL_THREAD_ATTACH:case DLL_THREAD_DETACH:case DLL_PROCESS_DETACH:break;}return TRUE;
}

c++的实现包好了导出为 dll，以及测试

效果图如下：

A星(AStar)算法的实现相关推荐

查找算法的实现c语言,查找算法的实现(C语言版)
<查找算法的实现(C语言版)>由会员分享,可在线阅读,更多相关<查找算法的实现(C语言版)(6页珍藏版)>请在人人文库网上搜索. 1.芬梨受交峙东喊济雏狈违晕裂赵檀芬逛温乌摄阿 ...
计算机图形学区域填充,计算机图形学区域填充算法的实现
. '. 实验四区域填充算法的实现班级 08信计学号 58 姓名陈瑞雪分数一.实验目的和要求: 1.掌握区域填充算法基本知识 2.理解区域的表示和类型,能正确区分四连通和八连通的区域 3.了解区域填 ...
OpenCV中图像旋转(warpAffine)算法的实现过程
在OpenCV中,目前并没有现成的函数直接用来实现图像旋转,它是用仿射变换函数cv::warpAffine来实现的,此函数目前支持4种插值算法,最近邻.双线性.双三次.兰索斯插值,如果传进去的参数为基 ...
JAVA实现中点画线_实验1-中点画线和Bresenham画线算法的实现
<实验1-中点画线和Bresenham画线算法的实现>由会员分享,可在线阅读,更多相关<实验1-中点画线和Bresenham画线算法的实现(9页珍藏版)>请在人人文库网上搜索. ...
python边缘检测代码_python Canny边缘检测算法的实现
图像边缘信息主要集中在高频段,通常说图像锐化或检测边缘,实质就是高频滤波.我们知道微分运算是求信号的变化率,具有加强高频分量的作用.在空域运算中来说,对图像的锐化就是计算微分.对于数字图像的离散信号, ...
干货回顾丨TensorFlow四种Cross Entropy算法的实现和应用
交叉熵介绍交叉熵(Cross Entropy)是Loss函数的一种(也称为损失函数或代价函数),用于描述模型预测值与真实值的差距大小,常见的Loss函数就是均方平方差(Mean Squared Er ...
C++基础代码--20余种数据结构和算法的实现
C++基础代码--20余种数据结构和算法的实现过年了,闲来无事,翻阅起以前写的代码,无意间找到了大学时写的一套C++工具集,主要是关于数据结构和算法.以及语言层面的工具类.过去好几年了,现在几乎已经 ...
选择性模糊及其算法的实现。
选择性模糊及其算法的实现. 我们常见的模糊算法比如均值模糊.高斯模糊等其基本的过程都是计算一个像素周边的的某个领域内,相关像素的某个特征值的累加和及对应的权重,然后得到结果值.比如均值模糊的各像素的权 ...
ML之SVM：基于Js代码利用SVM算法的实现根据Kaggle数据集预测泰坦尼克号生存人员
ML之SVM:基于Js代码利用SVM算法的实现根据Kaggle数据集预测泰坦尼克号生存人员目录实验数据设计思路实现代码(部分代码) 实验数据设计思路实现代码(部分代码) /**js代码实 ...

A星(AStar)算法的实现

A星(AStar)算法的实现相关推荐

最新文章

热门文章