优化这行代码: %step7.重连接,以Pnew为父节点 for i = i:size(T.v,2)-1 dist = sqrt((Pnew(1) - T.v(i).x)^2 + (Pnew(2) - T.v(i).y)^2); if dist < r && i ~= minInd %计算当前点经过Pnew的代价,如果更小就更新 this_cost = dist + tmp_cost; if this_cost < T.v(i).cost %如果没有发生碰撞 this_p = [T.v(i).x,T.v(i).y]; if iscollision2(this_p,Pnew,dist,Img) continue; end T.v(i).cost = this_cost; T.v(i).xPre = Pnew(1); T.v(i).yPre = Pnew(2); T.v(i).indPre = k; end end end
时间: 2024-04-27 08:22:50 浏览: 20
同样可以使用向量化运算,将循环改为矩阵运算:
dis = sqrt((Pnew(1) - [T.v(:).x]').^2 + (Pnew(2) - [T.v(:).y]').^2);
valid_ind = find(dis < r & (1:length(T.v) ~= minInd));
for i = valid_ind
this_cost = dis(i) + tmp_cost;
if this_cost < T.v(i).cost
this_p = [T.v(i).x,T.v(i).y];
if iscollision2(this_p,Pnew,dis(i),Img)
continue;
end
T.v(i).cost = this_cost;
T.v(i).xPre = Pnew(1);
T.v(i).yPre = Pnew(2);
T.v(i).indPre = k;
end
end
这样可以使代码更加简洁高效。
相关问题
优化这行代码: %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
这样可以使代码更加简洁高效。
优化这行代码: %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
这样可以减少代码行数,使代码更加简洁高效。
相关推荐
![](https://img-home.csdnimg.cn/images/20210720083646.png)
![docx](https://img-home.csdnimg.cn/images/20210720083331.png)
![](https://csdnimg.cn/download_wenku/file_type_ask_c1.png)
![](https://csdnimg.cn/download_wenku/file_type_ask_c1.png)
![](https://csdnimg.cn/download_wenku/file_type_ask_c1.png)
![](https://csdnimg.cn/download_wenku/file_type_ask_c1.png)
![](https://csdnimg.cn/download_wenku/file_type_ask_c1.png)
![](https://csdnimg.cn/download_wenku/file_type_ask_c1.png)
![](https://csdnimg.cn/download_wenku/file_type_ask_c1.png)
![](https://csdnimg.cn/download_wenku/file_type_ask_c1.png)
![](https://csdnimg.cn/download_wenku/file_type_ask_c1.png)
![](https://csdnimg.cn/download_wenku/file_type_ask_c1.png)
![](https://csdnimg.cn/download_wenku/file_type_ask_c1.png)
![](https://csdnimg.cn/download_wenku/file_type_ask_c1.png)
![](https://csdnimg.cn/download_wenku/file_type_ask_c1.png)