Use colnorm, part of rvctools/common rather than norm2.

petercorke · Feb 22, 2012 · 2c3a11d · 2c3a11d
1 parent ec03bbe
commit 2c3a11d
Show file tree

Hide file tree

Showing 4 changed files with 17 additions and 7 deletions.
diff --git a/Bug2.m b/Bug2.m
@@ -112,7 +112,7 @@ function navigate_init(bug, robot)
             if bug.step == 1
                 % Step 1.  Move along the M-line toward the goal
 
-                if norm2(bug.goal - robot) == 0 % are we there yet?
+                if colnorm(bug.goal - robot) == 0 % are we there yet?
                     return
                 end
 
@@ -137,7 +137,7 @@ function navigate_init(bug, robot)
             if bug.step == 2
                 % Step 2.  Move around the obstacle until we reach a point
                 % on the M-line closer than when we started.
-                if norm2(bug.goal-robot) == 0 % are we there yet?
+                if colnorm(bug.goal-robot) == 0 % are we there yet?
                     return
                 end
 
@@ -154,7 +154,7 @@ function navigate_init(bug, robot)
                 if abs( [robot' 1]*bug.mline') <= 0.5
                     bug.message('(%d,%d) moving along the M-line', n);
                     % are closer than when we encountered the obstacle?
-                    if norm2(robot-bug.goal) < norm2(bug.H(bug.j,:)'-bug.goal)
+                    if colnorm(robot-bug.goal) < colnorm(bug.H(bug.j,:)'-bug.goal)
                         % back to moving along the M-line
                         bug.j = bug.j + 1;
                         bug.step = 1;

diff --git a/RandomPath.m b/RandomPath.m
@@ -148,7 +148,7 @@ function setgoal(driver)
             steer = d_heading;
 
             % if nearly at goal point, choose the next one
-            d = norm2(driver.veh.x(1:2) - driver.goal);
+            d = colnorm(driver.veh.x(1:2) - driver.goal);
             if d < driver.closeenough
                 driver.setgoal();
             elseif d > driver.d_prev

diff --git a/Vehicle.m b/Vehicle.m
@@ -192,7 +192,7 @@ function add_driver(veh, driver)
             veh.x(1) = veh.x(1) + u(1)*veh.T*cos(veh.x(3));
             veh.x(2) = veh.x(2) + u(1)*veh.T*sin(veh.x(3));
             veh.x(3) = veh.x(3) + u(1)*veh.T/veh.L * u(2);
-            odo = [norm2(veh.x(1:2)-xp(1:2)) veh.x(3)-xp(3)];
+            odo = [colnorm(veh.x(1:2)-xp(1:2)) veh.x(3)-xp(3)];
             veh.odometry = odo;
 
             veh.x_hist = [veh.x_hist; veh.x'];   % maintain history
@@ -258,7 +258,7 @@ function add_driver(veh, driver)
 
         function odo = gstep(veh, varargin)
             odo = veh.step(varargin{:});
-            veh.showrobot();
+            veh.plot();
             drawnow
         end
 
@@ -313,7 +313,11 @@ function add_driver(veh, driver)
             end
             veh.visualize();
             for i=1:nsteps
+                if nargout > 0
                 veh.step();
+                else
+                    veh.gstep();
+                end
             end
             p = veh.x_hist;
         end
@@ -415,4 +419,10 @@ function display(nav)
 
     end % method
 
+    methods(Static)
+        function plot_vehicle(x,y,th)
+            disp('static plot method');
+        end
+    end
+
 end % classdef
diff --git a/makemap.m b/makemap.m
@@ -96,7 +96,7 @@
                     drawnow
                     hold off
 
-                    r = round(norm2(point1-point2));
+                    r = round(colnorm(point1-point2));
                     c = kcircle(r);
 
                     world_prev = world;