Fatal编程技术网
  • cplusplus/
  • C++ 2”之间的碰撞检测；“线性”；WGS84中的移动对象

C++ 2”之间的碰撞检测；“线性”；WGS84中的移动对象

c++ math vector 3d

C++ 2”之间的碰撞检测；“线性”；WGS84中的移动对象,c++,math,vector,3d,wgs84,C++,Math,Vector,3d,Wgs84,[由Spektre根据评论完成重新编辑] 我在3D中有两个起点和速度向量（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.9

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

我在3D中有两个起点和速度向量（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
这里正确地将位置转换为笛卡尔坐标（使用在线转换器链接的波纹管）：

使用下面链接QA中的我的转换（差异可能由不同的椭球和/或浮点错误引起）：

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

我真正需要的是，如果碰撞发生在某个指定的时间内，如
\u min\t

当心速度在本地
北、东、高/上方向的
[km/h]
速度！有关将此类速度转换为笛卡尔坐标的更多信息，请参阅相关：


    

  • 


要验证/检查WGS84位置变换，可以使用以下在线计算器：


    

  • 


如果可能的话，我希望避免使用网格、基本体或类似的东西



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


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

编辑：新测试用例


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



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;


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


最后一个测试用例：


    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



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;


最终测试用例：


    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



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;


最终代码：


    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



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;



const double\u max\u d=2500；//最大距离m
m_时间=3600.0；
int i，e，n；
双t，dt；
双x，y，z，d0，d1=0；
双p0[3]，p1[3]，v0[3]，v1[3]；
双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]=速度指数y；
vel1[2]=速度poi.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_水平（p0，v0，pos0，vel0）；
WGS84toXYZ_posvel（p1，v1，pos1，vel1）；
dt=1；//[秒]初始时间步长（精度=dt/10^（n-1）
n=5；//精度循环

对于（t=0.0，i=0；i您没有显示任何代码，因此我将只给出主要思想，并将代码留给您。如果您尝试了一些代码但遇到了问题，请回来，但请显示您的努力和代码

有多种方法可以解决您的问题。一种方法是为每个对象设置参数方程，在时间
t
中给出两个函数。将这些函数的结果设置为相等，然后在时间上进行求解。对于给出三个问题的三维坐标，每个坐标一个，并且
t
的值不太可能>这三个方程都相同。如果它们相同，那就是碰撞的时间

另一种允许某些浮点舍入误差的方法是将参考系更改为其中一个对象的参考系。减去两个速度向量，比如说
v2-v1
，就得到了第二个对象相对于第一个对象的速度。现在求出从现在静止的第一个对象到li的距离移动的第二个对象的NE。如果你不知道怎么做，在你最喜欢的搜索引擎中查找“点到线的距离”。然后你看这个距离是否足够小，你可以认为它是一个碰撞——你不可能得到完美的碰撞，零距离，给定浮点舍入误差。如果它足够小，你就不能。然后，查看该碰撞是在将来发生的还是在过去发生的。您可能希望找到该点在直线上的投影作为最后一次计算的中间值


清楚吗？
[Edit5]如果需要旧资料，请完成重新编辑查看修订历史记录

正如Nico Schertler指出的，检查线与线的相交是疯狂的，因为在相同位置和时间相交两条轨迹的概率几乎为零（即使包括舍入精度重叠）。相反，你应该在每条轨迹上找到足够近的位置（以碰撞）这两个物体同时存在。另一个问题是，你的轨迹根本不是线性的。是的，在booth WGS84和笛卡尔坐标中，它们在短时间内是线性的，但随着时间的增加，轨迹围绕地球弯曲。另外，你的输入值单位速度使这变得有点困难，所以让我重述一下标准化我将从现在开始处理的d值：


  • 输入
    
由两个对象组成。对于每个对象，已知其实际位置（在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
    order/convention！！！
    • 

  • 输出
    
您需要计算两个对象“碰撞”的时间，以后可以添加额外的约束以解决问题。如前所述，我们不是在搜索交点，而是在满足碰撞条件（如距离小于某个阈值）的情况下搜索“最近”的方法
    • 
经过一些教学和测试，我决定在WGS84空间中使用迭代方法。这带来了一些问题，例如如何将
    [m/s]
    中WGS84空间中的速度转换为
    [rad/s]
    在WGS84空间中。该比率随物体高度和纬度而变化。实际上，我们需要计算angl