Collision detection between two objects

问题:

The collision is not working According to that post Collision detection between 2 "linearly" moving objects in WGS84, I have the following data

EDIT: I have updated the data for a collision that should occur in 10 seconds.

m_sPosAV = {North=48.276111971715515 East=17.921031349301817 Altitude=6000.0000000000000 }
Poi_Position = {North=48.806113707277042 East=17.977161602106488 Altitude=5656.0000000000000 }  

velocity.x =  -189.80000000000001   // m/s
velocity.y =  -39.800000000000004  // m/s
velocity.z =  9   // m/s

m_sVelAV = {x=1.0000000000000000 y=1.0000000000000000 z=0.00000000000000000 } // m/s 



void WGS84toXYZ(double &x, double &y, double &z, double lon, double lat, double alt)
{
    const double  _earth_a = 6378141.4;       // [m] equator radius
    const double  _earth_b = 6356755.0;       // [m] polar radius
    double  a, b, h, l, c, s;
    a = lon;
    b = lat;
    h = alt;
    c = cos(b);
    s = sin(b);
    h = h + sqrt((_earth_a*_earth_a*c*c) + (_earth_b*_earth_b*s*s));
    z = h*s;
    l = h*c;
    x = l*cos(a);
    y = l*sin(a);
}


bool CPoiFilterCollision::collisionDetection(const CPoiItem& poi)
{
    const double _min_t = 10; // min_time
    const double _max_d = 500; // max_distance
    const double _max_t = 0.001; // max_time
    double dt;
    double d0, d1;
    double xAv, yAv, zAv;
    double xPoi, yPoi, zPoi;
    double x, y, z;
    double Ux, Uy, Uz;    // [m]
    double Vx, Vy, Vz;    // [m]
    double Wx, Wy, Wz;    // [m]
    double da = 1.567e-7; // [rad] angular step ~ 1.0 m in lon direction
    double dl = 1.0;
    const double deg = pi / 180.0;


     // [m] altitide step 1.0 m
    WGS84toXYZ(xAv, yAv, zAv, m_sPosAV.GetLongitude(), m_sPosAV.GetLatitude(), m_sPosAV.GetAltitude()); // actual position
    WGS84toXYZ(xPoi, yPoi, zPoi, poi.Position().GetLongitude(), poi.Position().GetLatitude(), poi.Position().GetAltitude()); // actual position

    WGS84toXYZ(Ux, Uy, Uz, m_sPosAV.GetLongitude() + da, m_sPosAV.GetLatitude(), m_sPosAV.GetAltitude()); // lon direction Nort
    WGS84toXYZ(Vx, Vy, Vz, m_sPosAV.GetLongitude(), m_sPosAV.GetLatitude() + da, m_sPosAV.GetAltitude()); // lat direction East
    WGS84toXYZ(Wx, Wy, Wz, m_sPosAV.GetLongitude(), m_sPosAV.GetLatitude(), m_sPosAV.GetAltitude() + dl); // alt direction High/Up
    Ux -= xAv; Uy -= yAv; Uz -= zAv;
    Vx -= xAv; Vy -= yAv; Vz -= zAv;
    Wx -= xAv; Wy -= yAv; Wz -= zAv;
    normalize(Ux, Uy, Uz);
    normalize(Vx, Vy, Vz);
    normalize(Wx, Wy, Wz);

    double vx = m_sVelAV.x*Ux + m_sVelAV.y*Vx + m_sVelAV.z*Wx;
    double vy = m_sVelAV.x*Uy + m_sVelAV.y*Vy + m_sVelAV.z*Wy;
    double vz = m_sVelAV.x*Uz + m_sVelAV.y*Vz + m_sVelAV.z*Wz;



    const QList<QVariant> velocity = poi.Property(QLatin1String("VELOCITY")).toList();

    if (velocity.size() == 3)
    {
        dt = _max_t;
        x = xAv - xPoi;
        y = yAv - yPoi;
        z = zAv - zPoi;
        d0 = sqrt((x*x) + (y*y) + (z*z));
        x = xAv - xPoi + (vx - velocity.at(0).toDouble())*dt;
        y = yAv - yPoi + (vy - velocity.at(1).toDouble())*dt;
        z = zAv - zPoi + (vz - velocity.at(2).toDouble())*dt;
        d1 = sqrt((x*x) + (y*y) + (z*z));

        if (d0 <= _max_d)
        {
            return true;
        }

        if (d0 <= d1)
        {
            return false;
        }

        double t = (_max_d - d0)*dt / (d1 - d0);

        if (t < _min_t)
        {
            qDebug() << "Collision at time " << t;
            return true;
        }
    }
    return false;
}