diff options
Diffstat (limited to 'libraries/TinyGPS/TinyGPS.cpp')
| -rw-r--r-- | libraries/TinyGPS/TinyGPS.cpp | 19 |
1 files changed, 19 insertions, 0 deletions
diff --git a/libraries/TinyGPS/TinyGPS.cpp b/libraries/TinyGPS/TinyGPS.cpp index d4ee60f..db0b49a 100644 --- a/libraries/TinyGPS/TinyGPS.cpp +++ b/libraries/TinyGPS/TinyGPS.cpp @@ -301,3 +301,22 @@ float TinyGPS::distance_between (float lat1, float long1, float lat2, float long delta = atan2(delta, denom);
return delta * 6372795;
}
+
+/* static */
+float TinyGPS::bearing_between (float lat1, float long1, float lat2, float long2)
+{
+ float delta = radians(long1-long2);
+ float sdlong = sin(delta);
+ float cdlong = cos(delta);
+ lat1 = radians(lat1);
+ lat2 = radians(lat2);
+ float slat1 = sin(lat1);
+ float clat1 = cos(lat1);
+ float slat2 = sin(lat2);
+ float clat2 = cos(lat2);
+ float y = sdlong * clat1;
+ float x = clat2*slat1 - slat2*clat1*cdlong;
+ // Yeah, the mod operation should look funny.
+ // I'm implicitly swapping the order of the arguments...
+ return (int) (degrees(atan2(y, x)) + 180) % 360;
+}
|