Debugging, modified some helper functions.

findDubinsLength.m

@@ -1,25 +1,26 @@
-function [ length ] = findDubinsLength( p_s, x_s, p_e, x_e, r, debugMode)
+function [ length ] = findDubinsLength( p_s, x_s, p_e, x_e, r, createPlot)
 %FINDDUBINSLENGTH Find the length of the shortest Dubins path
 %   Parameters:
 %       p_s     Start position as a 1-by-2 matrix
 %       x_s     Start angle in radians
 %       p_e     End position as a 1-by-2 matrix
 %       x_e     End angle in radians
 %       r       Turn radius
-%       debugMode   Plots Dubins circles TODO plot course
+%       createPlot  Optional argument to draw a plot of the scenario
 %   Returns:
 %       length  Length of the path
 %
-DEBUG_VERBOSE = 0;
-debugMode = 0;
+DUBINS_DEBUG = 0;
+DEBUG_VERBOSE = 0; % plots dubins trajectories
 %============= Input Validation ===============
 if nargin < 1
     error('No input arguments given!');
 elseif nargin > 6
     error('Too many arguments given!');
 end
-if (isempty(debugMode) ||  strcmpi(debugMode,'off'))
-    debugMode = 0;
+
+if nargin < 6
+    createPlot = 0;
 end
 
 if norm(p_s - p_e) < 3*r
@@ -35,8 +36,8 @@
     p_e = [p_e 0]; 
 end
 
-theta_s = heading2Theta(x_s)
-theta_e = heading2Theta(x_e)
+theta_s = heading2Theta(x_s);
+theta_e = heading2Theta(x_e);
 
 % Inline function for right-handed rotation of theta about z-axis
 % TODO be sure we aren't using anonymous functions anywhere. They are slow.
@@ -53,73 +54,87 @@
 %c_re = p_e' + r*rotm(pi/2) * [cos(x_e) sin(x_e) 0]';
 %c_le = p_e' + r*rotm(-pi/2) * [cos(x_e) sin(x_e) 0]';
 % 
-c_rs = p_s' + r*[cos(theta_s - pi/2) sin(theta_s - pi/2) 0]'
-c_ls = p_s' + r*[cos(theta_s + pi/2) sin(theta_s + pi/2) 0]'
-c_re = p_e' + r*[cos(theta_e - pi/2) sin(theta_e - pi/2) 0]'
-c_le = p_e' + r*[cos(theta_e + pi/2) sin(theta_e + pi/2) 0]'
+c_rs = p_s' + r*[cos(theta_s - pi/2) sin(theta_s - pi/2) 0]';
+c_ls = p_s' + r*[cos(theta_s + pi/2) sin(theta_s + pi/2) 0]';
+c_re = p_e' + r*[cos(theta_e - pi/2) sin(theta_e - pi/2) 0]';
+c_le = p_e' + r*[cos(theta_e + pi/2) sin(theta_e + pi/2) 0]';
+
+% Should be this:
+%c_rs = p_s' + r*[cos(x_s + pi/2) sin(x_s + pi/2) 0]';
+%c_ls = p_s' + r*[cos(x_s - pi/2) sin(x_s - pi/2) 0]';
+%c_re = p_e' + r*[cos(x_e + pi/2) sin(x_e + pi/2) 0]';
+%c_le = p_e' + r*[cos(x_e - pi/2) sin(x_e - pi/2) 0]';
+
 
 %============ Calculate Lengths ===============
-if (debugMode)
-    figure();
+if (DUBINS_DEBUG & DEBUG_VERBOSE)
+    c_rs, c_ls, c_re, c_le
+end
+if createPlot
     plotScenario(p_s, x_s, p_e, x_e, c_rs, c_ls, c_re, c_le,r)
 end
 
 % Case I, R-S-R
