BFS(广度优先搜索算法)实现

1. BFS算法

BFS(广度优先搜索算法)，搜索过程是从起始点一层一层的访问周围点，没有涉及到代价函数，是一种盲目搜索，搜索周围点的顺序是人为确定的。一般使用队列(先入先出)这种数据结构实现。但我的代码中使用链表这个数据结构实现，链表也能完成先入先出的操作。

我的代码中的主要算法过程：

1. 创建OPEN与CLOSED列表，代表已访问的点以未访问的点
2. 添加初始节点到OPEN中
3. 判断OPEN列表是否为空，为空则结束搜索，未找到路径，(循环1)
   1. 取出OPEN中最前面的点，记为cueent_node，将其从OPEN中删除，并添加到CLOSED中
   2. 获取current_node周围点(子节点)，并遍历（循环2）
      1. 判断周围点是否在CLOSED中，在其中则continue 。
      2. 判断周围单是否在OPEN中，在其中则continue。
      3. 到此步则说明该子节点是新节点，因此构建子节点与父节点之间关系，并直接将其添加到OPEN中
      4. 判断该子节点是否为终点，为终点则直接返回。
4. 返回带有父节点信息的终点。
5. 从终点遍历到起点，保存每个点，最终形成路径集合。

2. 代码实现

地图类：

#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

**Bfs实现：**只要过程在Bfs::search函数中

#ifndef BFS_H_
#define BFS_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 Bfs {
 public:
  Bfs(){};
  Bfs(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;
  // 访问的点在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;

  mymap::MyMap map_;
  std::list<Point*> open_list_;
  std::list<Point*> close_list_;
};

#endif  // BFS_H_

#include "bfs.h"

#include <cmath>

std::list<Point*> Bfs::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* Bfs::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()) {
    // 原本bfs是用队列来构建的，此处是list也可以
    // 获取先进入的节点
    Point* current_point = open_list_.front();
    // 弹出该节点并加入到已访问中
    open_list_.pop_front();
    close_list_.push_back(current_point);
    // 遍历邻居
    std::list<Point*> neighbors = get_neighbors(*current_point);
    for (auto neighbor : neighbors) {
      // 由于存储的是指针，所以没有办法直接通过指针不同来区分是否已访问
      // 邻居已访问
      if (is_in_closelist(*neighbor)) continue;
      // 不能在openlist中
      if (is_in_openlist(*neighbor)) continue;
      neighbor->parent_ = current_point;
      open_list_.push_back(neighbor);
      // 判断邻居是否为终点
      if (neighbor->x_ == end_point.x_ && neighbor->y_ == end_point.y_) {
        return neighbor;
      }
    }
  }
  // 没有找到路径则为空指针
  return nullptr;
}

std::list<Point*> Bfs::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(&current_point, new Point(i, j))) {
        neighbors.push_back(new Point(i, j));
      }
    }
  }
  return neighbors;
}

bool Bfs::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 Bfs::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 Bfs::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;
}

#include <vector>
// #include <unistd.h>

#include "bfs.h"
#include "matplotlibcpp.h"
#include "mymap.h"

namespace plt = matplotlibcpp;

int main() {
  mymap::MyMap map(51, 51);
  Bfs bfs(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> close_list_x;
  vector<int> close_list_y;

  std::list<Point*> path = bfs.get_path(start_point, end_point);
  std::list<Point*> close_list = bfs.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 == 50) {
      plt::plot(close_list_x, close_list_y, "yo");
      plt::pause(0.01);
      i = 0;
    }
  }
  // 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;
}

3. 仿真结果

源代码中将整个过程显示成动画了，这里只是最后的结果，从CLOSED(黄色点)中可以看出，搜索是一层一层搜索的。