Merge branch 'main' of https://github.com/JH159753/GameAILab1

Boid.pde

@@ -1,6 +1,6 @@
 /// In this file, you will have to implement seek and waypoint-following
 /// The relevant locations are marked with "TODO"
-
+import java.util.*;
 class Crumb
 {
   PVector position;
@@ -18,6 +18,61 @@ class Crumb
 
 class Boid
 {
+<<<<<<< HEAD
+  Crumb[] crumbs = {};
+  int last_crumb;
+  float acceleration;
+  float rotational_acceleration;
+  KinematicMovement kinematic;
+  PVector target;
+
+  Boid(PVector position, float heading, float max_speed, float max_rotational_speed, float acceleration, float rotational_acceleration)
+  {
+    this.kinematic = new KinematicMovement(position, heading, max_speed, max_rotational_speed);
+    this.last_crumb = millis();
+    this.acceleration = acceleration;
+    this.rotational_acceleration = rotational_acceleration;
+  }
+
+  void update(float dt)
+  {
+    
+    if (waypoints != null) {
+      for (int i = 0; i<waypoints.size(); i++)
+      {
+        text(i, waypoints.get(i).x + 10, waypoints.get(i).y + 10);
+      }
+    }
+    if (target != null)
+    {
+      // TODO: Implement seek here
+      
+
+
+      //This makes a vector with the direction our boid needs to go to
+      PVector direction = PVector.sub(target, kinematic.position);
+
+      //atan2(direction.y, direction.x) will return the direction we need to go in radians
+
+      //print direction we need to go and the direction we are facing right now
+      //println(atan2(direction.y, direction.x) + " " + normalize_angle_left_right(kinematic.getHeading()));
+
+      float directionalThreshold = .1;
+      float angleToTarget = normalize_angle_left_right(atan2(direction.y, direction.x) - normalize_angle_left_right(kinematic.getHeading()));
+      float arrivalThreshold = 60.0;
+
+      //This just draws a circle for visual debugging purposes
+      circle(target.x, target.y, 3);
+
+      //prints the angle to the target
+      //println(angleToTarget);
+
+      //if the angle is larger than the threshold in the positive direction, rotate counterclockwise
+      if (angleToTarget >= .1) {
+       //println("positive angle");
+        kinematic.increaseSpeed(0.0, 2);
+
+=======
    Crumb[] crumbs = {};
    int last_crumb;
    float acceleration;
@@ -36,6 +91,7 @@ class Boid
      this.acceleration = acceleration;
      this.rotational_acceleration = rotational_acceleration;
    }
+>>>>>>> 3d16b646a807cb7a2384072f4c267c5888644f96
 
    void update(float dt)
    {
@@ -86,6 +142,39 @@ class Boid
             kinematic.increaseSpeed(0.0, kinematic.getRotationalVelocity()); 
           }
         }
+<<<<<<< HEAD
+      }
+
+
+
+      //Sometimes our Boid just goes and does weird things and I don't know why
+
+      //if the target is outside its arrival threshold, accelerate.
+      //if the target is inside its arrival threshold, accelerate backwards until the speed is 0.
+      if (direction.mag() > arrivalThreshold) {
+        //println("main if");
+        kinematic.increaseSpeed(.5, 0);
+      } else if (direction.mag() < arrivalThreshold) {
+        //Need more specific code here to handle arrivals correctly
+
+        if (kinematic.getSpeed() < 40 && direction.mag() > 30) {
+          //println("if 1");
+          kinematic.increaseSpeed(1, 0);
+        } else if (kinematic.getSpeed() < 20 && direction.mag() > 15) {
+          //println("if .75");
+          kinematic.increaseSpeed(.75, 0);
+        } else if (kinematic.getSpeed() < 10 && direction.mag() > 5) {
+          //println("if .5");
+          kinematic.increaseSpeed(.5, 0);
+        } else if (kinematic.getSpeed() < 5 && direction.mag() < 5) {
+          //println("if -kin");
+          //This should ensure that the boid's speed can be dropped to exactly 0 so we don't have stuttering
+
+          kinematic.increaseSpeed(-kinematic.getSpeed(), 0);
+        } else {
+          //println("else");
+          kinematic.increaseSpeed(-1, 0);
+=======
         
         
         
@@ -207,6 +296,7 @@ class Boid
           
           }
           
+>>>>>>> 3d16b646a807cb7a2384072f4c267c5888644f96
         }
         
         
@@ -251,6 +341,64 @@ class Boid
         
      }
      
+<<<<<<< HEAD
+  //  //println("func count " + count);
+  //  if(count > waypoints.size() - 1){
+  //  this.target = waypoints.get(0);
+  //  return;
+  //  }
+  //  else {
+  //  // TODO: change to follow *all* waypoints
+  //  println("count " + count);
+  //  this.target = waypoints.get(count);
+  //  PVector temp = waypoints.remove(count);
+  //  count++;
+  //  //count--;
+    
+  //  follow(waypoints);
+  //  }
+    
+  //}
+  void follow(ArrayList<PVector> waypoints)
+  {
+    if(waypoints.size() == 0) return;
+    println("vector " + waypoints);   
+    println("reverse vector " + waypoints);
+    int count = 0;
+    PVector stop = waypoints.get(0);
+    this.seek(stop);
+    
+    
+    
+    
+    PVector temp = waypoints.remove(0);
+    println("temp vector " + waypoints);
+    //follow(waypoints);
+   
+    //this.target = waypoints.get(0);
+     //do{
+       
+       
+     // println("in while " + count);
+     ////this.target = waypoints.get(count);
+     //this.target = waypoints.get(count);
+     //if(PVector.sub(this.target,this.kinematic.position).mag() < 40){
+     //   count++;
+     //}
+    
+  
+     //}while(count < waypoints.size());
+    //count++;
+    //for(int i = 1; i < waypoints.size(); i++){
+    //    println("dist " + PVector.sub(this.target,this.kinematic.position).mag());
+    //    if(PVector.sub(this.target,this.kinematic.position).mag() < 40){
+    //    this.seek(waypoints.get(i));
+    //    this.target = waypoints.get(i);
+    //    }
+    
+    }
+
+=======
      // place crumbs, do not change     
      if (LEAVE_CRUMBS && (millis() - this.last_crumb > CRUMB_INTERVAL))
      {
@@ -313,4 +461,5 @@ class Boid
       
       
    }
+>>>>>>> 3d16b646a807cb7a2384072f4c267c5888644f96
 }

