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);
}
}
/*
Perform Simulation
*/
public void simulate(float dt) {
// Accelerate angular speed
float requiredSpeed = targetAngularSpeed - angularSpeed;
float angularAccel = requiredSpeed / dt;
angularAccel = PApplet.constrain(angularAccel, -maxAngularAccel, maxAngularAccel);
// Limit Angular speed
angularSpeed += angularAccel * dt;
angularSpeed = PApplet.constrain(angularSpeed, -maxAngularSpeed, maxAngularSpeed);
// Orientation Simulation
orientation += angularSpeed * dt;
// Position simulation
PVector worldRequiredSpeed = targetSpeed.get();
worldRequiredSpeed.rotate(orientation);
worldRequiredSpeed.sub(speed);
// PVector worldRequiredSpeed = worldTargetSpeed.get();
float dSpeed = worldRequiredSpeed.mag();
float dAcell = dSpeed / dt;
float dForce = Math.min(dAcell * getMass(), motorForce);
worldRequiredSpeed.normalize();
worldRequiredSpeed.mult(dForce);
force.add(worldRequiredSpeed);
super.simulate(dt);
}
public void checkThirtyDegreeRule(
ArrayList<Cam> cameras, ArrayList<Character> characters, int selectedIdx) {
if (cameras == null || cameras.isEmpty()) {
println("No cameras in the scene!");
}
if (characters.size() != 2) {
println("Only two characters supported for now");
// TODO (sanjeet): Hack! Fix this once more characters are allowed
}
Cam selectedCamera = cameras.get(selectedIdx);
// TODO The characters.get results in a runtime error because there aren't currently any
// characters allocated in the input file.
Character ch1 = characters.get(0);
Character ch2 = characters.get(1);
// Obtaining (x,y,z) for characters and selected camera
PVector ch1Location = ch1.getTranslation();
PVector ch2Location = ch2.getTranslation();
PVector selectedCameraLocation = selectedCamera.getTranslation();
PVector cameraPoint = new PVector();
cameraPoint.add(selectedCameraLocation);
for (int i = 0; i < 100; i++) {
cameraPoint.add(selectedCamera.getZAxis());
}
PVector intersection =
getTwoLinesIntersection(
new PVector(ch1Location.x, ch1Location.z),
new PVector(ch2Location.x, ch2Location.z),
new PVector(selectedCameraLocation.x, selectedCameraLocation.z),
new PVector(cameraPoint.x, cameraPoint.z));
PVector diff = PVector.sub(selectedCameraLocation, intersection);
diff.normalize();
FloatBuffer fb = selectedCamera.modelViewMatrix;
float[] mat = fb.array();
float[] fbMatrix = new float[mat.length];
for (int i = 0; i < fbMatrix.length; i++) {
fbMatrix[i] = mat[i];
}
fbMatrix[0] = -diff.x;
fbMatrix[1] = diff.y;
fbMatrix[2] = -diff.z;
fbMatrix[9] = diff.x;
fbMatrix[10] = diff.y;
fbMatrix[11] = diff.z;
fbMatrix[13] = intersection.x;
fbMatrix[14] = intersection.y;
fbMatrix[15] = intersection.z;
PMatrix3D matrix = new PMatrix3D();
matrix.set(fbMatrix);
matrix.transpose();
pushMatrix();
applyMatrix(matrix);
rotateY(radians(30));
line(0, 0, 0, 0, 0, 1000);
rotateY(radians(-2 * 30));
line(0, 0, 0, 0, 0, 1000);
popMatrix();
for (int i = 0; i < cameras.size(); i++) {
if (i == selectedIdx) {
continue;
}
if (!cameras.get(i).isInView(ch1Location) && !cameras.get(i).isInView(ch2Location)) {
continue;
}
PVector currCamLocation = cameras.get(i).getTranslation();
PVector vect1 = PVector.sub(currCamLocation, intersection);
PVector vect2 = PVector.sub(selectedCameraLocation, intersection);
float dotP = vect1.dot(vect2) / (vect1.mag() * vect2.mag());
if (acos(dotP) <= PI / 6) {
cameras.get(i).setColor(255, 0, 0);
} else {
cameras.get(i).setColor(0, 0, 255);
}
}
}