.gitattributes

@@ -0,0 +1,2 @@
+# Auto detect text files and perform LF normalization
+* text=auto

Boid.pde

@@ -24,6 +24,10 @@ class Boid
   float rotational_acceleration;
   KinematicMovement kinematic;
   PVector target;
+  PVector direction;
+  ArrayList<PVector> waypoints;
+  boolean stillInRadius = true;
+  int currentTarget = 0;
 
   Boid(PVector position, float heading, float max_speed, float max_rotational_speed, float acceleration, float rotational_acceleration)
   {
@@ -38,6 +42,188 @@ class Boid
     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;
+      //You have to normalize this too or the boid goes the wrong way sometimes
+      float angleToTarget = normalize_angle_left_right(atan2(direction.y, direction.x) - normalize_angle_left_right(kinematic.getHeading()));
+      float arrivalThreshold = 150.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 > 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, -kinematic.getRotationalVelocity());
+        } else if (kinematic.getRotationalVelocity() < 0) {
+          kinematic.increaseSpeed(0.0, kinematic.getRotationalVelocity());
+        }
+      }
+
+
+      //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
+        //TODO: change this to slow down less / not at all if the angle to the next target is not large
+
+        //This handles starting / stopping if there are more targets
+
+        //This ensures that we don't crash because waypoints is null
+        if (waypoints != null) {
+
+          //this checks if there's another target to go to
+          if (currentTarget + 1 < waypoints.size()) {
+
+
+
+            //if so, change the speed depending on the angle to the next target
+
+            //We can calculate the angle to the next target with the use of two vectors: one from our location to current target, one from current target to next target
+            //use the dot product of those two vectors; this gives us the angle between them, and we can use that to calculate how much we should slow down
+
+            //direction is our boid to target vector
+            //this is at direction
+            //current target to next target is here:
+            PVector currentTargetToNext = PVector.sub(waypoints.get(currentTarget+1), target);
+
+            //i'm not sure this is the best way to do this, it might be simpler to calculate the angle, but this should work too
+
+            //holds dot product of targets
+            float dotProductOfTargets = PVector.dot(currentTargetToNext, direction);
+
+            //Dividing by both their magnitudes will normalize our result between -1 and 1
+            dotProductOfTargets = dotProductOfTargets / (currentTargetToNext.mag() * direction.mag());
+
+            //Add 1, divide by 2, this will cause our result to be between 0 and 1
+            //If this is closer to 0, slow down more. If it's closer to 1, slow down less.
+            dotProductOfTargets = (dotProductOfTargets + 1) / 2;
+
+            //use an ideal speed for our boid, to tell it to either speed up or slow down whether it's going faster than this or not
+            float idealSpeed = (dotProductOfTargets) * 80 + 15;
+
+            float maxSpeed = 100 * pow(((PI - abs(angleToTarget)) / PI), 10);
+            //println(maxSpeed);
+
+            if (idealSpeed > maxSpeed) {
+              idealSpeed = maxSpeed;
+            }
+
+            if (kinematic.getSpeed() < idealSpeed) {
+              kinematic.increaseSpeed(1, 0);
+            } else if (kinematic.getSpeed() > idealSpeed) {
+              kinematic.increaseSpeed(-1, 0);
+            }
+          } else {
+
+            //if no more targets to check, do the normal calculation
+
+            //kinematic.getSpeed() is how fast we're moving, direction.mag() is how far are we from target
+            //Ideal speed here should be 80 at dist 85, and reduce linearly from there, hitting 0 at 5 units?
+            //This can be changed later if it isn't good
+            
+            
+            float idealSpeed = (1 * direction.mag() - 5);
+            
+            if (idealSpeed < 0) {
+              idealSpeed = 0;
+            }
+
+            if (kinematic.getSpeed() < idealSpeed) {
+              kinematic.increaseSpeed(1, 0);
+            } else if (kinematic.getSpeed() > idealSpeed) {
+              kinematic.increaseSpeed(-1, 0);
+            }
+          }
+        } else {
+
+          //if waypoints is null, do normal things
+          println("waypoints is null");
+
+          //This code should trigger if there's only one target left
+
+          //kinematic.getSpeed() is how fast we're moving, direction.mag() is how far are we from target
+          //Ideal speed here should be 80 at dist 85, and reduce linearly from there, hitting 0 at 5 units?
+          //This can be changed later if it isn't good
+
+          float idealSpeed = 1 * direction.mag() + 10;
+
+          //if idealSpeed is "negative" we should just set it to 0
+          if (idealSpeed < 0) {
+            idealSpeed = 0;
+          }
+
+          println(idealSpeed);
+
+          //use this to know how off the target speed we are, and slow down accordingly
+          //This will be positive if the ideal speed is higher than current speed, negative if ideal speed is lower.
+          float speedOffset = (idealSpeed - kinematic.getSpeed());
+
+          if (abs(speedOffset) < 1) {
+            kinematic.increaseSpeed(speedOffset, 0);
+          } else if (idealSpeed < speedOffset) {
+            kinematic.increaseSpeed(1, 0);
+          } else if (idealSpeed > speedOffset) {
+            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);
+
+      //handling going to multiple targets
+
+      //initial check exists because waypoints will be null for a single target
+      if (waypoints != null) {
+        //If within 5 units, move to next target
+        if (direction.mag() < 5) {
+          //This ensures that the same target can't trigger moving to the next target twice
+          if (stillInRadius == false) {
+            //this ensures that waypoints get cleared after finishing checking all targets
+            if (currentTarget < waypoints.size() - 1) {
+              currentTarget++;
+            } else {
+              currentTarget = 0;
+              waypoints = null;
+            }
+          }
+          stillInRadius = true;
+          if (waypoints != null) {
+            seek(waypoints.get(currentTarget));
+          }
+        } else {
+          stillInRadius = false;
+        }
+      }
     }
 
     // place crumbs, do not change
