RRT-Connect算法c++实现

以下是RRT-Connect算法的C++实现示例：

#include <iostream>
#include <cmath>
#include <vector>
#include <random>
#include <chrono>

using namespace std;

struct Point {
    double x;
    double y;
};

class RRT_Connect {
public:
    RRT_Connect(Point start, Point goal, double step_size, double goal_tolerance, double obstacle_tolerance) {
        start_ = start;
        goal_ = goal;
        step_size_ = step_size;
        goal_tolerance_ = goal_tolerance;
        obstacle_tolerance_ = obstacle_tolerance;
        srand(time(nullptr));
    }

    vector<Point> plan() {
        vector<Point> path;

        tree1_.push_back(start_);
        tree2_.push_back(goal_);

        int iter = 0;
        while (iter++ < max_iteration_) {
            Point random_point = getRandomPoint();
            int nearest1 = getNearest(tree1_, random_point);
            int nearest2 = getNearest(tree2_, random_point);

            Point new_point1 = steer(tree1_[nearest1], random_point);
            Point new_point2 = steer(tree2_[nearest2], random_point);

            if (isCollisionFree(tree1_[nearest1], new_point1)) {
                tree1_.push_back(new_point1);
                if (connect(new_point1, tree2_, nearest2)) {
                    getPath(tree1_.size() - 1, tree2_.size() - 1, path);
                    return path;
                }
            }

            if (isCollisionFree(tree2_[nearest2], new_point2)) {
                tree2_.push_back(new_point2);
                if (connect(new_point2, tree1_, nearest1)) {
                    getPath(tree1_.size() - 1, tree2_.size() - 1, path);
                    return path;
                }
            }
        }

        return path;
    }

private:
    Point start_;
    Point goal_;
    double step_size_;
    double goal_tolerance_;
    double obstacle_tolerance_;
    vector<Point> tree1_;
    vector<Point> tree2_;
    int max_iteration_ = 10000;

    double distance(Point a, Point b) {
        return sqrt(pow(a.x - b.x, 2) + pow(a.y - b.y, 2));
    }

    Point getRandomPoint() {
        static default_random_engine generator(chrono::system_clock::now().time_since_epoch().count());
        static uniform_real_distribution<double> distribution(0.0, 1.0);
        double x = distribution(generator);
        double y = distribution(generator);
        Point p = { x, y };
        return p;
    }

    int getNearest(vector<Point>& tree, Point p) {
        int nearest = 0;
        double min_distance = distance(tree[0], p);
        for (int i = 1; i < tree.size(); i++) {
            double d = distance(tree[i], p);
            if (d < min_distance) {
                nearest = i;
                min_distance = d;
            }
        }
        return nearest;
    }

    Point steer(Point from, Point to) {
        double d = distance(from, to);
        if (d <= step_size_) {
            return to;
        }
        double ratio = step_size_ / d;
        double x = from.x + (to.x - from.x) * ratio;
        double y = from.y + (to.y - from.y) * ratio;
        Point p = { x, y };
        return p;
    }

    bool isCollisionFree(Point a, Point b) {
        double d = distance(a, b);
        int steps = ceil(d / step_size_);
        double dx = (b.x - a.x) / steps;
        double dy = (b.y - a.y) / steps;
        for (int i = 0; i < steps; i++) {
            double x = a.x + i * dx;
            double y = a.y + i * dy;
            if (isObstacle({ x, y })) {
                return false;
            }
        }
        return true;
    }

    bool isObstacle(Point p) {
        // return true if p is inside the obstacle
        return distance(p, { 0.5, 0.5 }) < obstacle_tolerance_;
    }

    bool connect(Point p, vector<Point>& tree, int nearest) {
        while (true) {
            Point new_point = steer(tree[nearest], p);
            if (isCollisionFree(tree[nearest], new_point)) {
                tree.push_back(new_point);
                if (distance(new_point, goal_) < goal_tolerance_) {
                    return true;
                }
                p = new_point;
                nearest = tree.size() - 1;
            }
            else {
                return false;
            }
        }
    }

    void getPath(int last1, int last2, vector<Point>& path) {
        path.push_back(tree1_[last1]);
        while (last1 != 0) {
            last1 = getParent(tree1_, last1);
            path.push_back(tree1_[last1]);
        }
        reverse(path.begin(), path.end());

        while (last2 != 0) {
            last2 = getParent(tree2_, last2);
            path.push_back(tree2_[last2]);
        }
    }

    int getParent(vector<Point>& tree, int i) {
        double min_distance = distance(tree[i], tree[0]);
        int parent = 0;
        for (int j = 1; j < i; j++) {
            double d = distance(tree[j], tree[i]);
            if (d < min_distance) {
                min_distance = d;
                parent = j;
            }
        }
        return parent;
    }
};

int main() {
    Point start = { 0.1, 0.1 };
    Point goal = { 0.9, 0.9 };
    double step_size = 0.05;
    double goal_tolerance = 0.01;
    double obstacle_tolerance = 0.05;

    RRT_Connect planner(start, goal, step_size, goal_tolerance, obstacle_tolerance);
    vector<Point> path = planner.plan();

    for (int i = 0; i < path.size(); i++) {
        cout << path[i].x << " " << path[i].y << endl;
    }

    return 0;
}

请注意，这只是一个简单的实现示例，实际使用中还需要考虑更多的细节和调整，例如设置合适的参数和调整随机数生成器的种子以获得更好的性能，以及考虑更复杂的障碍物形状和大小。