working buildPRM function

buildPRM.m

@@ -22,6 +22,9 @@
 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 
 function buildPRM (rangeQ1Q2, nbPoints, L1, L2, MapFilename)
+  
+  hold off;
+  i = 1;
   Points=zeros(2, nbPoints);
   MatrixOfLinks=zeros(nbPoints, nbPoints);
   alpha=[0;0];
@@ -29,25 +32,39 @@ function buildPRM (rangeQ1Q2, nbPoints, L1, L2, MapFilename)
   a=[L1;L2];
   jointNumber=[1;2];
 
-  figure 1
+  figure 1; hold on;
-  b=drawCircle(0, 0, L2);
+  b=drawCircle(0, 0, L1+L2); %teacher's functions for drawing circles
-  c=drawCircle(0, 0, L1);%teacher's functions for drawing circles
+  hold on;
+  c=drawCircle(0, 0, L2-L1);
+  hold on;
+  %creates the lines defining the prohibited areas
+  top_line = createLine([0,L1,1,0]);
+  bottom_line = createLine([0,-L1,1,0]);
+  drawLine(top_line);
+  hold on;
+  drawLine(bottom_line);
+  center_box=[L2 L2; -L2 L2; -L2 -L2; L2 -L2]
+  drawPolygon(center_box);
+  hold on;
   
-  poly_a=circleToPolygon([0 0 L2], 32);%create a polygon for matgeom with the circle info (smaller one)
-  poly_b=circleToPolygon([0 0 L1], 32);%bigger one radius=2
   
-  for i=1:nbPoints % won't give 10 points if intersection happens
+  poly_a=circleToPolygon([0 0 L2-L1], 32);%create a polygon for matgeom with the circle info (smaller one)
+  poly_b=circleToPolygon([0 0 L1+L2], 32);%bigger one radius=3
 
+  while i <= nbPoints % will work until 10 valid points are found (no intersection)
+
+    %creates random angle values for Q1 and Q2
     Q=[rand()*(rangeQ1Q2(1,2)-rangeQ1Q2(1,1))+rangeQ1Q2(1,1);rand()*(rangeQ1Q2(2,2)-rangeQ1Q2(2,1))+rangeQ1Q2(2,1)];
-
     theta=[Q(1,1);Q(2,1)];
+    
     OutOfRange=0; %set the boolean
 
     bTee=dh2ForwardKinematics(theta, d, a, alpha, jointNumber); %FW kinematics
     jTee=bTee(1:2, 4); %only retrieve the x and y (2D) values
 
+    %checks if the end effector is in a prohibited zone
     if (jTee(2,1)>=L1)
-      OutOfRange=1; %is out if in that area
+      OutOfRange=1; %is not valid if in that area
     elseif (jTee(2,1)<=-L1)
       OutOfRange=1;
     elseif (abs(jTee(1,1)) <= L2 && abs(jTee(2,1)) <=L2)
@@ -55,34 +72,47 @@ function buildPRM (rangeQ1Q2, nbPoints, L1, L2, MapFilename)
     endif
 
     if (OutOfRange==0)
+      
       Points(1:2, i)=jTee; %assign the current random point to the Points matrix
       MatrixOfLinks(i, i)=1; %it doesnt intersect with the obstacles when compared to itself
+      
+      if i == 1
+        drawPoint(jTee(1,1), jTee(2,1)); %draw the point
+      endif
+      
       if i>=2 %i=1 useless
-        for j=1:i %cmare with the other points if there is an intersection
+        
+        for j=1:i %compare with the other points to check if it can be connected to them
+          
           intersect=0; %set no intersection to start with
+          %takes the coordinates of the current and past points
+          current_point=[Points(1, i), Points(2, i)]; 
+          previous_point=[Points(1, j), Points(2, j)];
           
-          dx=Points(1,i)-Points(1, j); %dx for creating the line - dif between x actual and x at n-1
+          %creates a line-segment between the points before checking if that line-segment instersects an obstacle
-          dy=Points(2,i)-Points(2, j);
+          L = createEdge(current_point, previous_point);
-          LINE=[Points(1, i), Points(2, i), dx, dy]; % creation of the vector LINE
+          %intersect returns a vector of points of intersection meaning that if it is empty, there is no intersection
-
+          if (isempty(intersectEdgePolygon(L, poly_a))!=1 | isempty(intersectEdgePolygon(L, poly_b))!=1 | isempty(intersectEdgePolygon(L, center_box))!=1)
-          L = createLine(LINE); %creation of the line with matgeom
-          hold on %plotting the line
-          drawLine(L);
-
-          if (isempty(intersectLinePolygon(L, poly_a))!=1)%intersect returns a vector of points of intersection
-            %meaning that if it is empty, there is no intersection
             intersect=1; % intersection happenned
-          else
+            
+          else %if there is no intersection, plot the line-segment and the point and adds it to the list of valid points
             MatrixOfLinks(i, j)=1;
             MatrixOfLinks(j,i)=1;
-            hold on
+            hold on;%plotting the line
+            drawEdge(L);
+            hold on;
             drawPoint(jTee(1,1), jTee(2,1)); %draw the point
           endif
         endfor
       endif
+      i= i + 1;
     endif
-  endfor
+  end
   
+  MatrixOfLinks
   Points %display the points vector to check on the graph
   
+  %saves the results into a .mat file
+  save(strcat(MapFilename,'.mat'), 'Points', 'MatrixOfLinks')
+
 end

test_file.m

@@ -7,6 +7,7 @@ clear all
 ##These are all of the tries I made with the matgeom library to try and develop the code
 figure 1
 a=drawCircle(0, 0, 1);
+hold on;
 b=drawCircle(0, 0, 2);
 
 ##figure 2