diff --git a/buildRRT.m b/buildRRT.m
index 630e5ff..067d6ae 100644
--- a/buildRRT.m
+++ b/buildRRT.m
@@ -110,7 +110,10 @@ function [nbNodes, obstacle, points] = buildRRT()
     
     endif
     [minDist, minIndex] = min(distArr); % we save which point is the closest one from the new r point and we note the distance between them
-      
+    if min(distArr) < pdefL
+      pdefL = min(distArr);
+      disp(minIndex);
+    endif
     if ~isnan(minDist) % if the line between p and r can be drawn (does not intersect an obstacle in X Y cartesian space)
       q1_p = points(1, minIndex);
       q2_p = points(2, minIndex);
@@ -129,9 +132,6 @@ function [nbNodes, obstacle, points] = buildRRT()
       figure 1
       hold on
       plot(points(1, n), points(2, n)) % we plot the new values and add a description to it
-      if pdefL == min(distArr) % this condition is valid only for the last iteration
-        text(points(1, n+1), points(2, n+1), 'G', 'FontSize', 20);
-      endif
       if n>1 % valid for every iteration except the first  and last ones
         text(points(1, n), points(2, n), int2str(n-1), 'FontSize', 20);
       endif
@@ -141,5 +141,6 @@ function [nbNodes, obstacle, points] = buildRRT()
       drawnow
     endif
   endwhile
+  text(points(1, WhileCond+1), points(2, WhileCond+1), 'G', 'FontSize', 20);
   nbNodes = WhileCond-1;
 endfunction
\ No newline at end of file
diff --git a/checkingLine.m b/checkingLine.m
index 465bdd9..eab3fe5 100644
--- a/checkingLine.m
+++ b/checkingLine.m
@@ -67,7 +67,9 @@ function dist = checkingLine(GapValue, L1, L2, n, j, points, threshold)
         Xtest = jTee(1,4);
         Ytest = jTee(2,4);
         % if a sampled point is colliding and we haven't already noted it, we do
-        if dist !=NaN && (Ytest >= L1 - threshold || Ytest <= -L1 + threshold || (Xtest>=-L2 - threshold && Xtest<=L2 - threshold && Ytest>=-L2 - threshold && Ytest<=L2 + threshold)) %verifie les obstacles
+        if dist !=NaN && (Ytest >= L1 - threshold || Ytest <= -L1 + threshold || (Xtest>=-L2 - threshold && Xtest<=L2 - threshold && Ytest>=-L2 - threshold && Ytest<=L2 + threshold))
+          % In order to avoid trajectories hitting the obstacle due to some rounding problems
+          % we defined the obstacle area using a threshold
           dist = NaN;
         endif