From f38f86ab8c202690f09f59e87ead759bd2661cf9 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Thu, 1 Jan 2015 15:17:10 +1100
Subject: [PATCH] AP_Math: added location_path_proportion()

this can be used for glide slope calculations
---
 libraries/AP_Math/AP_Math.h    |  8 ++++++++
 libraries/AP_Math/location.cpp | 30 ++++++++++++++++++++++++++++++
 2 files changed, 38 insertions(+)

diff --git a/libraries/AP_Math/AP_Math.h b/libraries/AP_Math/AP_Math.h
index b2bed58206..19a3d8bbbe 100644
--- a/libraries/AP_Math/AP_Math.h
+++ b/libraries/AP_Math/AP_Math.h
@@ -115,6 +115,14 @@ bool        location_passed_point(const struct Location & location,
                                   const struct Location & point1,
                                   const struct Location & point2);
 
+/*
+  return the proportion we are along the path from point1 to
+  point2. This will be less than >1 if we have passed point2
+ */
+float location_path_proportion(const struct Location &location,
+                               const struct Location &point1,
+                               const struct Location &point2);
+
 //  extrapolate latitude/longitude given bearing and distance
 void        location_update(struct Location &loc, float bearing, float distance);
 
diff --git a/libraries/AP_Math/location.cpp b/libraries/AP_Math/location.cpp
index e1c7681c69..bd5810ddc6 100644
--- a/libraries/AP_Math/location.cpp
+++ b/libraries/AP_Math/location.cpp
@@ -114,6 +114,36 @@ bool location_passed_point(const struct Location &location,
     return false;
 }
 
+
+/*
+  return the proportion we are along the path from point1 to
+  point2, along a line parallel to point1<->point2.
+
+  This will be less than >1 if we have passed point2
+ */
+float location_path_proportion(const struct Location &location,
+                               const struct Location &point1,
+                               const struct Location &point2)
+{
+    Vector2f loc1(location.lat, location.lng);
+    Vector2f pt1(point1.lat, point1.lng);
+    Vector2f pt2(point2.lat, point2.lng);
+    float angle = (loc1 - pt2).angle(pt1 - pt2);
+    float dist_p1_p2 = get_distance(point1, point2);
+    float dist_l_p2 = get_distance(location, point2);
+    if (dist_p1_p2 <= 0) {
+        return 1.0f;
+    }
+    if (isinf(angle)) {
+        if (get_distance(location, point2) == 0) {
+            return 1.0f;
+        }
+        return 0.0f;
+    }
+    float x1 = cosf(angle) * dist_l_p2;
+    return (dist_p1_p2 - x1) / dist_p1_p2;
+}
+
 /*
  *  extrapolate latitude/longitude given bearing and distance
  * Note that this function is accurate to about 1mm at a distance of