2“线性"之间的碰撞检测WGS84 中的移动物体 [英] Collision detection between 2 "linearly" moving objects in WGS84

问题描述

[由 Spektre 完成重新编辑] 根据评论

我在 3D (WGS84) 中有两个起点和速度向量，我如何检查它们是否在指定时间内在 3D 中发生碰撞?

样本输入:

//WGS84 对象位置const double deg=M_PI/180.0;双 pos0[3]={17.76 *deg,48.780 *deg,6054.0};//lon[rad],lat[rad],alt[m]双 pos1[3]={17.956532816382374*deg,48.768667387202690*deg,3840.0};//lon[rad],lat[rad],alt[m]//WGS84 速度 [km/h] 不是 [deg/sec]！！！双vel0[3]={- 29.346910862289782, 44.526061886823861,0.0};//[km/h] lon,lat,alt双vel1[3]={- 44.7 ,-188.0 ,0.0};//[km/h] lon,lat,alt

这里正确地将位置转换为笛卡尔(使用下面链接的在线转换器):

double pos0[3]={ 4013988.58505233,1285660.27718040,4779026.13957769 };//[米]双 pos1[3]={ 4009069.35282446,1299263.86628867,4776529.76526759 };//[米]

这些使用我从下面链接的 QA 转换而来(差异可能是由不同的椭球和/或浮点错误引起的):

double pos0[3] = { 3998801.90188399, 1280796.05923908, 4793000.78262020 };//[米]双 pos1[3] = { 3993901.28864493, 1294348.18237911, 4790508.28581325 };//[米]双vel0[3] = { 11.6185787807449, 41.1080659685389, 0 };//[公里/小时]双vel1[3] = { 17.8265828114202,-173.3281435179590, 0 };//[公里/小时]

我的问题是:如何检测物体是否会碰撞以及何时碰撞?

我真正需要的是是否在指定的时间内发生碰撞，例如 _min_t.

请注意，速度在 [km/h] 中位于本地 North,East,High/Up 向量方向！有关将此类速度转换为笛卡尔坐标的更多信息，请参阅相关内容:

