avail.m

来自「规则障碍物环境下的机器人路径规划matlab源码！」· M 代码 · 共 31 行

function [x,y]=avail(x0,y0,x1,y1,x2,y2,x3,y3)
if x0~=x1&y0~=y1&x2~=x3&y2~=y3
x=((y2-y0)*(x1-x0)*(x3-x2)+x0*(y1-y0)*(x3-x2)-x2*(y3-y2)*(x1-x0))/((y1-y0)*(x3-x2)-(y3-y2)*(x1-x0));
y=y0+(y1-y0)/(x1-x0)*(x-x0);
elseif x0==x1&x3~=x2
    x=x0;
    y=(y3-y2)/(x3-x2)*(x-x2)+y2;
elseif y0==y1&y3~=y2
    y=y0;
    x=(y-y2)/(y3-y2)*(x3-x2)+x2;
elseif x2==x3&x1~=x0
    x=x2;
    y=(y1-y0)/(x1-x0)*(x-x0)+y0;
elseif y2==y3&y1~=y0
    y=y2;
    x=(y-y0)/(y1-y0)*(x1-x0)+x0;
elseif (x0==x1&y0==y1)|(x2==x3&y2==y3)
    x=nan;
    y=nan;
else
    x=nan;
    y=nan;
    
    end