X-Git-Url: https://git.mxchange.org/?a=blobdiff_plain;f=simgear%2Froute%2Froute.cxx;h=7a1a6c0836a2655f25cad14e361846541b523c63;hb=4c2dd553f2e5065b0fad55aa28915bf7c4a204e3;hp=c4e66a8da513947ae5c7c2e5ce5ec234df660356;hpb=a354c841f1ecd7560c2049eccf25ffeca2ec96aa;p=simgear.git

diff --git a/simgear/route/route.cxx b/simgear/route/route.cxx
index c4e66a8d..7a1a6c08 100644
--- a/simgear/route/route.cxx
+++ b/simgear/route/route.cxx
@@ -20,6 +20,9 @@
 //
 // $Id$
 
+#ifdef HAVE_CONFIG_H
+#  include <simgear_config.h>
+#endif
 
 #include <plib/sg.h>
 
@@ -38,51 +41,22 @@ SGRoute::SGRoute() {
 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;
-    }
-}
-
 /** Update the length of the leg ending at waypoint index */
-void SGRoute::update_distance(int index)
+void SGRoute::update_distance_and_track(int index)
 {
-    SGWayPoint& curr = route[ index ];
-    double course, dist;
-
-    if ( index == 0 ) {
-	dist = 0;
-    } else {
-	const SGWayPoint& prev = route[ index - 1 ];
-	curr.CourseAndDistance( prev, &course, &dist );
-    }
-
-    curr.set_distance( dist );
+  SGWayPoint& curr = route[ index ];
+  double course, dist;
+
+  if ( index == 0 ) {
+    dist = 0;
+    course = 0.0;
+  } else {
+    const SGWayPoint& prev = route[index - 1];
+    curr.CourseAndDistance( prev, &course, &dist );
+  }
+
+  curr.set_distance(dist);
+  curr.set_track(course);
 }
 
 /**
@@ -97,9 +71,9 @@ void SGRoute::add_waypoint( const SGWayPoint &wp, int n ) {
     } else {
         route.insert( route.begin() + n, 1, wp );
         // update distance of next leg if not at end of route
-        update_distance( n + 1 );
+        update_distance_and_track( n + 1 );
     }
-    update_distance( n );
+    update_distance_and_track( n );
 }
 
 /** Delete waypoint with index n  (last one if n < 0) */
@@ -113,5 +87,13 @@ void SGRoute::delete_waypoint( int n ) {
     route.erase( route.begin() + n );
     // update distance of next leg if not at end of route
     if ( n < size - 1 )
-        update_distance( n );
+        update_distance_and_track( n );
+}
+
+double SGRoute::total_distance() const {
+  double total = 0.0;
+  for (unsigned int i=0; i<route.size(); ++i) {
+    total += route[i].get_distance();
+  }
+  return total;
 }