k = size(T.v,2) + 1; T.v(k).x = goal(1)-0.5; T.v(k).y = goal(2)-0.5; T.v(k).xfather = Qnew(1)-0.5; T.v(k).yfather = Qnew(2)-0.5; T.v(k).cost = T.v(k-1).cost + dis2goal; T.v(k).indPre = k - 1; disp('find path!'); break; end

这段代码的作用是将当前节点 `Qnew` 添加到 RRTstar 树中，并检查是否到达目标节点 `goal`。

第一行代码 `k = size(T.v,2) + 1` 是获取 RRTstar 树节点数，并将节点数加一作为当前节点 `Qnew` 的索引号 `k`。

接下来几行代码是将当前节点 `Qnew` 的信息存储到 RRTstar 树的结构体数组 `T.v` 中，包括节点的坐标 `(Qnew(1)-0.5, Qnew(2)-0.5)`、父节点的坐标 `(Qnear(1)-0.5, Qnear(2)-0.5)`、到达当前节点的代价 `T.v(k-1).cost + dis2goal`，以及当前节点的前驱节点索引 `k-1`。

然后代码使用 `disp` 函数输出信息 "find path!"，表示已经找到一条从起点到目标点的路径。

最后一行代码 `break` 是跳出循环，结束 RRTstar 算法的运行。

相关问题

优化这行代码： %step8.检查是否到达目标点附近 dis2goal = sqrt((Pnew(1) - goal(1))^2 + (Pnew(2) - goal(2))^2); if dis2goal < threshold %将goal插入树中 k = size(T.v,2) + 1; T.v(k).x = goal(1); T.v(k).y = goal(2); T.v(k).xPre = Pnew(1); T.v(k).yPre = Pnew(2); T.v(k).cost = T.v(k-1).cost + dis2goal; T.v(k).indPre = k - 1; disp('find path!'); break; end end

同样可以使用向量化运算，将if语句改为三目运算符：

dis2goal = sqrt((Pnew(1) - goal(1))^2 + (Pnew(2) - goal(2))^2);
flag = dis2goal < threshold;
k = flag*(size(T.v,2) + 1);
T.v(k).x = flag*goal(1);
T.v(k).y = flag*goal(2);
T.v(k).xPre = flag*Pnew(1);
T.v(k).yPre = flag*Pnew(2);
T.v(k).cost = flag*(T.v(k-1).cost + dis2goal);
T.v(k).indPre = flag*(k - 1);
if flag
disp('find path!');
break;
end

这样可以减少代码行数，使代码更加简洁高效。

path = []; path = [path;goal]; index = T.v(end).indPre; while true path = [path;[T.v(index).x-0.5,T.v(index).y-0.5]]; index = T.v(index).indPre; if index == -1 break; end end

这段代码是用来生成路径的。我们可以看到，首先定义一个空的路径数组 `path`，将终点 `goal` 加入到 `path` 中。然后通过 `T.v(end).indPre` 找到树中最后一个节点的前驱节点的索引，进入 `while` 循环。在循环中，将前驱节点的坐标（`T.v(index).x-0.5,T.v(index).y-0.5`）加入到 `path` 中，然后将当前节点的索引更新为前驱节点的索引，如果当前节点已经是起点，则跳出循环。最终生成的 `path` 数组即为从起点到终点的路径。