书名：代码本色：用编程模拟自然系统
作者：Daniel Shiffman
译者：周晗彬
ISBN：978-7-115-36947-5
第6章目录

6.8　路径跟随

讨论Craig Reynolds的路径跟随算法。

1、澄清

本节讨论的是路径跟随，并不是路径寻找。

• 路径寻找是一个研究性的话题（在人工智能领域），主要用于计算迷宫内两点的最短距离。
• 而在路径跟随中，路径已经存在，我们只是让小车沿着路径移动而已。

2、路径跟随算法的定义

图6-20

• 首先，我们要定义路径。定义路径的方法有很多种，一种简单的方法就是将它定义为一系列相连的点。

• 最简单的路径就是两点之间的线段。

  
  
  图6-22　简单的路径

• 我们设想路径有半径，如果路径是一条道路，半径就是道路的宽度。
  半径越小，小车就会越紧密地跟随路线；
  半径越大，小车的可偏离程度也就越大。

3、路径类

Path.pde

class Path {
  // A Path is line between two points (PVector objects)
  PVector start;   路径由两点定义：起点和终点
  PVector end;
  float radius; 路径有半径，即路的宽度

  Path() {
    // Arbitrary radius of 20  
    radius = 20;  选择任意值初始化路径
    start = new PVector(0,height/3);
    end = new PVector(width,2*height/3);
  }

  // Draw the path  显示路径
  void display() {

    strokeWeight(radius*2);
    stroke(0,100);
    line(start.x,start.y,end.x,end.y);

    strokeWeight(1);
    stroke(0);
    line(start.x,start.y,end.x,end.y);
  }
}

4、计算小车与路径之间的距离

• 现在，假设有一辆小车（如下图所示）位于半径之外，正以一定的速度运动。

  
  
  图6-23

• 首先我们要预测：如果小车以恒定的速度运动，过一段时间后它会出现在什么位置。

• 一旦有了这个位置，我们就可以计算它与路径之间的距离。
  如果距离太远，表明我们在偏离路径，应该转向，朝着路径所在的方向运动；
  如果距离足够近，表明我们正在沿着路径运动。

• 如何计算点到线的距离？这是个关键问题。
  点到线的距离就等于点和线之间的法线长度。法线指的是从该点延伸并垂直于该线的向量。

  
  
  图6-24

• 先弄清楚已知条件，我们知道有一个向量从路径的起始位置延伸至小车的预测位置。
  我们还可以定义一个向量（称为B），它从路径的起始位置指向终点。
  根据三角函数的基本知识，路径起始位置与法线交点之间距离等于：
  

  图6-25

• 如果我们知道夹角θ，就可以轻易地求出法线交点

• “标量投影”， 就是向量A到B的标量投影。

• 有了路径的法线交点之后，我们应该根据它确定小车是否应该转向，如何进行转向。
  Reynolds的算法指出：如果小车偏离了路径（也就是预测位置和法线交点的距离大于路径的半径），它就应该转向。

  
  
  图6-27

5、如何转向

• Reynolds的算法选取路径上位于法线交点前方的某个点作为目标位置（参见图6-20）。简单起见，我们把法线交点当做目标位置，这样也能正常工作

• 由于路径向量（称为向量“B”）也是已知的，我们可以轻易地寻找到Reynolds的“沿着路径向前的某个点”。

  
  
  图6-28

• 将上述实现放在一起，就可以在Vehicle类中加入以下转向功能

6、示例

示例代码6-5　简单的路径跟随

boolean debug = true;

// A path object (series of connected points)
Path path;

// Two vehicles
Vehicle car1;
Vehicle car2;

void setup() {
  size(640, 360);
  path = new Path();

  // Each vehicle has different maxspeed and maxforce for demo purposes
  car1 = new Vehicle(new PVector(0, height/2), 2, 0.02);
  car2 = new Vehicle(new PVector(0, height/2), 3, 0.05);
}

void draw() {
  background(255);
  // Display the path
  path.display();
  // The boids follow the path
  car1.follow(path);
  car2.follow(path);
  // Call the generic run method (update, borders, display, etc.)
  car1.run();
  car2.run();
  
  // Check if it gets to the end of the path since it's not a loop
  car1.borders(path);
  car2.borders(path);
  
  // Instructions
  fill(0);
  text("Hit space bar to toggle debugging lines.", 10, height-30);
}

public void keyPressed() {
  if (key == ' ') {
    debug = !debug;
  }
}

Vehicle.pde

class Vehicle {

  // All the usual stuff
  PVector position;
  PVector velocity;
  PVector acceleration;
  float r;
  float maxforce;    // Maximum steering force
  float maxspeed;    // Maximum speed

