緯度に基づいて地球表面の2点間の距離を計算する

#include   
#include   
#include   
  
#define EARTH_RADIUS  6371004  
#define PI 3.1415926  
  
void SystemInit(void)  
{  
    return;  
}  
  
#if 0  
double rad(double d)  
{  
    return d * PI / 180.0;  
}  
#else  
#define rad(d) ((d)*PI/180.0)  
#endif  
  
double getDistanceVer1(double lat1, double lng1, double lat2, double lng2)  
{  
    double radLat1 = rad(lat1);  
    double radLat2 = rad(lat2);  
    double radLng1 = rad(lng1);  
    double radLng2 = rad(lng2);  
    double s = acos(sin(radLat1)*sin(radLat2)+cos(radLat1)*cos(radLat2)*cos(radLng1-radLng2));  
    s = s * EARTH_RADIUS;  
    return s;  
}  
  
double getDistanceVer2(double lat1, double lng1, double lat2, double lng2)  
{  
    double radLat1 = rad(lat1);  
    double radLat2 = rad(lat2);  
    double a = radLat1 - radLat2;  
    double b = rad(lng1) - rad(lng2);  
    double s = 2 * asin(sqrt(pow(sin(a/2),2) + cos(radLat1)*cos(radLat2)*pow(sin(b/2),2)));  
    s = s * EARTH_RADIUS;  
    return s;  
}  
  
int main(void)  
{  
    unsigned char ret = 0;  
    double lat1,lng1,lat2,lng2,distance1, distance2;  
  
    int test = 0;  
  
    if(test == 1)  
    {  
        lat1 = 34.197761;  
        lng1 = 108.880089;  
        lat2 = 34.197718;  
        lng2 = 108.881170;  
    }  
    else if(test == 2)  
    {  
        lat1 = 0.000007;  
        lng1 = 109.327419;  
        lat2 = 0.000006;  
        lng2 = 109.328319;  
    }  
    else if(test == 3)  
    {  
        lat1 = -68.148228;  
        lng1 = 86.509809;  
        lat2 = -68.148455;  
        lng2 = 86.507481;  
    }  
    else  
    {  
        lat1 = 34.198000;  
        lng1 = 108.880000;  
        lat2 = 34.198000;  
        lng2 = 108.882000;  
    }  
    distance1=getDistanceVer1(lat1,lng1,lat2,lng2);  
  
    distance2=getDistanceVer2(lat1,lng1,lat2,lng2);  
  
    ret = (distance1 - distance2);  
  
    return ret;