modification of the buildPRM, and tries with the matgeom library on test_files

buildPRM.m

@@ -17,56 +17,72 @@
 %
 % Adrien Lasserre (adrien.lasserre@ecam.fr) & Gwenn Durpoix-Espinasson (g.durpoix-espinasson@ecam.fr)
 % 06/12/2022
+%
+% Check library matgeom: https://octave.sourceforge.io/matgeom/overview.html
 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 
 function buildPRM (rangeQ1Q2, nbPoints, L1, L2, MapFilename)
-  Points=zeros(1, nbPoints);
+  Points=zeros(2, nbPoints);
   MatrixOfLinks=zeros(nbPoints, nbPoints);
   alpha=[0;0];
   d=[0;0];
   a=[L1;L2];
   jointNumber=[1;2];
 
-  for i=1:nbPoints
+  figure 1
+  b=drawCircle(0, 0, L2);
+  c=drawCircle(0, 0, L1);%teacher's functions for drawing circles
 
-    [q1;q2]=[rand()*(rangeQ1Q2(1,2)-rangeQ1Q2(1,1))+rangeQ1Q2(1,1);rand()*(rangeQ1Q2(2,2)-rangeQ1Q2(2,1))+rangeQ1Q2(2,1)];
+  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
 
-    theta=[q1;q2];
+  for i=1:nbPoints % won't give 10 points if intersection happens
-    OutOfRange=0;
 
-    bTee=dh2ForwardKinematics(theta, d, a, alpha, jointNumber);
+    Q=[rand()*(rangeQ1Q2(1,2)-rangeQ1Q2(1,1))+rangeQ1Q2(1,1);rand()*(rangeQ1Q2(2,2)-rangeQ1Q2(2,1))+rangeQ1Q2(2,1)];
-    jTee=bTee(1:2, 4);
+
+    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
 
     if (jTee(2,1)>=L1)
+      OutOfRange=1; %is out if in that area
+    elseif (jTee(2,1)<=-L1)
       OutOfRange=1;
-    else if (jTee(2,1)<=-L1)
+    elseif (abs(jTee(1,1)) <= L2 && abs(jTee(2,1)) <=L2)
-      OutOfRange=1;
-    else if (abs(jTee(1,1)) <= L2 && abs(jTee(2,1)) <=L2)
       OutOfRange=1;
     endif
 
     if (OutOfRange==0)
-      Points(i)=jTee;
+      Points(1:2, i)=jTee; %assign the current random point to the Points matrix
-      MatrixOfLinks(i, i)=1;
+      MatrixOfLinks(i, i)=1; %it doesnt intersect with the obstacles when compared to itself
-      for j=1:i
+      if i>=2 %i=1 useless
-        intersect=0;
+        for j=1:i %cmare with the other points if there is an intersection
-        if
+          intersect=0; %set no intersection to start with
 
-          intersect=1;
+          dx=Points(1,i)-Points(1, j); %dx for creating the line - dif between x actual and x at n-1
+          dy=Points(2,i)-Points(2, j);
+          LINE=[Points(1, i), Points(2, i), dx, dy]; % creation of the vector LINE
 
-        else
+          L = createLine(LINE); %creation of the line with matgeom
+          hold on %plotting the line
+          drawLine(L);
 
-          MatrixOfLinks(i, j)=1;
+          if (isempty(intersectLinePolygon(L, poly_a))!=1)%intersect returns a vector of points of intersection
-          MatrixOfLinks(j,i)=1;
+            %meaning that if it is empty, there is no intersection
-
+            intersect=1; % intersection happenned
-        endif
+          else
-
+            MatrixOfLinks(i, j)=1;
-
+            MatrixOfLinks(j,i)=1;
-
+            hold on
-      endfor
+            drawPoint(jTee(1,1), jTee(2,1)); %draw the point
+          endif
+        endfor
+      endif
     endif
-
   endfor
 
+  Points %display the points vector to check on the graph
 
-endfunction
+end

drawCircle.m

@@ -8,9 +8,9 @@ function h = drawCircle(x,y,r)
 %	- y: the y-coordinate of the circle center (in m)
 %	- r: the radius of the circle center (in m)
 %
-% Outputs: 
+% Outputs:
 %	- h: a reference to the plot figure
-%	
+%
 %
 % author: Guillaume Gibert, guillaume.gibert@ecam.fr
 % date: 14/09/2021
@@ -27,4 +27,4 @@ xunit = r * cos(th) + x;
 yunit = r * sin(th) + y;
 
 % plots the samples
-h = plot(xunit, yunit, 'r');
+h = plot(xunit, yunit, 'b');

test_file.m

@@ -0,0 +1,63 @@
+clc
+close all
+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);
+b=drawCircle(0, 0, 2);
+
+##figure 2
+poly_a=circleToPolygon([0 0 1], 32);
+poly_b=circleToPolygon([0 0 2], 32);
+##drawPolygon(poly_a, 'b')
+##drawPolygon(poly_b, 'b')
+
+Points = zeros(2, 10);
+jTee=[3;4];
+Points(1:2,1)=[1.5;0]
+Points(1:2, 2)=[1, 1]
+Points(1:2, 3) = [0; -1.5];
+dx=Points(1,1)-Points(1, 2);
+dy=Points(2,1)-Points(2, 2);
+LINE=[Points(1, 1), Points(2, 1), dx, dy]
+
+L = createLine(LINE);
+hold on
+drawLine(L)
+hold on
+drawPoint(Points(1,1), Points(2, 1))
+hold on
+drawPoint(Points(1,2), Points(2, 2))
+
+intersectLinePolygon(L, poly_a)
+
+dx=Points(1,1)-Points(1, 3);
+dy=Points(2,1)-Points(2, 3);
+LINE=[Points(1, 1), Points(2, 1), dx, dy]
+
+L = createLine(LINE);
+hold on
+drawLine(L)
+hold on
+drawPoint(Points(1,2), Points(2, 2))
+hold on
+drawPoint(Points(1,3), Points(2, 3))
+
+intersectLinePolygon(L, poly_a)
+
+isempty(intersectLinePolygon(L, poly_a))!=1
+
+
+
+## This should be how we apply the function:
+####rangeQ1Q2=[-5, -5; 5, 5];
+####nbPoints=10;
+####L1=2;
+####L2=1;
+####MapFilename='HelloWorld';
+####
+####buildPRM (rangeQ1Q2, nbPoints, L1, L2, MapFilename)
+