51 lines
1.2 KiB
Matlab
51 lines
1.2 KiB
Matlab
function dist = checkingLine(GapValue, L1, L2, n, j, points)
|
|
q1 = points(1,j);
|
|
q2 = points(2,j);
|
|
q1_ = points(1,n);
|
|
q2_ = points(2,n);
|
|
|
|
|
|
dist = 0;
|
|
if n==j
|
|
dist = NaN;
|
|
endif
|
|
if q1 != q1_
|
|
|
|
%defining Q2 as a function of Q1
|
|
A = (q2_- q2)/(q1_ - q1);
|
|
B = q2 - A * q1;
|
|
Q2 = @(Q1) A*Q1+B;
|
|
|
|
%getting the sampling direction
|
|
if q1 > q1_
|
|
gap = -GapValue;
|
|
else
|
|
gap = GapValue;
|
|
endif
|
|
|
|
|
|
%getting the sample points
|
|
for g=q1:gap:q1_
|
|
Q1test = g;
|
|
Q2test = Q2(g);
|
|
|
|
%Is the end effector colliding with obstacle
|
|
[Xtest, Ytest]=MyFK(L1,L2,Q1test,Q2test);
|
|
|
|
%filling the obstacle matrix
|
|
if dist !=NaN && (Ytest >= L1 || Ytest <= -L1 || (Xtest>=-L2 && Xtest<=L2 && Ytest>=-L2 && Ytest<=L2)) %verifie les obstacles
|
|
dist = NaN;
|
|
|
|
%{
|
|
X_space = [X_space Xtest];
|
|
Y_space = [Y_space Ytest];
|
|
%}
|
|
endif
|
|
|
|
endfor
|
|
if dist ==0 && n!=j
|
|
dist = sqrt( (points(1,j)-points(1,n))^2 + (points(2,j)-points(2,n))^2);
|
|
endif
|
|
endif
|
|
|
|
endfunction |