4. Dijkstra算法
Dijkstra是一种贪婪算法，选择待访问点的标准是离起始点近的点优先访问。
我所编写的代码的流程：
- 创建
OPEN与CLOSED列表，代表已访问的点以未访问的点 - 添加初始节点到
OPEN中 - 判断
OPEN列表是否为空，为空则结束搜索，未找到路径，(循环1)- 取出
OPEN中离起始点最近的点，记为cueent_node，将其从OPEN中删除，并添加到CLOSED中 - 获取
current_node周围点(子节点)，并遍历（循环2）- 判断周围点是否在
CLOSED中，在其中则continue。 - 判断周围单是否在
OPEN中，在其中则continue。 - 到此步则说明该子节点是新节点，因此构建子节点与父节点之间关系，并直接将其添加到
OPEN中，实际代码中是通过G,F维持距离信息。 - 判断该子节点是否为终点，为终点则直接返回。
- 判断周围点是否在
- 取出
- 返回带有父节点信息的终点。
- 从终点遍历到起点，保存每个点，最终形成路径集合。
4.1 代码实现
地图实现：
#ifndef MYMAP_H_
#define MYMAP_H_
#include <vector>
using std::vector;
namespace mymap {
class MyMap {
public:
// 构造函数
MyMap(){};
MyMap(const int map_xlength, const int map_ylength);
// 设置平行与X轴的一排障碍物，[x_begin, x_end]
void SetObsLineX(const int x_begin, const int x_end, const int y);
// 设置平行与Y轴的一排障碍物，[y_begin, y_end]
void SetObsLineY(const int y_begin, const int y_end, const int x);
// 获取(x, y)处的地图状态，1为有障碍物，0为无障碍物
bool GetMapPointState(const int x, const int y) const;
// 获取成员变量接口
vector<vector<int>> map() const { return map_; };
int map_xlength() const { return map_xlength_; };
int map_ylength() const { return map_ylength_; };
private:
vector<vector<int>> map_; // 地图矩阵
int map_xlength_; // 地图列数，长
int map_ylength_; // 地图行数，宽
int unfree = 1; // 地图点状态
int free = 0;
};
} // namespace mymap
#endif // MYMAP_H
#include "mymap.h"
namespace mymap {
// 构造函数，构建指定长宽的地图大小
MyMap::MyMap(const int map_xlength, const int map_ylength)
: map_xlength_(map_xlength), map_ylength_(map_ylength) {
map_.resize(map_ylength_);
for (auto &e : map_) {
e.resize(map_xlength_);
}
int x_scale = map_xlength_ / 5;
int y_scale = map_ylength_ /5;
SetObsLineX(0, map_xlength_ - 1, 0);
SetObsLineX(0, map_xlength_ - 1, map_ylength_ - 1);
SetObsLineY(0, map_ylength_ - 1, 0);
SetObsLineY(0, map_ylength_ - 1, map_xlength_ - 1);
SetObsLineX(x_scale, x_scale * 2, y_scale * 3);
SetObsLineX(x_scale * 3, x_scale * 4, y_scale * 2);
SetObsLineX(0, x_scale, y_scale * 2);
SetObsLineX(x_scale * 4, x_scale * 5, y_scale * 3);
SetObsLineY(0, y_scale * 3, x_scale * 2);
SetObsLineY(y_scale * 2, y_scale * 5, x_scale * 3);
}
void MyMap::SetObsLineX(const int x_begin, const int x_end, const int y) {
for (int i = x_begin; i <= x_end; ++i) {
map_[i][y] = unfree;
}
}
void MyMap::SetObsLineY(const int y_begin, const int y_end, const int x) {
for (int i = y_begin; i <= y_end; ++i) {
map_[x][i] = unfree;
}
}
bool MyMap::GetMapPointState(const int x, const int y) const {
return map_[x][y];
}
} // namespace mymap
Dijkstra算法实现，主要部分在search，与我之前写的Astar算法只有一行的差别
#ifndef DIJKSTRA_H_
#define DIJKSTRA_H_
#include <list>
#include <vector>
#include "mymap.h"
struct Point {
int x_;
int y_;
double F_;
double G_;
double H_;
Point* parent_;
Point(int x, int y)
: x_(x), y_(y), F_(0.0), G_(0.0), H_(0.0), parent_(nullptr){};
};
class Dijkstra {
public:
Dijkstra(){};
Dijkstra(mymap::MyMap map) : map_(map){};
// 获取整个路径，也就是搜索完以后的路径点集
std::list<Point*> get_path(const Point& start_point, const Point& end_point);
// 获取成员变量
std::list<Point*> open_list() const { return open_list_; };
std::list<Point*> close_list() const { return close_list_; };
private:
// 从起点开始搜索路径，返回带有父节点信息的终点
Point* search(const Point& start_point, const Point& end_point);
// 获取当前点的周围点
std::list<Point*> get_neighbors(const Point& current_point) const;
// 获取openlist中f最小的点
Point* get_minF_point() const;
// 访问的点在open中
bool is_in_openlist(const Point& current_point) const;
// 访问的点在close中
bool is_in_closelist(const Point& current_point) const;
// 当前点到下一个点之间是否可以通过
bool is_collision(const Point* firstPoint, const Point* secondPoint) const;
// 计算F
double calculate_F(Point& current_point);
// 递推G
double calculate_G(const Point& previous_point, Point& current_point);
mymap::MyMap map_;
std::list<Point*> open_list_;
std::list<Point*> close_list_;
};
#endif // DIJKSTRA_H_
#include "dijkstra.h"
#include <cmath>
std::list<Point*> Dijkstra::get_path(const Point& start_point,
const Point& end_point) {
// 执行搜索，返回带有父节点信息的终点
Point* end_with_parent = this->search(start_point, end_point);
std::list<Point*> path;
// 从终点查找到起点
while (end_with_parent) {
path.push_back(end_with_parent);
end_with_parent = end_with_parent->parent_;
}
return path;
}
Point* Dijkstra::search(const Point& start_point, const Point& end_point) {
// 重新构建新的起始节点与终点，因为传入的是const不能改变内容
Point* start = new Point(start_point);
// 起点进入待访问
open_list_.push_back(start);
// 开始搜索
while (!open_list_.empty()) {
// 获取待访问列表中f最小的点
Point* current_point = this->get_minF_point();
// 从待访问中删除，并添加到已访问
open_list_.remove(current_point);
close_list_.push_back(current_point);
// 获取当前点的周围点
std::list<Point*> neighbors = this->get_neighbors(*current_point);
// 遍历周围点
for (auto& neighbor : neighbors) {
if (this->is_in_closelist(*neighbor)) {
continue;
}
// 当周围点在open列表中，则更新GHF并添加到open列表中
if (!this->is_in_openlist(*neighbor)) {
neighbor->parent_ = current_point;
neighbor->G_ = this->calculate_G(*current_point, *neighbor);
neighbor->F_ = this->calculate_F(*neighbor);
// 添加到open中
open_list_.push_back(neighbor);
} else {
// 如果已经在open列表中，则计算G值，用最小的G来更新父节点
if (this->calculate_G(*current_point, *neighbor) < neighbor->G_) {
neighbor->parent_ = current_point;
neighbor->G_ = this->calculate_G(*current_point, *neighbor);
neighbor->F_ = this->calculate_F(*neighbor);
// 此处不更新H是因为，H只与某点自身与终点有关，而在添加到openl中的时候H就已经计算了
}
}
// 访问的周围点是终点，这句要在neighbor更新并添加到open之后
if (neighbor->x_ == end_point.x_ && neighbor->y_ == end_point.y_) {
return neighbor;
}
}
}
// 查找失败则为空指针
return nullptr;
}
std::list<Point*> Dijkstra::get_neighbors(const Point& current_point) const {
std::list<Point*> neighbors;
int mid_x = current_point.x_;
int mid_y = current_point.y_;
// 上下左右四个点
// // 左边的点
// if (mid_x - 1 >= 0 and !map_.GetMapPointState(mid_x - 1, mid_y)) {
// neighbors.push_back(new Point(mid_x - 1, mid_y));
// }
// // 右面的点
// if (mid_x + 1 <= map_.map_xlength() and
// !map_.GetMapPointState(mid_x + 1, mid_y)) {
// neighbors.push_back(new Point(mid_x + 1, mid_y));
// }
// // 上面的点
// if (mid_y + 1 <= map_.map_ylength() and
// !map_.GetMapPointState(mid_x, mid_y + 1)) {
// neighbors.push_back(new Point(mid_x, mid_y + 1));
// }
// // 下面的点
// if (mid_y - 1 >= 0 and !map_.GetMapPointState(mid_x, mid_y - 1)) {
// neighbors.push_back(new Point(mid_x, mid_y - 1));
// }
// 八个点情况
// 遍历时点应该在地图范围内，并且中间的那个点，以及对角线路线不能被卡住
for (int i = mid_x - 1; i <= mid_x + 1; i++) {
for (int j = mid_y - 1; j <= mid_y + 1; j++) {
if (i >= 0 and j >= 0 and i <= map_.map_xlength() - 1 and
j <= map_.map_ylength() - 1 and
!is_collision(¤t_point, new Point(i, j))) {
neighbors.push_back(new Point(i, j));
}
}
}
return neighbors;
}
Point* Dijkstra::get_minF_point() const {
Point* res = open_list_.front();
// 遍历然后存最小的点
for (auto& p : open_list_) {
if (p->F_ < res->F_) {
res = p;
}
}
return res;
}
bool Dijkstra::is_in_openlist(const Point& current_point) const {
for (auto p : open_list_) {
if (p->x_ == current_point.x_ && p->y_ == current_point.y_) {
return true;
}
}
return false;
}
bool Dijkstra::is_in_closelist(const Point& current_point) const {
for (auto p : close_list_) {
if (p->x_ == current_point.x_ && p->y_ == current_point.y_) {
return true;
}
}
return false;
}
bool Dijkstra::is_collision(const Point* firstPoint,
const Point* secondPoint) const {
//如果访问的两个点本身就是障碍
if (this->map_.GetMapPointState(firstPoint->x_, firstPoint->y_) or
this->map_.GetMapPointState(secondPoint->x_, secondPoint->y_)) {
return true;
}
//两个点不是同一个点
if (firstPoint->x_ != secondPoint->x_ and firstPoint->y_ != secondPoint->y_) {
//如果两个点是反对角线上，则对角线不能有障碍物
if (secondPoint->x_ - firstPoint->x_ == firstPoint->y_ - secondPoint->y_) {
//对角线，左下角点
Point leftDwon(std::min(firstPoint->x_, secondPoint->x_),
std::min(firstPoint->y_, secondPoint->y_));
//对角线，右上角点
Point rightUp(std::max(firstPoint->x_, secondPoint->x_),
std::max(firstPoint->y_, secondPoint->y_));
//左下角点与右上角点均不能有障碍物
return this->map_.GetMapPointState(leftDwon.x_, leftDwon.y_) or
this->map_.GetMapPointState(rightUp.x_, rightUp.y_);
} else {
//当两点在对角线上时，反对角线不能有障碍物
//左上角
Point leftUp(std::min(firstPoint->x_, secondPoint->x_),
std::max(firstPoint->y_, secondPoint->y_));
//右下角
Point rightDwon(std::max(firstPoint->x_, secondPoint->x_),
std::min(firstPoint->y_, secondPoint->y_));
//左上角与右下角均不能有障碍物
return this->map_.GetMapPointState(leftUp.x_, leftUp.y_) or
this->map_.GetMapPointState(rightDwon.x_, rightDwon.y_);
}
}
//其他状态表示没有障碍物
return false;
}
// 计算F=H+G
double Dijkstra::calculate_F(Point& current_point) {
return current_point.H_ + current_point.G_;
}
// 计算G，欧拉距离，当前点的G = 父节点G + 两点之间的代价
double Dijkstra::calculate_G(const Point& previous_point, Point& current_point) {
double extra_G =
std::sqrt(std::pow((current_point.x_ - previous_point.x_), 2) +
std::pow((current_point.y_ - previous_point.y_), 2));
return previous_point.G_ + extra_G;
}
mian函数
#include <vector>
// #include <unistd.h>
#include "dijkstra.h"
#include "matplotlibcpp.h"
#include "mymap.h"
namespace plt = matplotlibcpp;
int main() {
mymap::MyMap map(51, 51);
Dijkstra dijkstra(map);
Point start_point(5, 5);
Point end_point(45, 45);
vector<int> start_and_end_point_x{start_point.x_, end_point.x_};
vector<int> start_and_end_point_y{start_point.y_, end_point.y_};
vector<int> path_x;
vector<int> path_y;
vector<int> open_list_x;
vector<int> open_list_y;
vector<int> close_list_x;
vector<int> close_list_y;
std::list<Point*> path = dijkstra.get_path(start_point, end_point);
std::list<Point*> close_list = dijkstra.close_list();
for (auto p : path) {
path_x.push_back(p->x_);
path_y.push_back(p->y_);
}
// for (auto p : close_list) {
// close_list_x.push_back(p->x_);
// close_list_y.push_back(p->y_);
// }
std::vector<int> free_x, unfree_x, free_y, unfree_y;
for (int i = 0; i < map.map_xlength(); ++i) {
for (int j = 0; j < map.map_ylength(); ++j) {
if (map.GetMapPointState(i, j) == 1) {
unfree_x.push_back(i);
unfree_y.push_back(j);
} else {
free_x.push_back(i);
free_y.push_back(j);
}
}
}
vector<int> x{10, 20};
vector<int> y{10, 20};
plt::figure();
plt::plot(unfree_x, unfree_y, "ko");
// plt::plot(close_list_x, close_list_y, "yo");
int i = 0;
for (auto p : close_list) {
close_list_x.push_back(p->x_);
close_list_y.push_back(p->y_);
i++;
if (i == 10) {
plt::plot(close_list_x, close_list_y, "yo");
plt::pause(0.01);
i = 0;
}
}
plt::plot(open_list_x, open_list_y, "co");
// plt::plot(free_x, free_y, "wo");
plt::plot(path_x, path_y, "ro");
plt::plot(start_and_end_point_x, start_and_end_point_y, "gs");
plt::show();
return 0;
}
4.2 仿真结果
157
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