Boid10308162985695495767.autosave

@@ -0,0 +1,99 @@
+/// In this file, you will have to implement seek and waypoint-following
+/// The relevant locations are marked with "TODO"
+
+class Crumb
+{
+  PVector position;
+  Crumb(PVector position)
+  {
+     this.position = position;
+  }
+  void draw()
+  {
+     fill(255);
+     noStroke(); 
+     circle(this.position.x, this.position.y, CRUMB_SIZE);
+  }
+}
+
+class Boid
+{
+   Crumb[] crumbs = {};
+   int last_crumb;
+   float acceleration;
+   float rotational_acceleration;
+   KinematicMovement kinematic;
+   PVector target;
+   
+   Boid(PVector position, float heading, float max_speed, float max_rotational_speed, float acceleration, float rotational_acceleration)
+   {
+     this.kinematic = new KinematicMovement(position, heading, max_speed, max_rotational_speed);
+     this.last_crumb = millis();
+     this.acceleration = acceleration;
+     this.rotational_acceleration = rotational_acceleration;
+   }
+
+   void update(float dt)
+   {
+     if (target != null)
+     {  
+        // TODO: Implement seek here
+        print(kinematic.getHeading());
+        
+     }
+     
+     // place crumbs, do not change     
+     if (LEAVE_CRUMBS && (millis() - this.last_crumb > CRUMB_INTERVAL))
+     {
+        this.last_crumb = millis();
+        this.crumbs = (Crumb[])append(this.crumbs, new Crumb(this.kinematic.position));
+        if (this.crumbs.length > MAX_CRUMBS)
+           this.crumbs = (Crumb[])subset(this.crumbs, 1);
+     }
+     
+     // do not change
+     this.kinematic.update(dt);
+     
+     draw();
+   }
+   
+   void draw()
+   {
+     for (Crumb c : this.crumbs)
+     {
+       c.draw();
+     }
+     
+     fill(255);
+     noStroke(); 
+     float x = kinematic.position.x;
+     float y = kinematic.position.y;
+     float r = kinematic.heading;
+     circle(x, y, BOID_SIZE);
+     // front
+     float xp = x + BOID_SIZE*cos(r);
+     float yp = y + BOID_SIZE*sin(r);
+     
+     // left
+     float x1p = x - (BOID_SIZE/2)*sin(r);
+     float y1p = y + (BOID_SIZE/2)*cos(r);
+     
+     // right
+     float x2p = x + (BOID_SIZE/2)*sin(r);
+     float y2p = y - (BOID_SIZE/2)*cos(r);
+     triangle(xp, yp, x1p, y1p, x2p, y2p);
+   } 
+   
+   void seek(PVector target)
+   {
+      this.target = target;
+      
+   }
+   
+   void follow(ArrayList<PVector> waypoints)
+   {
+      // TODO: change to follow *all* waypoints
+      this.target = waypoints.get(0);
+      
+   }
+}

Boid1037558311854801587.autosave

