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

diff --git a/simgear/route/route.cxx b/simgear/route/route.cxx
index f07bb9e3..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,33 +41,59 @@ SGRoute::SGRoute() {
 SGRoute::~SGRoute() {
 }
 
+/** Update the length of the leg ending at waypoint index */
+void SGRoute::update_distance_and_track(int index)
+{
+  SGWayPoint& curr = route[ index ];
+  double course, dist;
 
-// 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 );
+  if ( index == 0 ) {
+    dist = 0;
+    course = 0.0;
+  } else {
+    const SGWayPoint& prev = route[index - 1];
+    curr.CourseAndDistance( prev, &course, &dist );
+  }
 
-	return sqrt( sgdClosestPointToLineDistSquared( p, p0, d ) );
+  curr.set_distance(dist);
+  curr.set_track(course);
+}
 
+/**
+ * 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 {
-	// 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;
+        route.insert( route.begin() + n, 1, wp );
+        // update distance of next leg if not at end of route
+        update_distance_and_track( n + 1 );
     }
+    update_distance_and_track( 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_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;
 }