0
$\begingroup$

I'm doing collision of two airplanes in WGS84 coordinates. I can find the collision detection using two bounding spheres, but I would like to set a min-time for the collision, if that time passes the min time, then there is a collision

here is how I'm doing the collision detection using spheres

math::VectorXYZd dist = math::InternalCoord::DifferenceInMeter(math::InternalCoord(m_sPosAV), math::InternalCoord(poi.Position()));
double lenght = dist.Length();
Sphere avSphere;
Sphere poiSphere;
avSphere.radius = 190; //190m
poiSphere.radius = 50; // 50m

if (doesItCollide(poiSphere, avSphere, lenght) && time_of_collision < min_time)
{
    //qDebug() << "collision sphere";
    //return false;
}

Consider the velocity of the two flights are in Cartesian coordinates.

EDIT WITH VALUES:

p = {x=-160796.41247833753 y=-17194.909085652325 z=141603.15824651718 }
v = {x=31.117163358698527 y=164.21864498875618 z=-14.090094769606420 }
speed = 167.73363403936480

dProjected = -58559.840314409346
rASquared = 42773350692.106567
rD = -nan(ind)
  • 0
    As an aside (probably not affecting the answer): aircraft don't fly straight-line paths in the WGS84 x-y-z coordinate system; they tend to fly paths determined either by altitude above mean sea level or by pressure altitude, which will tend to "curve" their paths. Of course when you're a few seconds away from a potential collision that amount of curvature doesn't matter much, but you didn't mention how much time was involved.2017-01-11
  • 0
    That's why I'm calculating a bounding sphere, so I'm not actually estimating an actual collision, I just want to know if the two bounding sphere intersects within a specific pre determined time2017-01-11

1 Answers 1

0

It's not clear, but I guess you are computing the path of some reference point (such as center of mass) of each aircraft, and you consider it to be a "possible collision" if the spheres centered at the two reference points touch.

This is equivalent to "possible collision" if the distance between the center points is less than $r_C = r_1+r_2$ where $r_1$ and $r_2$ are the radii of the spheres. So I will stop referring to the spheres and simply consider the distance between the two center points, and when it becomes less than or equal to $r_C$.

Assuming two objects each move at constant velocity (constant speed and direction), and assuming we have the position of each object at time $t=0$, let $r(t)$ be the distance between them at arbitrary time $t$. We want to know if there is $t$ such that $r(t)\leq r_C$, and if so, what is the minimum such value of $t$.

Take the difference of the two velocity vectors (first aircraft's velocity minus the second aircraft's velocity). This is the velocity of the first aircraft relative to the second. Call this vector $v$. Take the vector difference of the positions of the aircraft (first aircraft's position minus the second aircraft's position, that is, the vector from the second aircraft to the first) at time $t=0$. Call this vector $p$.

Project the vector $p$ onto the line of vector $v$. Call this new vector $p_\parallel$. The projection can be computed as follows: $$ p_\parallel = \frac{p\cdot v}{\lVert v\rVert^2} v. $$ The length of $p_\parallel$ in the direction of $v$ is $$ \frac{p\cdot v}{\lVert v\rVert}. $$ If this "length" is positive, $p_\parallel$ is in the same direction as $v$, rather than opposite, the aircraft are approaching each other. Let $$ t_A = \frac {\lVert p_\parallel \rVert}{\lVert v \rVert}. $$ Assuming you measured distances and velocities in consistent coordinates, (such as meters and meters per second), $t_A$ is the time to the closest approach of the two center points.

The distance at the time of closest approach is $r_A$, where $$ r_A^2 = \lVert p \rVert^2 - \lVert p_\parallel \rVert^2. $$

The distance at the first time of possible collision (the value of $t$ that we want to find) is $r_C$. Of course if $r_A > r_C$ there is no possible collision. But if the aircraft are not yet in possible collision, if they are approaching each other, and if $r_A \leq r_C$, the situation looks something like the figure below, where $A$ is the point of closest approach and $C$ is the point of first possible collision.

