C++RRT算法代码

以下是一个简单的C++示例代码，演示了如何实现基本的Rapidly-exploring Random Tree (RRT)算法：

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

struct Node {
    double x;
    double y;
};

// 计算两个点之间的欧氏距离
double distance(const Node& a, const Node& b) {
    return std::sqrt(std::pow(a.x - b.x, 2) + std::pow(a.y - b.y, 2));
}

// 在RRT树中找到最近邻节点的索引
int findNearest(const std::vector<Node>& tree, const Node& randomPoint) {
    double minDist = std::numeric_limits<double>::max();
    int nearestIndex = -1;
    for (int i = 0; i < tree.size(); i++) {
        double dist = distance(tree[i], randomPoint);
        if (dist < minDist) {
            minDist = dist;
            nearestIndex = i;
        }
    }
    return nearestIndex;
}

// RRT算法主函数
std::vector<Node> rrt(const Node& start, const Node& goal, int maxIterations, double stepSize) {
    std::vector<Node> tree;
    tree.push_back(start);

    std::srand(std::time(nullptr));

    for (int i = 0; i < maxIterations; i++) {
        Node randomPoint;
        randomPoint.x = std::rand() % 10;
        randomPoint.y = std::rand() % 10;

        int nearestIndex = findNearest(tree, randomPoint);
        Node nearestPoint = tree[nearestIndex];

        double directionX = (randomPoint.x - nearestPoint.x) / distance(randomPoint, nearestPoint);
        double directionY = (randomPoint.y - nearestPoint.y) / distance(randomPoint, nearestPoint);

        Node newPoint;
        newPoint.x = nearestPoint.x + stepSize * directionX;
        newPoint.y = nearestPoint.y + stepSize * directionY;

        tree.push_back(newPoint);

        // 判断是否接近目标点
        if (distance(newPoint, goal)