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;
}
/**
* Sets the Quaternion from the three rotated vectors of an orthogonal basis.
*
* <p>The three vectors do not have to be normalized but must be orthogonal and direct (i,e.,
* {@code X^Y=k*Z, with k>0}).
*
* @param X the first PVector
* @param Y the second PVector
* @param Z the third PVector
* @see #fromRotationMatrix(float[][])
* @see #Quaternion(PVector, PVector)
*/
public final void fromRotatedBasis(PVector X, PVector Y, PVector Z) {
float m[][] = new float[3][3];
float normX = X.mag();
float normY = Y.mag();
float normZ = Z.mag();
for (int i = 0; i < 3; ++i) {
m[i][0] = (X.array())[i] / normX;
m[i][1] = (Y.array())[i] / normY;
m[i][2] = (Z.array())[i] / normZ;
}
fromRotationMatrix(m);
}
// 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);
}
/*
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 addVertex(float x, float y, float z, float u, float v) {
PVector vert = new PVector(x, y, z);
PVector texCoord = new PVector(u, v);
PVector vertNorm = PVector.div(vert, vert.mag());
vertices.add(vert);
texCoords.add(texCoord);
normals.add(vertNorm);
}
/**
* Returns the normalized axis direction of the rotation represented by the Quaternion.
*
* <p>The result is null for an identity Quaternion.
*
* @see #angle()
*/
public final PVector axis() {
PVector res = new PVector(this.x, this.y, this.z);
float sinus = res.mag();
if (sinus > 1E-8f) res.div(sinus);
if (PApplet.acos(this.w) <= HALF_PI) return res;
else {
res.x = -res.x;
res.y = -res.y;
res.z = -res.z;
return res;
}
}
/**
* Calculate a force to push particle away from repeller
*
* @param ptcl the Particle to push
* @return PVector direction
*/
public PVector pushParticle(Particle ptcl) {
PVector dir = PVector.sub(getLoc(), ptcl.getLoc()); // Calculate direction of
// force
float d = dir.mag(); // Distance between objects
dir.normalize(); // Normalize vector (distance doesn't matter here, we
// just want this vector for direction)
d = PApplet.constrain(d, 5, 100); // Keep distance within a reasonable
// range
float force = -1 * G / (d * d); // Repelling force is inversely
// proportional to distance
dir.mult(force); // Get force vector --> magnitude * direction
return dir;
}
private void calcCam(boolean is3D) {
if (rotVector.mag() != 0 || panVector.mag() != 0 || camDist != prevCamDist) {
prevCamDist = camDist;
camRot += rotVector.x;
if ((camPitch + rotVector.y) < 0.99 && (camPitch + rotVector.y) > 0.01)
camPitch += rotVector.y;
if (is3D) {
PVector camPan =
new PVector(
(panVector.x * (float) Math.sin(Math.PI * 0.5 + (Math.PI * camRot)))
+ (panVector.y * (float) Math.sin(Math.PI * 1.0 + (Math.PI * camRot))),
(panVector.y * (float) Math.cos(Math.PI * 1.0 + (Math.PI * camRot)))
+ (panVector.x * (float) Math.cos(Math.PI * 0.5 + (Math.PI * camRot))));
camPan.mult(0.05f * PVector.dist(camPos, camCenter));
camCenter.x += camPan.x;
camCenter.y += camPan.y;
camPan.x = 0;
camPan.y = 0;
} else {
camCenter.x -= 0.05f * (camPos.z - camCenter.z) * panVector.x;
camCenter.y += 0.05f * (camPos.z - camCenter.z) * panVector.y;
}
panVector.x = 0;
panVector.y = 0;
rotVector.x = 0;
rotVector.y = 0;
camPos.x =
camCenter.x
- camDist * (float) Math.sin(Math.PI * camRot) * (float) Math.sin(Math.PI * camPitch);
camPos.y =
camCenter.y
- camDist * (float) Math.cos(Math.PI * camRot) * (float) Math.sin(Math.PI * camPitch);
camPos.z = camCenter.z - camDist * (float) Math.cos(Math.PI * camPitch);
}
}
/**
* Sets the Quaternion as a rotation of {@link #axis() axis} and {@link #angle() angle} (in
* radians).
*
* <p>The {@code axis} does not need to be normalized. A null {@code axis} will result in an
* identity Quaternion.
*
* @param axis the PVector representing the axis
* @param angle the angle in radians
*/
public void fromAxisAngle(PVector axis, float angle) {
float norm = axis.mag();
if (norm < 1E-8f) {
// Null rotation
this.x = 0.0f;
this.y = 0.0f;
this.z = 0.0f;
this.w = 1.0f;
} else {
float sin_half_angle = PApplet.sin(angle / 2.0f);
this.x = sin_half_angle * axis.x / norm;
this.y = sin_half_angle * axis.y / norm;
this.z = sin_half_angle * axis.z / norm;
this.w = PApplet.cos(angle / 2.0f);
}
}
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);
}
}
}
