Created
August 18, 2020 01:42
-
-
Save Nekodigi/e400606adf1d09fdc50bc9886534657b to your computer and use it in GitHub Desktop.
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
// The Nature of Code | |
// Daniel Shiffman | |
// http://natureofcode.com | |
// Path Following | |
// Via Reynolds: // http://www.red3d.com/cwr/steer/PathFollow.html | |
// Using this variable to decide whether to draw all the stuff | |
boolean debug = false; | |
// A path object (series of connected points) | |
Path path; | |
// Two vehicles | |
ArrayList<Vehicle> cars = new ArrayList<Vehicle>(); | |
void setup() { | |
size(640, 360); | |
blendMode(ADD); | |
colorMode(HSB, 360, 100, 100); | |
// Call a function to generate new Path object | |
newPath(); | |
// Each vehicle has different maxspeed and maxforce for demo purposes | |
for(int i=0; i<100; i++){ | |
cars.add(new Vehicle(new PVector(random(width), random(width)), 2, 0.04)); | |
} | |
background(0); | |
} | |
void draw() { | |
//background(255); | |
fill(0, 1); | |
rect(0, 0, width, height); | |
// Display the path | |
//path.display(); | |
// The boids follow the path | |
for(Vehicle car : cars){ | |
car.follow(path); | |
car.run(); | |
car.borders(path); | |
} | |
// 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; | |
} | |
} | |
public void mousePressed() { | |
newPath(); | |
} | |
// The Nature of Code | |
// Daniel Shiffman | |
// http://natureofcode.com | |
// Path Following | |
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(); | |
} | |
} | |
// The Nature of Code | |
// Daniel Shiffman | |
// http://natureofcode.com | |
// Path Following | |
// Vehicle class | |
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(ms, 0); | |
} | |
// Main "run" function | |
public 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 = position.copy(); | |
// 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(); | |
stroke(float(frameCount)/100%360, sin(float(frameCount)/10)*50+50, sin(float(frameCount)/100)*50+50, 100); | |
point(position.x, position.y); | |
} | |
// 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); | |
} | |
} | |
} |
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment