RRT-connect.cpp

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

// 定义状态点
struct Point {
    double x, y;

    Point(double x, double y) : x(x), y(y) {}

    // 计算两点间距离
    double distanceTo(const Point& other) const {
        return std::hypot(x - other.x, y - other.y);
    }
};

// 定义树节点
struct Node {
    Point point;
    Node* parent;

    Node(Point point) : point(point), parent(nullptr) {}
};

class Environment {
public:
    // 假设我们有一个函数来判断一个点是否在障碍物内部
    bool isInsideObstacle(const Point& p) {
        // 这里需要实现障碍物的碰撞检测逻辑
        // 例如，对于简单的矩形障碍物，你需要判断点是否在矩形内部
        return false;
    }

    // 检查路径段是否与任何障碍物相交
    bool isCollisionFree(const Point& from, const Point& to) {
        // 这里需要实现路径段的碰撞检测逻辑
        // 你可以通过插值方式在两点间取若干点，然后用isInsideObstacle检查这些点
        return true;
    }
};

std::vector<Point> smoothPath(const std::vector<Point>& path, Environment& env) {
    std::vector<Point> smoothedPath;
    // 添加起始点
    smoothedPath.push_back(path.front());

    // 实现路径平滑逻辑
    // 例如，尝试连接路径上相隔较远的点，如果它们之间的连接是自由的，则跳过中间的点
    for (size_t i = 0; i < path.size(); ++i) {
        for (size_t j = path.size() - 1; j > i; --j) {
            if (env.isCollisionFree(path[i], path[j])) {
                // 如果路径是自由的，添加到平滑路径并跳过中间的点
                smoothedPath.push_back(path[j]);
                i = j; // 跳到这个点，继续平滑剩余的路径
                break;
            }
        }
    }

    // 添加终点
    smoothedPath.push_back(path.back());
    return smoothedPath;
}

// RRT树定义
class RRTree {
public:
    std::vector<Node*> nodes;

    ~RRTree() {
        for (auto* node : nodes) {
            delete node;
        }
    }

    void addNode(Node* node) {
        nodes.push_back(node);
    }

    Node* nearestNeighbor(const Point& point) {
        Node* nearest = nullptr;
        double minDistance = std::numeric_limits<double>::max();

        for (auto* node : nodes) {
            double distance = node->point.distanceTo(point);
            if (distance < minDistance) {
                minDistance = distance;
                nearest = node;
            }
        }

        return nearest;
    }
};

// 随机数生成
std::random_device rd;
std::mt19937 gen(rd());
std::uniform_real_distribution<> dis(0.0, 1.0);

// 随机采样函数
Point sample(double minX, double maxX, double minY, double maxY) {
    return Point(dis(gen) * (maxX - minX) + minX, dis(gen) * (maxY - minY) + minY);
}

// 主算法实现
bool connect(RRTree& tree, const Point& point, double stepSize, Environment& env) {
    Node* nearest = tree.nearestNeighbor(point);
    Point direction(point.x - nearest->point.x, point.y - nearest->point.y);
    double length = std::sqrt(direction.x * direction.x + direction.y * direction.y);
    
    // 检查新点是否在障碍物内部或者新点与最近点之间的路径是否有碰撞
    Point stepPoint(nearest->point.x + stepSize * direction.x / length,
                    nearest->point.y + stepSize * direction.y / length);
    if (!env.isInsideObstacle(stepPoint) && env.isCollisionFree(nearest->point, stepPoint)) {
        Node* newNode = new Node(stepPoint);
        newNode->parent = nearest;
        tree.addNode(newNode);
        return true;
    }
    return false;
}

// RRT-Connect算法主函数
void rrtConnect(const Point& start, const Point& goal, double stepSize, double minX, double maxX, double minY, double maxY, Environment& env) {
    RRTree Ta, Tb;
    Ta.addNode(new Node(start));
    Tb.addNode(new Node(goal));

    while (true) {
        // 从Ta向随机点扩展
        Point randPoint = sample(minX, maxX, minY, maxY);
        if (connect(Ta, randPoint, stepSize, env)) {
            // 如果Ta能连接到Tb
            if (connect(Tb, Ta.nodes.back()->point, stepSize, env)) {
                std::cout << "Path found!" << std::endl;
                return;
            }
        }
        // 交换Ta和Tb
        std::swap(Ta, Tb);
    }
}

int main() {
    Point start(0, 0);
    Point goal(10, 10);
    double stepSize = 0.5;
    double minX = 0;
    double maxX = 10;
    double minY = 0;
    double maxY = 10;
    Environment env;
    rrtConnect(start, goal, stepSize, minX, maxX, minY, maxY, env);

    return 0;
}