@@ -0,0 +1,189 @@
+/// In this file, you will have to implement seek and waypoint-following
+/// The relevant locations are marked with "TODO"
+
+class Crumb
+{
+  PVector position;
+  Crumb(PVector position)
+  {
+    this.position = position;
+  }
+  void draw()
+  {
+    fill(255);
+    noStroke();
+    circle(this.position.x, this.position.y, CRUMB_SIZE);
+  }
+}
+
+class Boid
+{
+  Crumb[] crumbs = {};
+  int last_crumb;
+  float acceleration;
+  float rotational_acceleration;
+  KinematicMovement kinematic;
+  PVector target;
+
+  Boid(PVector position, float heading, float max_speed, float max_rotational_speed, float acceleration, float rotational_acceleration)
+  {
+    this.kinematic = new KinematicMovement(position, heading, max_speed, max_rotational_speed);
+    this.last_crumb = millis();
+    this.acceleration = acceleration;
+    this.rotational_acceleration = rotational_acceleration;
+  }
+
+  void update(float dt)
+  {
+    if (target != null)
+    {
+      // TODO: Implement seek here
+
+
+      //This makes a vector with the direction our boid needs to go to
+      PVector direction = PVector.sub(target, kinematic.position);
+
+      //atan2(direction.y, direction.x) will return the direction we need to go in radians
+
+      //print direction we need to go and the direction we are facing right now
+      //println(atan2(direction.y, direction.x) + " " + normalize_angle_left_right(kinematic.getHeading()));
+
+      float directionalThreshold = .1;
+      float angleToTarget = normalize_angle_left_right(atan2(direction.y, direction.x) - normalize_angle_left_right(kinematic.getHeading()));
+      float arrivalThreshold = 60.0;
+
+      //This just draws a circle for visual debugging purposes
+      circle(target.x, target.y, 3);
+
+      //prints the angle to the target
+      //println(angleToTarget);
+
+      //if the angle is larger than the threshold in the positive direction, rotate counterclockwise
+      if (angleToTarget >= .1) {
+        println("positive angle");
+        kinematic.increaseSpeed(0.0, 2);
+
+        //if the angle is smaller than the threshold in the negative direction, rotate clockwise
+      } else if (angleToTarget < -.1) {
+        kinematic.increaseSpeed(0.0, -1);
+
+        //if the angle is within our threshold, stop our rotational velocity by rotating opposite
+      } else if (directionalThreshold > angleToTarget) {
+
+        if (kinematic.getRotationalVelocity() > 0) {
+          kinematic.increaseSpeed(0.0, -1);
+        } else if (kinematic.getRotationalVelocity() < 0) {
+          kinematic.increaseSpeed(0.0, 1);
+        }
+
+        
+        
+        
+        //Sometimes our Boid just goes and does weird things and I don't know why 
+        
+        //if the target is outside its arrival threshold, accelerate. 
+        //if the target is inside its arrival threshold, accelerate backwards until the speed is 0.
+        if (direction.mag() > arrivalThreshold) {
+          kinematic.increaseSpeed(1,0);
+        } else if (direction.mag() < arrivalThreshold) {
+          //Need more specific code here to handle arrivals correctly
+          
+          if (kinematic.getSpeed() < 40 && direction.mag() > 30) {
+            kinematic.increaseSpeed(1,0);
+          } else if (kinematic.getSpeed() < 20 && direction.mag() > 15) {
+            kinematic.increaseSpeed(.75,0);
+          } else if (kinematic.getSpeed() < 10 && direction.mag() > 5) {
+            kinematic.increaseSpeed(.5,0);
+          } else if (kinematic.getSpeed() < 5 && direction.mag() < 5) {
+            //This should ensure that the boid's speed can be dropped to exactly 0 so we don't have stuttering
+            kinematic.increaseSpeed(-kinematic.getSpeed(),0);
+          } else {
+            kinematic.increaseSpeed(-1,0);
+          }
+          
+          
+
+        }
+      }
+
+
+
+      //drawing a line for testing purposes
+      //line(kinematic.position.x, kinematic.position.y, kinematic.position.x + direction.x, kinematic.position.y + direction.y);
+    }
+
+    // place crumbs, do not change
+    if (LEAVE_CRUMBS && (millis() - this.last_crumb > CRUMB_INTERVAL))
+    {
+      this.last_crumb = millis();
+      this.crumbs = (Crumb[])append(this.crumbs, new Crumb(this.kinematic.position));
+      if (this.crumbs.length > MAX_CRUMBS)
+        this.crumbs = (Crumb[])subset(this.crumbs, 1);
+    }
+
+    // do not change
+    this.kinematic.update(dt);
+
+    draw();
+  }
+
+  void draw()
+  {
+    for (Crumb c : this.crumbs)
+    {
+      c.draw();
+    }
+
+    fill(255);
+    noStroke();
+    float x = kinematic.position.x;
+    float y = kinematic.position.y;
+    float r = kinematic.heading;
+    circle(x, y, BOID_SIZE);
+    // front
+    float xp = x + BOID_SIZE*cos(r);
+    float yp = y + BOID_SIZE*sin(r);
+
+    // left
+    float x1p = x - (BOID_SIZE/2)*sin(r);
+    float y1p = y + (BOID_SIZE/2)*cos(r);
+
+    // right
+    float x2p = x + (BOID_SIZE/2)*sin(r);
+    float y2p = y - (BOID_SIZE/2)*cos(r);
+    triangle(xp, yp, x1p, y1p, x2p, y2p);
+  }
+
+  void seek(PVector target)
+  {
+    this.target = target;
+  }
+int count = 0;
+ 
+  //void follow(ArrayList<PVector> waypoints)
+  //{
+     
+  //  //println("func count " + count);
+  //  if(count > waypoints.size() - 1){
+  //  this.target = waypoints.get(0);
+  //  return;
+  //  }
+  //  else {
+  //  // TODO: change to follow *all* waypoints
+  //  println("count " + count);
+  //  this.target = waypoints.get(count);
+  //  PVector temp = waypoints.remove(count);
+  //  count++;
+  //  //count--;
+    
+  //  follow(waypoints);
+  //  }
+    
+  //}
+  void follow(ArrayList<PVector> waypoints)
+  {
+    this.target = waypoints.get(0);
+     
+    
+  }
+}

Boid10419224425232634497.autosave

