Calculating distance between two points accurately

Forum 2005-2010 (read only) Software Syntax & Programs
10

http://www.movable-type.co.uk/scripts/latlong.html

The haversine formula 'remains particularly well-conditioned for numerical computation even at small distances' – unlike calculations based on the spherical law of cosines. The 'versed sine' is 1-cos[ch952], and the 'half-versed-sine' (1-cos[ch952])/2 = sin²([ch952]/2) as used above. It was published by R W Sinnott in Sky and Telescope, 1984, though known about for much longer by navigators. (For the curious, c is the angular distance in radians, and a is the square of half the chord length between the points).

long cnt = 1;

void setup()
{
  Serial.begin(115200);
}

void loop()
{
  cnt++;
  Serial.print(millis() / 1000);
  Serial.print("\t");
  Serial.print(cnt);
  Serial.print("\t");  
  Serial.print(cnt * 1000 / millis());
  Serial.print("\t");  
  float x = HaverSine(0, 0, 0, 180);
  Serial.println(x, 5);
} 

float HaverSine(float lat1, float lon1, float lat2, float lon2)
{
  float ToRad = PI / 180.0;
  float R = 6371;   // radius earth in Km
  
  float dLat = (lat2-lat1) * ToRad;
  float dLon = (lon2-lon1) * ToRad; 
  
  float a = sin(dLat/2) * sin(dLat/2) +
        cos(lat1 * ToRad) * cos(lat2 * ToRad) * 
        sin(dLon/2) * sin(dLon/2); 
        
  float c = 2 * atan2(sqrt(a), sqrt(1-a)); 
  
  float d = R * c;
  return d;
}