    // Constructor initialize all values
  Vehicle( PVector l, float ms, float mf) {
    position = l.get();
    r = 4.0;
    maxspeed = ms;
    maxforce = mf;
    acceleration = new PVector(0, 0);
    velocity = new PVector(maxspeed, 0);
  }

  // Main "run" function
  void run() {
    update();
    display();
  }


  // This function implements Craig Reynolds' path following algorithm
  // http://www.red3d.com/cwr/steer/PathFollow.html
  void follow(Path p) {

    // Predict position 50 (arbitrary choice) frames ahead
    PVector predict = velocity.get();  　第1步：预测小车的未来位置
    predict.normalize();
    predict.mult(50);
    PVector predictpos = PVector.add(position, predict); 

    // Look at the line segment
    PVector a = p.start;  第2步：在路径上寻找法线交点
    PVector b = p.end;

    // Get the normal point to that line
    PVector normalPoint = getNormalPoint(predictpos, a, b);

    // Find target point a little further ahead of normal
    PVector dir = PVector.sub(b, a);   第3步：沿着路径前进一段距离，将其设为目标
    dir.normalize();
    dir.mult(10);  // This could be based on velocity instead of just an arbitrary 10 pixels
    PVector target = PVector.add(normalPoint, dir);

    // How far away are we from the path?
    float distance = PVector.dist(predictpos, normalPoint);   第4步：如果我们脱离了路径，就寻找之前设定的目标，然后回归路径
    // Only if the distance is greater than the path's radius do we bother to steer
    if (distance > p.radius) {
      seek(target);
    }


    // Draw the debugging stuff
    if (debug) {
      fill(0);
      stroke(0);
      line(position.x, position.y, predictpos.x, predictpos.y);
      ellipse(predictpos.x, predictpos.y, 4, 4);

      // Draw normal position
      fill(0);
      stroke(0);
      line(predictpos.x, predictpos.y, normalPoint.x, normalPoint.y);
      ellipse(normalPoint.x, normalPoint.y, 4, 4);
      stroke(0);
      if (distance > p.radius) fill(255, 0, 0);
      noStroke();
      ellipse(target.x+dir.x, target.y+dir.y, 8, 8);
    }
  }


  // A function to get the normal point from a point (p) to a line segment (a-b)
  // This function could be optimized to make fewer new Vector objects
  PVector getNormalPoint(PVector p, PVector a, PVector b) {
    // Vector from a to p
    PVector ap = PVector.sub(p, a);
    // Vector from a to b
    PVector ab = PVector.sub(b, a);
    ab.normalize(); // Normalize the line
    // Project vector "diff" onto line by using the dot product
    ab.mult(ap.dot(ab));
    PVector normalPoint = PVector.add(a, ab);
    return normalPoint;
  }


  // Method to update position
  void update() {
    // Update velocity
    velocity.add(acceleration);
    // Limit speed
    velocity.limit(maxspeed);
    position.add(velocity);
    // Reset accelertion to 0 each cycle
    acceleration.mult(0);
  }

  void applyForce(PVector force) {
    // We could add mass here if we want A = F / M
    acceleration.add(force);
  }


  // A method that calculates and applies a steering force towards a target
  // STEER = DESIRED MINUS VELOCITY
  void seek(PVector target) {
    PVector desired = PVector.sub(target, position);  // A vector pointing from the position to the target

    // If the magnitude of desired equals 0, skip out of here
    // (We could optimize this to check if x and y are 0 to avoid mag() square root
    if (desired.mag() == 0) return;

    // Normalize desired and scale to maximum speed
    desired.normalize();
    desired.mult(maxspeed);
    // Steering = Desired minus Velocity
    PVector steer = PVector.sub(desired, velocity);
    steer.limit(maxforce);  // Limit to maximum steering force

      applyForce(steer);
  }

  void display() {
    // Draw a triangle rotated in the direction of velocity
    float theta = velocity.heading2D() + radians(90);
    fill(175);
    stroke(0);
    pushMatrix();
    translate(position.x, position.y);
    rotate(theta);
    beginShape(PConstants.TRIANGLES);
    vertex(0, -r*2);
    vertex(-r, r*2);
    vertex(r, r*2);
    endShape();
    popMatrix();
  }

  // Wraparound
  void borders(Path p) {
    if (position.x > p.end.x + r) {
      position.x = p.start.x - r;
      position.y = p.start.y + (position.y-p.end.y);
    }
  }
}

7、运行结果

getNormalPoint()函数。（求解法线交点）