@@ -0,0 +1,175 @@
+/// In this file, you will have to implement seek and waypoint-following
+/// The relevant locations are marked with "TODO"
+
+class Crumb
+{
+  PVector position;
+  Crumb(PVector position)
+  {
+     this.position = position;
+  }
+  void draw()
+  {
+     fill(255);
+     noStroke(); 
+     circle(this.position.x, this.position.y, CRUMB_SIZE);
+  }
+}
+
+class Boid
+{
+   Crumb[] crumbs = {};
+   int last_crumb;
+   float acceleration;
+   float rotational_acceleration;
+   KinematicMovement kinematic;
+   PVector target;
+   
+   Boid(PVector position, float heading, float max_speed, float max_rotational_speed, float acceleration, float rotational_acceleration)
+   {
+     this.kinematic = new KinematicMovement(position, heading, max_speed, max_rotational_speed);
+     this.last_crumb = millis();
+     this.acceleration = acceleration;
+     this.rotational_acceleration = rotational_acceleration;
+   }
+
+   void update(float dt)
+   {
+     if (target != null)
+     {  
+        // TODO: Implement seek here
+        
+        
+        //This makes a vector with the direction our boid needs to go to
+        PVector direction = PVector.sub(target, kinematic.position);
+        
+        //atan2(direction.y, direction.x) will return the direction we need to go in radians
+        
+        //print direction we need to go and the direction we are facing right now
+        //println(atan2(direction.y, direction.x) + " " + normalize_angle_left_right(kinematic.getHeading()));
+        
+        float directionalThreshold = .1;
+        float angleToTarget = normalize_angle(atan2(direction.y, direction.x)) - kinematic.getHeading();
+        float arrivalThreshold = 60.0;
+        
+        //This just draws a circle for visual debugging purposes
+        //circle(target.x, target.y, arrivalThreshold);
+        
+        //prints the angle to the target
+        //println(angleToTarget);
+        
+        //if the angle is larger than the threshold in the positive direction, rotate counterclockwise
+        if (angleToTarget > directionalThreshold) {
+          kinematic.increaseSpeed(0.0, 1);
+          
+        //if the angle is smaller than the threshold in the negative direction, rotate clockwise
+        } else if (angleToTarget < -directionalThreshold) {
+          kinematic.increaseSpeed(0.0, -1);
+          
+        //if the angle is within our threshold, stop our rotational velocity by rotating opposite
+        } else if (directionalThreshold > angleToTarget) {
+          
+          if (kinematic.getRotationalVelocity() > 0) {
+            kinematic.increaseSpeed(0.0, -1);
+          }
+          else if (kinematic.getRotationalVelocity() < 0) {
+            kinematic.increaseSpeed(0.0, 1); 
+          }
+        }
+        
+        
+        
+        //Slight flaw: since the arrival threshold is so big, the boid just won't move if its target is that close. 
+        
+        //if the target is outside its arrival threshold, accelerate. 
+        //if the target is inside its arrival threshold, accelerate backwards until the speed is 0.
+        if (direction.mag() > arrivalThreshold) {
+          kinematic.increaseSpeed(1,0);
+        } else if (direction.mag() < arrivalThreshold) {
+          //Need more specific code here to handle arrivals correctly
+          
+          if (kinematic.getSpeed() < 40 && direction.mag() > 30) {
+            kinematic.increaseSpeed(1,0);
+          } else if (kinematic.getSpeed() < 20 && direction.mag() > 15) {
+            kinematic.increaseSpeed(.75,0);
+          } else if (kinematic.getSpeed() < 10 && direction.mag() > 5) {
+            kinematic.increaseSpeed(.5,0);
+          } else if (kinematic.getSpeed() < 5 && direction.mag() < 3) {
+            kinematic.increaseSpeed(.25,0);
+          } else {
+            kinematic.increaseSpeed(-1,0);
+          }
+          
+          
+        }
+        
+        
+        
+        //drawing a line for testing purposes
+        //line(kinematic.position.x, kinematic.position.y, kinematic.position.x + direction.x, kinematic.position.y + direction.y);
+        
+        
+        
+        
+        
+        
+        
+        
+        
+     }
+     
+     // place crumbs, do not change     
+     if (LEAVE_CRUMBS && (millis() - this.last_crumb > CRUMB_INTERVAL))
+     {
+        this.last_crumb = millis();
+        this.crumbs = (Crumb[])append(this.crumbs, new Crumb(this.kinematic.position));
+        if (this.crumbs.length > MAX_CRUMBS)
+           this.crumbs = (Crumb[])subset(this.crumbs, 1);
+     }
+     
+     // do not change
+     this.kinematic.update(dt);
+     
+     draw();
+   }
+   
+   void draw()
+   {
+     for (Crumb c : this.crumbs)
+     {
+       c.draw();
+     }
+     
+     fill(255);
+     noStroke(); 
+     float x = kinematic.position.x;
+     float y = kinematic.position.y;
+     float r = kinematic.heading;
+     circle(x, y, BOID_SIZE);
+     // front
+     float xp = x + BOID_SIZE*cos(r);
+     float yp = y + BOID_SIZE*sin(r);
+     
+     // left
+     float x1p = x - (BOID_SIZE/2)*sin(r);
+     float y1p = y + (BOID_SIZE/2)*cos(r);
+     
+     // right
+     float x2p = x + (BOID_SIZE/2)*sin(r);
+     float y2p = y - (BOID_SIZE/2)*cos(r);
+     triangle(xp, yp, x1p, y1p, x2p, y2p);
+   } 
+   
+   void seek(PVector target)
+   {
+      this.target = target;
+      
+   }
+   
+   void follow(ArrayList<PVector> waypoints)
+   {
+      // TODO: change to follow *all* waypoints
+      this.target = waypoints.get(0);
+      
+   }
+}

Boid17200078021981803956.autosave

