Upload New File

Matlab Code/getNewConfig.m

+function q_new = getNewConfig(q_near, q_rand, dist_inc, obs_points, dx)
+
+% get unit vec
+vec = (q_rand - q_near)./norm(q_rand - q_near);
+total_len = norm(q_rand - q_near);
+% scale unit vec
+vec = dx.*vec;
+
+% check for collisions
+step_num = ceil(min(dist_inc, total_len)/dx); % number of steps of dx to get to the distance increment
+test_pt = q_near; % starting position for collision detection
+for i=1:step_num
+    
+    test_pt = test_pt + vec; % take a step
+    
+    % look for collisions (there is a faster way to do this, i am sure)
+    dists = sqrt((test_pt(1) - obs_points(:,1)).^2 + (test_pt(2) - obs_points(:,2)).^2);
+    if any(dists <= dx)
+        % collision!
+        if i == 1
+            q_new = []; % if its on the first iteration, no new config
+            return;
+        end
+        q_new = test_pt - vec;
+        return;
+    end
+    
+end
+
+% we made it!
+q_new = test_pt;
+
+end
\ No newline at end of file