Обнаружение столкновений между 2 линейно движущимися объектами в WGS84

[Edit5] Завершите повторное редактирование, если вам нужны старые источники, посмотрите историю изменений

Как отметил Нико Шертлер, проверка пересечения линии с линией - безумие, поскольку вероятность пересечения двух траекторий в одном и том же положении и времени почти равна нулю (даже с учетом перекрытий точности округления). Вместо этого вы должны найти место на каждой траектории, которое достаточно близко (чтобы столкнуться), и оба объекта находятся там относительно в одно и то же время. Другая проблема в том, что ваши траектории вообще не линейны. Да, они могут казаться линейными в течение короткого времени в кабине WGS84 и декартовыми, но с увеличением времени траектория изгибается вокруг Земли. Кроме того, ваши единицы входных значений для скорости делают это немного сложнее, поэтому позвольте мне резюмировать нормализованные значения, с которыми я буду иметь дело с этого момента:

1. Ввод

   состоит из двух предметов. Для каждого из них известно его фактическое положение (в WGS84 [rad]) и фактическая скорость [m/s], но не в декартовом пространстве, а в локальных осях WGS84. Например что-то вроде этого:

   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 };
   

   Осторожно, координаты мин указаны в Long,Lat,Alt порядке / условных обозначениях !!!

2. вывод

   Вам необходимо вычислить время, в течение которого два объекта "сталкиваются". Дополнительные ограничения к решению могут быть добавлены позже. Как упоминалось ранее, мы ищем не пересечение, а «ближайший» подход, который удовлетворяет условиям столкновения (например, расстояние меньше некоторого порога).

После некоторого обучения и тестирования я решил использовать итеративный подход в пространстве WGS84. Это вызывает некоторые проблемы, например, как преобразовать скорость из [m/s] в пространстве WGS84 в [rad/s] в пространстве WGS84. Это соотношение меняется с высотой и широтой объекта. На самом деле нам нужно вычислить изменение угла по осям long и lat, которые "точно" равны 1m пройденному расстоянию, а затем умножить на него скорости. Это можно аппроксимировать уравнением длины дуги:

l = dang.R

Где R - фактический радиус углового перемещения, ang - изменение угла, а l - пройденное расстояние, поэтому, когда l=1.0 тогда:

dang = 1.0/R

Если у нас есть декартово положение x,y,z (z - ось вращения Земли), то:

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

Теперь мы можем перебирать позиции с течением времени, что может быть использовано для приблизительного определения времени ближайшего сближения. Однако нам нужно ограничить максимальный временной шаг, чтобы не упустить большую часть кривизны Земли. Этот предел зависит от используемой скорости и точности цели. Итак, вот алгоритм поиска подхода:

1. инициализация

   установите начальный временной шаг на верхний предел, например dt=1000.0, и вычислите фактическое положение объектов кабины в декартовом пространстве. На основании этого вычислите их расстояние d1.

2. итерация

   установите d0=d1, затем вычислите фактические скорости в WGS84 для фактических позиций и добавьте speed*dt к каждому фактическому WGS84 положению объекта. Теперь просто вычислите фактические положения в декартовом пространстве и вычислите расстояние до них d1

   если d0>d1, значит, мы приближаемся к ближайшему подходу, поэтому снова перейдите к # 2.
   В случае d0==d1 траектории параллельны, поэтому верните время подхода t=0.0
   В случае d0<d1 мы уже пересекли ближайший подход, поэтому установите dt = -0.1*dt, а если dt>=desired_accuracy перейти к # 2, в противном случае остановитесь.

3. восстановить лучший t

   После итерации в # 2 мы должны восстановить лучшее время назад, поэтому верните t+10.0*dt;

Теперь у нас есть время наибольшего сближения t. Остерегайтесь: он может быть отрицательным (если предметы удаляются друг от друга). Теперь вы можете добавить свои ограничения, например

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

Вот источник C ++ для этого:

//---------------------------------------------------------------------------
#include <math.h>
//---------------------------------------------------------------------------
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<n;i++)
     for (e=0;;e=1)
        {
        d0=d1;
        // compute xyz distance
        WGS84toXYZ(q0,p0);
        WGS84toXYZ(q1,p1);
        vector_sub(q0,q0,q1);
        d1=vector_len(q0);
        // nearest approach crossed?
        if (e)
            {
            if (d0<d1){ dt*=-0.1; break; }                  // crossing trajectories
            if (fabs(d0-d1)<=1e-10) { i=n; t=0.0; break; }  // parallel trajectories
            }
        // apply time step
        t+=dt;
        WGS84_m2rad(da,db,p0);
        p0[0]+=vel0[0]*dt*da;
        p0[1]+=vel0[1]*dt*db;
        p0[2]+=vel0[2]*dt;
        WGS84_m2rad(da,db,p1);
        p1[0]+=vel1[0]*dt*da;
        p1[1]+=vel1[1]*dt*db;
        p1[2]+=vel1[2]*dt;
        }
    t+=10.0*dt; // recover original t
//  if ((d0<_max_d)&&(t>=0.0)&&(t<=_max_T)) return collision; else return no_collision;
    return t;
    }
//---------------------------------------------------------------------------

Вот обзор примера:

Красный - это объект0, а зеленый - это объект1. Белые квадраты представляют положение при вычисленном столкновении в момент времени t_coll [s] с расстоянием d_coll [m]. Желтые квадраты - это позиции в определенное пользователем время t_anim [s] с расстоянием d_anim [m], которое контролируется пользователем в целях отладки. Как видите, этот подход работает и в течение 36 часов ...

Надеюсь, я не забыл скопировать что-то (если да, прокомментируйте меня, и я добавлю)

Вы не показываете никакого кода, поэтому я просто изложу основные идеи, а код оставлю вам. Вернитесь, если вы попробуете какой-то код и застрянете, но покажите свои усилия и свой код до сих пор.

Есть несколько способов решить вашу проблему. Один из способов - задать параметрические уравнения для каждого объекта, задав ваши две функции во времени t. Установите одинаковые результаты этих функций и решите на время. Для трехмерных координат это дает вам три вопроса, по одному для каждой координаты, и очень маловероятно, что значения t будут одинаковыми для всех трех уравнений. Если они совпадают, это время вашего столкновения.

Другой способ, который допускает некоторые ошибки округления с плавающей запятой, - это изменить систему отсчета на систему координат одного из объектов. Вы вычитаете два вектора скорости, скажем v2-v1, и теперь у вас есть скорость второго объекта относительно первого объекта. Теперь найдите расстояние от теперь уже неподвижного первого объекта до линии движущегося второго объекта. Если вы не знаете, как это сделать, поищите «расстояние от точки до линии» в своей любимой поисковой системе. Затем вы видите, достаточно ли это расстояние, чтобы рассматривать его как столкновение - вы вряд ли получите идеальное столкновение, нулевое расстояние, учитывая ошибки округления с плавающей запятой. Если он достаточно мал, вы увидите, произойдет ли это столкновение в будущем или было достигнуто в прошлом. Вы можете найти проекцию точки на линии в качестве промежуточного значения для этого последнего вычисления.

Это ясно?