【GPS】 根据GPS坐标求取两点间距离算法

 以下为通过经纬度坐标值求取两点间距离的算法，计算通过c++实现，在计算过程中已考虑到地球的弧度，经验证算法正确可行，精度为0.001米。

#include "pch.h"
#include <iostream>
#define _USE_MATH_DEFINES
#include <math.h>
#include <iomanip>

using namespace std;

double Earth_Radius = 6378.137;
double OutDistance = 0;
double OutAzimuth  = 0;

double Angle2Rad(double Angle)
{
	return Angle * M_PI / 180.0;
}

double Rad2Angle(double Rad)
{
	return Rad * 180.0 / M_PI;
}

/* Lat:纬度		 Lng:经度 */
void GetDistance(double Lat1, double Lng1, double Lat2, double Lng2,double *P_OutDistance,double *P_OutAzimuth) 
{
	double RadLat1 = Angle2Rad(Lat1);
	double RadLat2 = Angle2Rad(Lat2);
	double RadLng1 = Angle2Rad(Lng1);
	double RadLng2 = Angle2Rad(Lng2);

	double Lat_Dif = RadLat1 - RadLat2;
	double Lng_Dif = RadLng1 - RadLng2;

	/**----------- Calculate Distance-----------**/
	double Distance = 2 * asin(sqrt(pow(sin(Lat_Dif / 2.0), 2)
					+ cos(RadLat1) * cos(RadLat2)
					* pow(sin(Lng_Dif / 2.0), 2)));

	Distance = Distance * Earth_Radius;
	//Distance = round(Distance * 10000) / 10000;
	Distance = Distance * 1000;

	/**----------- Calculate Azimuth-----------**/
	double Azimuth = sin(RadLat1) * sin(RadLat2)
					+ cos(RadLat1) * cos(RadLat2) * cos(RadLng2- RadLng1);
	Azimuth = sqrt(1 - pow(Azimuth, 2));
	Azimuth = cos(RadLat2) * sin(RadLng2 - RadLng1) / Azimuth; 
	Azimuth = Rad2Angle(asin(Azimuth));

	*P_OutDistance = Distance;
	*P_OutAzimuth  = Azimuth;
}

void main(void)
{
	GetDistance(0, 0, 0, 0,&OutDistance,&OutAzimuth);
	cout << "Distance = " << setprecision(7) <<  OutDistance << endl;
	cout << "Azimuth = "  << setprecision(4) <<  OutAzimuth  << endl;
	getchar();
}