-theta = heading2Theta(findHeadingFrom(c_rs,c_re))
+theta = findHeadingFrom(c_rs,c_re);
 L1 = norm(c_rs - c_re) + r*wrapTo2Pi(2*pi + wrapTo2Pi(theta - pi/2) - wrapTo2Pi(x_s - pi/2))...
-    + r*wrapTo2Pi(2*pi + wrapTo2Pi(x_e - pi/2) - wrapTo2Pi(theta - pi/2))
+    + r*wrapTo2Pi(2*pi + wrapTo2Pi(x_e - pi/2) - wrapTo2Pi(theta - pi/2));
+if (DUBINS_DEBUG & DEBUG_VERBOSE)
+    theta
+    L1
+end
 
 % Case II, R-S-L
-len = norm(c_le - c_rs)
-theta = heading2Theta(findHeadingFrom(c_rs,c_le))
-theta2 = theta - pi/2 + asin((2*r)/len)
+len = norm(c_le - c_rs);
+theta = findHeadingFrom(c_rs,c_le);
+theta2 = theta - pi/2 + asin((2*r)/len);
 L2 = sqrt(len^2 - 4*r^2)+r*wrapTo2Pi(2*pi + wrapTo2Pi(theta2) - wrapTo2Pi(x_s - pi/2))...
-    + r*wrapTo2Pi(2*pi + wrapTo2Pi(theta2 + pi) - wrapTo2Pi(x_e + pi/2))
+    + r*wrapTo2Pi(2*pi + wrapTo2Pi(theta2 + pi) - wrapTo2Pi(x_e + pi/2));
+if (DUBINS_DEBUG & DEBUG_VERBOSE)
+    L2
+end
 
 % Case III, L-S-R
-len = norm(c_re - c_ls)
-theta = heading2Theta(findHeadingFrom(c_ls,c_re))
-theta2 = acos((2*r)/len)
+len = norm(c_re - c_ls);
+theta = findHeadingFrom(c_ls,c_re);
+theta2 = acos((2*r)/len);
 
 if (2*r/len) > 1 || (2*r/len) < -1
     error('Error in case III');
 end
 
 L3 = sqrt(len^2 - 4*r^2) + r*wrapTo2Pi(2*pi + wrapTo2Pi(x_s + pi/2) - wrapTo2Pi(theta + theta2))...
-    + r*wrapTo2Pi(2*pi + wrapTo2Pi(x_e - pi/2) - wrapTo2Pi(theta + theta2 - pi))
+    + r*wrapTo2Pi(2*pi + wrapTo2Pi(x_e - pi/2) - wrapTo2Pi(theta + theta2 - pi));
+if (DUBINS_DEBUG & DEBUG_VERBOSE)
+    L3
+end
 
 % Case IV, L-S-L
-theta = heading2Theta(findHeadingFrom(c_ls,c_le))
+theta = findHeadingFrom(c_ls,c_le);
 L4 = norm(c_ls - c_le) + r*wrapTo2Pi(2*pi + wrapTo2Pi(x_s + pi/2) - wrapTo2Pi(theta + pi/2))...
-    + r*wrapTo2Pi(2*pi + wrapTo2Pi(theta + pi/2) - wrapTo2Pi(x_e + pi/2))
-
+    + r*wrapTo2Pi(2*pi + wrapTo2Pi(theta + pi/2) - wrapTo2Pi(x_e + pi/2));
+if (DUBINS_DEBUG & DEBUG_VERBOSE)
+    L4
+end
 
 % Return the length of the minimum length Dubins path
 length = min([L1, L2, L3, L4]);
 
-%if (debugMode & DEBUG_VERBOSE)
-    fprintf('Given [%0.1f, %0.1f, %0.1f] to [%0.1f, %0.1f, %0.1f],\n',...
-        p_s(1), p_s(2), x_s, p_e(1), p_e(2), x_e);
-    fprintf('L1=%0.2f, L2=%0.2f, L3=%0.2f, L4=%0.2f\n', L1, L2, L3, L4);
-
-    fprintf('Shortest Dubins path with r %.2f has cost %0.2f\n',...
-        r, length);
-%end
-
-
 end
 
 
 %% 
