From a354c841f1ecd7560c2049eccf25ffeca2ec96aa Mon Sep 17 00:00:00 2001
From: mfranz <mfranz>
Date: Fri, 6 Apr 2007 09:54:35 +0000
Subject: [PATCH] Csaba HALASZ: - fix bug that messed up leg distances after
 inserting and deleting waypoints   not at the end of the route - move
 add_waypoint() and delete_waypoint from hxx to cxx - beef up routetest

---
 simgear/route/route.cxx     | 49 ++++++++++++++++++++++++++++++++++++-
 simgear/route/route.hxx     | 27 +++-----------------
 simgear/route/routetest.cxx | 26 +++++++++++++++++---
 3 files changed, 75 insertions(+), 27 deletions(-)

diff --git a/simgear/route/route.cxx b/simgear/route/route.cxx
index f07bb9e3..c4e66a8d 100644
--- a/simgear/route/route.cxx
+++ b/simgear/route/route.cxx
@@ -49,7 +49,7 @@ double SGRoute::distance_off_route( double x, double y ) const {
 	int n1 = current_wp;
 	sgdVec3 p, p0, p1, d;
 	sgdSetVec3( p, x, y, 0.0 );
-	sgdSetVec3( p0, 
+	sgdSetVec3( p0,
 		    route[n0].get_target_lon(), route[n0].get_target_lat(),
 		    0.0 );
 	sgdSetVec3( p1,
@@ -68,3 +68,50 @@ double SGRoute::distance_off_route( double x, double y ) const {
 	return 0;
     }
 }
+
+/** Update the length of the leg ending at waypoint index */
+void SGRoute::update_distance(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 );
+}
+
+/**
+ * Add waypoint (default), or insert waypoint at position n.
+ * @param wp a waypoint
+ */
+void SGRoute::add_waypoint( const SGWayPoint &wp, int n ) {
+    int size = route.size();
+    if ( n < 0 || n >= size ) {
+        n = size;
+        route.push_back( wp );
+    } 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( n );
+}
+
+/** Delete waypoint with index n  (last one if n < 0) */
+void SGRoute::delete_waypoint( int n ) {
+    int size = route.size();
+    if ( size == 0 )
+        return;
+    if ( n < 0 || n >= size )
+        n = size - 1;
+
+    route.erase( route.begin() + n );
+    // update distance of next leg if not at end of route
+    if ( n < size - 1 )
+        update_distance( n );
+}
diff --git a/simgear/route/route.hxx b/simgear/route/route.hxx
index 5d93992d..1f7c0323 100644
--- a/simgear/route/route.hxx
+++ b/simgear/route/route.hxx
@@ -55,6 +55,8 @@ private:
     route_list route;
     int current_wp;
 
+    void update_distance(int index);
+
 public:
 
     /** Constructor */
@@ -73,21 +75,7 @@ public:
      * Add waypoint (default), or insert waypoint at position n.
      * @param wp a waypoint
      */
-    void add_waypoint( const SGWayPoint &wp, int n = -1 ) {
-        if ( n < 0 || n >= (int)route.size() )
-            route.push_back( wp );
-        else
-            route.insert( route.begin() + n, 1, wp );
-
-	int size = route.size();
-	if ( size > 1 ) {
-	    SGWayPoint next_to_last = route[ size - 2 ];
-	    double tmpd, tmpc;
-	    wp.CourseAndDistance( next_to_last, &tmpc, &tmpd );
-	    route[size - 1].set_distance( tmpd );
-	}
-    }
-
+    void add_waypoint( const SGWayPoint &wp, int n = -1 );
     /**
      * Get the number of waypoints (i.e. route length )
      * @return route length
@@ -152,14 +140,7 @@ public:
     inline void delete_first() { delete_waypoint(0); }
 
     /** Delete waypoint waypoint with index n  (last one if n < 0) */
-    void delete_waypoint( int n = 0 ) {
-        if ( !route.size() )
-            return;
-        if ( n < 0 || n >= (int)route.size() )
-            n = route.size() - 1;
-
-        route.erase( route.begin() + n );
-    }
+    void delete_waypoint( int n = 0 );
 
     /**
      * Calculate perpendicular distance from the current route segment
diff --git a/simgear/route/routetest.cxx b/simgear/route/routetest.cxx
index 1d8be5af..dc586c0a 100644
--- a/simgear/route/routetest.cxx
+++ b/simgear/route/routetest.cxx
@@ -8,8 +8,20 @@
 
 SG_USING_STD(cout);
 SG_USING_STD(endl);
- 
-int main() {
+
+void dump_route(const SGRoute& route, const char* message)
+{
+    cout << "Route dump: " << message << endl;
+    for (int i = 0; i < route.size(); i++) {
+        const SGWayPoint wp = route.get_waypoint(i);
+        cout << "\t#" << i << " " << wp.get_id() << " (" << wp.get_target_lat()
+                << ", " << wp.get_target_lon() << ") @" << wp.get_target_alt()
+                << " dist: " << wp.get_distance() << endl;
+    }
+}
+
+int main()
+{
     SGRoute route;
 
     route.add_waypoint( SGWayPoint(0, 0, 0, SGWayPoint::CARTESIAN, "Start") );
@@ -17,7 +29,8 @@ int main() {
     route.add_waypoint( SGWayPoint(2, 0, 0, SGWayPoint::CARTESIAN, "2") );
     route.add_waypoint( SGWayPoint(2, 2, 0, SGWayPoint::CARTESIAN, "3") );
     route.add_waypoint( SGWayPoint(4, 2, 0, SGWayPoint::CARTESIAN, "4") );
-   
+
+    dump_route(route, "Init");
     route.set_current( 1 );
 
     cout << "( 0.5, 0 ) = " << route.distance_off_route( 0.5, 0 ) << endl;
@@ -29,5 +42,12 @@ int main() {
     cout << "( 2, 4 ) = " << route.distance_off_route( 2, 4 ) << endl;
     cout << "( 2.5, 4 ) = " << route.distance_off_route( 2.5, 4 ) << endl;
 
+    SGWayPoint wp2 = route.get_waypoint(2);
+    route.delete_waypoint(2);
+    dump_route(route, "removed WP2");
+
+    route.add_waypoint(wp2, 3);
+    dump_route(route, "added back WP2 after WP3");
+
     return 0;
 }
-- 
2.39.5