/// Using this variable to decide whether to draw all the stuff
boolean debug = true;
// A path object (series of connected points)
Path path;
// vehicle array
ArrayList<Vehicle> vehicles;
void setup() {
size(640, 360);
// Call a function to generate new Path object
newPath();
vehicles = new ArrayList<Vehicle>();
for (int i = 0; i < 100; i++) {
vehicles.add(new Vehicle(new PVector(mouseX, mouseY), 2, 0.04));
}
}
void draw() {
background(255);
// Display the path
path.display();
// The boids follow the path
for (int i= vehicles.size()-1; i >= 0; i--) {
Vehicle v = vehicles.get(i);
v.follow(path);
// Call the generic run method (update, borders, display, etc.)
v.run();
if (v.position.x > width){vehicles.remove(i);}
// Instructions
fill(0);
text("Hit space bar to toggle debugging lines.\nClick the mouse to generate a new path.", 10, height-30);
}
}
void newPath() {
// A path is a series of connected points
// A more sophisticated path might be a curve
path = new Path();
path.addPoint(-20, height/2);
path.addPoint(random(0, width/2), random(0, height));
path.addPoint(random(width/2, width), random(0, height));
path.addPoint(width+20, height/2);
}
public void keyPressed() {
if (key == ' ') {
debug = !debug;
}
if (key == 'p') {
newPath();
}
}
void mousePressed() {
vehicles.add(new Vehicle(new PVector(mouseX, mouseY), 2, 0.04));
}
/class Path {
// A Path is an arraylist of points (PVector objects)
ArrayList<PVector> points;
// A path has a radius, i.e how far is it ok for the boid to wander off
float radius;
Path() {
// Arbitrary radius of 20
radius = 20;
points = new ArrayList<PVector>();
}
// Add a point to the path
void addPoint(float x, float y) {
PVector point = new PVector(x, y);
points.add(point);
}
PVector getStart() {
return points.get(0);
}
PVector getEnd() {
return points.get(points.size()-1);
}
// Draw the path
void display() {
// Draw thick line for radius
stroke(175);
strokeWeight(radius*2);
noFill();
beginShape();
for (PVector v : points) {
vertex(v.x, v.y);
}
endShape();
// Draw thin line for center of path
stroke(0);
strokeWeight(1);
noFill();
beginShape();
for (PVector v : points) {
vertex(v.x, v.y);
}
endShape();
}
}
class Vehicle {
// All the usual stuff
PVector position;
PVector velocity;
PVector acceleration;
float r;
float maxforce; // Maximum steering force
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();
}
// This function implements Craig Reynolds' path following algorithm
// http://www.red3d.com/cwr/steer/PathFollow.html
void follow(Path p) {
// Predict position 50 (arbitrary choice) frames ahead
// This could be based on speed
PVector predict = velocity.get();
predict.normalize();
predict.mult(50);
PVector predictpos = PVector.add(position, predict);
// Now we must find the normal to the path from the predicted position
// We look at the normal for each line segment and pick out the closest one
PVector normal = null;
PVector target = null;
float worldRecord = 1000000; // Start with a very high record distance that can easily be beaten
// Loop through all points of the path
for (int i = 0; i < p.points.size()-1; i++) {
// Look at a line segment
PVector a = p.points.get(i);
PVector b = p.points.get(i+1);
// Get the normal point to that line
PVector normalPoint = getNormalPoint(predictpos, a, b);
// This only works because we know our path goes from left to right
// We could have a more sophisticated test to tell if the point is in the line segment or not
if (normalPoint.x < a.x || normalPoint.x > b.x) {
// This is something of a hacky solution, but if it's not within the line segment
// consider the normal to just be the end of the line segment (point b)
normalPoint = b.get();
}
// How far away are we from the path?
float distance = PVector.dist(predictpos, normalPoint);
// Did we beat the record and find the closest line segment?
if (distance < worldRecord) {
worldRecord = distance;
// If so the target we want to steer towards is the normal
normal = normalPoint;
// Look at the direction of the line segment so we can seek a little bit ahead of the normal
PVector dir = PVector.sub(b, a);
dir.normalize();
// This is an oversimplification
// Should be based on distance to path & velocity
dir.mult(10);
target = normalPoint.get();
target.add(dir);
}
}
// Only if the distance is greater than the path's radius do we bother to steer
if (worldRecord > p.radius) {
seek(target);
}
// Draw the debugging stuff
if (debug) {
// Draw predicted future position
stroke(0);
fill(0);
line(position.x, position.y, predictpos.x, predictpos.y);
ellipse(predictpos.x, predictpos.y, 4, 4);
// Draw normal position
stroke(0);
fill(0);
ellipse(normal.x, normal.y, 4, 4);
// Draw actual target (red if steering towards it)
line(predictpos.x, predictpos.y, normal.x, normal.y);
if (worldRecord > p.radius) fill(255, 0, 0);
noStroke();
ellipse(target.x, target.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
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();
}
}