-function plotCircle(x,y,r)
+function plotCircle(x,y,r,args)
 %x and y are the coordinates of the center of the circle
 %r is the radius of the circle
 %0.01 is the angle step, bigger values will draw the circle faster but
 %you might notice imperfections (not very smooth)
 ang=0:0.01:2*pi; 
 xp=r*cos(ang);
 yp=r*sin(ang);
-plot(x+xp,y+yp);
+if ~iscell(args)
+    args = {[args]};
+end
+plot(x+xp,y+yp,args{:});
 end
 
 %%
@@ -130,10 +145,11 @@ function plotScenario(p_s, x_s, p_e, x_e, c_rs, c_ls, c_re, c_le, r)
 %scatter([c_s(1) c_e(1)],[c_s(2) c_e(2)],'r+');
 %plot([c_s(1) c_e(1)],[c_s(2) c_e(2)],'k--');
 % Plot circles
-plotCircle(c_rs(1), c_rs(2), r);
-plotCircle(c_re(1), c_re(2), r);
-plotCircle(c_ls(1), c_ls(2), r);
-plotCircle(c_le(1), c_le(2), r);
+formatArgs={':k','LineWidth',0.005};
+plotCircle(c_rs(1), c_rs(2), r, formatArgs);
+plotCircle(c_re(1), c_re(2), r, formatArgs);
+plotCircle(c_ls(1), c_ls(2), r, formatArgs);
+plotCircle(c_le(1), c_le(2), r, formatArgs);
 text(c_rs(1), c_rs(2), 'c_{rs}', 'FontSize', 12);
 text(c_re(1), c_re(2), 'c_{re}', 'FontSize', 12);
 text(c_ls(1), c_ls(2), 'c_{ls}', 'FontSize', 12);
@@ -151,8 +167,6 @@ function plotScenario(p_s, x_s, p_e, x_e, c_rs, c_ls, c_re, c_le, r)
 % Plot headings
 hAx = gca;
 scatter([p_s(1) p_e(1)], [p_s(2) p_e(2)], 'r');
-p_s(1:2)
-x_s
 drawHeadingArrow(hAx, p_s(1:2), x_s, r/3, 'b');
 drawHeadingArrow(hAx, p_e(1:2), x_e, r/3, 'b');
 
@@ -164,4 +178,3 @@ function plotScenario(p_s, x_s, p_e, x_e, c_rs, c_ls, c_re, c_le, r)
 M = [cos(theta) sin(theta) 0; -sin(theta) cos(theta) 0; 0 0 1]';
 end
 
-

findHeadingFrom.m

@@ -9,22 +9,22 @@
 end
 
 %============== Finding Angle ==================
-p1_x = p1(1);
-p1_y = p1(2);
-p2_x = p2(1);
-p2_y = p2(2);
+x1 = p1(1);
+y1 = p1(2);
+x2 = p2(1);
+y2 = p2(2);
 
 % Calculate angle of line between p1 and p2
-if (p1_x <= p2_x) && (p1_y < p2_y)
-    psi = atan((p2_x - p1_x)/(p2_y - p1_y));
-elseif (p1_x < p2_x) && (p1_y >= p2_y)
-    psi = atan((p1_y - p2_y)/(p2_x - p1_x)) + pi/2;
-elseif (p2_x <= p1_x) && (p2_y < p1_y)
-    psi = atan((p1_x - p2_x)/(p1_y - p2_y)) + pi;
-elseif (p2_x < p1_x) && (p1_y <= p2_y)
-    psi = atan((p2_y - p1_y)/(p1_x - p2_x)) + 3*pi/2;
+if (x1 <= x2) && (y1 < y2)
+    psi = atan((x2 - x1)/(y2 - y1));
+elseif (x1 < x2) && (y1 >= y2)
+    psi = atan((y1 - y2)/(x2 - x1)) + pi/2;
+elseif (x2 <= x1) && (y2 < y1)
+    psi = atan((x1 - x2)/(y1 - y2)) + pi;
+elseif (x2 < x1) && (y1 <= y2)
+    psi = atan((y2 - y1)/(x1 - x2)) + 3*pi/2;
 else
