Sphere collision with time of collision in WGS84 Coordinates

Question

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)

Answer 1

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.

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<bool, double>
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<bool, double>(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<bool, double>(false, 0.0);
  }
  double rASquared = pSquared - dProjected * dProjected;
  if (rCSquared < rASquared)
  {
    // The aircraft never get close enough to collide
    return pair<bool, double>(false, 0.0);
  }

  // Return the time until possible collision
  double rD = sqrt(rCSquared - rASquared);
  double tC = (dProjected - rD) / length(v);
  return pair<bool, double>(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".