// 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();
}