RRT adds direct line

buildPRM.m

@@ -144,11 +144,6 @@ function [nbNode, obstacle, points] = buildPRM()
             if obstacle(j, n) !=NaN && (Ytest >= L1 || Ytest <= -L1 || (Xtest>=-L2 && Xtest<=L2 && Ytest>=-L2 && Ytest<=L2)) %verifie les obstacles
               obstacle(j,n) = NaN;
               obstacle(n,j) = NaN;
-              
-              %{
-              X_space = [X_space Xtest];
-              Y_space = [Y_space Ytest];
-              %}
             endif
             
           endfor
@@ -170,35 +165,4 @@ function [nbNode, obstacle, points] = buildPRM()
     endif
   endwhile
   nbNode = WhileCond-2;
-  
-  %points
-  %{
-  figure 2
-  axis([-3 3 -3 3]);
-  hold all
-  for i=1:columns(X_space)
-    if i==1
-      text(X_space(i), Y_space(i), 'S', 'FontSize', 20);
-    elseif i==2
-      text(X_space(i), Y_space(i), 'G', 'FontSize', 20);
-    else
-      text(X_space(i), Y_space(i), '*', 'FontSize', 20);
-    endif
-    %drawnow
-  endfor
-  
-  figure 2
-  hold all
-  xL = 3*cos(0:0.01*pi:2*pi);
-  yL = 3*sin(0:0.01*pi:2*pi);
-  cL = [0 0 0];
-  plot(xL,yL,cL);
-  
-  x = cos(0:0.01*pi:2*pi);
-  y = sin(0:0.01*pi:2*pi);
-  c = [0 0 0];
-  plot(x,y,c);
-  
-  DrawObstacles
-  %}
 endfunction

buildRRT.m

@@ -109,58 +109,4 @@ function [nbNode, obstacle, points] = buildRRT()
       drawnow
     endif
   endwhile
-##  disp('avant')
-##  obstacle
-##  for i=1:columns(obstacle)
-##    for j=1:columns(obstacle)
-##      if ~isnan(checkingLine(GapValue, L1, L2, i, j, points))
-##        obstacle(i,j) = checkingLine(GapValue, L1, L2, i, j, points);
-##        
-##        q1 = points(1,1);
-##        q2 = points(2,1);
-##        q1_ = points(1,j);
-##        q2_ = points(2,j);
-##        
-##        Xplot = [q1, q1_];
-##        Yplot = [q2, q2_];
-##        plot(Xplot, Yplot, 'Color', 'b')
-##        drawnow
-##        
-##      endif
-##    endfor
-##  endfor
-##  disp('apres')
-##  obstacle
-  
-  
-  nbNode = WhileCond-1;
-  %{
-  for i=1:columns(obstacle)
-    [minTab(i,1) minTab(i,2)] = min(obstacle(1:i,i));
-  endfor
-  
-  for i=1:rows(minTab)
-    if ~isnan(minTab(i,1))
-      q1_p = points(1,minTab(i,2));
-      q2_p = points(2,minTab(i,2));
-      
-      q1_r = points(1, i);
-      q2_r = points(2, i);
-      
-      q1_pdefL = pdefL * (q1_r - q1_p) / minTab(i,1)
-      q2_pdefL = pdefL * (q2_r - q2_p) / minTab(i,1) % from thales theorem
-      
-      q1_pdefL = q1_r;
-      q2_pdefL = q2_r;
-      
-      
-      Xplot = [q1_p, q1_pdefL];
-      Yplot = [q2_p, q2_pdefL];
-      figure 1
-      plot(Xplot, Yplot, 'Color', 'b')
-      drawnow
-    endif
-    
-  endfor
-  %}
 endfunction

planPathRRT.m

@@ -3,6 +3,7 @@ function planPathRRT
   GapValue=5;
   L1=2;
   L2=1;
+  nodeTrajCut = [];
   [nbNode, visGraph, points] = buildRRT();
   for i=1:columns(visGraph)
     for j=1:columns(visGraph)
@@ -16,14 +17,21 @@ function planPathRRT
   Q1plot = [];
   Q2plot = [];
   
-  nodeTrajectory = [columns(points) nodeTrajectory];
+  nodeTrajectory = [columns(points) nodeTrajectory]
 
+  for i=columns(nodeTrajectory):-1:1
+    i
+    if ~isnan(checkingLine(GapValue, L1, L2, 1, nodeTrajectory(i), points))
+      nodeTrajCut = [nodeTrajCut nodeTrajectory(i)];
+    endif
+  endfor
+  nodeTrajCut
+  nodeTrajectory = [16 max(nodeTrajCut) 1]
   for i=1:columns(nodeTrajectory)
     Q1plot = [Q1plot points(1, nodeTrajectory(i))];
     Q2plot = [Q2plot points(2, nodeTrajectory(i))];
   endfor
   
-  
   figure 1
   axis([-180 180 -180 180]);
   title('q1 q2 Joint Space');