@@ -85,13 +271,13 @@ class Boid
   void seek(PVector target)
   {
     this.target = target;
-      
   }
 
   void follow(ArrayList<PVector> waypoints)
   {
-      // TODO: change to follow *all* waypoints
-      this.target = waypoints.get(0);
 
+    this.waypoints = waypoints;
+
+    seek(waypoints.get(0));
   }
 }

Flocking.pde

@@ -1,7 +1,24 @@
 
 /// called when "f" is pressed; should instantiate additional boids and start flocking
+Boid[] billies = new Boid[8];
 void flock()
 {
+  int lasttr = 0;
+  println("flock called");
+  float dt = (millis() - lasttr)/1000.0;
+  lasttr = millis();
+  PVector target = new PVector(mouseX, mouseY);
+  
+  for(int i = 0; i < 7; i++)
+  {
+     billies[i] = new Boid(new PVector(100 + i*100, 500), BILLY_START_HEADING, BILLY_MAX_SPEED, BILLY_MAX_ROTATIONAL_SPEED, BILLY_MAX_ACCELERATION, BILLY_MAX_ROTATIONAL_ACCELERATION);
+     println("billy " + billies[i].toString()); 
+
+  billies[i].update(dt);
+  billies[i].seek(target);
+  }
+  
+
 }
 
 /// called when "f" is pressed again; should remove the flock

Map.pde

@@ -58,12 +58,13 @@ class Wall
    
    void draw()
    {
-       strokeWeight(3);
+       
+       strokeWeight(1);
        line(start.x, start.y, end.x, end.y);
        if (SHOW_WALL_DIRECTION)
        {
           PVector marker = PVector.add(PVector.mult(start, 0.2), PVector.mult(end, 0.8));
-          circle(marker.x, marker.y, 5);
+          circle(marker.x, marker.y, 6);
        }
    }
 }
@@ -114,8 +115,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 +140,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
@@ -274,7 +286,7 @@ class Map
       }
    }
    
