dimanche 1 janvier 2017

C++ Find distance (in km) between point and line expressed in lat,long in degrees

I read and implemented the posts(Minimum distance between a point and a line in latitude, longitude, http://ift.tt/19xYT03, http://ift.tt/19xYT03) for finding the shortest distance between a point and a line segment expressed in latitude, longitude in degrees. However all these implementations are giving wrong results. Can someone please help me understand as to where am I going wrong?

//IMPLEMENTATION 1
double pointLineDistBearing(double* point, double* segment)
{
    double p = 0.017453292519943295;    // Math.PI / 180
    double lat1=segment[0], lon1=segment[1], lat2=segment[2], lon2=segment[3], lat3=point[0], lon3=point[1];
    double y = sin(lon3 - lon1) * cos(lat3);
    double x = cos(lat1) * sin(lat3) - sin(lat1) * cos(lat3) * cos(lat3 - lat1);
    double bearing1 = (atan2(y, x))*p;
    bearing1 = 360 - (bearing1 + 360 % 360);

    double y2 = sin(lon2 - lon1) * cos(lat2);
    double x2 = cos(lat1) * sin(lat2) - sin(lat1) * cos(lat2) * cos(lat2 - lat1);
    double bearing2 = p*(atan2(y2, x2));
    bearing2 = 360 - (bearing2 + 360 % 360);

    double lat1Rads = p*(lat1);
    double lat3Rads = p*(lat3);
    double dLon = p*(lon3 - lon1);

    double distanceAC = acos(sin(lat1Rads) * sin(lat3Rads)+cos(lat1Rads)*cos(lat3Rads)*cos(dLon)) * 6371;  
    double min_distance = fabs(asin(sin(distanceAC/6371)*sin(p*(bearing1)-p*(bearing2))) * 6371);

    cout<<"min dist="<<min_distance<<"\n";

}
//---------------------------------------------------------------------------
double haversineDistanceLoading(double lat0, double long0, double lat1, double long1) //lat1=polygon0[0], long1=polygon0[1]
{
  double p = 0.017453292519943295;    // Math.PI / 180
  double a = 0.5 - cos((lat1 - lat0) * p)/2 + 
          cos(lat0 * p) * cos(lat1 * p) * 
          (1 - cos((long1 - long0) * p))/2;

  return 12742 * asin(pow(a,0.5)); // 2 * R; R = 6371 km    
}
//---------------------------------------------------------------------------
// IMPLEMENTATION 2
double pointLineDistBearing2(double* point, double* segment)
{
    double p = 0.017453292519943295;    // Math.PI / 180
    double pi=22.0/7.0;
    double a_lat=segment[0]*p, a_lon=segment[1]*p, b_lat=segment[2]*p, b_lon=segment[3]*p, pointLat=point[0], pointLon=point[1];

    double delta_lon = b_lon - a_lon;
    double B_x = cos(b_lat) * cos(delta_lon); 
    double B_y = cos(b_lat) * sin(delta_lon);
    double mid_lat = atan2(sin(a_lat) + sin(b_lat), sqrt((pow((cos(a_lat) + B_x),2) + pow(B_y,2))));
    double mid_lon = a_lon + atan2(B_y, cos(a_lat) + B_x);
    double mid_lon3pi = (mid_lon + 3*pi);
    double pi2 = (2*3.14);
    double mid_lon3pi2pi = int(mid_lon3pi) % int(pi2);
    mid_lon= mid_lon3pi2pi - pi;
    //Normalise
    //mid_lon = ((mid_lon + 3*pi) % (2*pi)) - pi; //double mid_lon = (mid_lon + 3*pi) % (2*pi) - pi;

    double mid_Latitude=(mid_lat/p), mid_Longitude=(mid_lon/p);

    cout<<"minDist="<<haversineDistanceLoading(mid_Latitude, mid_Longitude, pointLat, pointLon)<<"\n";

    return haversineDistanceLoading(mid_Latitude, mid_Longitude, pointLat, pointLon);
}

Aucun commentaire:

Enregistrer un commentaire