added skeleton for collison algo

fixed relative loc formula so we take buggy frame into account

Software/offline/Collision_Avoidance/avoid_collision.m

@@ -1,5 +1,29 @@
 function new_delta = avoid_collision(x, delta, relative_locs)
 
+% define tolderance, step size, max iter, etc
 
+% initialize new_delta to max right
+% todo run this again but use full left as well, see if they converge in
+% the same place
+
+% while we haven't reached max iterations
+% determine the gradient
+
+% get the radius of the circle drawn with delta'
+
+% use the circle formula on each landmark to figure out if it's on the path
+
+% if it's within the tolerance, consider it in the way
+
+% use the difference as the input to the derivative of the gaussian
+
+% calculate the derivative and scale according to distance away (prioritize
+% closer objects)
+
+% add the difference between delta' and delta for the regularizer
+
+% run gradient step
+
+% once we exhaust iterations, return the new delta
 
 end

Software/offline/Collision_Avoidance/get_relative_object_locs.m

@@ -1,5 +1,23 @@
 function relative_locs = get_relative_object_locs(x, obstacle_locs)
 
-% TODO figure out how to get relative INCLUDING the buggy's orientation
+xloc = [ x(1) x(2) ];
+num_obstacles = size(obstacle_locs, 1);
+
+% get the world frame's relative locations
+relative_locs_world_frame = obstacle_locs - ones(num_obstacles,1)*xloc;
+
+% but we need the relative locations from the point of view from the
+% buggy's frame
+% see the docs.pdf on relative coordinates for the terminology
+d = sum(relative_locs_world_frame .^ 2, 2);
+
+psi = x(4);
+theta = atan2(relative_locs_world_frame(:,2), relative_locs_world_frame(:,1));
+phi = theta - ones(size(theta))*psi;
+
+a = d .* cos(phi);
+b = d .* sin(phi);
+
+relative_locs = [ a b ];
 
 end

Software/offline/Collision_Avoidance/update_sim_gui.m

@@ -1,6 +1,5 @@
 function update_sim_gui(figure_num, obstacles, goal, prev_locs, x)
 
-
 figure(figure_num);
 clf;
 hold on
@@ -9,5 +8,4 @@ function update_sim_gui(figure_num, obstacles, goal, prev_locs, x)
 scatter(prev_locs(:, 1), prev_locs(:, 2), 36, 'k', '+');
 scatter(x(1), x(2), 36, 'r', '+');
 
-
 end