daniel shiffman

(This is a draft tutorial, work-in-progress excerpt from what will hopefully be my upcoming nature of code book.)

Path Following Example Applet
All Steering Behavior Examples

PATH FOLLOWING

The goal of this section is to work out the algorithm (along with accompanying mathematics) and code for Craig Reynolds’ Path Following example. (See: http://www.red3d.com/cwr/steer/PathFollow.html.) Before we can do this, however, we have to spend the time to learn about another piece of Vector math that we ignored back in Chapter X (probably chapter 1??) — The Dot Product.

THE DOT PRODUCT

With scalar values (regular ol’ numbers), arithmetic operations are easy — add, subtract, multiply, divide. This is the stuff of elementary school math class. With vectors things become slightly more complicated. Neverthless, in Chapter 1(?) were able to happily make sense of vector math with some formulas and common sense and a few formulas

Notice how our chapter 1 understanding of vector multiplication involves multiplying a vector by a scalar number. When we want a vector to be twice as large (but facing the same direction) we multiply it by 2. When we want it to be 1/2 the size, we multiply it by 0.5.

However, there are two multiplication-like operations that can be done with vectors that are useful in certain scenarios — the dot product and the cross product. For now we’re going to focus only on the dot product, which is defined as follows. Assume vectors V and U:

V = (vx,vy)
U = (ux,uy)

THE DOT PRODUCT:
A • B = ax*bx + ay* by

For example, if we have the following two vectors:

A = (-3,5)
B = (10,1)

A • B= -3*10 + 5*1 = -30 + 5 = -25

Notice how the result of the dot product is a scalar value (a single number) and not a vector.

In Processing, this would translate to:

PVector a = new PVector(-3,5);
PVector b = new PVector(10,1);

float n = a.dot(b);
println(n);

And if we were to look in the guts of the PVector source, we’d find a pretty simple implementation of this function:

public float dot(PVector v) {
  return x*v.x + y*v.y + z*v.z;
}

OK, BUT WHY?

So, this is simple enough, but the real question is — why do we need the dot product and when is it going to be useful for us in code?

One of the more common uses of the dot product is to use it to find the angle between two vectors. Another way the dot product can be expressed is:

THE DOT PRODUCT:
A • B = |A| * |B| * cos(θ)

In other words, A dot B is equal to the magnitude of A times magnitude of B times cosine of the θ (with θ defined as the angle between the two vectors.)

The two forumulas for dot product can be derived from one another with basic trigonometry (see: http://mathworld.wolfram.com/DotProduct.html), but for our purposes we can be happy with operating on the assumption that:

A • B = |A| * |B| * cos(θ)
A • B = ax*bx + ay* by

both hold true and therefore:

ax*bx + ay*by = |A| * |B| * cos(θ)

Now, let’s start with the following problem, we have the following vectors A and B:

A = (10,2)
B = (4,-3)

We now have situation where we know everything but theta. We know the components of the vector (ax,ay,bx,by) and we can calculate the magnitude of each vector as we did in Chapter 1 with the pythagorean theorem. We can therefore solve for cos(theta):

cos(θ) = A • B / |A| * |B|

Once we’ve solved for cosine of theta, we can then take the inverse cosine (often expressed as arccosine) to solve for theta, i.e.

θ = arccos (A • B / |A| * |B|)

Let’s now do the math with actual numbers:

|A| = 10.2
|B| = 5

therefore:

theta = arccos ( 10*4 + 2*-3 / 10.2 * 5 )
theta = arccos ( 34 / 51 )
theta = ~ 48 degrees

The Processing version of this would be:

PVector a = new PVector(10,2);
PVector b = new PVector(4,-3);

float theta = acos(a.dot(b) / (a.mag() * b.mag()));
println(degrees(theta) + " degrees");
println(theta + " radians");

And, again, if we were to dig into the guts of the Processing source code, we could see that there is a function that implments this exact algorithm.

  static public float angleBetween(PVector v1, PVector v2) {
    float dot = v1.dot(v2);
    float theta = (float) Math.acos(dot / (v1.mag() * v2.mag()));
    return theta;
  }

Here’s a sketch demonstrating how dot product can give us the angle between two vectors:

// Angle Between Two Vectors

// Using the dot product to compute the angle between two vectors

PFont f;

void setup() {
  size(200,200);
  smooth();
  f = createFont("Arial",16,true);
}

void draw() {
  background(255);

  // A "vector" (really a point) to store the mouse location and screen center location
  PVector mouseLoc = new PVector(mouseX,mouseY);
  PVector centerLoc = new PVector(width/2,height/2);  

  // Aha, a vector to store the displacement between the mouse and center
  PVector v = PVector.sub(mouseLoc,centerLoc);
  v.normalize();
  v.mult(75);

  PVector xaxis = new PVector(75,0);
  // Render the vector
  drawVector(v,centerLoc,1.0);
  drawVector(xaxis,centerLoc,1.0);

  float theta = PVector.angleBetween(v,xaxis);

  textFont(f);
  fill(0);
  text(int(degrees(theta)) + " degrees\n" + theta + " radians",10,160);

}

// Renders a vector object 'v' as an arrow and a location 'loc'
void drawVector(PVector v, PVector loc, float scayl) {
  pushMatrix();
  float arrowsize = 6;
  // Translate to location to render vector
  translate(loc.x,loc.y);
  stroke(0);
  strokeWeight(2);
  // Call vector heading function to get direction (pointing up is a heading of 0)
  rotate(v.heading2D());
  // Calculate length of vector & scale it to be bigger or smaller if necessary
  float len = v.mag()*scayl;
  // Draw three lines to make an arrow
  line(0,0,len,0);
  line(len,0,len-arrowsize,+arrowsize/2);
  line(len,0,len-arrowsize,-arrowsize/2);
  popMatrix();
}

A couple things to note here:

• If two vectors (A and B) are orthogonal (i.e. perpendicular), the dot product (A B ) is equal to zero.
• If two vectors are unit vectors then the dot product is simply equal to cosine of the angle between, i.e. A B = cos(theta) if A and B are of length 1.

RETURNING TO PATH FOLLOWING

Now that we’ve got a basic understanding of the dot product under our belt we can return to a discussion of Craig Reynolds’ path following algorithm. Here are the steps our vehicle must follow:

• 1. Predict the future. Compute the vehicle’s theoretical location N frames in the future.
• 2. How far away. Calculate the distance between the vehicle’s future location and the path. If it is within the path radius, do nothing, otherwise continue.
• 3. Find a target point on the path. Take the point on the path that is “normal” (more on this in a moment) to the vehicle’s future location. Then look ahead on the path and set a target location.
• 4. Steer. Set the vehicle’s steering force to seek that target.

Before we deal with the vehicle, let’s define what we mean by a Path. There are many ways we could implement a path, for us, the simplest will be to define a path as a series of connected points:

Just getting started, though, let’s make our path even simpler, just two points.

We’re also going to consider a path to have a radius. If we consider our path to be a road, the radius would determine how wide that road should be. With a smaller radius, our vehicles will have to follow the path more closely, a wider radius will allow them to stray a bit more.

Putting this into a class, we have:

class Path {

  // A Path is only two points, start and end
  PVector start;
  PVector end;

  // A path has a radius, i.e how far is it ok for the vehicle to wander off
  float radius;

  Path() {
    // Arbitrary radius of 20
    radius = 20;
    // Two arbitrary points
    start = new PVector(0,height/3);
    end = new PVector(width,2*height/3);
  }

  // Draw the path
  void display() {
    // Draw a thick line to show radius
    strokeWeight(radius*2);
    stroke(0,100);
    line(start.x,start.y,end.x,end.y);
    // Draw thin line
    strokeWeight(1);
    stroke(0);
    line(start.x,start.y,end.x,end.y);
  }
}

And we could show the path onscreen:

Path path;

void setup() {
  size(400,200);
  smooth();
  path = new Path();
}

void draw() {
  background(255);
  path.display();
}

Now, let’s assume we have a Vehicle (as depicted below) outside of the path’s radius, moving with a velocity v.

The first thing we want to do is predict, assuming a constant velocity, where that vehicle will be in the future:

// Start by making a copy of the velocity
PVector predict = vel.get();
// Normalize to unit vector
predict.normalize();
// Look 25 pixels ahead by scaling vector up
predict.mult(25);
// Add vector to location to find the predicted location
PVector predictLoc = PVector.add(loc, predict);

Once we have that location, it’s now our job to find out how far away from the path that predicted location is. If it’s very far away, well, then we’ve strayed from the path and need to steer back towards it. If it’s close, then we’re doing ok and are following the path nicely.

So, how do we find the distance between a point and a line? This concept is key. The distance between a point and line is defined as the length of the “normal” between that point and line. The normal is a vector that extends from that point and is perpendicular to the line.

Let’s figure out what we do know. We know we have a vector (call it A) which extends from the path’s starting point to the vehicle’s predicted location:

PVector a = PVector.sub(predictLoc,path.start);

We also know that we can define a vector (call it B) that points from the start of the path to the end.

PVector a = PVector.sub(path.end,path.start);

Now, with basic trigonometry, we know that the distance from the path’s start to the normal point is |A| * cos(theta).

If we knew theta, we could easily just define that normal point as follows:

// Normalize b
b.normalize();
// Scale it according to distance to normal point
b.mult(a.mag()*cos(theta));
// The normal point can be found by adding the scaled version of b to the path’s starting point
PVector normalPoint = PVector.add(path.start,b);

And if the dot product has taught us anything, it’s that given two vectors, we can get theta, the angle between.

// Find theta first!
float theta = PVector.angleBetween(a,b);

// Normalize b
b.normalize();
// Scale it according to distance to normal point
b.mult(a.mag()*cos(theta));
// The normal point can be found by adding the scaled version of b to the path’s starting point
PVector normalPoint = PVector.add(path.start,b);

While the above code will work, there’s one more simplification we can make to the above code. If you notice the desired magnitude for vector B is:

a.mag()*cos(theta)

or

|A|*cos(theta)

And if you recall

A • B = |A|*|B|*cos(theta)

Now, what if vector B is a unit vector, i.e. length 1? Then:

A • B = |A|*|B|*cos(theta)

or

A • B = |A|*cos(theta)

And what are we doing in our code? Normalizing b!

b.normalize();

Because of this fact, we can simplify our code to be:

// Find theta first!  (WE DON’T NEED THETA ANYMORE!)
float theta = PVector.angleBetween(a,b);

// Normalize b
b.normalize();
// Using the DOT PRODUCT NOW!
b.mult(a.dot(b));
// The normal point can be found by adding the scaled version of b to the path’s starting point
PVector normalPoint = PVector.add(path.start,b);

This process is commonly known as “scalar projection”

A • B = |A| * |B| * cos(θ).

|A| * cos(θ) is the scalar projection of A onto B. And if we normalize B before computing the dot product the scalar projection of A onto B is equal to A • B.

STEERING TOWARDS THE TARGET

Reynolds algorithm requires that we only steer towards the path if we are straying beyond its radius.

 float distance = PVector.dist(predictLoc, normalPoint);
 if (distance > path.radius) {
   seek(target);
 }

But what is the target?

Reynolds’ algorithm involves picking a point ahead of the normal on the path (See step #3 above). But for simplicity, we could just say that the target is the normal itself and this will work fairly well:

float distance = PVector.dist(predictLoc, normalPoint);
if (distance > path.radius) {
  seek(normalPoint);
}

We do, however, have the vector that is the line segment — B. So if we wanted to scoot ahead, we could say:

float distance = PVector.dist(predictLoc, normalPoint);
if (distance > path.radius) {
  // Normalize and scale b (pick 25 pixels arbitrarily)
  b.normalize();
  b.mult(25);
  PVector target = PVector.add(normalPoint,b);
  seek(target);
}

NEXT SECTION: Expanding the path as multiple line segments / curve.

NEXT SECTION: Adding separation forces to implement crowd path following.