distance.cpp

#include <list>
#include <iostream>
#include <cmath>

using namespace std;
 
inline void sincos(double x, double& resSin, double& resCos)
  {
    resSin = sin(x);
    resCos = cos(x);
  }
  
inline double toRad(double deg)
{
	return  deg * M_PI / 180;
}
  /**
    Calculating basic cost for the A* algorithm based on the
    spherical distance of two points on earth
    */
  double GetSphericalDistance2(double aLat, double aLon,
                               double bLat, double bLon)
  {
    double r=6371.01; // Average radius of earth
    double aLatRad=toRad(aLat);
    double bLatRad=toRad(bLat);
    double dLat=toRad(bLat-aLat);
    double dLon=toRad(bLon-aLon);

    double sindLonDiv2=sin(dLon/2);
    //double cosdLonDiv2;
    //sincos(dLon/2, sindLonDiv2, cosdLonDiv2);

    double a = sin(dLat/2)*sin(dLat/2)+
		cos(aLatRad)*cos(bLatRad)*
		sindLonDiv2*sindLonDiv2;

    double c = 2*atan2(sqrt(a),sqrt(1-a));

    return r*c;
  }
  
  /**
    Calculating basic cost for the A* algorithm based on the
    spherical distance of two points on earth
    */
  double GetSphericalDistance(double aLat, double aLon,
                              double bLat, double bLon)
  {
    double r=6371.01; // Average radius of earth
    double dLat=(bLat-aLat)*M_PI/180;
    double dLon=(bLon-aLon)*M_PI/180;

    double sindLonDiv2;
    double cosdLonDiv2;
    sincos(dLon/2, sindLonDiv2, cosdLonDiv2);

    double a = sin(dLat/2)*sin(dLat/2)+cosdLonDiv2*cosdLonDiv2*sindLonDiv2*sindLonDiv2;

    double c = 2*atan2(sqrt(a),sqrt(1-a));

    return r*c;
  }

int main(int argc, char **args){

  std::cout << "Prague to Brno" << std::endl;
  std::cout << GetSphericalDistance(50.05237, 14.44173, 49.1925, 16.60882) << std::endl;
  std::cout << GetSphericalDistance2(50.05237, 14.44173, 49.1925, 16.60882) << std::endl;
  std::cout << std::endl;

  std::cout << "Over date line" << std::endl;
  std::cout << GetSphericalDistance(0.0, +179.0, 0.0, -179.0) << std::endl;
  std::cout << GetSphericalDistance2(0.0, +179.0, 0.0, -179.0) << std::endl;
  std::cout << std::endl;

  std::cout << "Over north pole" << std::endl;
  std::cout << GetSphericalDistance(89.0, 0, 89.0, 180.0) << std::endl;
  std::cout << GetSphericalDistance2(89.0, 0, 89.0, 180.0) << std::endl;
  std::cout << std::endl;

  std::cout << "Cross the world" << std::endl;
  std::cout << GetSphericalDistance(0,0, 0,180) << std::endl;
  std::cout << GetSphericalDistance2(0,0, 0,180) << std::endl;
  return 0;
}