for i = i:size(T.v,2)-1 dist = sqrt((Qnew(1) - T.v(i).x)^2 + (Qnew(2) - T.v(i).y)^2); if dist < r && i ~= minInd
时间: 2024-03-04 14:51:50 浏览: 78
这段代码也是RRT算法的一部分,作用是检测从新节点Qnew到树中已有节点的路径上是否与障碍物相交。具体来说,for循环遍历树中所有已有节点,计算新节点Qnew到当前节点的距离dist。如果距离小于规定的阈值r,并且当前节点不是新节点的父节点(即排除回到父节点的情况),则说明Qnew到该节点的路径可能与障碍物相交,需要进行进一步的检测。
阅读全文