class Path { // A Path is an arraylist of points (PVector objects) ArrayList<Vector> 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<Vector>(); } // 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(); }}
for (int i = 0; i < p.points.size()-1; i++) { PVector a = p.points.get(i); PVector b = p.points.get(i+1); PVector normalPoint = getNormalPoint(predictLoc,a,b); 为每个线段探求法线交点
示例代码6-6 路径跟随
boolean debug = true;// A path object (series of connected points)Path path;// Two vehiclesVehicle car1;Vehicle car2;void setup() { size(640, 360); // Call a function to generate new Path object newPath(); // Each vehicle has different maxspeed and maxforce for demo purposes car1 = new Vehicle(new PVector(0, height/2), 2, 0.04); car2 = new Vehicle(new PVector(0, height/2), 3, 0.1);}void draw() { background(255); // Display the path path.display(); // The boids follow the path car1.follow(path); car2.follow(path); // Call the generic run method (update, borders, display, etc.) car1.run(); car2.run(); car1.borders(path); car2.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();}Vehicle .pde
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 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 = 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(); } // 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); } }}4、运行效果
Vector> 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<
