1 #include <cfg/compiler.h>
6 static const char *_compass_heading[] =
8 "N", "NNE", "NE", "ENE",
9 "E", "ESE", "SE", "SSE",
10 "S", "SSW", "SW", "WSW",
11 "W", "WNW", "NW", "NNW",
15 * Use the Haversine formula to calculate great-circle distances between the
18 * The Haversine formula remains particularly well-conditioned for numerical
19 * computation even at small distances, unlike calculations based on the
20 * spherical law of cosines.
22 float distance(float lat1, float lon1, float lat2, float lon2)
24 const float PLANET_RADIUS = 6371000;
25 float d_lat = deg2rad(lat2 - lat1);
26 float d_lon = deg2rad(lon2 - lon1);
28 float a = sin(d_lat / 2) * sin(d_lat / 2) +
29 cos(deg2rad(lat1)) * cos(deg2rad(lat2)) *
30 sin(d_lon / 2) * sin(d_lon / 2);
31 float c = 2 * atan2(sqrt(a), sqrt(1 - a));
33 return PLANET_RADIUS * c;
37 * Evaluate the bearing (also known as forward azimuth) using spherical law
40 * The bearing is a straight line along a great-circle arc from the start point
41 * to the destination point.
43 int bearing(float lat1, float lon1, float lat2, float lon2)
47 res = rad2deg(atan2(sin(deg2rad(lon2 - lon1)) *
48 cos(deg2rad(lat2)), cos(deg2rad(lat1)) *
49 sin(deg2rad(lat2)) - sin(deg2rad(lat1)) *
50 cos(deg2rad(lat2)) * cos(deg2rad(lon2) -
52 return ((int)res + 360) % 360;
55 const char *compass_heading(int bearing)
57 ASSERT(bearing >= 0 && bearing < 360);
61 return _compass_heading[(bearing << 4) / 360];