conn.m

来自「粒子群(PSO)路径规划。这个是一个局部路径规划」· M 代码 · 共 31 行

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

result = true;
theta = straightLine(x1,y1,x2,y2);

%进行半径为robtov的膨胀
%计算出机器人的四边形
x = robotv*cos(theta);
y = robotv*sin(theta);
roblinex = [x1+x,x2+x,x2-x,x1-x,x1+x];
robliney = [y1+y,y2+y,y2-y,y1-y,y1+y];
%判断连接处是否有障碍物，有的话返回false

for i = 1:size(obstaclesx,1)
    %求机器人多边形和障碍物多边形是否相交
 
    if poly_cross(roblinex,robliney,obstaclesx(i,:),obstaclesy(i,:))
        result = false;
        return;
    end
end