Fix a build order problem.

[simgear.git] / simgear / route / route.cxx

diff --git a/simgear/route/route.cxx b/simgear/route/route.cxx
index 6052e0d7612d5e6b692b4522971a37d0dd48170f..24264725158994ec6ac238c6df3c5832982b753e 100644 (file)
--- a/simgear/route/route.cxx
+++ b/simgear/route/route.cxx
@@ -21,6 +21,10 @@
 // $Id$
 
 
+#include <plib/sg.h>
+
+#include <simgear/math/vector.hxx>
+
 #include "route.hxx"
 
 
@@ -33,3 +37,34 @@ SGRoute::SGRoute() {
 // destructor
 SGRoute::~SGRoute() {
 }
+
+
+// Calculate perpendicular distance from the current route segment
+// This routine assumes all points are laying on a flat plane and
+// ignores the altitude (or Z) dimension.  For best results, use with
+// CARTESIAN way points.
+double SGRoute::distance_off_route( double x, double y ) const {
+    if ( current_wp > 0 ) {
+       int n0 = current_wp - 1;
+       int n1 = current_wp;
+       sgdVec3 p, p0, p1, d;
+       sgdSetVec3( p, x, y, 0.0 );
+       sgdSetVec3( p0, 
+                   route[n0].get_target_lon(), route[n0].get_target_lat(),
+                   0.0 );
+       sgdSetVec3( p1,
+                   route[n1].get_target_lon(), route[n1].get_target_lat(),
+                   0.0 );
+       sgdSubVec3( d, p0, p1 );
+
+       return sqrt( sgdClosestPointToLineDistSquared( p, p0, d ) );
+
+    } else {
+       // We are tracking the first waypoint so there is no route
+       // segment.  If you add the current location as the first
+       // waypoint and the actual waypoint as the second, then we
+       // will have a route segment and calculate distance from it.
+
+       return 0;
+    }
+}