Collision detection between 2 “linearly” moving objects in WGS84

Question

[Complete re-edit by Spektre] based on comments

I have two start points and velocity vectors in 3D (WGS84) how would I check if they are colliding i

Answer 1

[Edit5] Complete reedit in case you need old sources see the revision history

As Nico Schertler pointed out checking for line to line intersection is insanity as the probability of intersecting 2 trajectories at same position and time is almost none (even when including round-off precision overlaps). Instead you should find place on each trajectory that is close enough (to collide) and both objects are there at relatively same time. Another problem is that your trajectories are not linear at all. Yes they can appear linear for shor times in booth WGS84 and Cartesian but with increasing time the trajectory bends around Earth. Also your input values units for speed are making this a bit harder so let me recapitulate normalized values I will be dealing with from now:

1. Input

   consists of two objects. For each is known its actual position (in WGS84 [rad]) and actual speeds [m/s] but not in Cartesian space but WGS84 local axises instead. For example something like this:

   const double kmh=1.0/3.6;
   const double deg=M_PI/180.0;
   const double rad=180.0/M_PI;
   //                      lon            lat      alt
   double pos0[3]={  23.000000*deg, 48.000000*deg,2500.000000 };
   double pos1[3]={  23.000000*deg, 35.000000*deg,2500.000000 };
   double vel0[3]={ 100.000000*kmh,-20.000000*kmh,   0.000000*kmh };
   double vel1[3]={ 100.000000*kmh, 20.000000*kmh,   0.000000*kmh };
   

   Beware mine coordinates are in Long,Lat,Alt order/convention !!!

2. output

   You need to compute the time in which the two objects "collide" Additional constrains to solution can be added latter on. As mentioned before we are not searching for intersection but "closest" approach instead that suffice collision conditions (like distance is smaller then some threshold).

After some taught and testing I decided to use iterative approach in WGS84 space. That brings up some problems like how to convert speed in [m/s] in WGS84 space to [rad/s] in WGS84 space. This ratio is changing with object altitude and latitude. In reality we need to compute angle change in long and lat axises that are "precisely" equal to 1m traveled distance and then multiply the velocities by it. This can be approximated by arc-length equation:

l = dang.R

Where R is actual radius of angular movement, ang is the angle change and l is traveled distance so when l=1.0 then:

dang = 1.0/R

If we have Cartesian position x,y,z (z is Earth rotation axis) then:

Rlon = sqrt (x*x + y*y)
Rlat = sqrt (x*x + y*y + z*z)

Now we can iterate positions with time which can be used to approximate closest approach time. We need to limit the max time step however so we do not miss to much of the Earth curvature. This limit is dependent on used speeds and target precision. So here the algorithm to find the approach:

1. init

   set initial time step to the upper limit like dt=1000.0 and compute actual positions of booth objects in Cartesian space. From that compute their distance d1.

2. iteration

   set d0=d1 then compute actual speeds in WGS84 for actual positions and add speed*dt to each objects actual WGS84 position. Now just compute actual positions in Cartesian space and compute their distance d1

   if d0>d1 then it menas we are closing to the closest approach so goto #2 again.
   In case d0==d1 the trajectories are parallel so return approach time t=0.0
   In case d0 we already crossed the closest approach so set dt = -0.1*dt and if dt>=desired_accuracy goto #2 otherwise stop.

3. recover best t

   After the iteration in #2 we should recover the best time back so return t+10.0*dt;

Now we have closest approach time t. Beware it can be negative (if the objects are going away from each other). Now you can add your constrains like

if (d0<_max_d)
 if ((t>=0.0)&&(t<=_max_T))
  return collision ...

Here C++ source for this:

