0

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;
}
Community
  • 1
  • 1
andre
  • 63
  • 2
  • 9
  • 1
    1. Do not post duplicate questions. If the first questions answer is still not good enough for you do not accept it as answered, instead add more details to your original Question in form of [edit]. 2. you have `lon,lat` in degrees and `double da = 1.567e-7; // [rad]` in radians so which one your `sin,cos` implementation needs? (also using the non linear angle Spherical to Cartesian conversion the updated one in my answer is better more precise but that should not be a too big deal for this) and 3. what does it mean not working? – Spektre Dec 22 '16 at 15:17
  • 1
    Possible duplicate of [Collision detection between 2 "linearly" moving objects in WGS84](http://stackoverflow.com/questions/41101107/collision-detection-between-2-linearly-moving-objects-in-wgs84) – Spektre Dec 22 '16 at 15:19
  • I use the sin and cos of the standard libarary so I think the sin and cos accepts radians – andre Dec 22 '16 at 15:24
  • The collision does not occur and the time of collision is so large and its not within the limit of the time collision however there is a collision between the two objects happen – andre Dec 22 '16 at 15:24
  • @then check it and CONVERT ALL ANGLES TO UNITS REQUIRED otherwise the result is nonsense – Spektre Dec 22 '16 at 15:24
  • so I should convert da to degrees by multiplying by 180/PI ? – andre Dec 22 '16 at 15:25
  • I'm so sorry If I disturbed you :( – andre Dec 22 '16 at 15:27
  • to check that just try `sin(0.5)` and if result is `~0.479` then use radians if `~0.0087` then use degrees. The upper case is so you do not miss it as you have several times for now ... – Spektre Dec 22 '16 at 15:27
  • the result is 0.479, what should I change in the code to let all angles to be in radians ? – andre Dec 22 '16 at 15:29
  • also are you sure your data will collide in some reasonable time (if too distant then you need to take into account also the curvature of earth which lead to a bit different approach) – Spektre Dec 22 '16 at 15:29
  • yes the data collide, where should I change in the code, to convert the angles to radians ? m_Pos.GetLongtitude(),..etc ? – andre Dec 22 '16 at 15:30
  • didn't see that comment simply `77.697421087742512 -> 77.697421087742512*M_PI/180.0` does not matter where in code you convert it .... but before computation all units should be the same (and the right ones) – Spektre Dec 22 '16 at 15:33
  • I have converted all the to radians. Time T does not decrease until it hits min_time. When the collision occurs, say I set min_t to 10, the time T is going to increase until it's 10 secs. The time should be decrease until it hits zero, or I'm wrong ? – andre Dec 22 '16 at 15:40
  • what are you writing about colision time is constant ... unless you are animating and computing it for example 1 time per second in such case you should not detect any collision until your _min_time and then `t=~_min_time` after that would decrease each second by `~1.0sec` until hit `~0.0` and after that also no collision should be detected ... if no alteration of course nor speed was present ... – Spektre Dec 22 '16 at 15:52
  • but what I need is a time that decreases until a collision occurs, so if there is a collision in 10 secs, I should detect it – andre Dec 22 '16 at 15:56
  • Collision at time 3.6373 Collision at time 5.96366 Collision at time 3.2334 Collision at time 3.08982 Collision at time 4.29415 Collision at time 3.0456 Collision at time 5.5345 Collision at time 3.69332 Collision at time 6.88666 Collision at time 4.64974 Collision at time 8.25097 Collision at time 5.74245 Collision at time 9.63327 Collision at time 6.92364 Collision at time 8.1611 Collision at time 9.43742 – andre Dec 22 '16 at 16:00
  • This is sample data for min_time is 10 – andre Dec 22 '16 at 16:01
  • what sample rate? – Spektre Dec 22 '16 at 16:03
  • Should I sample the collision code ? – andre Dec 22 '16 at 16:04
  • I need the time to decrease until the collision occurs. – andre Dec 22 '16 at 16:04
  • lol I asked how many samples per second is there in your data (so it does make any sense from physical side ) .... – Spektre Dec 22 '16 at 16:06
  • :D, its 1000 sampler/second – andre Dec 22 '16 at 16:12
  • the input data is wrong ... if you just look at the positions they so far away from each other so there is no chance they will collide inside 10sec limit not to mention angular distance is so big you have to take into account curvature of Earth ... 1000 samples per second should lead to very similar times you got something wrong ... possibly the position update is done on wrong units – Spektre Dec 22 '16 at 16:13
  • The input data is not for collision data. Do you want a data where they collide together _ – andre Dec 22 '16 at 16:14
  • Then why have you used that data? (mistake or some other reason you did not mention but asking about ....) – Spektre Dec 22 '16 at 16:16
  • I have updated the data. The collision should occur in 10 seconds – andre Dec 22 '16 at 16:20
  • not possible as the objects are `~59km` appart and speed is `< 720km/h` there is no way they collide in 10 seconds .... if you are testing on such premises there is no surprise it is not working ... – Spektre Dec 22 '16 at 16:28
  • How to take account the curvature of Earth ? what should I change ? – andre Dec 22 '16 at 16:45
  • you should iterate time with small enough interval or change the equations to curve that is intersection of plane and WGS84 ellipsoid or try to compute linearly in spherical coordinates (but that needs some angular bounds corrections). if your checking time is inside few minutes or even one hour then you can ignore this but for distances like 500km is this noticeable ... – Spektre Dec 22 '16 at 17:09
  • Can you explain more about the collision time. What I need is the time should decrease until the collision occurres – andre Dec 22 '16 at 17:11
  • @Spektre The second problem which I noticed, is the the velocity of the second object not the AV, is in its local coordinates, how would I transform its velocity to the coordinates of the AV. If you see, its velocity in its local coordinates is -186,..etc – andre Dec 23 '16 at 08:01
  • first get exactly which velocity is in which coordinate system and units not just blindly converting to whatever.... right now the speed `(1,1,0)[m/s]` is nonsense as it is 5 km/h which is too slow for any plane (unless it is a parachute/glide and even then it is too slow with no altitude change for spherical and no z component for cartessian which is highly improbable) – Spektre Dec 23 '16 at 08:11
  • yea the plane is too slow, its 110 knotens/sec that's its maximum speed. The problem is with the second velocity object is its in local coordinates, how would I transform it to the plane coordinate ? – andre Dec 23 '16 at 08:15
  • I was writing about the second velocity `(1.0,1.0,0.0)[m/s]` not `110` where did `[knots/s]` come from when you have `[m/s]` in comments. What does it mean plane local coordinates? and what does mean plane coordinates? you cant throw such therms without specifying them no one except you could know what they mean ... so if I do not know which one is which I can not advice how to transform in between ...And I suspect you do not know which one is which either .... – Spektre Dec 23 '16 at 08:23
  • @Spektre I mean the maximum speed of the plane is 110 knotens/hour, the m/s I get it in code. The plane has velocity in lan,lot,alt in m/s as I showed in the data above. The object is a ship and it has the velocity in its local coordinates, not related to the coordinates of the plane itself. as you see its vx = -186. but the vx of the plane is 1m/s – andre Dec 23 '16 at 08:29
  • how can ship be in 6000m above sea level? unless it is Zeppelin or Balloon but even those should not get higher then 2000m or risc freezing to death. Also how would ship collide with plane? And you still did not clarify what is plane local? if it is tighted to the object itself then you need also its orientation (in form of either transform matrix or 3 basis vectors like forward,Up,Left) if ti is in Cartessian then no need for transformation as you got it in the coordinates you need. If it is something else you need to specify .... – Spektre Dec 23 '16 at 08:41
  • the plane local is tighted to the object itself. its in spherical coordinates velocity lat,lon,alt, and I convert it using your WXYZ to velocity in Cartesian coordinates. Would you clarify more about the time of collision ? should It decrease until zero which is the time of collision ? If I want that behavior how would I do it. I want to set a time for collision, and if there is a collision in 10 secs, then it should alret – andre Dec 23 '16 at 08:53
  • if tighted to plane then it is not in spherical coordinates !!! but in the objects local axises instead which are which depends on how someone encoded it which I can only guess .... forget about collisions and time when your data is not sorted out yet (nothing can work when your input data is not properly converted to common coordinate system and units) – Spektre Dec 23 '16 at 09:00
  • The collision works when the two objects are close together. What I need is a time that decreases until the collision happens. so instead of min_time, I need max time. How would I do that ? Thanks so much and happy new year @Spektre – andre Dec 23 '16 at 13:02

0 Answers0