From 468f85484b462f68971fd4fb02bfe8ad3bb255bc Mon Sep 17 00:00:00 2001
From: Jesse Morgan <jesse@jesterpm.net>
Date: Wed, 13 Aug 2014 07:36:28 -0700
Subject: Adding bearing method to TinyGPS.

---
 libraries/TinyGPS/TinyGPS.cpp | 19 +++++++++++++++++++
 libraries/TinyGPS/TinyGPS.h   |  5 +++++
 2 files changed, 24 insertions(+)

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;
+}
diff --git a/libraries/TinyGPS/TinyGPS.h b/libraries/TinyGPS/TinyGPS.h
index 9c75ad0..1ffdbdd 100644
--- a/libraries/TinyGPS/TinyGPS.h
+++ b/libraries/TinyGPS/TinyGPS.h
@@ -111,6 +111,11 @@ class TinyGPS
 
     static float distance_between (float lat1, float long1, float lat2, float long2);
 
+    /**
+     * Return the bearing in degrees from lat1/long1 to lat2/long2.
+     */
+    static float bearing_between (float lat1, float long1, float lat2, float long2);
+
 private:
     enum {_GPS_SENTENCE_GPGGA, _GPS_SENTENCE_GPRMC, _GPS_SENTENCE_OTHER};
     
-- 
cgit v1.2.3