// A function to rotate a vector public void rotateVector(PVector v, float theta) { float m = v.mag(); float a = v.heading2D(); a += theta; v.x = m * PApplet.cos(a); v.y = m * PApplet.sin(a); }
public void avoid(ArrayList obstacles) { // Make a vector that will be the position of the object // relative to the Boid rotated in the direction of boid's velocity PVector closestRotated = new PVector(sight + 1, sight + 1); float closestDistance = 99999; Obstacle avoid = null; // Let's look at each obstacle for (int i = 0; i < obstacles.size(); i++) { Obstacle o = (Obstacle) obstacles.get(i); float d = PVector.dist(loc, o.loc); PVector dir = vel.get(); dir.normalize(); PVector diff = PVector.sub(o.loc, loc); // Now we use the dot product to rotate the vector that points from boid to obstacle // Velocity is the new x-axis PVector rotated = new PVector(diff.dot(dir), diff.dot(getNormal(dir))); // Is the obstacle in our path? if (PApplet.abs(rotated.y) < (o.radius + r)) { // Is it the closest obstacle? if ((rotated.x > 0) && (rotated.x < closestRotated.x)) { closestRotated = rotated; avoid = o; } } } // Can we actually see the closest one? if (PApplet.abs(closestRotated.x) < sight) { // The desired vector should point away from the obstacle // The closer to the obstacle, the more it should steer PVector desired = new PVector(closestRotated.x, -closestRotated.y * sight / closestRotated.x); desired.normalize(); desired.mult(closestDistance); desired.limit(maxspeed); // Rotate back to the regular coordinate system rotateVector(desired, vel.heading2D()); // Draw some debugging stuff if (debug) { stroke(0); line(loc.x, loc.y, loc.x + desired.x * 10, loc.y + desired.y * 10); avoid.highlight(true); } // Apply Reynolds steering rules desired.sub(vel); desired.limit(maxforce); acc.add(desired); } }
public void render() { // Draw a triangle rotated in the direction of velocity float theta = vel.heading2D() + PApplet.radians(90); fill(100); stroke(0); pushMatrix(); translate(loc.x, loc.y); rotate(theta); beginShape(PConstants.TRIANGLES); vertex(0, -r * 2); vertex(-r, r * 2); vertex(r, r * 2); endShape(); if (debug) { stroke(50); line(0, 0, 0, -sight); } popMatrix(); }