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