53 lines
1.3 KiB
C++
53 lines
1.3 KiB
C++
#include "distance.hpp"
|
|
|
|
#include "../std/algorithm.hpp"
|
|
#include "../std/cmath.hpp"
|
|
|
|
|
|
namespace earth
|
|
{
|
|
|
|
double const PI = atan(1.0) * 4.0;
|
|
|
|
double Degree2Rad(double deg)
|
|
{
|
|
return deg / 180.0 * PI;
|
|
}
|
|
|
|
double Rad2Degree(double rad)
|
|
{
|
|
return rad * 180.0 / PI;
|
|
}
|
|
|
|
double GetDistanceOnSphere(double lat1Deg, double lon1Deg, double lat2Deg, double lon2Deg)
|
|
{
|
|
double const lat1 = Degree2Rad(lat1Deg);
|
|
double const lat2 = Degree2Rad(lat2Deg);
|
|
double const lon1 = Degree2Rad(lon1Deg);
|
|
double const lon2 = Degree2Rad(lon2Deg);
|
|
|
|
double const dlat = sin((lat2 - lat1) / 2.0);
|
|
double const dlon = sin((lon2 - lon1) / 2.0);
|
|
double const y = dlat * dlat + dlon * dlon * cos(lat1) * cos(lat2);
|
|
return 2.0 * atan2(sqrt(y), sqrt(max(0.0, 1.0 - y)));
|
|
}
|
|
|
|
void GetOffsetOnSphere(double lat1Deg, double lon1Deg, double azimuth, double angDist,
|
|
double & lat2Deg, double & lon2Deg)
|
|
{
|
|
double const lat1 = Degree2Rad(lat1Deg);
|
|
double const lon1 = Degree2Rad(lon1Deg);
|
|
|
|
double const sinLat1 = sin(lat1);
|
|
double const cosLat1 = cos(lat1);
|
|
double const sinA = sin(angDist);
|
|
double const cosA = cos(angDist);
|
|
|
|
double const lat2 = asin(sinLat1*cosA + cosLat1*sinA*cos(azimuth));
|
|
double const lon2 = lon1 + atan2(sin(azimuth)*sinA*cosLat1, cosA - sinLat1*sin(lat2));
|
|
|
|
lat2Deg = Rad2Degree(lat2);
|
|
lon2Deg = Rad2Degree(lon2);
|
|
}
|
|
|
|
}
|