c++ - WGS84中2个 “linearly”运动对象之间的碰撞检测

标签 c++ math vector 3d wgs84

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

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

输入样例:

// 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

在这里，正确地将头寸转换为笛卡尔坐标(使用在线转换器链接的波纹管):
double pos0[3]={ 4013988.58505233,1285660.27718040,4779026.13957769 }; // [m]
double pos1[3]={ 4009069.35282446,1299263.86628867,4776529.76526759 }; // [m]

这些是使用我从下面的链接的质量检查中进行的转换(差异可能是由于不同的椭球和/或浮点错误引起的):
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]

我的问题是:如何检测对象是否碰撞以及何时碰撞？

我真正需要的是如果在指定的时间内发生冲突，例如_min_t。

当心速度是在本地[km/h] vector 方向上的North,East,High/Up中!有关将此类速度转换为笛卡尔坐标的更多信息，请参见相关:
  • How to convert a spherical velocity coordinates into cartesian

    • 要验证/检查WGS84位置转换，您可以使用以下在线计算器:
  • online WGS84 to XYZ

    • 如果可能的话，我想避免使用网格，基元或类似的东西。

    这是安德烈(Andre)尝试解决原始帖子中留下的这个问题(基于我的回答，但缺少速度转换):
    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;
}

    这应该是有效的直角坐标转换位置和速度，但是由于x, y, z参数的顺序错误而转换错误。上面的数据按正确的lon, lat, alt和x, y, z顺序排列，这显然不是:
    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;

    更不用说速度仍然不在笛卡尔空间上...

    编辑:新测试案例
    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 }

    其他测试案例:
        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

    测试案例3冲突时间为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

    最新测试案例:
    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;

    最终测试用例:
    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

    最终代码:
    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]如果需要旧资源，请完成编辑，请参阅修订历史记录

    正如Nico Schertler指出的那样，检查线与线的相交是不明智的，因为在相同位置和时间相交2条轨迹的可能性几乎为零(即使包括舍入精度重叠在内)。相反，您应该在每个轨迹上找到足够近的位置(可发生碰撞)，并且两个对象都相对同时存在。另一个问题是您的轨迹根本不是线性的。是的，它们可以在WGS84和笛卡尔坐标系中出现线性时间，但是随着时间的增加，轨迹会绕地球弯曲。另外，您输入的速度值单位会使此操作变得更困难，因此让我概括一下我将从现在开始要处理的标准化值:

  • 输入

    由两个对象组成。已知每个位置的实际位置(在 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顺序/约定!!!
  • 输出

    您需要计算两个对象“碰撞”的时间，稍后可以在解决方案上添加其他约束。如前所述，我们不是在寻找交集，而是在寻找最接近的碰撞条件(例如距离小于某个阈值)来满足碰撞条件。

    • 经过一些教导和测试后，我决定在 WGS84 空间中使用迭代方法。这带来了一些问题，例如如何将 WGS84 空间中的[m/s]中的速度转换为 WGS84 空间中的[rad/s]。该比率随物体的高度和纬度而变化。实际上，我们需要计算“精确”等于long行进距离的lat和1m轴的 Angular 变化，然后将速度乘以它。这可以通过弧长方程来近似:
    l = dang.R

    其中R是角运动的实际半径，ang是 Angular 变化，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)

    现在我们可以用时间迭代位置，该时间可以用来近似最接近的进场时间。但是，我们需要限制最大时间步长，因此我们不会错过太多的地球曲率。此限制取决于使用的速度和目标精度。所以在这里找到算法的方法:
  • 初始化

    将初始时间步长设置为dt=1000.0之类的上限，并计算笛卡尔对象在笛卡尔空间中的实际位置。从中计算出它们的距离d1。
  • 迭代

    设置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 ，否则停止。
  • 恢复最佳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;
    }
//---------------------------------------------------------------------------

    这里是示例概述:

    overview

    红色是对象0，绿色是对象1。白色方块表示在时间t_coll [s]与距离d_coll [m]的计算碰撞中的位置。黄色方块是用户定义的时间t_anim [s]上的位置，其距离d_anim [m]由用户控制以进行调试。如您所见，这种方法也可以在36个小时内起作用...

    希望我不会忘记复制某些内容(如果是，请评论我，我会添加它)

    关于c++ - WGS84中2个 “linearly”运动对象之间的碰撞检测，我们在Stack Overflow上找到一个类似的问题： https://stackoverflow.com/questions/41101107/

    上一篇：c++ - 如何让 C++ 中的其他函数可以访问监听器？

    下一篇：c++ - 使用正则表达式拆分字符串，忽略大括号内出现的定界符

    相关文章：

    c++ - 在cuda内核中创建一个 vector

    c++ - 如何从顺序遍历中列出二叉搜索树中每个节点的子节点？

    javascript - 如何使用 if 语句从 select 标记中提取值？ (Javascript)

    c++ - <vector> 搞乱了我的快速排序

    c++ - 使用 OpenCL 的欧氏距离

    c++ - 我应该#include 在我的库的命名空间中吗？

    c++ - Box2D 在不移动圆的情况下改变圆所面对的角度

    c++ - 具有非均匀缩放的 3x3 矩阵旋转

    c++ - pop_back() 返回值？

    c++ - 指针实际上并不指向对象本身？

    ©2024 IT工具网  联系我们