@@ -0,0 +1,198 @@
+/// In this file, you will have to implement seek and waypoint-following
+/// The relevant locations are marked with "TODO"
+
+class Crumb
+{
+  PVector position;
+  Crumb(PVector position)
+  {
+    this.position = position;
+  }
+  void draw()
+  {
+    fill(255);
+    noStroke();
+    circle(this.position.x, this.position.y, CRUMB_SIZE);
+  }
+}
+
+class Boid
+{
+  Crumb[] crumbs = {};
+  int last_crumb;
+  float acceleration;
+  float rotational_acceleration;
+  KinematicMovement kinematic;
+  PVector target;
+
+  Boid(PVector position, float heading, float max_speed, float max_rotational_speed, float acceleration, float rotational_acceleration)
+  {
+    this.kinematic = new KinematicMovement(position, heading, max_speed, max_rotational_speed);
+    this.last_crumb = millis();
+    this.acceleration = acceleration;
+    this.rotational_acceleration = rotational_acceleration;
+  }
+
+  void update(float dt)
+  {
+    if (target != null)
+    {
+      // TODO: Implement seek here
+
+
+      //This makes a vector with the direction our boid needs to go to
+      PVector direction = PVector.sub(target, kinematic.position);
+
+      //atan2(direction.y, direction.x) will return the direction we need to go in radians
+
+      //print direction we need to go and the direction we are facing right now
+      //println(atan2(direction.y, direction.x) + " " + normalize_angle_left_right(kinematic.getHeading()));
+
+      float directionalThreshold = .1;
+      float angleToTarget = normalize_angle_left_right(atan2(direction.y, direction.x) - normalize_angle_left_right(kinematic.getHeading()));
+      float arrivalThreshold = 60.0;
+
+      //This just draws a circle for visual debugging purposes
+      circle(target.x, target.y, 3);
+
+      //prints the angle to the target
+      //println(angleToTarget);
+
+      //if the angle is larger than the threshold in the positive direction, rotate counterclockwise
+      if (angleToTarget >= .1) {
+        println("positive angle");
+        kinematic.increaseSpeed(0.0, 2);
+
+        //if the angle is smaller than the threshold in the negative direction, rotate clockwise
+      } else if (angleToTarget < -.1) {
+        kinematic.increaseSpeed(0.0, -1);
+
+        //if the angle is within our threshold, stop our rotational velocity by rotating opposite
+      } else if (directionalThreshold > angleToTarget) {
+
+        if (kinematic.getRotationalVelocity() > 0) {
+          kinematic.increaseSpeed(0.0, -1);
+        } else if (kinematic.getRotationalVelocity() < 0) {
+          kinematic.increaseSpeed(0.0, 1);
+        }
+      }
+
+
+
+      //Sometimes our Boid just goes and does weird things and I don't know why
+
+      //if the target is outside its arrival threshold, accelerate.
+      //if the target is inside its arrival threshold, accelerate backwards until the speed is 0.
+      if (direction.mag() > arrivalThreshold) {
+        //println("main if");
+        kinematic.increaseSpeed(.5, 0);
+      } else if (direction.mag() < arrivalThreshold) {
+        //Need more specific code here to handle arrivals correctly
+
+        if (kinematic.getSpeed() < 40 && direction.mag() > 30) {
+          //println("if 1");
+          kinematic.increaseSpeed(1, 0);
+        } else if (kinematic.getSpeed() < 20 && direction.mag() > 15) {
+          //println("if .75");
+          kinematic.increaseSpeed(.75, 0);
+        } else if (kinematic.getSpeed() < 10 && direction.mag() > 5) {
+          //println("if .5");
+          kinematic.increaseSpeed(.5, 0);
+        } else if (kinematic.getSpeed() < 5 && direction.mag() < 5) {
+          //println("if -kin");
+          //This should ensure that the boid's speed can be dropped to exactly 0 so we don't have stuttering
+
+          kinematic.increaseSpeed(-kinematic.getSpeed(), 0);
+        } else {
+          println("else");
+          kinematic.increaseSpeed(-1, 0);
+        }
+      }
+
+
+
+      //drawing a line for testing purposes
+      //line(kinematic.position.x, kinematic.position.y, kinematic.position.x + direction.x, kinematic.position.y + direction.y);
+    }
+
+    // place crumbs, do not change
+    if (LEAVE_CRUMBS && (millis() - this.last_crumb > CRUMB_INTERVAL))
+    {
+      this.last_crumb = millis();
+      this.crumbs = (Crumb[])append(this.crumbs, new Crumb(this.kinematic.position));
+      if (this.crumbs.length > MAX_CRUMBS)
+        this.crumbs = (Crumb[])subset(this.crumbs, 1);
+    }
+
+    // do not change
+    this.kinematic.update(dt);
+
+    draw();
+  }
+
+  void draw()
+  {
+    for (Crumb c : this.crumbs)
+    {
+      c.draw();
+    }
+
+    fill(255);
+    noStroke();
+    float x = kinematic.position.x;
+    float y = kinematic.position.y;
+    float r = kinematic.heading;
+    circle(x, y, BOID_SIZE);
+    // front
+    float xp = x + BOID_SIZE*cos(r);
+    float yp = y + BOID_SIZE*sin(r);
+
+    // left
+    float x1p = x - (BOID_SIZE/2)*sin(r);
+    float y1p = y + (BOID_SIZE/2)*cos(r);
+
+    // right
+    float x2p = x + (BOID_SIZE/2)*sin(r);
+    float y2p = y - (BOID_SIZE/2)*cos(r);
+    triangle(xp, yp, x1p, y1p, x2p, y2p);
+  }
+
+  void seek(PVector target)
+  {
+    this.target = target;
+  }
+int count = 0;
+ 
+  //void follow(ArrayList<PVector> waypoints)
+  //{
+     
+  //  //println("func count " + count);
+  //  if(count > waypoints.size() - 1){
+  //  this.target = waypoints.get(0);
+  //  return;
+  //  }
+  //  else {
+  //  // TODO: change to follow *all* waypoints
+  //  println("count " + count);
+  //  this.target = waypoints.get(count);
+  //  PVector temp = waypoints.remove(count);
+  //  count++;
+  //  //count--;
+    
+  //  follow(waypoints);
+  //  }
+    
+  //}
+  void follow(ArrayList<PVector> waypoints)
+  {
+    this.target = waypoints.get(0);
+      // println("distance " + PVector.sub(this.target,this.kinematic.position).mag());
+    for (int i = 1; i < waypoints.size(); i++){
+      println("distance " + PVector.sub(this.target,this.kinematic.position).mag());
+      if(PVector.sub(this.target,this.kinematic.position).mag() < 4)
+       this.target = waypoints.get(i);
+        
+    }
+    }
+  
+}

Map.pde

