Upload New File

Matlab Code/checkCollisions.m

+function [collision_flag, end_point] = checkCollisions(q_start, q_end, dx, obs_points)
+
+step_num = ceil(norm(q_start - q_end)./dx); % number of steps of dx to get to the distance increment
+vec = (q_end - q_start)./norm(q_start - q_end);
+vec = vec.*dx;
+pt = q_start;
+collision_flag = false;
+
+for i=1:step_num
+   
+    % look for collisions (there is a faster way to do this, i am sure)
+    dists = sqrt((pt(1) - obs_points(:,1)).^2 + (pt(2) - obs_points(:,2)).^2);
+    if any(dists <= dx)
+        % collision!
+        collision_flag = true;
+        end_point = pt;
+        return;
+    end
+    
+    % otherwise take a step
+    pt = pt + vec;
+    
+end
+
+end_point = pt;
+
+end
\ No newline at end of file