Modified Files:

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

diff --git a/simgear/route/route.cxx b/simgear/route/route.cxx
index 6052e0d7612d5e6b692b4522971a37d0dd48170f..f07bb9e3875ab7abf123b487a62c3ac7a168f1e4 100644 (file)
--- a/simgear/route/route.cxx
+++ b/simgear/route/route.cxx
@@ -16,11 +16,15 @@
 //
 // You should have received a copy of the GNU General Public License
 // along with this program; if not, write to the Free Software
-// Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+// Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
 //
 // $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;
+    }
+}