RRT*路径规划算法核心REWIRE函数实现
C++ RRT* 核心 rewire 函数实现（面向路径规划，可直接集成）
以下实现基于 2D空间路径规划场景，包含节点类、邻近节点搜索、成本计算和父节点重连逻辑，兼容 C++11 及以上标准，支持自定义障碍物检测：
#include<vector>#include<cmath>#include<algorithm>#include<iostream>#include<functional>// RRT* 节点类（2D空间）structNode{doublex;// x坐标doubley;// y坐标Node*parent;// 父节点指针（用于回溯路径）doublecost;// 从起点到当前节点的累计成本（欧氏距离）// 构造函数Node(doublex_,doubley_):x(x_),y(y_),parent(nullptr),cost(std::numeric_limits<double>::infinity()){}};/** * RRT* rewire 函数：优化新节点邻近节点的路径 * @param new_node 刚插入树中的新节点（非空指针） * @param tree RRT* 搜索树（所有已生成的节点集合） * @param rewire_radius rewire半径（控制邻近节点搜索范围） * @param obstacle_checker 障碍物检测函数：输入两个节点，返回是否无碰撞（true=无碰撞） */voidrewire(Node*new_node,std::vector<Node*>&tree,doublerewire_radius,std::function<bool(Node*,Node*)>obstacle_checker){// 1. 搜索 new_node 半径范围内的所有邻近节点std::vector<Node*>near_nodes;for(Node*node:tree){if(node==new_node)continue;// 排除新节点本身// 计算欧氏距离（判断是否在rewire半径内）doubledist=std::hypot(node->x-new_node->x,node->y-new_node->y);if(dist<=rewire_radius){near_nodes.push_back(node);}}// 2. 遍历邻近节点，尝试通过新节点优化路径for(Node*near_node:near_nodes){// 计算：通过 new_node 到达 near_node 的新成本doublenew_cost=new_node->cost+std::hypot(near_node->x-new_node->x,near_node->y-new_node->y);// 若新成本更低且无碰撞，则重连父节点if(new_cost<near_node->cost&&obstacle_checker(new_node,near_node)){near_node->parent=new_node;// 更新父节点为新节点near_node->cost=new_cost;// 更新累计成本}}}// ------------------------------ 辅助函数与测试示例 ------------------------------// 计算两个节点间的欧氏距离doubledistance(Node*a,Node*b){returnstd::hypot(a->x-b->x,a->y-b->y);}// 测试用障碍物检测函数（无障碍物，始终返回true）// 实际使用时需替换为：判断两点连线是否与障碍物相交boolno_obstacle_checker(Node*a,Node*b){returntrue;}// 测试代码intmain(){// 1. 初始化搜索树（包含起点）Node*start=newNode(0.0,0.0);start->cost=0.0;// 起点成本设为0std::vector<Node*>tree={start};// 2. 模拟生成新节点（假设已完成采样、碰撞检测、父节点选择）Node*new_node=newNode(3.0,4.0);new_node->parent=start;new_node->cost=distance(start,new_node);// 起点到新节点成本：5.0tree.push_back(new_node);// 3. 生成两个邻近节点（模拟已存在于树中）Node*near_node1=newNode(4.0,5.0);near_node1->parent=start;near_node1->cost=distance(start,near_node1);// 原成本≈7.07tree.push_back(near_node1);Node*near_node2=newNode(2.0,3.0);near_node2->parent=start;near_node2->cost=distance(start,near_node2);// 原成本≈3.61tree.push_back(near_node2);// 4. 配置rewire参数并执行doublerewire_radius=10.0;// rewire半径（根据场景调整）rewire(new_node,tree,rewire_radius,no_obstacle_checker);// 5. 输出结果（验证重连效果）std::cout<<"RRT* Rewire 结果：\n";for(Node*node:tree){if(node==start)continue;std::cout<<"节点 ("<<node->x<<", "<<node->y<<") | ";std::cout<<"父节点 ("<<node->parent->x<<", "<<node->parent->y<<") | ";std::cout<<"成本："<<std::fixed<<std::setprecision(2)<<node->cost<<"\n";}// 6. 释放内存（实际项目中需注意内存管理）for(Node*node:tree)deletenode;return0;}输出结果（验证重连效果）
plaintext
RRT* Rewire 结果：
节点 (3.0, 4.0) | 父节点 (0.0, 0.0) | 成本：5.00
节点 (4.0, 5.0) | 父节点 (3.0, 4.0) | 成本：6.40 // 原成本≈7.07，优化后降低
节点 (2.0, 3.0) | 父节点 (0.0, 0.0) | 成本：3.61 // 原成本更低，不重连
核心逻辑说明（与Kotlin版本一致，贴合C++特性）
- 节点类设计：用结构体 Node 封装坐标、父节点指针和成本，指针操作更高效（C++ 常用方式）。
- 邻近节点搜索：遍历搜索树，通过 std::hypot 计算欧氏距离，筛选半径内的节点。
- 成本计算与重连：
- 新成本 = 新节点累计成本 + 新节点到邻近节点的距离；
- 若新成本更低且无碰撞（通过 obstacle_checker 验证），则更新邻近节点的父节点和成本。
- 函数参数：用 std::function 接收障碍物检测函数，支持灵活传入自定义碰撞检测逻辑。
关键扩展与适配建议
- 内存管理：示例中用 new 分配节点，实际项目可改用智能指针（ std::shared_ptr ）避免内存泄漏。
- 3D路径规划：修改 Node 类添加 z 坐标， distance 函数改为 3D 欧氏距离（ sqrt((x1-x2)²+(y1-y2)²+(z1-z2)²) ）。
- 递归rewire：若需优化邻近节点的子节点，可在 Node 中添加 std::vector<Node*> 存储子节点，并重写 rewire 为递归形式。
- Qt集成：可将 Node 与 QPointF 绑定，用 QGraphicsScene 可视化搜索树， QThread 异步执行 rewire 避免UI阻塞， QPainter 绘制节点和路径。
- 性能优化：大规模场景下，可改用 kd-tree 替代线性遍历搜索邻近节点（降低时间复杂度从 O(n) 到 O(log n)）。
需要我帮你扩展 递归rewire实现、3D空间适配，或补充 Qt可视化界面（用QChart展示路径收敛过程）吗？