35 lines
661 B
Matlab
35 lines
661 B
Matlab
function distance = distBetweenPts(pt1, pt2)
|
|
%%%
|
|
%input:
|
|
%-pt1: [x1,y1]
|
|
%-pt2: [x2,y2]
|
|
%output:
|
|
%-distance between pt1 and pt2 (=NaN if no path)
|
|
x1=pt1(1);
|
|
x2=pt2(1);
|
|
y1=pt1(2);
|
|
y2=pt2(2);
|
|
L1=2;
|
|
L2=1;
|
|
distance = sqrt((x2-x1).^2 + (y2-y1).^2);
|
|
a = (y2-y1)/(x2-x1);
|
|
b=y1-a*x1;
|
|
|
|
%testing if there is an obstacle
|
|
if (x1>x2)
|
|
for (i=x2:0.01:x1)
|
|
j=a*i+b;
|
|
if (((i<(L1-L2))&&(i>(-L1+L2)))&&((j<(L1-L2))&&(j>(-L1+L2))))
|
|
distance="Nan";
|
|
endif
|
|
endfor
|
|
elseif (x1<x2)
|
|
for (i=x1:0.01:x2)
|
|
j=a*i+b;
|
|
if (((i<(L1-L2))&&(i>(-L1+L2)))&&((j<(L1-L2))&&(j>(-L1+L2))))
|
|
distance = "NaN";
|
|
endif
|
|
endfor
|
|
endif
|
|
endfunction
|