// 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 draw(PApplet canvas, float scale) {
PVector orient = PVector.fromAngle(orientation);
orient.mult(scale * getRadius());
float x = (float) position.x * scale;
float y = (float) position.y * scale;
float diameter = getRadius() * 2 * scale;
canvas.fill(teamColor);
canvas.stroke(0);
canvas.ellipse(x, y, diameter, diameter);
canvas.line(x, y, x + (float) orient.x, y + (float) orient.y);
// Delegate Decoration to Robot
float heading = orient.heading();
float drawScale = 100f / scale * getRadius();
canvas.translate(x, y);
canvas.rotate(heading);
canvas.scale(drawScale);
// TODO: How to resolve scale, so that teams don't have to mind it also...
decorateRobot(canvas);
canvas.scale(1f / drawScale);
canvas.rotate(-heading);
canvas.translate(-x, -y);
}
// tällä funktiolla on huono nimi ja toteutuskin o ruma.
// tämä on se juttu mikä tekee työt endpoints()ille.
// jos tilepisteen reunalla toinen tile vain yhdessä suunnassa,
// niin piste on yksinään. tähän tarttis jonkun kuvan selostamaan.
PVector head(int x, int y, int z) {
PVector pp = new PVector(x, y, z);
PVector[] dirs = {
new PVector(-1, 0, 0),
new PVector(1, 0, 0),
new PVector(0, -1, 0),
new PVector(0, 1, 0),
new PVector(0, 0, -1),
new PVector(0, 0, 1),
};
int blkfound = -1;
for (int i = 0; i < dirs.length; i++) {
PVector p = dirs[i];
PVector next = PVector.add(pp, p);
if (next.x >= 0
&& next.x < size
&& next.y >= 0
&& next.y < size
&& next.z >= 0
&& next.z < size) {
if (map[at(next)] != 0) {
if (blkfound != -1) return null;
blkfound = i;
}
}
}
if (blkfound != -1) return PVector.sub(pp, dirs[blkfound]);
return null;
}
public void checkCollision(Paddle[] pads) {
// check against walls
if (y >= height || y < 0) {
speed.y *= -1;
}
if (x >= width) {
restart();
paddles[1].addPoint();
timer = 40;
}
if (x < 0) {
restart();
paddles[0].addPoint();
timer = 40;
}
// check against paddles
for (int i = 0; i < pads.length; i++) {
if (x > pads[i].x - (pads[i].w / 2)
&& x < pads[i].x + (pads[i].w / 2)
&& y > pads[i].y - (pads[i].h / 2)
&& y < pads[i].y + (pads[i].h / 2)) {
speed.x *= -1;
float friction = 0.5f;
println(pads[i].vel.y);
speed.y += friction * pads[i].vel.y;
}
}
}
public void update(float newY) {
y = newY;
vel.x = x - px;
vel.y = y - py;
px = x;
py = y;
}
// Change robot's front/back left/right speed
public void setSpeed(float x, float y) {
if (Float.isNaN(x) || Float.isNaN(y)) {
// System.out.println("Excep: setSpeed "+x+" "+y);
return;
}
targetSpeed.set(x, y);
targetSpeed.limit(maxSpeed);
}
// Method to update location
public void update() {
// Update velocity
vel.add(acc);
// Limit speed
vel.limit(maxspeed);
loc.add(vel);
// Reset accelertion to 0 each cycle
acc.mult(0);
}
public PVector getSmoothedVelocity() {
PVector total = new PVector(0, 0, 0);
for (int i = 0; i < buffer.length; i++) {
total.x += buffer[i].x;
total.y += buffer[i].y;
total.z += buffer[i].z;
}
PVector smthd =
new PVector(total.x / buffer.length, total.y / buffer.length, total.z / buffer.length);
return smthd;
}
public void calculateForces(ArrayList objects) {
for (int i = 0; i < objects.size(); i++) {
CelestialObject obj = (CelestialObject) objects.get(i);
ArrayList forces = obj.getForces();
float totalForceX = 0;
float totalForceY = 0;
for (int j = 0; j < forces.size(); j++) {
totalForceX += ((PVector) forces.get(j)).x;
totalForceY += ((PVector) forces.get(j)).y;
}
PVector newAccel = new PVector(totalForceX / obj.getMass(), totalForceY / obj.getMass());
obj.setAcceleration(newAccel);
}
for (int i = 0; i < objects.size(); i++) {
CelestialObject obj1 = (CelestialObject) objects.get(i);
float forceX = 0;
float forceY = 0;
obj1.clearForces();
if (obj1.getClass() == Star.class) {
println(obj1.getVelocity());
continue;
}
for (int j = 0; j < objects.size(); j++) {
CelestialObject obj2 = (CelestialObject) objects.get(j);
if (i == j) continue;
PVector pvDistance = PVector.sub(obj2.getPosition(), obj1.getPosition());
// println("distance: x:" + pvDistance.x + " y:" + pvDistance.y);
float distance = sqrt(sq(pvDistance.y) + sq(pvDistance.x));
float angle = degrees(atan2(pvDistance.y, pvDistance.x));
float force = (G * obj1.getMass() * obj2.getMass()) / sq(distance);
forceX = force * cos(radians(angle));
forceY = force * sin(radians(angle));
// println("FORCES on " + obj1.getName() + ":" + forceX + "," + forceY);
obj1.addForce(new PVector(forceX, forceY));
println();
}
}
}
// Function to update location
public void update() {
// As long as we aren't dragging the pendulum, let it swing!
if (!dragging) {
float G = 0.4f; // Arbitrary universal gravitational constant
theta_acc =
(-1 * G / r)
* sin(
theta); // Calculate acceleration (see:
// http://www.myphysicslab.com/pendulum1.html)
theta_vel += theta_acc; // Increment velocity
theta_vel *= damping; // Arbitrary damping
theta += theta_vel; // Increment theta
}
loc.set(r * sin(theta), r * cos(theta), 0); // Polar to cartesian conversion
loc.add(origin); // Make sure the location is relative to the pendulum's origin
}
public void draw() {
stroke(0, 0, 0);
change = round(random(1, 10));
if (change == 10) {
direction = round(random(0, 8));
point(pointX, pointY);
point(pointX + 1, pointY + 1);
point(pointX - 1, pointY - 1);
point(pointX - 1, pointY + 1);
point(pointX + 1, pointY - 1);
stroke(200, 200, 200);
point(pointX + 1, pointY);
point(pointX - 1, pointY);
point(pointX, pointY + 1);
point(pointX, pointY - 1);
stroke(0, 0, 0);
}
if (direction == 1) {
speed.set(0, -1, 0);
}
if (direction == 2) {
speed.set(1, 0, 0);
}
if (direction == 3) {
speed.set(0, 1, 0);
}
if (direction == 4) {
speed.set(-1, 0, 0);
}
if (direction == 5) {
speed.set(-1, -1, 0);
}
if (direction == 6) {
speed.set(1, 1, 0);
}
if (direction == 7) {
speed.set(-1, 1, 0);
}
if (direction == 8) {
speed.set(1, -1, 0);
}
if (pointX == width) {
pointX = 1;
}
if (pointX == 0) {
pointX = width - 1;
}
if (pointY == height) {
pointY = 1;
}
if (pointY == 0) {
pointY = height - 1;
}
pointX = pointX + speed.x;
pointY = pointY + speed.y;
point(pointX, pointY);
}
// Constructor initialize all values
Boid(PVector l, float ms, float mf) {
loc = l.get();
r = 4.0f;
maxspeed = ms;
maxforce = mf;
acc = new PVector(0, 0);
vel = new PVector(maxspeed, 0);
}
/*
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 updateAll(PVector newPos) {
// update current joint position
currentPos = newPos;
// map velocity to depth image size - I can't find how to get the depth of this context
velocity.x = map(newPos.x - prevPos.x, -context.depthWidth(), context.depthWidth(), -1, 1);
velocity.y = map(newPos.y - prevPos.y, -context.depthHeight(), context.depthHeight(), -1, 1);
velocity.z = newPos.z - prevPos.z;
// overwrite the circular buffer
buffer[bufferIndex] = velocity;
// and incrememnt the buffer index
bufferIndex++;
if (bufferIndex >= buffer.length) {
bufferIndex = 0;
}
prevPos = newPos;
smoothedVelocity = getSmoothedVelocity();
}
/**
* Given two points p1, p2 and a line passing through a,b check whether p1 and p2 are on the
* same side of the line. In our case a,b are characters and p1, p2 are cameras
*/
public boolean isInSameHalfPlane(PVector p1, PVector p2, PVector a, PVector b) {
PVector copyP1 = new PVector(p1.x, p1.y, p1.z);
PVector copyP2 = new PVector(p2.x, p2.y, p2.z);
PVector copyA = new PVector(a.x, a.y, a.z);
PVector copyB = new PVector(b.x, b.y, b.z);
copyB.sub(copyA);
copyP1.sub(copyA);
copyP2.sub(copyA);
PVector p1a = copyB.cross(copyP1);
PVector p2a = copyB.cross(copyP2);
if (p1a.dot(p2a) > 0) {
return true;
}
return false;
}
public void drag() {
// If we are draging the ball, we calculate the angle between the
// pendulum origin and mouse location
// we assign that angle to the pendulum
if (dragging) {
PVector diff =
PVector.sub(origin, new PVector(mouseX, mouseY)); // Difference between 2 points
theta = atan2(-1 * diff.y, diff.x) - radians(90); // Angle relative to vertical axis
}
}
public void wallCollisions() {
// x
if (x > (bounds.x / 2)) {
velocity.x *= -1;
// velocity.x *= damping;
}
if (x < -(bounds.x / 2)) {
velocity.x *= -1;
// velocity.x *= damping;
}
// y
if (g) {
velocity.y += gravity;
if (y > bounds.y / 2) {
velocity.y *= -0.95f;
y = bounds.y / 2;
}
} else {
if (y > (bounds.y / 2)) {
velocity.y *= -1;
velocity.y *= damping;
}
if (y < -(bounds.y / 2)) {
velocity.y *= -1;
velocity.y *= damping;
}
}
// z
if (z > (bounds.z / 2)) {
velocity.z *= -1;
// velocity.z *= damping;
}
if (z < -(bounds.z / 2)) {
velocity.z *= -1;
// velocity.z *= damping;
}
}
public void setState(PVector position, float orientation, boolean resetAll) {
this.position = position.get();
this.orientation = orientation;
if (resetAll) {
this.force = new PVector();
this.accel = new PVector();
this.speed = new PVector();
}
}
public boolean isInView(PVector vect) {
PVector z = getZAxis();
PVector v1 = PVector.sub(vect, getTranslation());
float dotP = PVector.dot(v1, z) / (z.mag() * v1.mag());
if ((acos(dotP) * 180 / PI) <= fov) {
return true;
}
return false;
}
// maailmassa on välttämättä myös pisteitä joissa ei ole haaroja, laitetaan niiden päihin
// kolikkoja
// akseli ulospäin siitä suorasta osasta
// paluuarvot paikka1, akseli1, ...
ArrayList<PVector> endpoints() {
ArrayList<PVector> pts = new ArrayList<PVector>();
for (int z = 0; z < size; z += space + 1) {
for (int y = 0; y < size; y += space + 1) {
for (int x = 0; x < size; x += space + 1) {
PVector pos = head(x, y, z);
if (pos != null) {
pts.add(pos);
pts.add(PVector.sub(pos, new PVector(x, y, z)));
}
}
}
}
return pts;
}
// This constructor could be improved to allow a greater variety of pendulums
Pendulum(PVector origin_, float r_) {
// Fill all variables
origin = origin_.get();
r = r_;
theta = 0.0f;
// calculate the location of the ball using polar to cartesian conversion
float x = r * sin(theta);
float y = r * cos(theta);
loc = new PVector(origin.x + x, origin.y + y);
theta_vel = 0.0f;
theta_acc = 0.0f;
damping = 0.995f; // Arbitrary damping
ballr = 16.0f; // Arbitrary ball radius
}
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();
}
public void mouseDragged() {
if (paused) {
ArrayList objects = timeline.getStatefulObjects();
for (int i = 0; i < objects.size(); i++) {
CelestialObject obj = (CelestialObject) objects.get(i);
if (obj.isMouseOver()) {
dragging = true;
PVector pos = obj.getPosition();
pos.x = mouseX;
pos.y = mouseY;
timeline.reset();
timeline.setCurrentState(objects);
sliderTimeline.setValue(0);
break;
}
}
}
}
public void setAcceleration(PVector acceleration) {
this.acceleration = acceleration;
velocity.add(acceleration);
position.add(velocity);
}
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);
}
}
}
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);
}
}
// Wraparound
public void borders() {
if (loc.x < -r) loc.x = width + r;
if (loc.y < -r) loc.y = height + r;
if (loc.x > width + r) loc.x = -r;
if (loc.y > height + r) loc.y = -r;
}
// deep copy
public PositionRotation clone() {
return new PositionRotation(position.get(), rotation.get());
}
// Change robot forward/backward speed only
public void setSpeed(float x) {
targetSpeed.set(x, 0);
targetSpeed.limit(maxSpeed);
}