• 红色是对象 0，绿色是对象 1.白色方块表示在时间 t_coll [s] 与距离 d_coll [m] 计算碰撞时的位置.黄色方块是用户定义时间 t_anim [s] 与距离 d_anim [m] 的位置，由用户出于调试目的控制.正如您所看到的，这种方法也适用于 36 小时之类的时间......

  希望我没有忘记复制一些东西(如果是的话评论我我会添加它)

  [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 in 3D within some specified time?

  Sample input:

  // WGS84 objects positions
  const double deg=M_PI/180.0;
  double pos0[3]={17.76             *deg,48.780            *deg,6054.0}; // lon[rad],lat[rad],alt[m]
  double pos1[3]={17.956532816382374*deg,48.768667387202690*deg,3840.0}; // lon[rad],lat[rad],alt[m]
  // WGS84 speeds in [km/h] not in [deg/sec]!!!
  double vel0[3]={- 29.346910862289782,  44.526061886823861,0.0}; // [km/h] lon,lat,alt
  double vel1[3]={- 44.7              ,-188.0              ,0.0}; // [km/h] lon,lat,alt
  

  And here correctly transformed positions into Cartesian (using online convertor linked bellow):

  double pos0[3]={ 4013988.58505233,1285660.27718040,4779026.13957769 }; // [m]
  double pos1[3]={ 4009069.35282446,1299263.86628867,4776529.76526759 }; // [m]
  

  And these using my conversion from the linked QA below (difference may be caused by different ellipsoid and or floating point errors):

  double pos0[3] = { 3998801.90188399, 1280796.05923908, 4793000.78262020 }; // [m]
  double pos1[3] = { 3993901.28864493, 1294348.18237911, 4790508.28581325 }; // [m]
  double vel0[3] = { 11.6185787807449,  41.1080659685389, 0 }; // [km/h]
  double vel1[3] = { 17.8265828114202,-173.3281435179590, 0 }; // [km/h]
  

  My question is: How to detect if the objects will collide and when?

  What I really need is if collision occurs within some specified time like _min_t.

  Beware the speeds are in [km/h] in direction of local North,East,High/Up vectors! For more info about converting such speeds into Cartesian coordinates see related:

  • How to convert a spherical velocity coordinates into cartesian

  To validate/check WGS84 position transformations you can use the following online calculator:

  • online WGS84 to XYZ

  I would like to avoid using mesh, primitives or similar stuff if possible.

  ---

  This is Andre's attempt to solve this (based on my answer but missing the speed transformation) left from the original post:

  bool collisionDetection()
  {
      const double _min_t = 10.0; // min_time
      const double _max_d = 5000; // max_distance
      const double _max_t = 0.001; // max_time
      double dt;
      double x, y, z, d0, d1;
  
      VectorXYZd posObj1 = WGS84::ToCartesian(m_sPos1);
      VectorXYZd posObj2 = WGS84::ToCartesian(m_sPos2);
  
      const QList<QVariant> velocity;    
      if (velocity.size() == 3)
      {
          dt = _max_t;
          x = posObj1 .x - posObj2 .x;
          y = posObj1 .y - posObj2 .y;
          z = posObj1 .z - posObj2 .z;
          d0 = sqrt((x*x) + (y*y) + (z*z));
          x = posObj1 .x - posObj2 .x + (m_sVelAV.x - velocity.at(0).toDouble())*dt;
          y = posObj1 .y - posObj2 .y + (m_sVelAV.y - velocity.at(1).toDouble())*dt;
          z = posObj1 .z - posObj2 .z + (m_sVelAV.z - velocity.at(2).toDouble())*dt;
          d1 = sqrt((x*x) + (y*y) + (z*z));
          double t = (_max_d - d0)*dt / (d1 - d0);
  
          if (d0 <= _max_d)
          {
              return true;
          }
  
          if (d0 <= d1)
          {
              return false;
          }
  
          if (t < _min_t)
          {
            return true;
          }
      }
      return false;
  }
  

  And this is supposed to be valid Cartesian transformed positions and speeds but transformed wrongly due to wrong order of x, y, z parameters. The data above is in correct lon, lat, alt and x, y, z order this is obviously not:

  posObject2 = {x=1296200.8297778680 y=4769355.5802477235 z=4022514.8921807557 }
  posObject1 = {x=1301865.2949957885 y=4779902.8263504291 z=4015541.3863254949 }
  velocity object 2: x = -178, y = -50, z = 8
  velocity object 1: x = 0, y = -88, z = 0; 
  

  Not to mention velocities are still not on Cartesian space ...

  EDIT: NEW TEST CASE

  m_sPosAV = {North=48.970020901863471 East=18.038928517158574 Altitude=550.00000000000000 }
  
  m_position = {North=48.996515594886858 East=17.989637729707006 Altitude=550.00000000000000 }
  
  d0 = 4654.6937995573062
  d1 = 4648.3896597230259
  t = 65.904213878080199
  dt = 0.1
  velocityPoi = {x=104.92401431817457 y=167.91352303897233 z=0.00000000000000000 }
  m_sVelAV = {x=0.00000000000000000 y=0.00000000000000000 z=0.00000000000000000 }
  

  ANOTHER TEST CASE:

      m_sPosAV = {North=49.008020930461598 East=17.920928503349856 Altitude=550.00000000000000 }
      m_position = {North=49.017421151053824 East=17.989399013104570 Altitude=550.00000000000000 }
      d0 = 144495.56021027692
      d1 = 144475.91709961568
      velocityPoi = {x=104.92401431817457 y=167.91352303897233 z=0.00000000000000000 }
      m_sVelAV = {x=0.89000000000000001 y=0.00000000000000000 z=0.
  
  00000000000000000 }
  
      t = 733.05884538126884
  

  TEST CASE 3 COLLISION TIME IS 0

  m_sPosAV = {North=48.745020278145105 East=17.951529239281793 Altitude=4000.0000000000000 }
  m_position = {North=48.734919749542570 East=17.943535418223373 Altitude=4000.0000000000000 }
  
  v1 = {61.452929549676597, -58.567847120366054, 8.8118360639107198}
  v0 = {0.00000000000000000, 0.00000000000000000, 0.00000000000000000}
  
  pos0 = {0.85076109780503417, 0.31331329099350030, 4000.0000000000000}
  pos1 = {0.85058481032472799, 0.31317377249621559, 3993.0000000000000}
  d1 = 2262.4742373961790
  

  LAST TEST CASE:

  p0 = 0x001dc7c4 {3933272.5980855357, 4681348.9804422557, 1864104.1897091190}
  p1 = 0x001dc7a4 {3927012.3039519843, 4673002.8791717924, 1856993.0651808924}
  dt = 100;
  n = 6;
  v1 = 0x001dc764 {18.446446996578750, 214.19570794229870, -9.9777430316824578}
  v0 = 0x001dc784 {0.00000000000000000, 0.00000000000000000, 0.00000000000000000}
  const double _max_d = 2500; 
  double _max_T = 120;
  

  Final Test Case:

  m_sPosAV = {North=49.958099932390311 East=16.958899924978102 Altitude=9000.0000000000000 }
  m_position = {North=49.956106045262935 East=16.928683918401916 Altitude=9000.0000000000000 }
  
  p0 = 0x0038c434 {3931578.2438977188, 4678519.9203961492, 1851108.3449359399}
  p1 = 0x0038c414 {3933132.4705292359, 4679955.4705412844, 1850478.2954359739}
  vel0 = 0x0038c3b4 {0.00000000000000000, 0.00000000000000000, 0.00000000000000000}
  vel1 = 0x0038c354 {-55.900000000000006, 185.69999999999999, -8.0000000000000000}
  dt = 1;   // [sec] initial time step (accuracy = dt/10^(n-1)
  n = 5;        // accuracy loops
  

  FINAL CODE:

  const double _max_d = 2500; // max_distance m
      m_Time = 3600.0;
      int i, e, n;
      double t, dt;
      double x, y, z, d0, d1 = 0;
      double p0[3], p1[3], v0[3], v1[3];
      double vel0[3], pos0[3], pos1[3], vel1[3];
  
      vel0[0] = m_sVelAV.x;
      vel0[1] = m_sVelAV.y;
      vel0[2] = m_sVelAV.z;
  
      vel1[0] = velocityPoi.x;
      vel1[1] = velocityPoi.y;
      vel1[2] = velocityPoi.z;
  
  
      pos0[0] = (m_sPosAV.GetLatitude()*pi) / 180;
      pos0[1] = (m_sPosAV.GetLongitude()*pi) / 180;
      pos0[2] = m_sPosAV.GetAltitude();
  
      pos1[0] = (poi.Position().GetLatitude()*pi) / 180;
      pos1[1] = (poi.Position().GetLongitude()*pi) / 180;
      pos1[2] = poi.Position().GetAltitude();
  
  
      WGS84toXYZ_posvel(p0, v0, pos0, vel0);
      WGS84toXYZ_posvel(p1, v1, pos1, vel1);
      dt = 1;   // [sec] initial time step (accuracy = dt/10^(n-1)
      n = 5;        // accuracy loops
      for (t = 0.0, i = 0; i<n; i++)
          for (e = 1; t <= m_Time; t += dt)
          {
              d0 = d1;
              // d1 = relative distance in time t
              x = p0[0] - p1[0] + (v0[0] - v1[0])*t;
              y = p0[1] - p1[1] + (v0[1] - v1[1])*t;
              z = p0[2] - p1[2] + (v0[2] - v1[2])*t;
              d1 = sqrt((x*x) + (y*y) + (z*z));
              if (e) { e = 0; continue; }
              // if bigger then last step stop (and search with 10x smaller time step)
              if (d0<d1) { d1 = d0; t -= dt + dt; dt *= 0.1; if (t<0.0) t = 0.0; break; }
          }
      // handle big distance as no collision
      if (d1 > _max_d) return false;
      if (t >= m_Time) return false;
  
      qDebug() << "Collision at time t= " << t;
  

  解决方案

  [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<d1 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 <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;
      }
  //---------------------------------------------------------------------------
  

  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)

  这篇关于2“线性"之间的碰撞检测WGS84 中的移动物体的文章就介绍到这了，希望我们推荐的答案对大家有所帮助，也希望大家多多支持IT屋！