added computation of y=ax+b and test for intersection between links and prohibited areas

buildPRM.m

@@ -125,15 +125,32 @@ for l=1:size(q1q2_valid,2)
 			wTee = dh2ForwardKinematics(theta, d, a, alpha, 1);
 			B = wTee(1:2,end);
 			
+      intersects = false;
+      
 			% computes y =ax + b
+      if ((q1q2_valid(1,l)-q1q2_valid(1,m)) != 0)
+        a = (q1q2_valid(2,l)-q1q2_valid(2,m))/(q1q2_valid(1,l)-q1q2_valid(1,m));
+        b = q1q2_valid(1,l) - a * q1q2_valid(2,l);
+      else
+        x = q1q2_valid(1,l);
+
+
+      % Check for intersection with prohibited areas
+      if (a >= 0 && b >= L1) || (a <= 0 && b <= -L1) || (a == 0 && (b >= -L1 || b <= L1)) || (a ~= 0 && b == 0 && abs(a) <= L2)
+        intersects = true;
+      end
+      
+			if (intersects == false)
+        connectionMap(l,m) = 1;
+        plot([q1q2_valid(2,l); q1q2_valid(2,m)], [q1q2_valid(1,l); q1q2_valid(1,m)], 'b'); hold on;
+      else
+        connectionMap(l,m) = 0;
+      endif
 			
-			% if no collision
-			%connectionMap(l,m) = 1;
-			% else
-			%connectionMap(l,m) = 0;
+      
 		end	
 	end
 end
-
-
-
+q1q2_valid
+connectionMap
+end