From 4825e7b2805178e7c7682b2b0b5afa114d74f0a2 Mon Sep 17 00:00:00 2001 From: Reynaldo Morillo Date: Sun, 29 Apr 2018 20:46:31 -0400 Subject: [PATCH] Initial commit. This is essentially the code Yushuo, expect I moved the Path and Vehicle classes into their own files. --- path.pde | 43 ++++++ simulation.pde | 80 +++++++++++ vehicle.pde | 353 +++++++++++++++++++++++++++++++++++++++++++++++++ 3 files changed, 476 insertions(+) create mode 100644 path.pde create mode 100644 simulation.pde create mode 100644 vehicle.pde diff --git a/path.pde b/path.pde new file mode 100644 index 0000000..50cc65c --- /dev/null +++ b/path.pde @@ -0,0 +1,43 @@ +class Path { + + // A Path is an arraylist of points (PVector objects) + ArrayList 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 = 30; + points = new ArrayList(); + } + + // Add a point to the path + void addPoint(float x, float y) { + PVector point = new PVector(x, y); + points.add(point); + } + + // Draw the path + void display() { + strokeJoin(ROUND); + + // Draw thick line for radius + stroke(175); + strokeWeight(radius*2); + noFill(); + beginShape(); + for (PVector v : points) { + vertex(v.x, v.y); + } + endShape(CLOSE); + // Draw thin line for center of path + stroke(0); + strokeWeight(1); + noFill(); + beginShape(); + for (PVector v : points) { + vertex(v.x, v.y); + } + endShape(CLOSE); + } +} diff --git a/simulation.pde b/simulation.pde new file mode 100644 index 0000000..b783227 --- /dev/null +++ b/simulation.pde @@ -0,0 +1,80 @@ + +boolean debug = true; + +// A path object (series of connected points) +Path path; + +// Two vehicles +Vehicle car1; +Vehicle car2; + +void setup() +{ + size(1000, 1000); + // 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), 1, 0.04); + car2 = new Vehicle(new PVector(0, height/2), 1, 2); +} + +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); + //car1.borders(); + //car2.borders(); + // Instructions + fill(0); + +} + +// Here define the element of the track +/*void newPath() +{ + // A path is a series of connected points + // A more sophisticated path might be a curve, if you want to have a try. + path = new Path(); + path.addPoint(100, height/2); + path.addPoint(100,height/2+200); + path.addPoint(300,height/2+200); + path.addPoint(300,height/2); + // path.addPoint(200, height/2+5); +}*/ + + +void newPath() { + // A path is a series of connected points + // A more sophisticated path might be a curve + path = new Path(); + float offset = 30; + path.addPoint(offset,offset); + path.addPoint(width-offset,offset); + path.addPoint(width-offset,height-offset); + path.addPoint(width/2,height-offset*3); + path.addPoint(offset,height-offset); +} + + +public void keyPressed() +{ + if (key == ' ') { + debug = !debug; + } +} + +public void mousePressed() +{ + newPath(); +} diff --git a/vehicle.pde b/vehicle.pde new file mode 100644 index 0000000..77faec4 --- /dev/null +++ b/vehicle.pde @@ -0,0 +1,353 @@ +class Vehicle +{ + + // All the usual parameters; + PVector position; + PVector velocity; + PVector acceleration; + float r; + float maxforce; // Maximum steering force. Here I assume the weight of the car is 1. So it is actually the acceleration. + 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(); + } + + + //Here is the main part of the pure pursuit algorithm + int i=0; + int ii=0; + PVector a; + PVector b; + float angle; + PVector aa; + PVector bb; + PVector cc; + + // PVector a = p.points.get(0); + //PVector b = p.end; + void follow(Path p) + + { + if(i==0) + { + a = p.points.get(0); + b = p.points.get(1); + } + + //print(i); + if(i==p.points.size()-1) + { + + aa=a; + bb=b; + a = p.points.get(i); + b = p.points.get(0); + cc=b; + angle= PVector.angleBetween(PVector.sub( bb,aa) , PVector.sub(cc,bb) ); + angle=angle*180/PI; + print(angle); + print("\n"); + + } + else + { + aa=a; + bb=b; + a = p.points.get(i); + b = p.points.get(i+1); + cc=b; + angle= PVector.angleBetween(PVector.sub(bb,aa) , PVector.sub(cc,bb) ); + angle=angle*180/PI; + print(angle); + print("\n"); + if(ii!=0 ) + { + if(i<=p.points.size()-3) + { + aa=p.points.get(i); + bb=p.points.get(i+1); + cc=p.points.get(i+2); + angle= PVector.angleBetween(PVector.sub(bb,aa) , PVector.sub(cc,bb) ); + angle=angle*180/PI; + print(angle); + print("\n"); + } + //angle=200; + if (i==p.points.size()-2) + { + aa=p.points.get(i); + bb=p.points.get(i+1); + cc=p.points.get(i+2-p.points.size()); + angle= PVector.angleBetween(PVector.sub(bb,aa) , PVector.sub(cc,bb) ); + angle=angle*180/PI; + print(angle); + print("\n"); + } + + if (i==p.points.size()-1) + { + aa=p.points.get(i); + bb=p.points.get(i+1-p.points.size()); + cc=p.points.get(i+2-p.points.size()); + angle= PVector.angleBetween(PVector.sub(bb,aa) , PVector.sub(cc,bb) ); + angle=angle*180/PI; + print(angle); + print("\n"); + } + } + + + + } + //PVector a = p.points.get(i); + //PVector b = p.points.get(i+1); + // Predict position 50 (arbitrary choice) frames ahead + PVector predict = velocity.get(); + predict.normalize(); + predict.mult(50); + PVector predictpos = PVector.add(position, predict); + + + // Get the normal point to that line + PVector normalPoint = getNormalPoint(predictpos, a, b); + + // Find target point a little further ahead of normal + PVector dir = PVector.sub(b, a); + dir.normalize(); + dir.mult(1); // This could be based on velocity instead of just an arbitrary 10 pixels + PVector target = PVector.add(normalPoint, dir); + + // How far away are we from the path? + float distance = PVector.dist(predictpos, normalPoint); + + + + // The commented one is to show that, keep the car is in the track instead of staying in the middle of the line. + //if (distance > p.radius) + if (distance > 0) + { + seek(target); + //print(target); + //print(b); + // print('\n'); + // print(); + } + // Test if the target position of car is close to the end part of the line. + if(angle> 90) + { + //print("Fuck"); + if(abs(target.x-b.x)<1 && abs(target.y-b.y)<1) + //if(abs(position.x-b.x)<10 && abs(position.y-b.y)<10) + { + i++; + //float aa= PVector.angleBetween(PVector.sub( p.points.get(i),p.points.get(i-1)) , PVector.sub(p.points.get(i+1),p.points.get(i)) ); + //print(aa*180/PI); + print("/n"); + /*if(i==p.points.size()-1) + { + //i--; + i=0; + }*/ + //print(i); + /*if(abs(target.x-p.points.get(0).x)<5 && abs(target.y-p.points.get(0).y)<5) + { + i=0; + print(i); + }*/ + if(b==p.points.get(0)) + { + i=0; + } + + if(ii==0) + + { + if(abs(target.x - p.points.get(2).x )<5 && abs(target.y - p.points.get(2).y )<500) + { + position.x=position.x-100; + position.y=position.y-300; + i--; + ii++; + print("The push"); + print(i); + } + } + + } + } + + else + { + if(abs(position.x-b.x)<5 && abs(position.y-b.y)<5) + //if(abs(position.x-b.x)<10 && abs(position.y-b.y)<10) + { + i++; + //float aa= PVector.angleBetween(PVector.sub( p.points.get(i),p.points.get(i-1)) , PVector.sub(p.points.get(i+1),p.points.get(i)) ); + //print(aa*180/PI); + print("/n"); + /*if(i==p.points.size()-1) + { + //i--; + i=0; + }*/ + //print(i); + /*if(abs(target.x-p.points.get(0).x)<5 && abs(target.y-p.points.get(0).y)<5) + { + i=0; + print(i); + }*/ + + if(b==p.points.get(0)) + { + i=0; + } + if(ii==0) + + { + if(abs(target.x - p.points.get(2).x )<5 && abs(target.y - p.points.get(2).y )<500) + { + position.x=position.x-100; + position.y=position.y-300; + i--; + ii++; + print("The push"); + print(i); + } + } + + } + } + + + //Test if the car back to the original position. So that we need to set the we follow the track at the the beginning of the path + /*if(abs(position.x-p.points.get(0).x)<20 && abs(position.y-p.points.get(0).y)<20) + { + i=0; + print(i); + }*/ + + // Draw the debugging stuff + if (debug) + { + fill(0); + stroke(0); + line(position.x, position.y, predictpos.x, predictpos.y); + ellipse(predictpos.x, predictpos.y, 4, 4); + + // Draw normal position + fill(0); + stroke(0); + line(predictpos.x, predictpos.y, normalPoint.x, normalPoint.y); + ellipse(normalPoint.x, normalPoint.y, 4, 4); + stroke(0); + if (distance > p.radius) fill(255, 0, 0); + noStroke(); + ellipse(target.x+dir.x, target.y+dir.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, but I assume the weight is 1, so it is fine. + 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); + } + }*/ + /*void borders() { + if (position.x < -r) position.x = width+r; + //if (position.y < -r) position.y = height+r; + if (position.x > width+r) position.x = -r; + //if (position.y > height+r) position.y = -r; + }*/ + +}