c++实现rrt算法

以下是使用C++实现RRT算法的示例代码：

#include <iostream>
#include <cmath>
#include <vector>
#include <cstdlib>
#include <ctime>

using namespace std;

// 定义节点结构体
struct Node {
    double x, y;
    int parent;
};

// 定义RRT类
class RRT {
public:
    RRT(double x_min, double x_max, double y_min, double y_max, double expand_dis, int max_iter) {
        x_min_ = x_min;
        x_max_ = x_max;
        y_min_ = y_min;
        y_max_ = y_max;
        expand_dis_ = expand_dis;
        max_iter_ = max_iter;
        srand(time(NULL));        nodes_.push_back({x_min_, y_min_, -1});
    }

    // 添加节点
    void addNode(Node node) {
        nodes_.push_back(node);
    }

    // 获取节点
    Node getNode(int i) {
        return nodes_[i];
    }

    // 获取节点数量
    int getNodeNum() {
        return nodes_.size();
    }

    // 判断两个节点是否相连
    bool isConnect(Node n1, Node n2) {
        double dx = n1.x - n2.x;
        double dy = n1.y - n2.y;
        double d = sqrt(dx * dx + dy * dy);
        if (d > expand_dis_) {
            return false;
        }
        return true;
    }

    // 扩展树
    void expandTree() {
        for (int i = 0; i < max_iter_; i++) {
            Node rnd_node = getRandomNode();
            int nearest_node_id = getNearestNode(rnd_node);
            Node nearest_node = getNode(nearest_node_id);
            Node new_node = getNewNode(nearest_node, rnd_node);
            if (isConnect(new_node, nearest_node)) {
                addNode(new_node);
            }
        }
    }

    // 获取随机节点
    Node getRandomNode() {
        double x = x_min_ + (x_max_ - x_min_) * rand() / (RAND_MAX + 1.0);
        double y = y_min_ + (y_max_ - y_min_) * rand() / (RAND_MAX + 1.0);
        return {x, y, -1};
    }

    // 获取最近节点
    int getNearestNode(Node node) {
        int nearest_node_id = 0;
        double min_dist = sqrt(pow(node.x - nodes_[0].x, 2) + pow(node.y - nodes_[0].y, 2));
        for (int i = 1; i < getNodeNum(); i++) {
            double dist = sqrt(pow(node.x - nodes_[i].x, 2) + pow(node.y - nodes_[i].y, 2));
            if (dist < min_dist) {
                min_dist = dist;
                nearest_node_id = i;
            }
        }
        return nearest_node_id;
    }

    // 获取新节点
    Node getNewNode(Node nearest_node, Node rnd_node) {
        double theta = atan2(rnd_node.y - nearest_node.y, rnd_node.x - nearest_node.x);
        double x = nearest_node.x + expand_dis_ * cos(theta);
        double y = nearest_node.y + expand_dis_ * sin(theta);
        return {x, y, nearest_node.parent};
    }

    // 获取路径
    vector<Node> getPath(int start_id, int end_id) {
        vector<Node> path;
        int node_id = end_id;
        while (node_id != start_id) {
            path.push_back(getNode(node_id));
            node_id = getNode(node_id).parent;
        }
        path.push_back(getNode(start_id));
        return path;
    }

private:
    double x_min_, x_max_, y_min_, y_max_;
    double expand_dis_;
    int max_iter_;
    vector<Node> nodes_;
};

int main() {
    RRT rrt(0, 10, 0, 10, 1, 1000);
    rrt.expandTree();
    vector<Node> path = rrt.getPath(0, rrt.getNodeNum() - 1);
    for (int i = path.size() - 1; i >= 0; i--) {
        cout << "(" << path[i].x << ", " << path[i].y << ")" << endl;
    }
    return 0;
}