@@ -114,8 +114,15 @@ class Obstacle
 // visible screen (or not too far outside)
 boolean isPointInPolygon(PVector point, ArrayList<Wall> walls)
 {
+   int inside = 0;
+   int outside = 0;
+   for (int x = 0; x < 5; ++x)
+   {
+       for (int y = 0; y < 5; ++y)
+       {
+           if (x + y == 0) continue;
          // we create a test point "far away" horizontally
-   PVector testpoint = PVector.add(point, new PVector(width*2, 0));
+         PVector testpoint = PVector.add(point, new PVector(2*width*(x-3), 2*width*(y-2)));
          
          // Then we count how often the line from the given point
          // to our test point intersects the polygon outline
@@ -132,7 +139,11 @@ boolean isPointInPolygon(PVector point, ArrayList<Wall> walls)
          // so if we "know" that the testpoint is outside
          // and odd number means we exited one more time 
          // than we entered.
-   return (count%2) == 1;
+            if (count%2 == 1) inside++;
+            else outside++;
+       }
+   }
+   return inside > outside;
 }
 
 class Map
@@ -283,4 +294,29 @@ class Map
        }
        return true;
    }
+   PVector percentFromPoint(PVector from, PVector to, float percent)
+   {
+     //p1 + ((p2 - p1) * percent)
+     return PVector.add(from, PVector.mult(PVector.sub(to, from),percent));
+   }
+   
+   boolean intersectsWall(PVector from, PVector to)
+   {  
+      //5% of the way from the start
+      PVector start = percentFromPoint(from, to, 0.01);
+      
+      //95% of the way from the start
+      PVector end = percentFromPoint(from, to, 0.99);
+      
+      if (!isReachable(start)) return true;
+     
+      //println("Start: " + start);
+      //println("End: " + end);
+     
+      for (Wall w : walls)
+      {
+         if (w.crosses(start, end)) return true;
+      }
+      return false;
+   }
 }

NavMesh.pde

@@ -1,33 +1,423 @@
-// Useful to sort lists by a custom key
+import java.util.*;
-import java.util.Comparator;
 
-
-/// In this file you will implement your navmesh and pathfinding. 
-
-/// This node representation is just a suggestion
 class Node
 {
-   int id;
+  String id;
   ArrayList<Wall> polygon;
+  ArrayList<Integer> indices = new ArrayList<Integer>();
   PVector center;
-   ArrayList<Node> neighbors;
+  ArrayList<Node> neighbours = new ArrayList<Node>();
-   ArrayList<Wall> connections;
+  
+  
+  Node(String id, ArrayList<Wall> polygon)
+  {
+    this.id = id;
+    this.polygon = polygon;
+    center = findCenter();
+  }
+  
+  PVector findCenter()
+  {
+    int x_avg = 0;
+    int y_avg = 0;
+    
+    for(Wall w: polygon) {
+      x_avg += w.start.x;
+      y_avg += w.start.y;
+    }
+    x_avg /=polygon.size();
+    y_avg /=polygon.size();
+    return new PVector(x_avg, y_avg);
+  }
+  
+  boolean isNeighbours(Node n)
+  {
+     int prev = indices.get(indices.size()-1);
+     for(Integer i: indices)
+     {
+       if (n.indices.contains(prev) && n.indices.contains(i)) return true;
+       prev = i;
+     }
+     
+     return false;
+  }
+    
 }
 
 