-   boolean isReachable(PVector point)
+   public boolean isReachable(PVector point)
    {
        if (!isPointInPolygon(point, outline)) return false;
        for (Obstacle o: obstacles)
@@ -283,4 +295,5 @@ class Map
        }
        return true;
    }
+   
 }

NavMesh.pde

@@ -1,33 +1,431 @@
-// Useful to sort lists by a custom key
+import java.util.*;
-import java.util.Comparator;
+Map origin_map;
-
-
-/// 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(i) && n.indices.contains(prev)) 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 rec_stack_count = 0;
+  int max_depth = 1000;
+  
+  
+  HashMap<PVector, Integer> vert_lookup_map = new HashMap<PVector, Integer>();
+  ArrayList<PVector> map_vecs = 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(i) && b.indices.contains(prev_index)) {
+         start = prev_index;
+         end = i;
+         break;
+       }
+       prev_index = i;
      }
 
-   ArrayList<PVector> findPath(PVector start, PVector destination)
+     
+     
+     PVector start_vect, end_vect;
+     start_vect = map_vecs.get(start);
+     end_vect = map_vecs.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);
+  }
+
+  
+  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)
+    {      
+      
+      for (Node b: nodes)
+      {
+         if (b.equals(a)) continue;
+         if (a.isNeighbours(b)) a.neighbours.add(b);
+      }
+    }
+  }
+  void setNodeIndices(Node node)
+  {
+     for(Wall w: node.polygon)
+     {
+         node.indices.add(vert_lookup_map.get(w.start));
+     }
+  }
+
+
+  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);
+    }
+    
+    //make polygon from index 1 to 2.
+    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)));
+    }
+    
+
+    
+    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;
+    } 
+
+    Node nodeA = new Node(rec_stack_count+"A", polygon_1);
+    setNodeIndices(nodeA);
+    nodes.add(nodeA);
+    
+
+    Node nodeB = new Node(rec_stack_count+"B", polygon_2);
+    setNodeIndices(nodeB);    
+    nodes.add(nodeB);
+    
+
+    rec_stack_count++;
+    if (rec_stack_count == max_depth) return;
+   
+    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(); i++)
+    {
+     // finding the reflex angle by finding where it turns right
+     int j = i + 1;
+     // for index out of bounds
+     if( j >= polygon.size()) j = 0;
+      if (polygon.get(i).normal.dot(polygon.get(j).direction) >= 0) {
+        return j;
+      }
+    }
+    
+    return -1;
+  }
+  PVector percentage(PVector from, PVector to, float percent)
+   {
+     //p1 + ((p2 - p1) * percent)
+      return PVector.mult(PVector.sub(to, from),percent);
+   }
+   
+   boolean intersectsWall(PVector from, PVector to)
+   {  
+      //threshold to see if wall intersects with 1% margin.
+      
+      PVector start = PVector.add(from, percentage(from, to, 0.01));
+      
+      
+      PVector end = PVector.add(from, percentage(from, to, 0.99));
+      
+      if (!map.isReachable(start)) return true;
+     
+      //println("Start: " + start);
+      //println("End: " + end);
+     
+      for (Wall w : map.walls)
+      {
+         if (w.crosses(start, end)) return true;
+      }
+      return false;
+   }
+  
+  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);
+    }
+    
+
+    PVector index_point = vertices.get(convex_index);
+
+   
+    int next_index = convex_index + 1;
+    if (next_index >= vertices.size()) next_index = 0;
+
+    int last_index = convex_index - 1;
+    if (last_index < 0) last_index = vertices.size() - 1;
+
+    for (int i = vertices.size()-1; i>=0; i--)
+    {
+      
+      if (i == next_index || i == convex_index || i == last_index) continue;
+
+      PVector point_vertex = vertices.get(i);
+      
+      if (!intersectsWall(index_point, point_vertex))
+      {
+        return i;
+      }
+    }
+    
+    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;
+    
+   
+    splitMap(node, min(convex_index, joining_index), max(convex_index, joining_index));
+  }
+
+  //creates a hashmap with key PVector and value Integer
+ 
+  void setVertexMap(Map map)
+  {
+    //clear all lookups and map vectors
+    map_vecs.clear();
+    vert_lookup_map.clear();
+    
+    for (int i = 0; i < map.walls.size(); i++)
+    {
+      vert_lookup_map.put(map.walls.get(i).start, i);
+      map_vecs.add(map.walls.get(i).start);
+     
+    }
+  }
+
+  void bake(Map map)
+  {    
+    //reset recursions and other values
+    
+    // to keep track of recursive calls
+    rec_stack_count = 0;
+    nodes.clear();
+    
+    
+    origin_map = map;
+    vert_lookup_map.clear();
+    map_vecs.clear();
+    
+    //make hashmap of vertices
+    setVertexMap(map);
+    
+    //create a node with the whole map walls
+    Node m = new Node("Map", map.outline);
+    setNodeIndices(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)
+  {
+   
+    ArrayList<SearchFrontier> frontier = new ArrayList<SearchFrontier>(); 
+    ArrayList<Node> visited_nodes = new ArrayList<Node>(); 
+    Node node_start = nodeFromPoint(start);
+    Node node_dest = nodeFromPoint(dest);
+    
+    
+    
+    SearchFrontier s = new SearchFrontier(node_start, null, node_dest.findCenter());
+    frontier.add(s);
+    visited_nodes.add(frontier.get(0).node);
+    
+   
+    //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)
+      {
+        
+        if (!visited_nodes.contains(neighbours))
+        {
+          frontier.add(new SearchFrontier(neighbours, first_frontier, node_dest.findCenter())); 
+        }
+      }
+
+      frontier.remove(0);
+      //sort via lambda function
+      
+      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)
+  {
+    
+    
+    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);
+    
+    return res;
   }
 
   
