Path Following

One of the steering behaviours implemented in the library makes the autonomous agent follow a user defined path. Although normally used in conjunction with navigation graphs and path finding algorithms it is possible to simply define a set of points for the agent to follow.

So a path comprises one or more waypoints, with the last one being the destination. The library keeps a list of current waypoints and as the agent reaches each one in turn it is removed from the list. New waypoints can be added to the end of the route or the current route can be replaced with a new one at any time.

The applet below is waiting for you to define a route for the dart to follow, simply click on the inner rectangle to add a waypoint.

Path following

The full code for this applet is shown here before we discuss those parts required to implement path following.

import java.util.*;

// PathFollowing_01
World world;
Vehicle dart;
StopWatch sw;
// List used to display path being followed
LinkedList<Vector2D> route = new LinkedList<Vector2D>();

public void setup() {
  size(640, 400);
  world = new World(width, height);
  sw = new StopWatch();

  dart = new Vehicle(
  new Vector2D(320, 170), // position
  10, // collision radius
  new Vector2D(0, 0), // velocity
  50, // maximum speed
  new Vector2D(1, 0), // heading
  1, // mass
  4, // turning rate
  200                     // max force
  ); 
  ArrowPic view = new ArrowPic(this);    
  dart.renderer(view);
  // Set path way-point and destinations distances then 
  // switch path following on.
  dart.AP().pathFactors(10, 1).pathOn();
  // The world will manage this vehicle
  world.add(dart);
  // Add the starting position
  route.add(new Vector2D(dart.pos()));

  sw.reset();
}

public void draw() {
  double deltaTime = sw.getElapsedTime();
  world.update(deltaTime);
  // Stop the vehicle at the end of the path
  if (dart.AP().pathRouteLength() == 0)
    dart.velocity(0, 0);
  // Ready to draw
  background(60, 128, 60);
  // Draw play area
  fill(100, 200, 100);
  noStroke();
  rect(20, 40, width-40, height-60);
  // Display instructions
  fill(255);
  text("Click inside the play area to extend the path", 20, 30);
  drawRoute();
  world.draw(deltaTime);
}

public void mouseClicked() {
  if (mouseX > 20 && mouseX < width - 20 && mouseY > 40 && mouseY <height - 20) {
    Vector2D wp = new Vector2D(mouseX, mouseY);
    dart.AP().pathAddToRoute(wp);
    route.add(wp);
  }
}

// Display the path being followed.
public void drawRoute() {
  Vector2D p0, p1;
  // Remove paths that have been travelled
  while (route.size () > dart.AP().pathRouteLength() + 1)
    route.removeFirst();
  // Draw the path to follow (if any)
  if (route.size() > 1) {
    stroke(0, 64, 0);
    strokeWeight(1);
    p0 = route.get(0);
    for (int i = 1; i < route.size(); i++) {
      p1 = route.get(i);
      line((float)p0.x, (float)p0.y, (float)p1.x, (float)p1.y);
      p0 = p1;
    }
  }
}

The import statement at line 1 is needed because Processing V2 no longer imports the java.util package by default, and we need it to create the linked list in line 8 which will be used to display the actual path being followed. This list is not used by the vehicle because the auto-pilot has its own list of waypoints.

So after we have created the vehicle and renderer (lines 15 - 26) we need to switch on path following -

  dart.AP().pathFactors(10, 1).pathOn();

Before switching on the 'path follow' behaviour this code sets the path factors. The first value represents how close the vehicle has to be to a waypoint for it to be considered reached (measured in world units). The second value is generally much smaller because it is the distance to use for the destination (final waypoint).

New waypoints are added in the mouseClicked method.

public void mouseClicked() {
  if (mouseX > 20 && mouseX < width - 20 && mouseY > 40 && mouseY <height - 20) {
    Vector2D wp = new Vector2D(mouseX, mouseY);
    dart.AP().pathAddToRoute(wp);
    route.add(wp);
  }
}

Lines 59 and 60 create the waypoint and adds it to the vehicles path. It is also added to the list used to draw the path (line 61).

Thats it really, except to say thet the pathAddToRoute method will accept various parameter types, including a Vector2D array and you should look at the AutoPilot class reference to find out about the others.