+class SearchFrontier{
+  Node node;
+  SearchFrontier prev_frontier;
+  float distanceToEnd;
+  float distanceToLast = 0;
+  
+  SearchFrontier(Node n, SearchFrontier from, PVector end)
+  {
+    this.node = n;
+    this.distanceToEnd = PVector.dist(n.center, end);
+    if (from != null)
+    {
+       this.prev_frontier = from;
+       this.distanceToLast = PVector.dist(n.center, from.node.center) + from.distanceToLast;
+    }
+      
+  }
+  
+  float heuristicSum()
+  {
+    return distanceToEnd + distanceToLast;
+  }
+}
+
 
 class NavMesh
 {
-   void bake(Map map)
+  ArrayList<Node> nodes = new ArrayList<Node>();
+  int recursionDepth = 0;
+  int maxDepth = 1000;
+  int pointAmount = 0;
+  
+  HashMap<PVector, Integer> vert_lookup_map = new HashMap<PVector, Integer>();
+  ArrayList<PVector> mapVectors = new ArrayList<PVector>();
+  
+  PVector midpoint(Node a, Node b)
   {
-       /// generate the graph you need for pathfinding
+     int start = 0;
+     int end = 0;
+    
+     int prev_index = a.indices.get(a.indices.size()-1);
+     for(Integer i: a.indices)
+     {
+       if (b.indices.contains(prev_index) && b.indices.contains(i)) {
+         start = prev_index;
+         end = i;
+         break;
+       }
+       prev_index = i;
+     }
+     println(a.id + " and " + b.id + " share indices " + start + " and " + end);
+     
+     
+     PVector start_vect, end_vect;
+     start_vect = mapVectors.get(start);
+     end_vect = mapVectors.get(end);
+     
+     return new PVector(start_vect.x + (end_vect.x - start_vect.x)/2,
+     start_vect.y + (end_vect.y - start_vect.y)/2);
   }
 
-   ArrayList<PVector> findPath(PVector start, PVector destination)
+  
+  void calculateAdjacencies()
   {
-      /// implement A* to find a path
+    for (Node n: nodes)
-      ArrayList<PVector> result = null;
+    {
-      return result;
+      n.neighbours.clear();
+    }
+      
+    //for(int i = 0; i < nodes.size(); i++){
+    
+    //if(i + 1 >= nodes.size()) continue;
+    //Node a = nodes.get(i);
+    //Node b = nodes.get(i + 1);
+    
+    //if(a.isneighbours(b)) a.neighbours.add(b);
+    
+    
+    //}
+    for (Node a: nodes)
+    {      
+      //this is terrible for efficiency i'm so sorry
+      for (Node b: nodes)
+      {
+         if (b.equals(a)) continue;
+         if (a.isNeighbours(b)) a.neighbours.add(b);
+      }
+    }
+  }
+  void setIndices(Node node)
+  {
+     for(Wall w: node.polygon)
+     {
+         node.indices.add(vert_lookup_map.get(w.start));
+     }
+  }
+
+  //assume index_1 < index_2
+  void splitMap(Node node, int index_1, int index_2)
+  {
+    
+    ArrayList<Wall> polygon_1 = new ArrayList<Wall>();
+    ArrayList<Wall> polygon_2 = new ArrayList<Wall>();
+    
+    //get the vertex positions from your original node
+    ArrayList<PVector> node_verts = new ArrayList<PVector>();
+    for(Wall w: node.polygon)
+    {
+      node_verts.add(w.start);
+    }
+    
+    //for polygon_1, just make a polygon from index A to B
+    for(int i = index_1; i<=index_2; i++)
+    {
+      //finishes the polygon
+      if (i == index_2) {
+        polygon_1.add( new Wall(node_verts.get(index_2), node_verts.get(index_1)) );
+        break;
+      }
+      
+      int next_index = i+1;
+      if (next_index > node_verts.size()-1) next_index = 0;
+      polygon_1.add( new Wall(node_verts.get(i), node_verts.get(next_index)) );
+    }
+    
+    //for polygon_2
+    //a little bit tricker, since poly b has a disjunction between vertex indices
+    //the loop is thus different for constructing b
+    //start from index_2 and go further until you hit index A. You are guaranteed to finish the polygon once you connect A and B.
+    int i = index_2;
+    boolean completedpolygon_2 = false;
+    while (!completedpolygon_2) {
+      if (i == index_1) {
+        polygon_2.add( new Wall(node_verts.get(index_1), node_verts.get(index_2)) );
+        completedpolygon_2 = true;
+        break;
+      }
+      
+      int next_index = i+1;
+      if (next_index > node_verts.size()-1) next_index = 0;
+      polygon_2.add( new Wall(node_verts.get(i), node_verts.get(next_index)) );
+      
+      i = next_index;
+    }
+    
+  
+    //we'll create a node to store poly a
+    Node nodeA = new Node(recursionDepth+"A", polygon_1);
+    setIndices(nodeA);
+    nodes.add(nodeA);
+    
+    //the same goes for b
+    Node nodeB = new Node(recursionDepth+"B", polygon_2);
+    setIndices(nodeB);    
+    nodes.add(nodeB);
+    
+    
+    
+    //this portion is not at all necessary for the program to function but it helps when debugging
+    recursionDepth++;
+    if (recursionDepth == maxDepth) return;
+    
+    //polygons are added to the node list, in order of A and B
+    //0.[NODE 0A] 
+    //1.[NODE 0B]
+    
+    //findReflexVertex will return -1 if the shape is all good
+    //remove the bad nodes from the list and add in two new ones
+    //order in the node list has no effect on neighboursing
+    //the node list functions identically to a bag in that regard
+    if (findReflexVertex(polygon_1) != -1) {
+      nodes.remove(nodeA);
+      convexDecomposition(nodeA);
+    }
+    if (findReflexVertex(polygon_2) != -1) {
+      nodes.remove(nodeB);
+      convexDecomposition(nodeB);
+    }
+  }
+
+
+  int findReflexVertex(ArrayList<Wall> polygon)
+  { 
+
+    for (int i = 0; i<polygon.size() - 1; i++)
+    {
+     // finding the reflex angle by finding where it turns right
+      if (polygon.get(i).normal.dot(polygon.get(i + 1).direction) >= 0) {
+        return i + 1;
+      }
+    }
+    
+    return -1;
+  }
+  
+  //given a reflexive index, find a vertex that you can go to without intersection another wall 
+  int joiningVertex(ArrayList<Wall> polygon, int convex_index)
+  {
+    //you need the PVectors for this one
+    ArrayList<PVector> vertices = new ArrayList<PVector>();
+    for(Wall w: polygon)
+    {
+      vertices.add(w.start);
+    }
+    
+    //our "bad" point
+    PVector pointAtIndex = vertices.get(convex_index);
+
+    //we don't need to consider the vertex's neighbours since they obviously can't be connected to
+    int next_index = convex_index + 1;
+    if (next_index >= vertices.size()) next_index = 0;
+
+    int lastIndex = convex_index - 1;
+    if (lastIndex < 0) lastIndex = vertices.size() - 1;
+
+    for (int potentialConnecting = vertices.size()-1; potentialConnecting>=0; potentialConnecting--)
+    {
+      //skip neighbours and the bad point
+      if (potentialConnecting == next_index || potentialConnecting == convex_index || potentialConnecting == lastIndex) continue;
+
+      PVector potentialConnectingPoint = vertices.get(potentialConnecting);
+      
+      if (!map.intersectsWall(pointAtIndex, potentialConnectingPoint))
+      {
+        return potentialConnecting;
+      }
+    }
+    
+    return -1;
+  }
+  
+  
+  void convexDecomposition(Node node)
+  {
+    int convex_index = findReflexVertex(node.polygon);
+    if (convex_index == -1) return;
+    
+    int joining_index = joiningVertex(node.polygon, convex_index);
+    if (joining_index == -1) return;
+    
+   // split polygons from small index to the max index
+    splitMap(node, min(convex_index, joining_index), max(convex_index, joining_index));
+  }
+
+  //creates a hashmap with key PVector and value Integer
+  //creating a hashmap for this removes the risk of directly comparing PVectors since it should look by reference instead of value
+  void setVertexMap(Map map)
+  {
+    //clear all lookups and map vectors
+    mapVectors.clear();
+    vert_lookup_map.clear();
+    
+    for (int i = 0; i < map.walls.size(); i++)
+    {
+      vert_lookup_map.put(map.walls.get(i).start, i);
+      mapVectors.add(map.walls.get(i).start);
+     
+    }
+  }
+
+  void bake(Map map)
+  {    
+    //reset recursions and other values
+    recursionDepth = 0;
+    nodes.clear();
+    pointAmount = map.walls.size();
+    
+    vert_lookup_map.clear();
+    mapVectors.clear();
+    
+    //make hashmap of vertices
+    setVertexMap(map);
+    
+    //create a node with the whole map walls
+    Node m = new Node("Map", map.outline);
+    setIndices(m);
+    
+   
+    convexDecomposition(m);
+    
+   
+    calculateAdjacencies();
+       
+    
+  }
+
+ Node nodeFromPoint(PVector p)
+  {
+    for (Node n: nodes)
+    {
+      if (isPointInPolygon(p,n.polygon))
+        return n;
+    }
+    
+    return null;
+  }
+  //Uses A* to find a path from start to dest
+  ArrayList<PVector> findPath(PVector start, PVector dest)
+  {
+    println("dest vec " + dest);
+    ArrayList<SearchFrontier> frontier = new ArrayList<SearchFrontier>(); 
+    ArrayList<Node> visited_nodes = new ArrayList<Node>(); 
+    Node node_start = nodeFromPoint(start);
+    Node node_dest = nodeFromPoint(dest);
+    println("dest node " + node_dest);
+    //for (Node n: nodes)
+    //{
+    //  if (isPointInPolygon(start,n.polygon)) node_start = n;
+    //  else if (isPointInPolygon(dest,n.polygon)) node_dest = n;
+    //  //else println("node_dest is null");
+        
+    //}
+   
+    
+    SearchFrontier s = new SearchFrontier(node_start, null, node_dest.findCenter());
+    frontier.add(s);
+    visited_nodes.add(frontier.get(0).node);
+    
+    println("frontier " + frontier);
+    //till the end of of frontier
+    while (frontier.get(0).node != node_dest)
+    {
+      
+      SearchFrontier first_frontier = frontier.get(0);
+      // add all the neighbours of first
+      
+      for (Node neighbours: first_frontier.node.neighbours)
+      {
+        println("loop");
+        if (!visited_nodes.contains(neighbours))
+        {
+          frontier.add(new SearchFrontier(neighbours, first_frontier, node_dest.findCenter())); 
+        }
+      }
+      //first in frontier no longer required
+      frontier.remove(0);
+      //sort via lambda function
+      //shorter paths have priority
+      frontier.sort((a,b) -> {
+        if (a.heuristicSum() > b.heuristicSum()) return 1;
+        else if (a.heuristicSum() < b.heuristicSum()) return -1;
+        else return 0;
+      });
+      //add the removed node to visited list
+      visited_nodes.add(first_frontier.node);
+    }
+    
+    return findDestPath(dest, node_start, frontier);
+  }
+  
+ 
+  
+  //given a list of frontiers, create a PVector path from the start to dest
+  ArrayList<PVector> findDestPath(PVector dest, Node node_start, ArrayList<SearchFrontier> genPath)
+  {
+    //we're going to build this list up from the end and then reverse it.
+    
+    ArrayList<PVector> res = new ArrayList<PVector>();
+    //add the end
+    res.add(dest);
+    SearchFrontier front = genPath.get(0);
+    while (front.node != node_start) {
+      PVector midPoint = midpoint(front.node, front.prev_frontier.node);
+      res.add(midPoint);
+      
+      //assign previous frontier to start
+      front = front.prev_frontier;
+    }
+    
+    
+    Collections.reverse(res);
+    println("result " + res);
+    return res;
   }
 
   
@@ -38,6 +428,19 @@ class NavMesh
 
   void draw()
   {
-      /// use this to draw the nav mesh graph
+    
+    strokeWeight(3);
+    
+    
+    for (Node n: nodes)
+    {
+      for (Wall w: n.polygon)
+      {
+         stroke(0,255,255);
+         strokeWeight(3);
+          line(w.start.x, w.start.y, w.end.x, w.end.y);
+          //w.draw();
+      }
+    }
   }
 }

