前言
《坦克动荡2》(Tank Trouble 2)是一款适用于PC平台的射击类网页游戏。游戏设定在一个迷宫般的竞技场中，玩家需要控制坦克，在对手摧毁自己之前先摧毁对方。- 本题目旨在各位自己实现
A-star算法，并为接下来学习并自行编写替换nav2中的Planner Server模块做准备。 - 本题目要求
C++，opencvC++基础知识。 《坦克动荡2》游戏链接
题目描述
- 如下图，是一张
《坦克动荡2》的游戏截图，我们要根据以下图片，完成下列一系列要求：
代价地图的读取- 使用
opencv-C++对下列图片进行读取 - 自行选择图像处理策略，以
黑白的形式对可行使区域和障碍物进行划分
- 使用
起点与终点的标定：- 选取绿色坦克作为机器人的起点
- 选取红色坦克作为路径规划的目标点
根据机器人足迹Footprint对地图进行膨胀- 使用圆形对绿色坦克进行概括(设置Footprint) ，对代价地图进行
一定系数的膨胀
- 使用圆形对绿色坦克进行概括(设置Footprint) ，对代价地图进行
对几个目标物体进行识别- 识别出三个道具的坐标
进行路径规划-
自行对下述代价地图进行
合适分辨率的切分(切分成格子) -
要求使用
A*算法，规划绿色坦克从起点位置前往终点位置 -
启发函数不做限制。 -
坦克可以当作
麦克纳姆模型，允许进行8联通移动，且不需要考虑初始位姿(可以自旋)，如下所示，当坦克位于九宫格的中央时候，可以前往周围任意8个位置 -
-
要求坦克在前往最终红色区域的路上，依次选择合适的路径
吃满三个道具，允许倒车和重复路径的行驶，且红色终点和道具位置均不会在行驶过程中改变。 -
完成上述任务的前提下
路径越短越好。
-
算法介绍
A*（A-star）路径规划算法是一种广泛应用于图搜索中的计算机算法，它旨在找到从起始点到目标点的最短路径。A_算法结合了最佳优先搜索的高性能和Dijkstra算法的优点，通过启发式方法来提高搜索效率
算法原理：
- 讲解推荐：Introduction to the A* Algorithm (redblobgames.com)
- 开放集（Open Set）：存放待检查的节点。
- 封闭集（Closed Set）：存放已经检查过的节点。
- 启发式函数（Heuristic Function）：用于估算从当前节点到目标节点的成本，通常表示为h(n)。
- 曼哈顿距离
- 欧式距离
- 路径成本（g(n)）：从起始节点到当前节点的实际成本。
- 总成本（f(n)）：由路径成本和启发式函数的估计值组成，即f(n) = g(n) + h(n)。
1.代价地图的读取
- 下面我们开始题目的处理，这里我们使用
opencv-C++进行图像提取，由于本题图像处理不是重点，这里代码就不讲究设计模式的使用了。 - 由于地图墙壁为固定颜色且和其他游戏物件颜色区分很大，这里我们就使用
hsv对地图墙壁进行提取,提取后进行一次开闭运算消除地图中的噪点
cv::Mat get_binary_game_map_from_screen(const cv::Mat& screen_frame)
{
cv::Mat maskGray;
cv::Scalar lowerGray(0, 0, 40), upperGray(0, 0, 77);
cv::Mat hsvImage = convert_to_hsv(screen_frame);
cv::inRange(hsvImage, lowerGray, upperGray, maskGray);
cv::Mat element = cv::getStructuringElement(cv::MORPH_RECT, cv::Size(3, 3));
cv::Mat opened_frame;
cv::morphologyEx(maskGray, opened_frame, cv::MORPH_OPEN, element);
cv::morphologyEx(opened_frame, maskGray, cv::MORPH_CLOSE, element);
cv::Mat inverted_image = cv::Mat::zeros(maskGray.size(), maskGray.type());
cv::bitwise_not(maskGray, inverted_image);
return inverted_image;
}
2.起点与终点的标定
- 对于颜色十分明显的红绿色坦克，我们采用同样的方式进行提取，并把这两个点画在原始图像上
- 这里我们简单使用几个函数完成上述要求
cv::Mat convert_to_hsv(const cv::Mat& bgrImage) {
if (bgrImage.empty())
{
return cv::Mat();
}
cv::Mat hsvImage;
cv::cvtColor(bgrImage, hsvImage, cv::COLOR_BGR2HSV);
return hsvImage;
}
cv::Mat create_red_mask(const cv::Mat& hsvImage) {
cv::Mat maskRed1, maskRed2, maskRed;
cv::Scalar lowerRed1(0, 70, 50), upperRed1(10, 255, 255);
cv::Scalar lowerRed2(170, 70, 50), upperRed2(180, 255, 255);
cv::inRange(hsvImage, lowerRed1, upperRed1, maskRed1);
cv::inRange(hsvImage, lowerRed2, upperRed2, maskRed2);
cv::add(maskRed1, maskRed2, maskRed);
return maskRed;
}
cv::Mat create_green_mask(const cv::Mat& hsvImage) {
cv::Mat maskGreen;
cv::Scalar lowerGreen(40, 40, 40), upperGreen(80, 255, 255);
cv::inRange(hsvImage, lowerGreen, upperGreen, maskGreen);
return maskGreen;
}
cv::Point getCenter(const std::vector<std::vector<cv::Point>>& contours)
{
for (const auto& contour : contours) {
cv::Moments m = cv::moments(contour);
if (m.m00 != 0) {
cv::Point center(m.m10 / m.m00, m.m01 / m.m00);
return center;
}
}
}
std::vector<std::vector<cv::Point>> find_contours(const cv::Mat& mask) {
std::vector<std::vector<cv::Point>> contours;
cv::findContours(mask, contours, cv::RETR_EXTERNAL, cv::CHAIN_APPROX_SIMPLE);
return contours;
}
- 合并起来，我们有
std::pair<cv::Point, cv::Point> get_tank_centers(const cv::Mat& screen_frame) {
cv::Mat hsvImage = convert_to_hsv(screen_frame);
cv::Mat maskRed = create_red_mask(hsvImage);
cv::Mat maskGreen = create_green_mask(hsvImage);
std::vector<std::vector<cv::Point>> contoursRed = find_contours(maskRed);
std::vector<std::vector<cv::Point>> contoursGreen = find_contours(maskGreen);
cv::Point centerRed = getCenter(contoursRed);
cv::Point centerGreen = getCenter(contoursGreen);
return { centerRed, centerGreen };
}
- 我们在主函数中调用上述函数
// 获取红色和绿色坦克的坐标
std::pair<cv::Point, cv::Point> tank_centers = get_tank_centers(screen_frame);
cv::Point centerRed = tank_centers.first;
cv::Point centerGreen = tank_centers.second;
// 在原图上绘制中心点
cv::circle(b_map, centerRed,5, cv::Scalar(0, 0, 255), -1); // 红色坦克中心点
cv::circle(b_map, centerGreen, 5, cv::Scalar(0, 255, 0), -1); // 绿色坦克中心点
3. 根据机器人足迹Footprint对地图进行膨胀
- 这里我们引入
ROS导航相关的内容进行辅助说明
机器人足迹的定义
机器人足迹（Footprint）是指机器人在移动时所占用的实际物理空间。它通常是一个二维的形状，描述了机器人在地面上留下的印记或轮廓。机器人足迹可以是圆形、矩形、多边形或其他任何形状，这取决于机器人的物理结构和设计。例如，一个轮式机器人可能有一个矩形的足迹，而一个多足机器人可能有更复杂的足迹形状。
地图膨胀的必要性
- 地图膨胀是将机器人足迹考虑在内的一个关键步骤，它对于路径规划和避障具有重要作用。以下是地图膨胀的原因：
- 在路径规划中，如果将机器人视为一个质点，那么规划的路径可能不适合实际机器人的尺寸。通过膨胀地图上的障碍物，可以确保机器人即使在最边缘的位置也不会与障碍物发生碰撞。
- 这里我们使用使用色块的最小外接圆作为机器人的足迹，并使用其半径配合
opencv-cpp的dilate函数来实现地图膨胀
double get_tank_footpint_radius(const cv::Mat& screen_frame)
{
cv::Mat hsvImage = convert_to_hsv(screen_frame);
cv::Mat maskRed = create_red_mask(hsvImage);
cv::Mat maskGreen = create_green_mask(hsvImage);
std::vector<std::vector<cv::Point>> contoursRed = find_contours(maskRed);
std::vector<std::vector<cv::Point>> contoursGreen = find_contours(maskGreen);
cv::Point2f center;
float radius1, radius2;
cv::minEnclosingCircle(contoursGreen[0], center, radius1);
cv::minEnclosingCircle(contoursRed[0], center, radius2);
return std::max(radius1,radius2);
}
void dilate_map(cv::Mat& binary_map, const float& radius) {
// 创建圆形结构元素
cv::Mat element = cv::getStructuringElement(cv::MORPH_ELLIPSE, cv::Size(2 * radius + 1, 2 * radius + 1));
cv::Mat inverted_map;
cv::bitwise_not(binary_map, inverted_map);
cv::dilate(inverted_map, inverted_map, element);
cv::bitwise_not(inverted_map, binary_map);
}
4. 对几个目标物体进行识别
- 通过观察发现目标物体和背景颜色相近，使用
hsv很难将其剥离，那么可以尝试底下几种方法- 正方形检测
- 模板匹配
- 这里我们先使用hsv进行筛选出地图和道具同时存在的情况并和上述提取出的村地图进行相减
cv::Mat create_v_mask(const cv::Mat& hsvImage) {
cv::Mat maskV;
cv::Scalar lowerV(0, 0, 59), upperV(0, 0, 179);
cv::inRange(hsvImage, lowerV, upperV, maskV);
return maskV;
}
cv::Mat get_v_enhanced_map(const cv::Mat& screen_frame) {
cv::Mat hsvImage = convert_to_hsv(screen_frame);
cv::Mat maskV = create_v_mask(hsvImage);
cv::Mat element = cv::getStructuringElement(cv::MORPH_RECT, cv::Size(3, 3));
cv::Mat opened_frame;
cv::morphologyEx(maskV, opened_frame, cv::MORPH_OPEN, element);
cv::morphologyEx(opened_frame, maskV, cv::MORPH_CLOSE, element);
// 将得到的掩码与二值化的地图相减
cv::Mat binary_map = get_binary_game_map_from_screen(screen_frame);
cv::Mat inverted_image = cv::Mat::zeros(maskV.size(), maskV.type());
cv::bitwise_not(maskV, inverted_image);
cv::Mat result = binary_map-inverted_image;
cv::morphologyEx(result, opened_frame, cv::MORPH_OPEN, element);
cv::morphologyEx(opened_frame, result, cv::MORPH_CLOSE, element);
return result;
}
- 然后对轮廓进行提取
cv::Mat drawGameObjectCentersOnOriginal(const cv::Mat& original_image, const std::vector<cv::Point>& centers) {
// 绘制三个点在原始图像上
for (const auto& center : centers) {
cv::circle(original_image, center, 5, cv::Scalar(0, 255, 0), -1);
}
return original_image;
}
std::vector<cv::Point> getGameObjectCenters(const cv::Mat& result) {
std::vector<cv::Point> centers;
// 进行边缘检测
cv::Mat edges;
cv::Canny(result, edges, 50, 150, 3);
// 找到轮廓
std::vector<std::vector<cv::Point>> contours;
cv::findContours(edges, contours, cv::RETR_EXTERNAL, cv::CHAIN_APPROX_SIMPLE);
// 计算每个轮廓的中心点
for (const auto& contour : contours) {
cv::Moments m = cv::moments(contour);
if (m.m00 != 0) {
cv::Point center(m.m10 / m.m00, m.m01 / m.m00);
centers.push_back(center);
if (centers.size() == 3) { // 只返回三个物体的中心坐标
break;
}
}
}
return centers;
}
5. 进行路径规划
5.1 对地图进行合适分辨率的切分(切分成格子)
- 我们搜先对地图进行一定比例的缩放,过大的分辨率会导致搜索速度缓慢
5.2 进行txt格式转换
- 我们把上述resize后的图片进行
txt文件存储，方法一会进行路径规划- T:道具
- S:绿色坦克起点
- G:红色坦克终点
- 0:可行驶区域
- 1:墙壁
std::ofstream file("map.txt");
if (!file.is_open()) {
std::cerr << "Error opening file." << std::endl;
return -1;
}
for (int row = 0; row < resized_image.rows; row++) {
for (int col = 0; col < resized_image.cols; col++) {
cv::Vec3b pixel = resized_image.at<cv::Vec3b>(row, col);
int blue = pixel[0];
int green = pixel[1];
int red = pixel[2];
// 根据像素值确定存储的字符
if (blue == 255 && green == 0 && red == 0) {
file << "T";
}
else if (blue == 0 && green == 255 && red == 0) {
file << "S";
}
else if (blue == 0 && green == 0 && red == 255) {
file << "G";
}
else if (blue == 255 && green == 255 && red == 255) {
file << "0";
}
else {
file << "1";
}
}
file << std::endl;
}
file.close();
- 完整txt数据获取
1111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111
1111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111
1111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111
1111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111
1111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111
1111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111
1111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111
1111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111
1111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111
1111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111
1111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111
1111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111
1111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111
1111111111111000000000000011111111111111111111000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000111111111111111111110000000000000000000000000000000000000000000000000000000000000000000000000000001111111111111
1111111111111000000000000011111111111111111111000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000111111111111111111110000000000000000000000000000000000000000000000000000000000000000000000000000001111111111111
1111111111111000000000000011111111111111111111000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000111111111111111111110000000000000000000000000000000000000000000000000000000000000000000000000000001111111111111
1111111111111000000000000011111111111111111111000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000111111111111111111110000000000000000000000000000000000000000000000000000000000000000000000000000001111111111111
1111111111111000000000000011111111111111111111000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000111111111111111111110000000000000000000000000000000000000000000000000000000000000000000000000000001111111111111
11111111111110000000000000111111111111111111110000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000001111111111111111111100000000000000000000000000000000000000000000000000000000000000000000000SS000001111111111111
11111111111110000000000000111111111111111111110000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000001111111111111111111100000000000000000000000000000000000000000000000000000000000000000000000SS000001111111111111
1111111111111000000000000011111111111111111111000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000111111111111111111110000000000000000000000000000000000000000000000000000000000000000000000000000001111111111111
1111111111111000000000000011111111111111111111000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000111111111111111111110000000000000000000000000000000000000000000000000000000000000000000000000000001111111111111
1111111111111000000000000011111111111111111111000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000111111111111111111110000000000000000000000000000000000000000000000000000000000000000000000000000001111111111111
1111111111111000000000000011111111111111111111000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000111111111111111111110000000000000000000000000000000000000000000000000000000000000000000000000000001111111111111
1111111111111000000000000011111111111111111111000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000111111111111111111110000000000000000000000000000000000000000000000000000000000000000000000000000001111111111111
1111111111111111111111111111111111111111111111111111111111111111111111000000000000000000000000000000010000000000000000000000000000000010000000000000000000000111111111111111111110000000000000000000000111111111111111111111111111111111100000000000000000000001111111111111
1111111111111111111111111111111111111111111111111111111111111111111111111100000000000000000000000111111111100000000000000000000001111111111000000000000000000111111111111111111110000000000000000001111111111111111111111111111111111111111110000000000000000001111111111111
1111111111111111111111111111111111111111111111111111111111111111111111111110000000000000000000011111111111110000000000000000000011111111111110000000000000000111111111111111111110000000000000000111111111111111111111111111111111111111111111100000000000000001111111111111
1111111111111111111111111111111111111111111111111111111111111111111111111111000000000000000000111111111111111000000000000000000111111111111111000000000000000111111111111111111110000000000000001111111111111111111111111111111111111111111111110000000000000001111111111111
1111111111111111111111111111111111111111111111111111111111111111111111111111100000000000000001111111111111111100000000000000001111111111111111100000000000000111111111111111111110000000000000011111111111111111111111111111111111111111111111111000000000000001111111111111
1111111111111111111111111111111111111111111111111111111111111111111111111111110000000000000001111111111111111110000000000000011111111111111111100000000000000111111111111111111110000000000000011111111111111111111111111111111111111111111111111000000000000001111111111111
1111111111111111111111111111111111111111111111111111111111111111111111111111110000000000000011111111111111111110000000000000011111111111111111110000000000000111111111111111111110000000000000111111111111111111111111111111111111111111111111111100000000000001111111111111
1111111111111111111111111111111111111111111111111111111111111111111111111111111000000000000011111111111111111110000000000000011111111111111111110000000000000111111111111111111110000000000000111111111111111111111111111111111111111111111111111100000000000001111111111111
1111111111111111111111111111111111111111111111111111111111111111111111111111111000000000000011111111111111111111000000000000111111111111111111110000000000000111111111111111111110000000000000111111111111111111111111111111111111111111111111111100000000000001111111111111
1111111111111111111111111111111111111111111111111111111111111111111111111111111000000000000011111111111111111111000000000000111111111111111111110000000000000111111111111111111110000000000000111111111111111111111111111111111111111111111111111100000000000001111111111111
1111111111111111111111111111111111111111111111111111111111111111111111111111111000000000000011111111111111111111000000000000111111111111111111110000000000000111111111111111111110000000000000111111111111111111111111111111111111111111111111111100000000000001111111111111
1111111111111111111111111111111111111111111111111111111111111111111111111111111000000000000011111111111111111111000000000000111111111111111111110000000000000111111111111111111110000000000000111111111111111111111111111111111111111111111111111100000000000001111111111111
1111111111111111111111111111111111111111111111111111111111111111111111111111111000000000000011111111111111111111000000000000111111111111111111110000000000000111111111111111111110000000000000111111111111111111111111111111111111111111111111111100000000000001111111111111
1111111111111111111111111111111111111111111111111111111111111111111111111111111000000000000011111111111111111111000000000000111111111111111111110000000000000111111111111111111110000000000000111111111111111111111111111111111111111111111111111100000000000001111111111111
1111111111111111111111111111111111111111111111111111111111111111111111111111110000000000000011111111111111111111000000000000111111111111111111110000000000000011111111111111111100000000000000111111111111111111111111111111111111111111111111111100000000000001111111111111
1111111111111111111111111111111111111111111111111111111111111111111111111111110000000000000011111111111111111111000000000000111111111111111111110000000000000011111111111111111100000000000000111111111111111111111111111111111111111111111111111100000000000001111111111111
1111111111111111111111111111111111111111111111111111111111111111111111111111100000000000000011111111111111111111000000000000111111111111111111110000000000000001111111111111111000000000000000111111111111111111111111111111111111111111111111111100000000000001111111111111
1111111111111111111111111111111111111111111111111111111111111111111111111111000000000000000011111111111111111111000000000000111111111111111111110000000000000000111111111111110000000000000000111111111111111111111111111111111111111111111111111100000000000001111111111111
1111111111111111111111111111111111111111111111111111111111111111111111111110000000000000000011111111111111111111000000000000111111111111111111110000000000000000011111111111100000000000000000111111111111111111111111111111111111111111111111111100000000000001111111111111
1111111111111111111111111111111111111111111111111111111111111111111111111000000000000000000011111111111111111111000000000000111111111111111111110000000000000000001111111110000000000000000000111111111111111111111111111111111111111111111111111100000000000001111111111111
1111111111111000000000000000000000000000000000000000000000000000000000000000000000000000000011111111111111111111000000000000111111111111111111110000000000000000000000000000000000000000000000111111111111111111110000000000001111111111111111111100000000000001111111111111
1111111111111000000000000000000000000000000000000000000000000000000000000000000000000000000011111111111111111111000000000000111111111111111111110000000000000000000000000000000000000000000000111111111111111111110000000000001111111111111111111100000000000001111111111111
1111111111111000000000000000000000000000000000000000000000000000000000000000000000000000000011111111111111111111000000000000111111111111111111110000000000000000000000000000000000000000000000111111111111111111110000000000001111111111111111111100000000000001111111111111
1111111111111000000000000000000000000000000000000000000000000000000000000000000000000000000011111111111111111111000000000000111111111111111111110000000000000000000000000000000000000000000000111111111111111111110000000000001111111111111111111100000000000001111111111111
1111111111111000000000000000000000000000000000000000000000000000000000000000000000000000000011111111111111111111000000000000111111111111111111110000000000000000000000000000000000000000000000111111111111111111110000000000001111111111111111111100000000000001111111111111
1111111111111000000000000000000000000000000000000000000000000000000000000000000000000000000011111111111111111111000000000000111111111111111111110000000000000000000000000000000000000000000000111111111111111111110000000000001111111111111111111100000000000001111111111111
111111111111100000000000000000000000000000000000000000000000000000000000000000000000000000001111111111111111111100000000000011111111111111111111000000T000000000000000000000000000000000000000111111111111111111110000000000001111111111111111111100000000000001111111111111
111111111111100000000000000000000000000000000000000000000000000000000000000000000000000000001111111111111111111100000000000011111111111111111111000000T000000000000000000000000000000000000000111111111111111111110000000000001111111111111111111100000000000001111111111111
1111111111111000000000000000000000000000000000000000000000000000000000000000000000000000000011111111111111111111000000000000111111111111111111110000000000000000000000000000000000000000000000111111111111111111110000000000001111111111111111111100000000000001111111111111
1111111111111000000000000000000000000000000000000000000000000000000000000000000000000000000011111111111111111111000000000000111111111111111111110000000000000000000000000000000000000000000000111111111111111111110000000000001111111111111111111100000000000001111111111111
1111111111111000000000000000000000000000000000000000000000000000000000000000000000000000000011111111111111111111000000000000111111111111111111110000000000000000000000000000000000000000000000111111111111111111110000000000001111111111111111111100000000000001111111111111
1111111111111000000000000000000000000000000000000000000000000000000000000000000000000000000011111111111111111111000000000000111111111111111111110000000000000000000000000000000000000000000000111111111111111111110000000000001111111111111111111100000000000001111111111111
1111111111111000000000000000000000000000000000000000000000000000000000000000000000000000000011111111111111111111000000000000111111111111111111110000000000000000000000000000000000000000000000111111111111111111110000000000001111111111111111111100000000000001111111111111
1111111111111000000000000000000001111110000000000000000000000000001111111111111111111111111111111111111111111111000000000000111111111111111111110000000000000000000011111111111111111111111111111111111111111111110000000000001111111111111111111100000000000001111111111111
1111111111111000000000000000000111111111110000000000000000000001111111111111111111111111111111111111111111111111000000000000111111111111111111110000000000000000011111111111111111111111111111111111111111111111110000000000001111111111111111111100000000000001111111111111
1111111111111000000000000000011111111111111000000000000000000011111111111111111111111111111111111111111111111111000000000000111111111111111111110000000000000000111111111111111111111111111111111111111111111111110000000000001111111111111111111100000000000001111111111111
1111111111111000000000000000111111111111111100000000000000000111111111111111111111111111111111111111111111111111000000000000111111111111111111110000000000000001111111111111111111111111111111111111111111111111110000000000001111111111111111111100000000000001111111111111
1111111111111000000000000000111111111111111110000000000000001111111111111111111111111111111111111111111111111111000000000000111111111111111111110000000000000011111111111111111111111111111111111111111111111111110000000000001111111111111111111100000000000001111111111111
1111111111111000000000000001111111111111111110000000000000001111111111111111111111111111111111111111111111111111000000000000111111111111111111110000000000000011111111111111111111111111111111111111111111111111110000000000001111111111111111111100000000000001111111111111
1111111111111000000000000001111111111111111111000000000000011111111111111111111111111111111111111111111111111111000000000000111111111111111111110000000000000111111111111111111111111111111111111111111111111111110000000000001111111111111111111100000000000001111111111111
1111111111111000000000000011111111111111111111000000000000011111111111111111111111111111111111111111111111111111000000000000111111111111111111110000000000000111111111111111111111111111111111111111111111111111110000000000001111111111111111111100000000000001111111111111
1111111111111000000000000011111111111111111111000000000000011111111111111111111111111111111111111111111111111111000000000000111111111111111111110000000000000111111111111111111111111111111111111111111111111111110000000000001111111111111111111100000000000001111111111111
1111111111111000000000000011111111111111111111000000000000011111111111111111111111111111111111111111111111111111000000000000111111111111111111110000000000000111111111111111111111111111111111111111111111111111110000000000001111111111111111111100000000000001111111111111
1111111111111000000000000011111111111111111111000000000000011111111111111111111111111111111111111111111111111111000000000000111111111111111111110000000000000111111111111111111111111111111111111111111111111111110000000000001111111111111111111100000000000001111111111111
1111111111111000000000000011111111111111111111000000000000011111111111111111111111111111111111111111111111111111000000000000111111111111111111110000000000000111111111111111111111111111111111111111111111111111110000000000001111111111111111111100000000000001111111111111
1111111111111000000000000011111111111111111111000000000000011111111111111111111111111111111111111111111111111111000000000000111111111111111111110000000000000111111111111111111111111111111111111111111111111111100000000000000111111111111111111100000000000001111111111111
1111111111111000000000000011111111111111111111000000000000011111111111111111111111111111111111111111111111111111000000000000111111111111111111110000000000000111111111111111111111111111111111111111111111111111100000000000000111111111111111111100000000000001111111111111
1111111111111000000000000011111111111111111111000000000000001111111111111111111111111111111111111111111111111111000000000000111111111111111111110000000000000011111111111111111111111111111111111111111111111111100000000000000111111111111111111100000000000001111111111111
1111111111111000000000000011111111111111111111000000000000001111111111111111111111111111111111111111111111111111000000000000111111111111111111110000000000000011111111111111111111111111111111111111111111111111000000000000000011111111111111111000000000000001111111111111
1111111111111000000000000011111111111111111111000000000000000111111111111111111111111111111111111111111111111111000000000000111111111111111111110000000000000001111111111111111111111111111111111111111111111111000000000000000011111111111111110000000000000001111111111111
1111111111111000000000000011111111111111111111000000000000000011111111111111111111111111111111111111111111111111000000000000111111111111111111110000000000000000111111111111111111111111111111111111111111111110000000000000000001111111111111100000000000000001111111111111
1111111111111000000000000011111111111111111111000000000000000001111111111111111111111111111111111111111111111111000000000000111111111111111111110000000000000000011111111111111111111111111111111111111111111000000000000000000000011111111111000000000000000001111111111111
1111111111111000000000000011111111111111111111000000000000000000011111111111111111111111111111111111111111111111000000000000111111111111111111110000000000000000000111111111111111111111111111111111111111100000000000000000000000000111111100000000000000000001111111111111
1111111111111000000000000011111111111111111111000000000000000000000000000000000000000000000011111111111111111111000000000000111111111111111111110000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000001111111111111
1111111111111000000000000011111111111111111111000000000000000000000000000000000000000000000011111111111111111111000000000000111111111111111111110000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000001111111111111
1111111111111000000000000011111111111111111111000000000000000000000000000000000000000000000011111111111111111111000000000000111111111111111111110000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000001111111111111
1111111111111000000000000011111111111111111111000000000000000000000000000000000000000000000011111111111111111111000000000000111111111111111111110000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000001111111111111
1111111111111000000000000011111111111111111111000000000000000000000000000000000000000000000011111111111111111111000000000000111111111111111111110000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000001111111111111
1111111111111000000000000011111111111111111111000000000000000000000000000000000000000000000011111111111111111111000000000000111111111111111111110000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000001111111111111
1111111111111000GG0000000011111111111111111111000000000000000000000000000000000000000000000011111111111111111111000000000000111111111111111111110000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000001111111111111
1111111111111000GG0000000011111111111111111111000000000000000000000000000000000000000000000011111111111111111111000000000000111111111111111111110000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000001111111111111
1111111111111000000000000011111111111111111111000000000000000000000000000000000000000000000011111111111111111111000000000000111111111111111111110000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000001111111111111
1111111111111000000000000011111111111111111111000000000000000000000000000000000000000000000011111111111111111111000000000000111111111111111111110000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000001111111111111
1111111111111000000000000011111111111111111111000000000000000000000000000000000000000000000011111111111111111111000000000000111111111111111111110000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000001111111111111
1111111111111000000000000011111111111111111111000000000000000000000000000000000000000000000011111111111111111111000000000000111111111111111111110000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000001111111111111
1111111111111000000000000011111111111111111111000000000000000000000000000000000000000000000011111111111111111111000000000000111111111111111111110000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000001111111111111
1111111111111000000000000011111111111111111111000000000000000000011111111111111111111111111111111111111111111111111111111111111111111111111111110000000000000000000111111111111111111111111111111111111111100000000000000000000000000111111100000000000000000001111111111111
1111111111111000000000000011111111111111111111000000000000000001111111111111111111111111111111111111111111111111111111111111111111111111111111110000000000000000011111111111111111111111111111111111111111111000000000000000000000011111111111000000000000000001111111111111
1111111111111000000000000011111111111111111111000000000000000011111111111111111111111111111111111111111111111111111111111111111111111111111111110000000000000000111111111111111111111111111111111111111111111110000000000000000001111111111111100000000000000001111111111111
1111111111111000000000000011111111111111111111000000000000000111111111111111111111111111111111111111111111111111111111111111111111111111111111110000000000000001111111111111111111111111111111111111111111111111000000000000000011111111111111110000000000000001111111111111
1111111111111000000000000011111111111111111111000000000000001111111111111111111111111111111111111111111111111111111111111111111111111111111111110000000000000011111111111111111111111111111111111111111111111111000000000000000011111111111111111000000000000001111111111111
1111111111111000000000000011111111111111111111000000000000001111111111111111111111111111111111111111111111111111111111111111111111111111111111110000000000000011111111111111111111111111111111111111111111111111100000000000000111111111111111111100000000000001111111111111
1111111111111000000000000011111111111111111111000000000000011111111111111111111111111111111111111111111111111111111111111111111111111111111111110000000000000111111111111111111111111111111111111111111111111111100000000000000111111111111111111100000000000001111111111111
1111111111111000000000000011111111111111111111000000000000011111111111111111111111111111111111111111111111111111111111111111111111111111111111110000000000000111111111111111111111111111111111111111111111111111100000000000000111111111111111111100000000000001111111111111
1111111111111000000000000011111111111111111111000000000000011111111111111111111111111111111111111111111111111111111111111111111111111111111111110000000000000111111111111111111111111111111111111111111111111111110000000000001111111111111111111100000000000001111111111111
1111111111111000000000000011111111111111111111000000000000011111111111111111111111111111111111111111111111111111111111111111111111111111111111110000000000000111111111111111111111111111111111111111111111111111110000000000001111111111111111111100000000000001111111111111
1111111111111000000000000011111111111111111111000000000000011111111111111111111111111111111111111111111111111111111111111111111111111111111111110000000000000111111111111111111111111111111111111111111111111111110000000000001111111111111111111100000000000001111111111111
1111111111111000000000000011111111111111111111000000000000011111111111111111111111111111111111111111111111111111111111111111111111111111111111110000000000000111111111111111111111111111111111111111111111111111110000000000001111111111111111111100000000000001111111111111
1111111111111000000000000011111111111111111111000000000000011111111111111111111111111111111111111111111111111111111111111111111111111111111111110000000000000111111111111111111111111111111111111111111111111111110000000000001111111111111111111100000000000001111111111111
1111111111111000000000000011111111111111111111000000000000011111111111111111111111111111111111111111111111111111111111111111111111111111111111110000000000000111111111111111111111111111111111111111111111111111110000000000001111111111111111111100000000000001111111111111
1111111111111000000000000011111111111111111111000000000000011111111111111111111111111111111111111111111111111111111111111111111111111111111111100000000000000111111111111111111111111111111111111111111111111111110000000000001111111111111111111100000000000001111111111111
1111111111111000000000000011111111111111111111000000000000011111111111111111111111111111111111111111111111111111111111111111111111111111111111100000000000000111111111111111111111111111111111111111111111111111110000000000001111111111111111111100000000000001111111111111
1111111111111000000000000011111111111111111111000000000000011111111111111111111111111111111111111111111111111111111111111111111111111111111111000000000000000111111111111111111111111111111111111111111111111111110000000000001111111111111111111100000000000001111111111111
1111111111111000000000000011111111111111111111000000000000011111111111111111111111111111111111111111111111111111111111111111111111111111111110000000000000000111111111111111111111111111111111111111111111111111110000000000001111111111111111111100000000000001111111111111
1111111111111000000000000011111111111111111111000000000000011111111111111111111111111111111111111111111111111111111111111111111111111111111100000000000000000111111111111111111111111111111111111111111111111111110000000000001111111111111111111100000000000001111111111111
1111111111111000000000000011111111111111111111000000000000011111111111111111111111111111111111111111111111111111111111111111111111111111100000000000000000000111111111111111111111111111111111111111111111111111110000000000001111111111111111111100000000000001111111111111
1111111111111000000000000011111111111111111111000000000000011111111111111111111000000000000000000000000000000000000000000000000000000000000000000000000000000111111111111111111110000000000000111111111111111111110000000000001111111111111111111100000000000001111111111111
1111111111111000000000000011111111111111111111000000000000011111111111111111111000000000000000000000000000000000000000000000000000000000000000000000000000000111111111111111111110000000000000111111111111111111110000000000001111111111111111111100000000000001111111111111
1111111111111000000000000011111111111111111111000000000000011111111111111111111000000000000000000000000000000000000000000000000000000000000000000000000000000111111111111111111110000000000000111111111111111111110000000000001111111111111111111100000000000001111111111111
1111111111111000000000000011111111111111111111000000000000011111111111111111111000000000000000000000000000000000000000000000000000000000000000000000000000000111111111111111111110000000000000111111111111111111110000000000001111111111111111111100000000000001111111111111
1111111111111000000000000011111111111111111111000000000000011111111111111111111000000000000000000000000000000000000000000000000000000000000000000000000000000111111111111111111110000000000000111111111111111111110000000000001111111111111111111100000000000001111111111111
111111111111100000000000001111111111111111111100000T000000011111111111111111111000000000000000000000000000000000000000000000000000000000000000000000000000000111111111111111111110000000000000111111111111111111110000000000001111111111111111111100000000000001111111111111
1111111111111000000000000011111111111111111111000000000000011111111111111111111000000000000000000000000000000000000000000000000000000000000000000000000000000111111111111111111110000000000000111111111111111111110000000000001111111111111111111100000000000001111111111111
1111111111111000000000000011111111111111111111000000000000011111111111111111111000000000000000000000000000000000000000000000000000000000000000000000000000000111111111111111111110000000000000111111111111111111110000000000001111111111111111111100000000000001111111111111
1111111111111000000000000011111111111111111111000000000000011111111111111111111000000000000000000000000000000000000000000000000000000000000000000000000000000111111111111111111110000000000000111111111111111111110000000000001111111111111111111100000000000001111111111111
1111111111111000000000000011111111111111111111000000000000011111111111111111111000000000000000000000000000000000000000000000000000000000000000000000000000000111111111111111111110000000000000111111111111111111110000000000001111111111111111111100000000000001111111111111
1111111111111000000000000011111111111111111111000000000000011111111111111111111000000000000000000000000000000000000000000000000000000000000000000000000000000111111111111111111110000000000000111111111111111111110000000000001111111111111111111100000000000001111111111111
1111111111111000000000000011111111111111111111000000000000011111111111111111111000000000000000000000000000000000000000000000000000000000000000000000000000000111111111111111111110000000000000111111111111111111110000000000001111111111111111111100000000000001111111111111
1111111111111000000000000011111111111111111111000000000000011111111111111111111000000000000000000000000000000000000000000000000000000000000000000000000000000111111111111111111110000000000000111111111111111111110000000000001111111111111111111100000000000001111111111111
1111111111111000000000000011111111111111111111111111111111111111111111111111111000000000000000000111111111111111111111111111111111111111111000000000000000000111111111111111111111111111111111111111111111111111111111111111111111111111111111111100000000000001111111111111
1111111111111000000000000011111111111111111111111111111111111111111111111111111000000000000000011111111111111111111111111111111111111111111110000000000000000111111111111111111111111111111111111111111111111111111111111111111111111111111111111100000000000001111111111111
1111111111111000000000000011111111111111111111111111111111111111111111111111111000000000000000111111111111111111111111111111111111111111111111000000000000000111111111111111111111111111111111111111111111111111111111111111111111111111111111111100000000000001111111111111
1111111111111000000000000011111111111111111111111111111111111111111111111111111000000000000001111111111111111111111111111111111111111111111111100000000000000111111111111111111111111111111111111111111111111111111111111111111111111111111111111100000000000001111111111111
1111111111111000000000000011111111111111111111111111111111111111111111111111111000000000000001111111111111111111111111111111111111111111111111100000000000000111111111111111111111111111111111111111111111111111111111111111111111111111111111111100000000000001111111111111
1111111111111000000000000011111111111111111111111111111111111111111111111111111000000000000011111111111111111111111111111111111111111111111111110000000000000111111111111111111111111111111111111111111111111111111111111111111111111111111111111100000000000001111111111111
1111111111111000000000000011111111111111111111111111111111111111111111111111111000000000000011111111111111111111111111111111111111111111111111110000000000000111111111111111111111111111111111111111111111111111111111111111111111111111111111111100000000000001111111111111
1111111111111000000000000011111111111111111111111111111111111111111111111111111000000000000011111111111111111111111111111111111111111111111111110000000000000111111111111111111111111111111111111111111111111111111111111111111111111111111111111100000000000001111111111111
1111111111111000000000000011111111111111111111111111111111111111111111111111111000000000000011111111111111111111111111111111111111111111111111110000000000000111111111111111111111111111111111111111111111111111111111111111111111111111111111111100000000000001111111111111
1111111111111000000000000011111111111111111111111111111111111111111111111111111000000000000011111111111111111111111111111111111111111111111111110000000000000111111111111111111111111111111111111111111111111111111111111111111111111111111111111100000000000001111111111111
1111111111111000000000000011111111111111111111111111111111111111111111111111111000000000000011111111111111111111111111111111111111111111111111110000000000000111111111111111111111111111111111111111111111111111111111111111111111111111111111111100000000000001111111111111
1111111111111000000000000011111111111111111111111111111111111111111111111111111000000000000011111111111111111111111111111111111111111111111111110000000000000111111111111111111111111111111111111111111111111111111111111111111111111111111111111100000000000001111111111111
1111111111111000000000000011111111111111111111111111111111111111111111111111111000000000000011111111111111111111111111111111111111111111111111110000000000000111111111111111111111111111111111111111111111111111111111111111111111111111111111111100000000000001111111111111
1111111111111000000000000001111111111111111111111111111111111111111111111111110000000000000011111111111111111111111111111111111111111111111111110000000000000111111111111111111111111111111111111111111111111111111111111111111111111111111111111100000000000001111111111111
1111111111111000000000000001111111111111111111111111111111111111111111111111110000000000000011111111111111111111111111111111111111111111111111100000000000000011111111111111111111111111111111111111111111111111111111111111111111111111111111111000000000000001111111111111
1111111111111000000000000000111111111111111111111111111111111111111111111111100000000000000011111111111111111111111111111111111111111111111111100000000000000001111111111111111111111111111111111111111111111111111111111111111111111111111111111000000000000001111111111111
1111111111111000000000000000011111111111111111111111111111111111111111111111000000000000000011111111111111111111111111111111111111111111111111000000000000000001111111111111111111111111111111111111111111111111111111111111111111111111111111110000000000000001111111111111
1111111111111000000000000000001111111111111111111111111111111111111111111110000000000000000011111111111111111111111111111111111111111111111110000000000000000000111111111111111111111111111111111111111111111111111111111111111111111111111111100000000000000001111111111111
1111111111111000000000000000000111111111111111111111111111111111111111111100000000000000000011111111111111111111111111111111111111111111111000000000000000000000001111111111111111111111111111111111111111111111111111111111111111111111111110000000000000000001111111111111
1111111111111000000000000000000000011111111111111111111111111111111111000000000000000000000011111111111111111111111111111111111111111110000000000000000000000000000000111111111111111111111111111111111111111111111111111111111111111111100000000000000000000001111111111111
1111111111111000000000000000000000000000000000000000000000000000000000000000000000000000000011111111111111111111000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000001111111111111
1111111111111000000000000000000000000000000000000000000000000000000000000000000000000000000011111111111111111111000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000001111111111111
1111111111111000000000000000000000000000000000000000000000000000000000000000000000000000000011111111111111111111000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000001111111111111
1111111111111000000000000000000000000000000000000000000000000000000000000000000000000000000011111111111111111111000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000001111111111111
1111111111111000000000000000000000000000000000000000000000000000000000000000000000000000000011111111111111111111000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000001111111111111
11111111111110000000000000000000000000000000000000000000000000000000000000000000000000000000111111111111111111110000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000T0000001111111111111
1111111111111000000000000000000000000000000000000000000000000000000000000000000000000000000011111111111111111111000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000001111111111111
1111111111111000000000000000000000000000000000000000000000000000000000000000000000000000000011111111111111111111000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000001111111111111
1111111111111000000000000000000000000000000000000000000000000000000000000000000000000000000011111111111111111111000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000001111111111111
1111111111111000000000000000000000000000000000000000000000000000000000000000000000000000000011111111111111111111000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000001111111111111
1111111111111000000000000000000000000000000000000000000000000000000000000000000000000000000011111111111111111111000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000001111111111111
1111111111111000000000000000000000000000000000000000000000000000000000000000000000000000000011111111111111111111000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000001111111111111
1111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111
1111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111
1111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111
1111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111
1111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111
1111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111
1111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111
1111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111
1111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111
1111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111
1111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111
1111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111
1111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111
1111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111
5.3 txt数据读取与地图数组的创建
- 我们创建一个地图类，完成一个初始化地图和地图展示的函数
class GameMap
{
public:
GameMap(const std::string& filename)
{
if (init_map(filename)==-1)
{
std::abort();
}
}
~GameMap()
{
// 清理分配的内存
delete[] mapData;
}
private:
// 创建一维数组保存地图数据
unsigned char* mapData;
int width = 0;
int height = 0;
};
- 初始化函数，这里我选择将地图存储为一维数组
mapData = new unsigned char[width * height]，这里效仿了Nav2的costmap2D存储方式，感兴趣的朋友可以去看看我的另一篇文章Nav2代价地图实现和原理–Nav2源码解读之CostMap2D(上)-CSDN博客
int init_map(const std::string& filename)
{
std::ifstream file(filename);
if (!file.is_open()) {
std::cerr << "无法打开文件 " << filename << std::endl;
return -1;
}
// 使用字符串流读取整个文件内容
std::stringstream buffer;
buffer << file.rdbuf();
file.close();
// 计算地图尺寸
std::string line;
while (std::getline(buffer, line)) {
if (line.empty()) continue; // 跳过空行
std::istringstream iss(line);
char ch;
int line_width = 0;
while (iss >> ch) {
++line_width;
}
if (line_width > width) width = line_width; // 更新最大宽度
++height; // 增加高度
}
// 重置流并重新读取文件内容
buffer.clear();
buffer.str("");
file.open(filename);
buffer << file.rdbuf();
file.close();
mapData = new unsigned char[width * height];
std::cout << "map init:" << width << "*" << height<<std::endl;
// 填充一维数组
int idx = 0;
while (std::getline(buffer, line)) {
std::istringstream iss(line);
char ch;
while (iss >> ch) {
mapData[idx++] = ch;
}
}
}
- 地图展示函数
int DisplayMap(int windowWidth, int windowHeight) {
// 创建OpenCV图像
cv::Mat img(height, width, CV_8UC3, cv::Scalar(255, 255, 255));
if (img.empty()) {
std::cerr << "无法创建图像" << std::endl;
delete[] mapData; // 清理分配的内存
return -1;
}
// 填充图像
int idx = 0;
for (int i = 0; i < height; ++i) {
for (int j = 0; j < width; ++j) {
unsigned char value = mapData[idx++];
switch (value) {
case 'T': // 道具
img.at<cv::Vec3b>(i, j) = cv::Vec3b(255, 0, 0); // 蓝色
break;
case 'S': // 绿色坦克起点
img.at<cv::Vec3b>(i, j) = cv::Vec3b(0, 255, 0); // 绿色
break;
case 'G': // 红色坦克终点
img.at<cv::Vec3b>(i, j) = cv::Vec3b(0, 0, 255); // 红色
break;
case '1': // 墙壁
img.at<cv::Vec3b>(i, j) = cv::Vec3b(0, 0, 0); // 黑色
break;
case '0': // 可行驶区域
img.at<cv::Vec3b>(i, j) = cv::Vec3b(255, 255, 255); // 白色
break;
default:
std::cerr << "未知符号: " << value << std::endl;
break;
}
}
}
// 显示图像
cv::namedWindow("Map", cv::WINDOW_NORMAL); // 创建一个可以手动调整大小的窗口
cv::resizeWindow("Map", windowWidth, windowHeight); // 设置窗口的初始大小
cv::imshow("Map", img);
cv::waitKey(0);
return 0;
}
- 同时我们写几个获取起点终点的函数
Point getStart() {
Point startPoint = { -1, -1 };
int idx = 0;
for (int i = 0; i < height; ++i) {
for (int j = 0; j < width; ++j) {
unsigned char value = mapData[idx++];
if (value == 'S') {
startPoint.x = i;
startPoint.y = j;
std::cout << "start goal found:" << startPoint.x << "," << startPoint.y << std::endl;
if (startPoint.x != -1 && startPoint.y != -1)
{
return startPoint;
}
}
}
}
return startPoint;
}
Point getGoal() {
Point goalPoint = { -1, -1 };
int idx = 0;
for (int i = 0; i < height; ++i) {
for (int j = 0; j < width; ++j) {
unsigned char value = mapData[idx++];
if (value == 'G') {
goalPoint.x = i;
goalPoint.y = j;
std::cout << "goal goal found:" << goalPoint.x << "," << goalPoint.y << std::endl;
if (goalPoint.x != -1 && goalPoint.y != -1)
{
return goalPoint;
}
}
}
}
return goalPoint;
}
5.4 路径规划
- 我们先来进行单独起点到终点的
A_star路径规划尝试 - 首先我们为
GameMap类添加几个函数用于辅助一会的算法-
由于题目规定的机器人是八领域，所以当返回一个点的领域时，需要返回上下左右以及斜着的一个八种情况，同时要考虑点在地图边缘或者是障碍物的情况。
-
同时我们设置
getCost,选择不将原地图mapData暴露出去，这是个良好的封装意识
-
std::vector<Point> getNeighbors(const Point& p) {
std::vector<Point> neighbors;
if (isValid(p.x - 1, p.y)) neighbors.push_back({ p.x - 1, p.y }); // 上
if (isValid(p.x + 1, p.y)) neighbors.push_back({ p.x + 1, p.y }); // 下
if (isValid(p.x, p.y - 1)) neighbors.push_back({ p.x, p.y - 1 }); // 左
if (isValid(p.x, p.y + 1)) neighbors.push_back({ p.x, p.y + 1 }); // 右
if (isValid(p.x - 1, p.y - 1)) neighbors.push_back({ p.x - 1, p.y - 1 }); // 左上
if (isValid(p.x - 1, p.y + 1)) neighbors.push_back({ p.x - 1, p.y + 1 }); // 右上
if (isValid(p.x + 1, p.y - 1)) neighbors.push_back({ p.x + 1, p.y - 1 }); // 左下
if (isValid(p.x + 1, p.y + 1)) neighbors.push_back({ p.x + 1, p.y + 1 }); // 右下
return neighbors;
}
// 判断位置是否有效的方法
bool isValid(int x, int y) {
if (x < 0 || x >= height || y < 0 || y >= width) return false;
return mapData[x * width + y] != '1'; // '1'代表墙壁
}
// 获取两点间成本的方法
int getCost(const Point& a, const Point& b) {
return (mapData[b.x * width + b.y] == '1') ? INT_MAX : 1; // 墙壁成本无限大
}
- 然后我们定义一个
Node节点类，包含点的坐标和这个点的总成本，稍后我们将把这个点存储到优先队列中
struct Node {
Point point;
int f; // 总成本，包括 g 值和 h 值
// 重载 < 运算符，用于优先队列排序
bool operator<(const Node& other) const {
return f > other.f;
}
};
5.4.1 A* 算法实现流程细节说明
- 我们来仔细分析以下
A*的实现流程 - A* 算法通过维护两个集合来工作：
- 开放集 (
frontier): 包含所有被考虑过的节点，这些节点还未被彻底探索。 - 关闭集 (
came_from和cost_so_far): 包含所有已知的从起点到这些节点的最短路径和成本。
- 开放集 (
- 算法的基本步骤如下：
- 将起点添加到
开放集中。 - 如果
开放集为空，则没有路径。否则，从开放集中选择具有最低f值的节点（f是从起点到当前节点的实际成本加上从当前节点到终点的估计成本）。 - 将该节点从
开放集移到关闭集。 - 对该节点的所有邻居进行以下操作：
- 如果邻居在关闭集中，忽略它。
- 计算从起点到邻居的新成本。
- 如果邻居不在开放集中或新成本更低，则更新其成本和前驱节点，并将其添加到开放集中。
- 重复步骤 2，直到找到终点或开放集为空。
- 将起点添加到
5.4.2 A* 算法代码实现
- 那下面我们来看具体实现：
heuristic为启发式函数：- 欧几里得距离
std::sqrt(std::pow(a.x - b.x, 2) + std::pow(a.y - b.y, 2)) - 曼哈顿距离
std::abs(a.x - b.x) + std::abs(a.y - b.y)
auto heuristic = [](const Point& a, const Point& b) { //return std::abs(a.x - b.x) + std::abs(a.y - b.y); return std::sqrt(std::pow(a.x - b.x, 2) + std::pow(a.y - b.y, 2)); };- 欧几里得距离
std::priority_queue<Node> frontier;开放集frontier.push({ start, 0 });把起点添加到开放集中
std::priority_queue<Node> frontier; frontier.push({ start, 0 });- 创建两个
unordered_mapcame_from用于存储每个节点的前驱节点cost_so_far用于存储从起点到每个节点的最短已知成本。
cost_so_far[start] = 0;初始化起点的前驱节点为它自己，成本为 0。std::unordered_map<Point, Point> came_from; std::unordered_map<Point, int> cost_so_far; came_from[start] = start; cost_so_far[start] = 0;- 当开放集非空时，循环继续。
- 从优先队列中取出
f值最低的节点。
while (!frontier.empty()) { Node current = frontier.top(); frontier.pop();- 如果当前节点是终点，则算法完成。
if (current.point == goal) { break; } - 对于当前节点的每个邻居：
- 计算从起点经过当前节点到邻居的新成本。
- 如果邻居不在
cost_so_far中，或者新成本更低，则更新cost_so_far和came_from，并将邻居添加到开放集中。
for (Point next : gamemap.getNeighbors(current.point))
{
int new_cost = cost_so_far[current.point] + gamemap.getCost(current.point, next); if (cost_so_far.find(next) == cost_so_far.end() || new_cost < cost_so_far[next])
{
cost_so_far[next] = new_cost;
int priority = new_cost + heuristic(next, goal);
frontier.push({ next, priority });
came_from[next] = current.point;
}
}
- 算法结束后，返回
came_from映射，其中包含了从起点到每个节点的最短路径信息。`
5.4.3 A* 算法完整代码
- 如下
std::unordered_map<Point, Point> aStarSearch(GameMap& gamemap, const Point& start, const Point& goal) {
auto heuristic = [](const Point& a, const Point& b) {
//return std::abs(a.x - b.x) + std::abs(a.y - b.y);
return std::sqrt(std::pow(a.x - b.x, 2) + std::pow(a.y - b.y, 2));
};
std::priority_queue<Node> frontier;
frontier.push({ start, 0 });
std::unordered_map<Point, Point> came_from;
std::unordered_map<Point, int> cost_so_far;
came_from[start] = start;
cost_so_far[start] = 0;
while (!frontier.empty()) {
Node current = frontier.top();
frontier.pop();
if (current.point == goal) {
break;
}
for (Point next : gamemap.getNeighbors(current.point)) {
int new_cost = cost_so_far[current.point] + gamemap.getCost(current.point, next);
if (cost_so_far.find(next) == cost_so_far.end() || new_cost < cost_so_far[next]) {
cost_so_far[next] = new_cost;
int priority = new_cost + heuristic(next, goal);
frontier.push({ next, priority });
came_from[next] = current.point;
}
}
}
return came_from;
}
5.4.4 得到A* 算法路径
- 我们写一个函数来获取路径
std::vector<Point> reconstruct_path(const Point& start, const Point& goal, const std::unordered_map<Point, Point>& came_from) {
std::vector<Point> path;
Point current = goal;
path.push_back(current);
while (current != start) {
current = came_from.at(current);
path.push_back(current);
}
std::reverse(path.begin(), path.end()); // 反转路径，使其从起点到终点
return path;
}
5.4.5 效果展示
- 其中红色表示搜索路径，黄色表示最短路径
-
欧式距离作为启发函数
-
曼哈顿距离作为启发函数
-
5.5 完成吃掉三个道具的任务实现
- 这里采用最懒的方法去全遍历，计算最短路径
std::vector<std::vector<Point>> generate_permutations(const std::vector<Point>& items) {
std::vector<std::vector<Point>> result;
std::function<void(int, std::vector<Point>&)> permute = [&](int start, std::vector<Point>& current) {
if (start == items.size()) {
result.push_back(current);
return;
}
for (int i = start; i < items.size(); ++i) {
std::swap(current[start], current[i]);
permute(start + 1, current);
std::swap(current[start], current[i]);
}
};
std::vector<Point> current(items);
permute(0, current);
return result;
}
void run()
{
auto start = game_map->getStart();
auto goals = game_map->getGameItem();
auto goal = game_map->getGoal();
std::vector<Point> shortest_path;
int shortest_path_length = INT_MAX;
std::unordered_map<Point, Point> final_came_from;
// 生成所有可能的目标点顺序组合
std::vector<std::vector<Point>> permutations = generate_permutations(goals);
for (const auto& order : permutations) {
std::vector<Point> current_path;
Point current = start;
int current_path_length = 0;
for (const auto& goal : order) {
auto came_from = planner->aStarSearch(*game_map, current, goal);
auto path = reconstruct_path(current, goal, came_from);
// 去除路径中的重复起点
if (!current_path.empty() && path.front() == current_path.back()) {
path.erase(path.begin());
}
current_path.insert(current_path.end(), path.begin(), path.end());
current = goal;
current_path_length += path.size() - 1; // 减去重复的终点
}
// 从最后一个目标点到终点的路径
final_came_from = planner->aStarSearch(*game_map, current, goal);
auto final_path = reconstruct_path(current, goal, final_came_from);
if (!final_path.empty() && final_path.front() == current_path.back()) {
final_path.erase(final_path.begin());
}
current_path.insert(current_path.end(), final_path.begin(), final_path.end());
current_path_length += final_path.size() - 1; // 减去重复的终点
std::cout << "current_path_length:"<< current_path_length << std::endl;
if (current_path_length < shortest_path_length) {
shortest_path_length = current_path_length;
shortest_path = current_path;
}
}
if (!shortest_path.empty()) {
cv::Mat img = game_map->getDisplayMap(windowWidth, windowHeight);
for (const auto& pair : final_came_from)
{
cv::circle(img, cv::Point(pair.second.y, pair.second.x), 1, cv::Scalar(0, 0, 255), 1);
}
for (const auto& point : shortest_path) {
cv::circle(img, cv::Point(point.y, point.x), 1, cv::Scalar(0, 255, 255), 1);
}
// 显示图像
cv::namedWindow("Map", cv::WINDOW_NORMAL);
cv::resizeWindow("Map", windowWidth, windowHeight);
cv::imshow("Map", img);
cv::waitKey(0);
}
else {
std::cerr << "没有找到路径。" << std::endl;
}
}
完整代码呈现
- 如下
#include <opencv2/opencv.hpp>
#include <fstream>
#include <vector>
#include <iostream>
#include <sstream>
struct Point {
int x;
int y;
// 重载 == 运算符
bool operator==(const Point& other) const {
return x == other.x && y == other.y;
}
bool operator!=(const Point& other) const {
return x != other.x || y != other.y;
}
bool operator<(const Point& other) const {
if (x == other.x) return y < other.y;
return x < other.x;
}
};
class GameMap
{
public:
GameMap(const std::string& filename)
{
if (init_map(filename) == -1)
{
std::abort();
}
}
~GameMap()
{
// 清理分配的内存
delete[] mapData;
}
int init_map(const std::string& filename)
{
std::ifstream file(filename);
if (!file.is_open()) {
std::cerr << "无法打开文件 " << filename << std::endl;
return -1;
}
// 使用字符串流读取整个文件内容
std::stringstream buffer;
buffer << file.rdbuf();
file.close();
// 计算地图尺寸
std::string line;
while (std::getline(buffer, line)) {
if (line.empty()) continue; // 跳过空行
std::istringstream iss(line);
char ch;
int line_width = 0;
while (iss >> ch) {
++line_width;
}
if (line_width > width) width = line_width; // 更新最大宽度
++height; // 增加高度
}
// 重置流并重新读取文件内容
buffer.clear();
buffer.str("");
file.open(filename);
buffer << file.rdbuf();
file.close();
mapData = new unsigned char[width * height];
std::cout << "map init:" << width << "*" << height << std::endl;
// 填充一维数组
int idx = 0;
while (std::getline(buffer, line)) {
std::istringstream iss(line);
char ch;
while (iss >> ch) {
mapData[idx++] = ch;
}
}
}
cv::Mat getDisplayMap(int windowWidth, int windowHeight) {
// 创建OpenCV图像
cv::Mat img(height, width, CV_8UC3, cv::Scalar(255, 255, 255));
if (img.empty()) {
std::cerr << "无法创建图像" << std::endl;
delete[] mapData; // 清理分配的内存
return cv::Mat();
}
// 填充图像
int idx = 0;
for (int i = 0; i < height; ++i) {
for (int j = 0; j < width; ++j) {
unsigned char value = mapData[idx++];
switch (value) {
case 'T': // 道具
img.at<cv::Vec3b>(i, j) = cv::Vec3b(255, 0, 0); // 黄色
break;
case 'S': // 绿色坦克起点
img.at<cv::Vec3b>(i, j) = cv::Vec3b(0, 255, 0); // 绿色
break;
case 'G': // 红色坦克终点
img.at<cv::Vec3b>(i, j) = cv::Vec3b(0, 0, 255); // 红色
break;
case '1': // 墙壁
img.at<cv::Vec3b>(i, j) = cv::Vec3b(0, 0, 0); // 黑色
break;
case '0': // 可行驶区域
img.at<cv::Vec3b>(i, j) = cv::Vec3b(255, 255, 255); // 白色
break;
default:
std::cerr << "未知符号: " << value << std::endl;
break;
}
}
}
return img;
}
Point getStart() {
Point startPoint = { -1, -1 };
int idx = 0;
for (int i = 0; i < height; ++i) {
for (int j = 0; j < width; ++j) {
unsigned char value = mapData[idx++];
if (value == 'S') {
startPoint.x = i;
startPoint.y = j;
std::cout << "start goal found:" << startPoint.x << "," << startPoint.y << std::endl;
if (startPoint.x != -1 && startPoint.y != -1)
{
return startPoint;
}
}
}
}
return startPoint;
}
Point getGoal() {
Point goalPoint = { -1, -1 };
int idx = 0;
for (int i = 0; i < height; ++i) {
for (int j = 0; j < width; ++j) {
unsigned char value = mapData[idx++];
if (value == 'G') {
goalPoint.x = i;
goalPoint.y = j;
std::cout << "goal goal found:" << goalPoint.x << "," << goalPoint.y << std::endl;
if (goalPoint.x != -1 && goalPoint.y != -1)
{
return goalPoint;
}
}
}
}
return goalPoint;
}
std::vector<Point> getGameItem() {
std::vector<Point> game_items;
int idx = 0;
for (int i = 0; i < height; ++i) {
for (int j = 0; j < width; ++j) {
unsigned char value = mapData[idx++];
if (value == 'T') {
Point goalPoint = { 0, 0 };
goalPoint.x = i;
goalPoint.y = j;
std::cout << "goal goal found:" << goalPoint.x << "," << goalPoint.y << std::endl;
game_items.push_back(goalPoint);
}
}
}
return game_items;
}
std::vector<Point> getNeighbors(const Point& p) {
std::vector<Point> neighbors;
if (isValid(p.x - 1, p.y)) neighbors.push_back({ p.x - 1, p.y }); // 上
if (isValid(p.x + 1, p.y)) neighbors.push_back({ p.x + 1, p.y }); // 下
if (isValid(p.x, p.y - 1)) neighbors.push_back({ p.x, p.y - 1 }); // 左
if (isValid(p.x, p.y + 1)) neighbors.push_back({ p.x, p.y + 1 }); // 右
if (isValid(p.x - 1, p.y - 1)) neighbors.push_back({ p.x - 1, p.y - 1 }); // 左上
if (isValid(p.x - 1, p.y + 1)) neighbors.push_back({ p.x - 1, p.y + 1 }); // 右上
if (isValid(p.x + 1, p.y - 1)) neighbors.push_back({ p.x + 1, p.y - 1 }); // 左下
if (isValid(p.x + 1, p.y + 1)) neighbors.push_back({ p.x + 1, p.y + 1 }); // 右下
return neighbors;
}
// 判断位置是否有效的方法
bool isValid(int x, int y) {
if (x < 0 || x >= height || y < 0 || y >= width) return false;
return mapData[x * width + y] != '1'; // '1'代表墙壁
}
// 获取两点间成本的方法
int getCost(const Point& a, const Point& b) {
return (mapData[b.x * width + b.y] == '1') ? INT_MAX : 1; // 墙壁成本无限大
}
private:
// 创建一维数组保存地图数据
unsigned char* mapData;
int width = 0;
int height = 0;
};
// 定义哈希函数
namespace std {
template <>
struct hash<Point> {
size_t operator()(const Point& k) const {
return hash<int>()(k.x) ^ (hash<int>()(k.y) << 1);
}
};
}
struct Node {
Point point;
int f; // 总成本，包括 g 值和 h 值
// 重载 < 运算符，用于优先队列排序
bool operator<(const Node& other) const {
return f > other.f;
}
};
class Planner
{
public:
std::unordered_map<Point, Point> aStarSearch(GameMap& gamemap, const Point& start, const Point& goal) {
auto heuristic = [](const Point& a, const Point& b) {
// 使用曼哈顿距离作为启发式函数
return std::abs(a.x - b.x) + std::abs(a.y - b.y);
// return std::sqrt(std::pow(a.x - b.x, 2) + std::pow(a.y - b.y, 2));
};
std::priority_queue<Node> frontier;
frontier.push({ start, 0 });
std::unordered_map<Point, Point> came_from;
std::unordered_map<Point, int> cost_so_far;
came_from[start] = start;
cost_so_far[start] = 0;
while (!frontier.empty()) {
Node current = frontier.top();
frontier.pop();
if (current.point == goal) {
break;
}
for (Point next : gamemap.getNeighbors(current.point)) {
int new_cost = cost_so_far[current.point] + gamemap.getCost(current.point, next);
if (cost_so_far.find(next) == cost_so_far.end() || new_cost < cost_so_far[next]) {
cost_so_far[next] = new_cost;
int priority = new_cost + heuristic(next, goal);
frontier.push({ next, priority });
came_from[next] = current.point;
}
}
}
return came_from;
}
};
std::vector<Point> reconstruct_path(const Point& start, const Point& goal, const std::unordered_map<Point, Point>& came_from) {
std::vector<Point> path;
Point current = goal;
path.push_back(current);
while (current != start) {
current = came_from.at(current);
path.push_back(current);
}
std::reverse(path.begin(), path.end()); // 反转路径，使其从起点到终点
return path;
}
class Controller
{
public:
Controller():
game_map(new GameMap("map.txt")),
planner(new Planner())
{
}
~Controller()
{
delete game_map;
delete planner;
}
std::vector<std::vector<Point>> generate_permutations(const std::vector<Point>& items) {
std::vector<std::vector<Point>> result;
std::function<void(int, std::vector<Point>&)> permute = [&](int start, std::vector<Point>& current) {
if (start == items.size()) {
result.push_back(current);
return;
}
for (int i = start; i < items.size(); ++i) {
std::swap(current[start], current[i]);
permute(start + 1, current);
std::swap(current[start], current[i]);
}
};
std::vector<Point> current(items);
permute(0, current);
return result;
}
void run()
{
auto start = game_map->getStart();
auto goals = game_map->getGameItem();
auto goal = game_map->getGoal();
std::vector<Point> shortest_path;
int shortest_path_length = INT_MAX;
std::unordered_map<Point, Point> final_came_from;
// 生成所有可能的目标点顺序组合
std::vector<std::vector<Point>> permutations = generate_permutations(goals);
for (const auto& order : permutations) {
std::vector<Point> current_path;
Point current = start;
int current_path_length = 0;
for (const auto& goal : order) {
auto came_from = planner->aStarSearch(*game_map, current, goal);
auto path = reconstruct_path(current, goal, came_from);
// 去除路径中的重复起点
if (!current_path.empty() && path.front() == current_path.back()) {
path.erase(path.begin());
}
current_path.insert(current_path.end(), path.begin(), path.end());
current = goal;
current_path_length += path.size() - 1; // 减去重复的终点
}
// 从最后一个目标点到终点的路径
final_came_from = planner->aStarSearch(*game_map, current, goal);
auto final_path = reconstruct_path(current, goal, final_came_from);
if (!final_path.empty() && final_path.front() == current_path.back()) {
final_path.erase(final_path.begin());
}
current_path.insert(current_path.end(), final_path.begin(), final_path.end());
current_path_length += final_path.size() - 1; // 减去重复的终点
std::cout << "current_path_length:"<< current_path_length << std::endl;
if (current_path_length < shortest_path_length) {
shortest_path_length = current_path_length;
shortest_path = current_path;
}
}
if (!shortest_path.empty()) {
cv::Mat img = game_map->getDisplayMap(windowWidth, windowHeight);
for (const auto& pair : final_came_from)
{
cv::circle(img, cv::Point(pair.second.y, pair.second.x), 1, cv::Scalar(0, 0, 255), 1);
}
for (const auto& point : shortest_path) {
cv::circle(img, cv::Point(point.y, point.x), 1, cv::Scalar(0, 255, 255), 1);
}
// 显示图像
cv::namedWindow("Map", cv::WINDOW_NORMAL);
cv::resizeWindow("Map", windowWidth, windowHeight);
cv::imshow("Map", img);
cv::waitKey(0);
}
else {
std::cerr << "没有找到路径。" << std::endl;
}
}
void run_direct2goal()
{
auto came_from = planner->aStarSearch(*game_map, game_map->getStart(), game_map->getGoal());
auto path = reconstruct_path(game_map->getStart(), game_map->getGoal(), came_from);
cv::Mat img = game_map->getDisplayMap(windowWidth, windowHeight);
for (const auto& pair : came_from) {
std::cout << "From (" << pair.first.x << ", " << pair.first.y << ") to ("
<< pair.second.x << ", " << pair.second.y << ")" << std::endl;
cv::circle(img, cv::Point(pair.second.y, pair.second.x), 1, cv::Scalar(0, 0, 255), 1);
}
for (const auto& pair : path)
{
cv::circle(img, cv::Point(pair.y, pair.x), 1, cv::Scalar(0, 255, 255), 1);
}
// 显示图像
cv::namedWindow("Map", cv::WINDOW_NORMAL); // 创建一个可以手动调整大小的窗口
cv::resizeWindow("Map", windowWidth, windowHeight); // 设置窗口的初始大小
cv::imshow("Map", img);
cv::waitKey(0);
}
private:
GameMap* game_map;
Planner* planner;
int windowWidth = 800; // 指定窗口的宽度
int windowHeight = 600; // 指定窗口的高度
};
#include <opencv2/core/utils/logger.hpp>
int main() {
cv::utils::logging::setLogLevel(cv::utils::logging::LOG_LEVEL_SILENT);
Controller* controller = new Controller;
//controller->run_direct2goal();
controller->run();
return 0;
}
总结
- 其实可以对
多目标设计A*算法的额外扩展，本文就不在实现了 - 如有错误，欢迎指正！！谢谢观看！
