You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
33 lines
1.1 KiB
33 lines
1.1 KiB
#include "lat_long.hh" |
|
|
|
float LatLong::distance_to(const LatLong &target) const { |
|
float lat1 = latitude * RAD_PER_DEG; |
|
float lat2 = target.latitude * RAD_PER_DEG; |
|
float long1 = longitude * RAD_PER_DEG; |
|
float long2 = target.longitude * RAD_PER_DEG; |
|
float clat1 = cosf(lat1); |
|
float clat2 = cosf(lat2); |
|
float a = powf(sinf((long2 - long1) / 2.f), 2.f) * clat1 * clat2 + |
|
powf(sinf((lat2 - lat1) / 2.f), 2.f); |
|
float d_over_r = 2 * atan2f(sqrtf(a), sqrtf(1 - a)); |
|
return d_over_r * EARTH_RAD; |
|
} |
|
|
|
float LatLong::bearing_toward(const LatLong &target) const { |
|
float dlong = (target.longitude - longitude) * RAD_PER_DEG; |
|
float sdlong = sinf(dlong); |
|
float cdlong = cosf(dlong); |
|
float lat1 = latitude * RAD_PER_DEG; |
|
float lat2 = target.latitude * RAD_PER_DEG; |
|
float slat1 = sinf(lat1); |
|
float clat1 = cosf(lat1); |
|
float slat2 = sinf(lat2); |
|
float clat2 = cosf(lat2); |
|
float num = sdlong * clat2; |
|
float denom = (clat1 * slat2) - (slat1 * clat2 * cdlong); |
|
float course = atan2f(num, denom); |
|
if (course < 0.0) { |
|
course += 2 * PI; |
|
} |
|
return course / RAD_PER_DEG; |
|
} |