自治智能体转向力之路径跟随算法

Craig Reynolds的路径跟随算法：
假设一小车在某处运动，则若小车以恒定速度继续运动，则可以预测过一段时间，小车所处的位置，可以通过这个位置计算小车与道路之间的距离，若距离足够近，则说明小车正在沿路径运动，如果距离太远，则表明小车离路径太远，则需要转向。

单段路径跟随：

1.计算小车未来位置

    // 首先创建速度向量的副本
    PVector predict = velocity.get(); 
    
    // 单位化向量，并将向量向前延申25像素
    predict.normalize(); 
    predict.mult(25);
    
    // 将向量加上当前位置，计算未来的位置
    PVector predictpos = PVector.add(position,predict);

2. 计算法线交点

  // 点p在线段ab上的法线交点位置计算
  PVector getNormalPoint(PVector p, PVector a, PVector b) {
    // 向量ap
    PVector ap = PVector.sub(p, a);
    // 向量ab
    PVector ab = PVector.sub(b, a);
    // 向量ab归一化
    ab.normalize(); 
    // Project vector "diff" onto line by using the dot product
    ab.mult(ap.dot(ab));
    PVector normalPoint = PVector.add(a, ab);
    return normalPoint;
  }

3. 计算点p到线段ab的距离

    /*  ---- 3. 计算小车与法线交点的距离 ----  */
    float distance = PVector.dist(predictpos, normalPoint);  
 

4. 若p到线段ab的距离大于某值，则转向

  /*  ---- 4. 当预测距离 > 能接受的最大距离，则作用转向力 ----  */
    if(distance > path.radius)
    { 
       // 计算道路的方向
      PVector dir = PVector.sub(b, a);
      dir.normalize();
      dir.mult(10);  // This could be based on velocity instead of just an arbitrary 10 pixels
      PVector target = PVector.add(normalPoint, dir);
 
      seek(target);
    }  

多路径跟随

为了寻找目标位置，我们必须找到线段上的法线交点。
交点的选择要满足两个条件：

1. 这个交点必须在路径内
   若不再路径内，则选择终点作为法线交点，并将路径设置为下一路径
2. 这个点为距小车最近的法线交点
   

// 路径跟随算法
  PVector follow(Path path)
  {
    /*  ---- 1. 预计小车的未来位置 ----  */
    // 首先创建速度向量的副本
    PVector predict = velocity.get(); 
    
    //