/**
* This file is part of miniCDx benchmark of oSCJ.
*
* miniCDx is free software: you can redistribute it and/or modify
* it under the terms of the GNU Lesser General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* miniCDx is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU Lesser General Public License for more details.
*
* You should have received a copy of the GNU Lesser General Public License
* along with miniCDx. If not, see <http://www.gnu.org/licenses/>.
*
*
* Copyright 2009, 2010
* @authors Daniel Tang, Ales Plsek, Kun Wei
*
* See: http://sss.cs.purdue.edu/projects/oscj/
*/
package hijac.cdx;
import hijac.cdx.Aircraft;
import hijac.cdx.Constants;
import hijac.cdx.Motion;
import hijac.cdx.Vector3d;
/**
* Objects of the <code>Motion</code> class provide all a-priori known
* information about what the Aircraft was doing between two frames. Also
* provides methods to do useful things with this object (so it's not just
* dead data).
*
* @author Filip Pizlo
*/
class Motion {
/**
* The aircraft that we are referring to.
*/
private final Aircraft aircraft;
/**
* The first position (from the last frame). Will be equal to
* <code>pos_two</code> if we do not have a record for this aircraft for
* any previous frames.
*/
private final Vector3d pos_one;
/**
* The second position (from the current frame).
*/
private final Vector3d pos_two;
public Motion() {
aircraft = new Aircraft();
pos_one = new Vector3d();
pos_two = new Vector3d();
}
/**
* Initialise with an aircraft and two positions.
*/
public Motion(Aircraft _aircraft, Vector3d _pos_one, Vector3d _pos_two) {
aircraft = _aircraft;
pos_one = _pos_one;
pos_two = _pos_two;
}
/**
* Initialise with an aircraft and one position.
*/
public Motion(Aircraft _aircraft, Vector3d _pos) {
this(_aircraft, _pos, _pos);
}
/**
* Retrieve the aircraft.
*/
public Aircraft getAircraft() {
return aircraft;
}
/**
* Retrieve the first position.
*/
public Vector3d getFirstPosition() {
return pos_one;
}
/**
* Retrieve the second position.
*/
public Vector3d getSecondPosition() {
return pos_two;
}
public String toString() {
return "Motion of " + getAircraft().toString() +
" from " + getFirstPosition().toString() +
" to " + getSecondPosition().toString();
}
public void copyfrom(
Aircraft _aircraft, Vector3d _pos_one, Vector3d _pos_two) {
for (int i = 0; i < aircraft.getCallsign().length; i++) {
aircraft.getCallsign()[i]= _aircraft.getCallsign()[i];
}
pos_one.x = _pos_one.x;
pos_one.y = _pos_one.y;
pos_one.z = _pos_one.z;
pos_two.x = _pos_two.x;
pos_two.y = _pos_two.y;
pos_two.z = _pos_two.z;
}
/**
* Find an intersection between this Motion and another Motion.
*
* @return a Vector3d object with the intersection point if an intersection
* was found, null otherwise.
*
* @author Jeff Hagelberg, Filip Pizlo
*/
public Vector3d findIntersection(Motion other) {
Vector3d i1 = new Vector3d(),
f1 = new Vector3d(),
i2 = new Vector3d(),
f2 = new Vector3d();
i1.set(getFirstPosition());
f1.set(getSecondPosition());
i2.set(other.getFirstPosition());
f2.set(other.getSecondPosition());
final float r = Constants.PROXIMITY_RADIUS;
final float vx1 = f1.x - i1.x;
final float vx2 = f2.x - i2.x;
final float vy1 = f1.y - i1.y;
final float vy2 = f2.y - i2.y;
final float vz1 = f1.z - i1.z;
final float vz2 = f2.z - i2.z;
/* This test is not a geometrical 3d intersection test, it takes the fact
* that the aircraft move into account; so it is more like a 4d test (it
* assumes that both of the aircraft have a constant speed over the tested
* interval). We thus have two points, each of them moving on its line
* segment at constant speed; we are looking for times when the distance
* between these two points is smaller than r.
*
* V1 is vector of aircraft 1
* V2 is vector of aircraft 2
*
* if a = 0 iff the planes are moving in parallel and have the same speed
* (can be zero - they may not be moving at all)
*
* a = (V2 - V1)^T * (V2 - V1) =< (V2 - V1), (V2 - V1) >=
* sqrt(|| V2 - V1 ||) */
final float a =
(vx2 - vx1) * (vx2 - vx1) +
(vy2 - vy1) * (vy2 - vy1) +
(vz2 - vz1) * (vz2 - vz1);
if (a != 0.0f) {
/* We are first looking for instances of time when the planes are exactly
* r from each other and at least one plane is moving; if the planes are
* moving in parallel and do not have constant speed, then if the faster
* starts behind the slower, we can have 2, 1, or 0 solutions; if the
* faster plane starts in front of the slower, we can have 0 or 1
* solutions.
*
* point P1 = I1 + vV1
* point P2 = I2 + vV2
* - looking for v such that dist(P1,P2) = || P1 - P2 || = r
*
* it follows that || P1 - P2 || = sqrt( < P1-P2, P1-P2 > )
* 0 = -r^2 + < P1 - P2, P1 - P2 >
* from properties of dot product
* 0 = -r^2 + <I1-I2,I1-I2> + v * 2<I1-I2, V1-V2> + v^2 *<V1-V2,V1-V2>
* so we calculate a, b, c - and solve the quadratic equation
* 0 = c + bv + av^2
*
* b = 2 * <I1-I2, V1-V2> */
final float b = 2.0f *
(i2.x * vx2 - i2.x * vx1 -
i1.x * vx2 + i1.x * vx1 +
i2.y * vy2 - i2.y * vy1 -
i1.y * vy2 + i1.y * vy1 +
i2.z * vz2 - i2.z * vz1 -
i1.z * vz2 + i1.z * vz1);
// c = -r^2 + (I2 - I1)^T * (I2 - I1)
final float c = -r * r +
(i2.x - i1.x) * (i2.x - i1.x) +
(i2.y - i1.y) * (i2.y - i1.y) +
(i2.z - i1.z) * (i2.z - i1.z);
final float discr = b * b - 4.0f * a * c;
if (discr < 0.0f) return null;
/* the left side */
final float v1 = (-b - (float) Math.sqrt(discr)) / (2.0f * a);
/* the right side */
final float v2 = (-b + (float) Math.sqrt(discr)) / (2.0f * a);
// FIXME: v1 <= v2 always holds, correct ? because v1 > v2 only if
// a < 0, which would mean <V1-V2,V1-V2> < 0, which is impossible.
if (v1 <= v2 &&
(v1 <= 1.0f && 1.0f <= v2 || v1 <= 0.0f && 0.0f <= v2 ||
0.0f <= v1 && v2 <= 1.0f)) {
// new: calculate the location of the collision; if it is
// outside of the bounds of the Simulation, don't do anything!
final float x1col = i1.x + vx1 * (v1 + v2) / 2.0f;
final float y1col = i1.y + vy1 * (v1 + v2) / 2.0f;
final float z1col = i1.z + vz1 * (v1 + v2) / 2.0f;
if (z1col > Constants.MIN_Z && z1col <= Constants.MAX_Z &&
x1col >= Constants.MIN_X && x1col <= Constants.MAX_X &&
y1col >= Constants.MIN_Y && y1col <= Constants.MAX_Y)
return new Vector3d(x1col, y1col, z1col);
}
}
else {
/* The planes have the same speeds and are moving in parallel (or they
* are not moving at all) they thus have the same distance all the time;
* we calculate it from the initial point
*
* dist = || i2 - i1 || = sqrt((i2 - i1)^T * (i2 - i1)) */
float dist =
(i2.x - i1.x) * (i2.x - i1.x) +
(i2.y - i1.y) * (i2.y - i1.y) +
(i2.z - i1.z) * (i2.z - i1.z);
dist = (float) Math.sqrt(dist);
//System.out.println("i1 = " + i1 + ", i2 = " + i2 + ", dist = " + dist);
if (dist <= r)
/*System.out.println(
"Planes were travelling in parallel. Collision.");*/
return getFirstPosition();
}
return null;
}
}