-    error('Unhandled case for p1=[%d %d] and p2=[%d %d].',p1_x,p1_y,p2_x,p2_y);
+    error('Unhandled case for p1=[%d %d] and p2=[%d %d].',x1,y1,x2,y2);
 end
 
 end % function

findPTPCost.m

@@ -9,6 +9,6 @@
     error('Too many arguments given!');
 end
 
-cost = findDubinsLength(p1, x1, p2, x2, r, 0);
+cost = findDubinsLength(p1, x1, p2, x2, r);
 
 end  % function findPTPCost

heading2Theta.m

@@ -16,4 +16,10 @@
 
 end %% function heading2Theta()
 
+%Note mod is actually:
+%
+%function [m] = myMod(x,y)
+%   n = floor(x./y);
+%   m = x - n.*y;
+%end
 

plotWaypointDubins.m

@@ -28,8 +28,6 @@
 
 [n, ~] = size(V);
 [m, ~] = size(E);
-fprintf ('n = %i\n', n);
-fprintf ('m = %i\n', m);
 
 if (n ~= length(X))
     error('Length of V and X do not match');
@@ -65,7 +63,7 @@
 C_total = 0;
 
 if strcmp(pathOptions.Debug,'on')
-    vertexOrder = order
+    vertexOrder = order;
 end
 
 for i=2:(m+1)
@@ -89,7 +87,7 @@
             c  = c + c_i;
         end % for
         C_total = C_total + c;
-        fprintf('Cost of leg (%i,%i) is %0.2f\n', order(i-1), vi, c);
+        %fprintf('Cost of leg (%i,%i) is %0.2f\n', order(i-1), vi, c);
     end
 
     % Update position

plotWaypointHeadings.m

@@ -28,10 +28,11 @@
 end
 
 [n, ~] = size(V);
-[nX, ~] = size(X);
+nX = length(X);
+%[nX, ~] = size(X);
 
 if (n ~= nX)
-    error('X dimensions do not match V');
+    error(sprintf('X (%d) dimensions should match V (%d)',nX,n));
 end
 
 if ~exist('pathOptions','var')

plotWaypointScenario.m

@@ -36,7 +36,18 @@
     error('Expected V to have 2 columns');
 end
 
-[m, ~] = size(E);
+[m, dimc] = size(E);
+
+%E
+%m
+%dimc
+%if dimc < 2
+%    error('Expected E to have at least 2 columns');
+%end
+
+%if strcmp(pathOptions.ShowEdgeCosts,'on') && dimc < 3
+%    error('Expected E to have 3 columns for showEdgeCosts option.');
+%end
 
 if isempty(subPlotDim)
     error('subPlotDim is empty!');
@@ -52,6 +63,7 @@
     pathOptions = PathOptions;
 end
 
+
 % 
 % %========= Rebuild V and E to include starting configuration ===========
 % [n,~] = size(V);

solveGreedyPointToPoint.m

@@ -86,8 +86,8 @@
     V_visited(idx) = 1;
     X(idx) = theta_i;
 
-    fprintf('Chose leg (%i,%i) with cost %0.2f and final heading %0.2f\n',...
-        lastIdx, idx, c_i, theta_i);
+    %fprintf('Chose leg (%i,%i) with cost %0.2f and final heading %0.2f\n',...
+    %    lastIdx, idx, c_i, theta_i);
 
     % Build an edge
     E(i-1,:) = [lastIdx idx c_i];