vehicle.pde

class Vehicle
{

  // All the usual parameters;
  PVector position;
  PVector velocity;
  PVector acceleration;
  float r;
  float maxforce;    // Maximum steering force. Here I assume the weight of the car is 1. So it is actually the acceleration.
  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();
  }


  //Here is the main part of the pure pursuit algorithm
  int i=0;
  int ii=0;
  PVector a;
  PVector b;
  float angle;
  PVector aa;
  PVector bb;
  PVector cc;

 
  void follow(Path p)

  {
    if(i==0)
    {
      a = p.points.get(0);
      b = p.points.get(1);
    }

    //Depend on the next steering angle, choose a modified steer choice instead of the constant look-ahead distance.
    
    if(i==p.points.size()-1)
    {

      aa=a;
      bb=b;
       a = p.points.get(i);
       b = p.points.get(0);
       cc=b;
       angle= PVector.angleBetween(PVector.sub( bb,aa) , PVector.sub(cc,bb) );
       angle=angle*180/PI;
       print(angle);
       print("\n");

    }
    else
    {
      aa=a;
      bb=b;
       a = p.points.get(i);
       b = p.points.get(i+1);
        cc=b;
       angle= PVector.angleBetween(PVector.sub(bb,aa) , PVector.sub(cc,bb) );
       angle=angle*180/PI;
       print(angle);
       print("\n");
       if(ii!=0  )
       {
         if(i<=p.points.size()-3)
         {
           aa=p.points.get(i);
           bb=p.points.get(i+1);
           cc=p.points.get(i+2);
           angle= PVector.angleBetween(PVector.sub(bb,aa) , PVector.sub(cc,bb) );
           angle=angle*180/PI;
           print(angle);
           print("\n");
         }
         
         
         if (i==p.points.size()-2)
         {
           aa=p.points.get(i);
           bb=p.points.get(i+1);
           cc=p.points.get(i+2-p.points.size());
           angle= PVector.angleBetween(PVector.sub(bb,aa) , PVector.sub(cc,bb) );
           angle=angle*180/PI;
           print(angle);
           print("\n");
         }

         if (i==p.points.size()-1)
         {
           aa=p.points.get(i);
           bb=p.points.get(i+1-p.points.size());
           cc=p.points.get(i+2-p.points.size());
           angle= PVector.angleBetween(PVector.sub(bb,aa) , PVector.sub(cc,bb) );
           angle=angle*180/PI;
           print(angle);
           print("\n");
         }
       }



    }
    
    PVector predict = velocity.get();
    predict.normalize();
    predict.mult(50);
    PVector predictpos = PVector.add(position, predict);


    // 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);
    dir.normalize();
    dir.mult(1);  // 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);



    // The commented one is to show that, keep the car is in the track instead of staying in the middle of the line.
    //if (distance > p.radius)
    if (distance > 0)
    {
      seek(target);
      //print(target);
      //print(b);
     // print('\n');
     // print();
    }
    // Test if the target position of car is close to the end part of the line.
    if(angle> 90)
    {
       //print("Fuck");
          if(abs(target.x-b.x)<1 && abs(target.y-b.y)<1)
        //if(abs(position.x-b.x)<10 && abs(position.y-b.y)<10)
            {
              i++;
            //float aa= PVector.angleBetween(PVector.sub( p.points.get(i),p.points.get(i-1)) , PVector.sub(p.points.get(i+1),p.points.get(i)) );
            //print(aa*180/PI);
            print("/n");
            /*if(i==p.points.size()-1)
            {
              //i--;
              i=0;
            }*/
            //print(i);
              /*if(abs(target.x-p.points.get(0).x)<5 && abs(target.y-p.points.get(0).y)<5)
              {
                i=0;
                print(i);
               }*/
               if(b==p.points.get(0))
               {
                 i=0;
               }

               if(ii==0)

               {
                 if(abs(target.x - p.points.get(2).x )<5 && abs(target.y - p.points.get(2).y )<500)
                 {
                   position.x=position.x-100;
                   position.y=position.y-300;
                   i--;
                   ii++;
                   print("The push");
                   print(i);
                 }
               }

            }
    }

    else
    {
      if(abs(position.x-b.x)<5 && abs(position.y-b.y)<5)
        
            {
              i++;
            
            print("/n");
            

               if(b==p.points.get(0))
               {
                 i=0;
               }
               if(ii==0)

               {
                 if(abs(target.x - p.points.get(2).x )<5 && abs(target.y - p.points.get(2).y )<500)
                 {
                   position.x=position.x-100;
                   position.y=position.y-300;
                   i--;
                   ii++;
                   print("The push");
                   print(i);
                 }
               }

            }
    }


    //Test if the car back to the original position. So that we need to set the we follow the track at the the beginning of the path
     /*if(abs(position.x-p.points.get(0).x)<20 && abs(position.y-p.points.get(0).y)<20)
    {
      i=0;
      print(i);
    }*/

    // 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, but I assume the weight is 1, so it is fine.
    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.getEnd().x + r)
    {
      position.x = p.getStart().x - r;
      position.y = p.getStart().y + (position.y-p.getEnd().y);
    }
  }*/
   /*void borders() {
    if (position.x < -r) position.x = width+r;
    //if (position.y < -r) position.y = height+r;
    if (position.x > width+r) position.x = -r;
    //if (position.y > height+r) position.y = -r;
  }*/

}