enter image description here

The smaller circles represent the spheres of radius $r_1$ and $r_2$, which are touching when their centers are at a distance $r_C$. The horizontal leg of the larger right triangle (from Aircraft $2$ to point $A$) represents the vector $p_\parallel$.

Let $$ r_D = \sqrt{r_C^2 - r_A^2}. $$ Then $r_D$ is how much the relative position changes between the first time of potential collision, $t_C$, and the closest approach, $t_A$. So $$ t_C = t_A - \frac{r_D}{\lVert v \rVert}. $$

Equivalently, you can compute $$ t_C = \frac{\lVert p_\parallel \rVert - r_D}{\lVert v \rVert}, $$ which allows you to skip the step where you compute $t_A$.

Note that this method works equally well in two dimensions or in three dimensions, as long as you do the vector operations correctly for the given number of dimensions.

I did not recognize your math library but here's a C++-based pseudocode of the function. The function returns two things: a Boolean which is true if the aircraft have a possible collision, and the first possible time of collision if there is one.

pair
findTimeToCollision(Vector p1, // 1st aircraft position
                    Vector v1, // 1st aircraft velocity
                    Vector p2, // 2nd aircraft position
                    Vector v2, // 2nd aircraft velocity
                    double rC) // distance for "collision"
{
  Vector p = p2.minus(p1);

  double rCSquared = rC * rC;
  double pSquared  = dotProduct(p, p);
  if (pSquared < rCSquared)
  {
    // The spheres intersect right now
    return pair(true, 0.0);
  }

  Vector v = v2.minus(v1);
  double vSquared   = dotProduct(v, v);
  double speed      = sqrt(vSquared);
  double dProjected = dotProduct(p, v) / speed ;
  if (dProjected <= 0)
  {
    // The aircraft are not approaching each other
    return pair(false, 0.0);
  }
  double rASquared = pSquared - dProjected * dProjected;
  if (rCSquared < rASquared)
  {
    // The aircraft never get close enough to collide
    return pair(false, 0.0);
  }

  // Return the time until possible collision
  double rD = sqrt(rCSquared - rASquared);
  double tC = (dProjected - rD) / length(v);
  return pair(true, tC);
}

This assumes some functions of vectors, mainly u.minus(v) gives the coordinate-by-coordinate vector difference $u - v$ and dotProduct(u, v) gives the dot product $u\cdot v$, equal to u.x * v.x + u.y * v.y + u.z * v.z if the vectors have three coordinates. I also included some optimizations using facts such as $\lVert v \rVert^2 = v\cdot v$ to save the expense of doing the same calculations multiple times. If you have distances in meters and velocities in meters per second then the function will return the number of seconds until "collision".

  • 0
    Thanks so much. Can you show me some pseudo code in whatever language you prefer ?2017-01-11
  • 0
    Please david I need a pseudo code. I see that you know C++/C. I'm using those languages.2017-01-11
  • 0
    is Rc the distance between the two flights ? in cartesian coordinates? but I want a collision of spheres togethers2017-01-11
  • 0
    I want to understand it graphically, would you draw a figure of the calculations ? If you don't mind. Thanks so much!!2017-01-11
  • 0
    I wish I can vote up more2017-01-11
  • 0
    I get tC is nan all the time2017-01-12
  • 0
    I have edited my question with the values, please take a look at them at tell me your opinion, I get Nan all the time2017-01-12
  • 0
    My first version of the pseudocode did the calculation only for the case where there is a possible collision. You tried it with numbers where the closest approach distance was more than the collision distance, and the answer should have been "no collision". I inserted some checks in the code that you might want to make to avoid NaN errors or negative "time to collision" values.2017-01-12
  • 0
    What's distance ? what's Rc ? is it the sum of the radius of the spherers ? or the distance between the flights ?2017-01-12
  • 0
    In the second paragraph: $r_C$ is the sum of the spheres' radii. From the fourth paragraph, the initial distance between the aircraft (at time $0$) is $\lVert p\rVert$.2017-01-12