@@ -38,6 +436,29 @@ class NavMesh
 
   void draw()
   {
-      /// use this to draw the nav mesh graph
+    
+    strokeWeight(3);
+    
+    
+    for (Node n: nodes)
+    {
+      for (Wall w: n.polygon)
+      {
+         stroke(255);
+         strokeWeight(2);
+         line(w.start.x, w.start.y, w.end.x, w.end.y);
+         
+          //w.draw();
+      }
+    }
+    for( Wall w: map.outline){
+        stroke(255,0,0);
+         strokeWeight(3);
+         line(w.start.x, w.start.y, w.end.x, w.end.y);
+    
+    
+    
+    }
+    
   }
 }

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;
 
@@ -22,7 +22,7 @@ boolean show_help = false;
 boolean flocking_enabled = false;
 
 void setup() {
-  size(800, 600);
+  size(1000, 800);
   
   billy = new Boid(BILLY_START, BILLY_START_HEADING, BILLY_MAX_SPEED, BILLY_MAX_ROTATIONAL_SPEED, BILLY_MAX_ACCELERATION, BILLY_MAX_ROTATIONAL_ACCELERATION);
   randomSeed(0);
@@ -37,14 +37,41 @@ void mousePressed() {
   if (mouseButton == LEFT)
   {
      
-     if (waypoints.size() == 0)
+     if (!entering_path) 
      {
+       
+        if (nm.nodes.size() > 0) // if map is pressed and nodes are still left
+        {
+          
+           waypoints = nm.findPath(billy.kinematic.position, target);
+         
+      
+           billy.follow(waypoints);
+        }
+        else { //target seeking on plain space
+           
            billy.seek(target);
         }
+     }
      else 
+     {
+        //finish the path
+        if (nm.nodes.size() > 0) //remaining map if left
+        {
+          PVector start_vectoint = waypoints.get(waypoints.size() -1);
+          ArrayList<PVector> finalRoute = nm.findPath(start_vectoint, target);
+          for (PVector p: finalRoute)
+          {
+            waypoints.add(p);
+          }
+        }
+        else //waypoints to be added
         {
           waypoints.add(target);
+        }
+        
         entering_path = false;
+       
         billy.follow(waypoints);
      }
   }

sketch.properties

@@ -0,0 +1 @@
+main=lab1.pde