RRT (Rapidly-Exploring Random Trees)

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

struct Point {
    double x;
    double y;
};

class Node {
public:
    Point point;
    int parent;
    
    Node(Point p, int parentIdx) : point(p), parent(parentIdx) {}
};

class RRTPlanner {
public:
    RRTPlanner(Point start, Point goal, double mapWidth, double mapHeight, int maxIterations, double stepSize)
        : startNode(start, -1), goalNode(goal, -1), maxIterations(maxIterations), stepSize(stepSize),
          mapWidth(mapWidth), mapHeight(mapHeight), randomGenerator(std::random_device()()) {
        nodes.push_back(startNode);
    }

    double distance(const Point& p1, const Point& p2) {
        return std::sqrt(std::pow(p2.x - p1.x, 2) + std::pow(p2.y - p1.y, 2));
    }

    Point generateRandomPoint() {
        std::uniform_real_distribution<double> xDist(0, mapWidth);
        std::uniform_real_distribution<double> yDist(0, mapHeight);
        return {xDist(randomGenerator), yDist(randomGenerator)};
    }

    int nearestNeighbor(const Point& target) {
        int nearestIdx = 0;
        double nearestDist = distance(nodes[0].point, target);
        for (int i = 1; i < nodes.size(); ++i) {
            double dist = distance(nodes[i].point, target);
            if (dist < nearestDist) {
                nearestDist = dist;
                nearestIdx = i;
            }
        }
        return nearestIdx;
    }

    Point steer(const Point& from, const Point& to, double maxDistance) {
        double dist = distance(from, to);
        if (dist <= maxDistance) {
            return to;
        } else {
            double ratio = maxDistance / dist;
            double newX = from.x + (to.x - from.x) * ratio;
            double newY = from.y + (to.y - from.y) * ratio;
            return {newX, newY};
        }
    }

    void extendTree() {
        Point randomPoint = generateRandomPoint();
        int nearestIdx = nearestNeighbor(randomPoint);
        Point nearestPoint = nodes[nearestIdx].point;
        Point newPoint = steer(nearestPoint, randomPoint, stepSize);

        // Check collision or validity of newPoint
        
        nodes.push_back(Node(newPoint, nearestIdx));
    }

    std::vector<Point> plan() {
        for (int i = 0; i < maxIterations; ++i) {
            extendTree();
        }

        // Generate path from goal to start using parent pointers
        std::vector<Point> path;
        int currentNodeIdx = nodes.size() - 1;
        while (currentNodeIdx != -1) {
            path.push_back(nodes[currentNodeIdx].point);
            currentNodeIdx = nodes[currentNodeIdx].parent;
        }
        return path;
    }

private:
    Node startNode;
    Node goalNode;
    int maxIterations;
    double stepSize;
    double mapWidth;
    double mapHeight;
    std::vector<Node> nodes;
    std::default_random_engine randomGenerator;
};

int main() {
    Point start = {10, 10};
    Point goal = {90, 90};
    double mapWidth = 100;
    double mapHeight = 100;
    int maxIterations = 1000;
    double stepSize = 5.0;

    RRTPlanner planner(start, goal, mapWidth, mapHeight, maxIterations, stepSize);
    std::vector<Point> path = planner.plan();

    // Print the path
    for (int i = path.size() - 1; i >= 0; --i) {
        std::cout << "Step " << path.size() - i << ": (" << path[i].x << ", " << path[i].y << ")\n";
    }

    return 0;
}