conn.m

来自「一个用粒子群做的路径规划。功能比较简单」· M 代码 · 共 28 行

%--------判断两个点的连接是否为连通状态--------------------
function result = Conn(x1,y1,x2,y2)
%----声明全局----------------------------
global rangOb;
global robotv;
%------------------

result = true;
[k,b] = straightLine(x1,y1,x2,y2);

%判断连接处是否有障碍物，有的话返回false
%因为机器人也是有体积的，所以对原来的障碍物进行膨胀
%进行[-0.01,+0.01]的膨胀
for px = x1:0.1:x2
    py = k*px+b;
    for  i = 1:size(rangOb)
       if px>=(rangOb(i,1)-robotv)...
                 && px <= (rangOb(i,1)+robotv)...
                 && py >= (rangOb(i,2)-robotv)...
                 &&  py<= (rangOb(i,2)+robotv)
          result = false;
          return;
       end
    end
end