lab1.pde

@@ -13,7 +13,7 @@ NavMesh nm = new NavMesh();
 
 boolean entering_path = false;
 
-boolean show_nav_mesh = false;
+boolean show_nav_mesh = true;
 
 boolean show_waypoints = false;
 
@@ -37,14 +37,41 @@ void mousePressed() {
   if (mouseButton == LEFT)
   {
      
-     if (waypoints.size() == 0)
+     if (!entering_path) //if you haven't made a path yet
      {
+       //println("node size " + nm.nodes.size());
+        if (nm.nodes.size() > 0) //if you're on a map
+        {
+           println("Pathfinding to single target");
+           waypoints = nm.findPath(billy.kinematic.position, target);
+           println("waypoints " + waypoints);
+           //Collections.reverse(waypoints);
+           billy.follow(waypoints);
+        }
+        else { //if you're not on a map
+           println("Simply seeking target");
            billy.seek(target);
         }
-     else
+     }
+     else //if you have a path
+     {
+        //finish the path
+        if (nm.nodes.size() > 0) //if you're on a map
+        {
+          PVector start_vectoint = waypoints.get(waypoints.size() -1);
+          ArrayList<PVector> finalRoute = nm.findPath(start_vectoint, target);
+          for (PVector p: finalRoute)
+          {
+            waypoints.add(p);
+          }
+        }
+        else //if you're not on a map
         {
           waypoints.add(target);
+        }
+        println("Finishing Path");
         entering_path = false;
+        println(waypoints);
         billy.follow(waypoints);
      }
   }