Just the start of the path planning implementation

path.pde

@@ -6,7 +6,7 @@ class Path {
   float radius;
 
   Path() {
-    // Arbitrary radius of 20
+    // Arbitrary radius of 30
     radius = 30;
     points = new ArrayList<PVector>();
   }
@@ -17,8 +17,7 @@ class Path {
     points.add(point);
   }
 
-  // Draw the path
-  void display() {
+  void drawRoad() {
     strokeJoin(ROUND);
 
     // Draw thick line for radius
@@ -30,6 +29,9 @@ class Path {
       vertex(v.x, v.y);
     }
     endShape(CLOSE);
+  }
+
+  void drawPathLine() {
     // Draw thin line for center of path
     stroke(0);
     strokeWeight(1);
@@ -40,4 +42,15 @@ class Path {
     }
     endShape(CLOSE);
   }
+
+  // Draw the path
+  void display(boolean drawRoadBool) {
+    // Check if the road needs to be drawn
+    if (drawRoadBool) {
+      drawRoad();
+    }
+    // Draw the path line
+    drawPathLine();
+  }
+
 }

pathplanner.pde

@@ -0,0 +1,27 @@
+class PathPlanner {
+  // Produces a path from a given track, startPoint, and EndPoint
+
+  Path track = null;  // Path of the track it's given
+  Path path = null;  // Path it produces
+  PVector startPoint = null;  // Start point on the path
+  PVector endPoint = null;  // Destination point
+
+  PathPlanner(Path atrack, PVector start, PVector destination) {
+    track = atrack;
+    startPoint = start;
+    endPoint = destination;
+    path = new Path();
+  }
+
+  void discritizeTrack() {
+    // Make the track a little more refined, to allow for more careful decisions
+
+  }
+
+  Path pathFinder() {
+    // Returns a Path through the track
+
+    return null;
+  }
+
+}

simulation.pde

@@ -1,16 +1,33 @@
 
+//////////////////////////////////////////////////////////////////
+// Global Variables
+//////////////////////////////////////////////////////////////////
+
+// Debug Variable
+// WARNING: Do not delete this variable, it's being used in some
+// classes
 boolean debug = true;
 
+//////////////////////////////////
+// Main Objects
+//////////////////////////////////
+
 // A path object (series of connected points)
 Path path;
 
 // Two vehicles
 Vehicle car1;
 Vehicle car2;
 
+//////////////////////////////////////////////////////////////////
+// Setup
+//////////////////////////////////////////////////////////////////
+
 void setup()
 {
+  // Set canvas size
   size(1000, 1000);
+
   // Call a function to generate new Path object
   newPath();
 
@@ -19,14 +36,22 @@ void setup()
   car2 = new Vehicle(new PVector(0, height/2), 1, 2);
 }
 
+//////////////////////////////////////////////////////////////////
+// Draw
+//////////////////////////////////////////////////////////////////
+
 void draw()
 {
+  // Background
   background(255);
-  // Display the path
-  path.display();
+
+  // Display the Road and center line path
+  path.display(true);
+
   // The boids follow the path
   car1.follow(path);
   car2.follow(path);
+
   // Call the generic run method (update, borders, display, etc.)
   car1.run();
   car2.run();
@@ -37,7 +62,6 @@ void draw()
   //car2.borders();
   // Instructions
   fill(0);
-
 }
 
 // Here define the element of the track
@@ -59,7 +83,7 @@ void newPath() {
   // A more sophisticated path might be a curve
   path = new Path();
   float offset = 30;
-   path.addPoint(offset,offset);
+  path.addPoint(offset,offset);
   path.addPoint(width-offset,offset);
   path.addPoint(width-offset,height-offset);
   path.addPoint(width/2,height-offset*3);
@@ -73,8 +97,3 @@ public void keyPressed()
     debug = !debug;
   }
 }
-
-public void mousePressed()
-{
-  newPath();
-}

vehicle.pde

@@ -10,7 +10,7 @@ class Vehicle
   float maxspeed;    // Maximum speed
 
     // Constructor initialize all values
-  Vehicle( PVector l, float ms, float mf)
+  Vehicle(PVector l, float ms, float mf)
   {
     position = l.get();
     r = 4.0;