//---------------------------------------------------------------------------
#include 
//---------------------------------------------------------------------------
const double kmh=1.0/3.6;
const double deg=M_PI/180.0;
const double rad=180.0/M_PI;
const double  _earth_a=6378137.00000;   // [m] WGS84 equator radius
const double  _earth_b=6356752.31414;   // [m] WGS84 epolar radius
const double  _earth_e=8.1819190842622e-2; //  WGS84 eccentricity
const double  _earth_ee=_earth_e*_earth_e;
//--------------------------------------------------------------------------
const double _max_d=2500.0;         // [m] collision gap
const double _max_T=3600000.0;      // [s] max collision time
const double _max_dt=1000.0;        // [s] max iteration time step (for preserving accuracy)
//--------------------------------------------------------------------------
//                      lon            lat      alt
double pos0[3]={  23.000000*deg, 48.000000*deg,2500.000000 }; // [rad,rad,m]
double pos1[3]={  23.000000*deg, 35.000000*deg,2500.000000 }; // [rad,rad,m]
double vel0[3]={ 100.000000*kmh,-20.000000*kmh,   0.000000*kmh }; // [m/s,m/s,m/s]
double vel1[3]={ 100.000000*kmh,+20.000000*kmh,   0.000000*kmh }; // [m/s,m/s,m/s]
//---------------------------------------------------------------------------
double divide(double x,double y)
        {
        if ((y>=-1e-30)&&(y<=+1e-30)) return 0.0;
        return x/y;
        }
void  vector_copy(double *c,double *a)         { for(int i=0;i<3;i++) c[i]=a[i];       }
double vector_len(double *a) { return sqrt((a[0]*a[0])+(a[1]*a[1])+(a[2]*a[2])); }
void  vector_len(double *c,double *a,double l)
        {
        l=divide(l,sqrt((a[0]*a[0])+(a[1]*a[1])+(a[2]*a[2])));
        c[0]=a[0]*l;
        c[1]=a[1]*l;
        c[2]=a[2]*l;
        }
void  vector_sub(double *c,double *a,double *b) { for(int i=0;i<3;i++) c[i]=a[i]-b[i]; }
//---------------------------------------------------------------------------
void WGS84toXYZ(double *xyz,double *abh)
    {
    double  a,b,h,l,c,s;
    a=abh[0];
    b=abh[1];
    h=abh[2];
    c=cos(b);
    s=sin(b);
    // WGS84 from eccentricity
    l=_earth_a/sqrt(1.0-(_earth_ee*s*s));
    xyz[0]=(l+h)*c*cos(a);
    xyz[1]=(l+h)*c*sin(a);
    xyz[2]=(((1.0-_earth_ee)*l)+h)*s;
    }
//---------------------------------------------------------------------------
void WGS84_m2rad(double &da,double &db,double *abh)
    {
    // WGS84 from eccentricity
    double p[3],rr;
    WGS84toXYZ(p,abh);
    rr=(p[0]*p[0])+(p[1]*p[1]);
    da=divide(1.0,sqrt(rr));
    rr+=p[2]*p[2];
    db=divide(1.0,sqrt(rr));
    }
//---------------------------------------------------------------------------
double collision(double *pos0,double *vel0,double *pos1,double *vel1)
    {
    int e,i,n;
    double p0[3],p1[3],q0[3],q1[3],da,db,dt,t,d0,d1,x,y,z;
    vector_copy(p0,pos0);
    vector_copy(p1,pos1);
    // find closest d1[m] approach in time t[sec]
    dt=_max_dt; // [sec] initial time step (accuracy = dt/10^(n-1)
    n=6;        // acuracy loops
    for (t=0.0,i=0;i=0.0)&&(t<=_max_T)) return collision; else return no_collision;
    return t;
    }
//---------------------------------------------------------------------------

Here an overview of example:

Red is object0 and Green is object1. The White squares represents position at computed collision at time t_coll [s] with distance d_coll [m]. Yellow squares are positions at user defined time t_anim [s] with distance d_anim [m] which is controlled by User for debugging purposes. As you can see this approach works also for times like 36 hours ...

Hope I did not forget to copy something (if yes comment me and I will add it)