Quick-RRT*算法c++实现

以下是 Quick-RRT* 算法的 C++ 实现代码：

#include <bits/stdc++.h>
using namespace std;

#define EPS 1e-4 // 容差
#define INF 1e9 // 无穷大

const int MAX_ITER = 1000; // 最大迭代次数
const int W = 640; // 地图宽
const int H = 480; // 地图高
const int K = 50; // 近邻点数
const double GOAL_RADIUS = 20.0; // 目标区域半径

// 二维点
struct Point {
    double x, y;
    Point() {}
    Point(double x_, double y_) : x(x_), y(y_) {}
};

// 定义节点
struct Node {
    int id;
    Point pt;
    vector<int> neighbor;
};

// 计算两点距离
double dist(Point a, Point b) {
    double dx = a.x - b.x;
    double dy = a.y - b.y;
    return sqrt(dx * dx + dy * dy);
}

// 判断两点之间是否存在障碍物
bool collides(Point a, Point b, vector<Point>& obstacles) {
    for (Point obs : obstacles) {
        if (dist(a, b) <= dist(a, obs) + dist(b, obs) + EPS) {
            return true;
        }
    }
    return false;
}

// 生成随机点
Point random_point() {
    return Point((double)rand() / RAND_MAX * W, (double)rand() / RAND_MAX * H);
}

// 找到最近的节点
int nearest_node(Point pt, vector<Node>& nodes) {
    double min_dist = INF;
    int best_node = -1;
    for (Node node : nodes) {
        double d = dist(pt, node.pt);
        if (d < min_dist) {
            min_dist = d;
            best_node = node.id;
        }
    }
    return best_node;
}

// 在节点集合中找到 k 个最近邻节点
vector<int> nearest_nodes(Point pt, vector<Node>& nodes, int k) {
    vector<pair<double, int>> dists;
    for (Node node : nodes) {
        double d = dist(pt, node.pt);
        dists.push_back(make_pair(d, node.id));
    }
    sort(dists.begin(), dists.end());
    vector<int> result;
    for (int i = 0; i < min(k, (int)dists.size()); i++) {
        result.push_back(dists[i].second);
    }
    return result;
}

// 判断是否到达目标区域
bool is_goal(Point pt, Point goal) {
    return dist(pt, goal) < GOAL_RADIUS;
}

// 从起点开始运行 Quick-RRT* 算法
vector<Point> quick_rrt_star(Point start, Point goal, vector<Point>& obstacles) {
    vector<Node> nodes;
    nodes.push_back({0, start, {}});
    for (int i = 0; i < MAX_ITER; i++) {
        Point rand_pt = random_point();
        int nearest_id = nearest_node(rand_pt, nodes);
        Node nearest_node = nodes[nearest_id];
        Point new_pt = rand_pt;
        if (collides(nearest_node.pt, rand_pt, obstacles)) {
            double theta = atan2(rand_pt.y - nearest_node.pt.y, rand_pt.x - nearest_node.pt.x);
            new_pt = Point(nearest_node.pt.x + cos(theta) * EPS, nearest_node.pt.y + sin(theta) * EPS);
            if (collides(nearest_node.pt, new_pt, obstacles)) {
                continue;
            }
        }
        vector<int> neighbor_ids = nearest_nodes(new_pt, nodes, K);
        Node new_node = {nodes.size(), new_pt, neighbor_ids};
        nodes.push_back(new_node);
        for (int id : neighbor_ids) {
            Node neighbor = nodes[id];
            if (!collides(neighbor.pt, new_pt, obstacles)) {
                double cost = dist(neighbor.pt, new_pt);
                bool better = true;
                for (int j : new_node.neighbor) {
                    Node other = nodes[j];
                    if (!collides(other.pt, new_pt, obstacles)) {
                        double new_cost = dist(other.pt, new_pt) + cost;
                        if (new_cost < dist(other.pt, new_node.pt)) {
                            better = false;
                            break;
                        }
                    }
                }
                if (better) {
                    new_node.neighbor.push_back(id);
                    neighbor.neighbor.push_back(new_node.id);
                }
            }
        }
        if (is_goal(new_pt, goal)) {
            vector<Point> path = {goal};
            int curr_id = new_node.id;
            while (curr_id != 0) {
                curr_id = nodes[curr_id].neighbor[0];
                path.push_back(nodes[curr_id].pt);
            }
            path.push_back(start);
            reverse(path.begin(), path.end());
            return path;
        }
    }
    return {};
}

// 绘制路径和障碍物
void draw_path(vector<Point>& path, vector<Point>& obstacles) {
    Mat img(H, W, CV_8UC3, Scalar(255, 255, 255));
    for (Point obs : obstacles) {
        circle(img, Point(obs.x, H - obs.y), 10, Scalar(0, 0, 0), -1);
    }
    for (int i = 0; i < (int)path.size() - 1; i++) {
        line(img, Point(path[i].x, H - path[i].y), Point(path[i + 1].x, H - path[i + 1].y), Scalar(0, 0, 255), 2);
    }
    imshow("Result", img);
    waitKey(0);
}

int main() {
    Point start(50, 50);
    Point goal(W - 50, H - 50);
    vector<Point> obstacles = {Point(200, 200), Point(400, 400)};
    vector<Point> path = quick_rrt_star(start, goal, obstacles);
    draw_path(path, obstacles);
    return 0;
}

注：以上代码使用 OpenCV 库进行图像绘制。请确保已正确安装 OpenCV 库。