优化这行代码： %step5.父节点重选择，在给定半径里面选择父节dian for i = i:size(T.v,2) dist = sqrt((Pnew(1) - T.v(i).x)^2 + (Pnew(2) - T.v(i).y)^2); if dist <= r %处理在给定园范围里面的 this_cost = dist + T.v(i).cost; if this_cost < tmp_cost %判断是否有障碍物 this_p = [T.v(i).x,T.v(i).y]; if iscollision2(this_p,Pnew,dist,Img) continue; end tmp_cost = this_cost; minInd = i; end end end

同样可以使用向量化运算，将循环改为矩阵运算：

dis = sqrt((Pnew(1) - [T.v(:).x]').^2 + (Pnew(2) - [T.v(:).y]').^2);
valid_ind = find(dis <= r);
for i = valid_ind
this_cost = dis(i) + T.v(i).cost;
if this_cost < tmp_cost
this_p = [T.v(i).x,T.v(i).y];
if iscollision2(this_p,Pnew,dis(i),Img)
continue;
end
tmp_cost = this_cost;
minInd = i;
end
end

这样可以使代码更加简洁高效。