X-Git-Url: https://git.mxchange.org/?a=blobdiff_plain;f=simgear%2Froute%2Froute.cxx;h=d0da1804035ba40b4a6038be5feaa9e7748288e1;hb=f3c131ffaf04d8e04595c1271f8a70c8a9d89f5f;hp=24264725158994ec6ac238c6df3c5832982b753e;hpb=c39e08cb2a8d5666f9d54af7d243fa525214d24e;p=simgear.git

diff --git a/simgear/route/route.cxx b/simgear/route/route.cxx
index 24264725..d0da1804 100644
--- a/simgear/route/route.cxx
+++ b/simgear/route/route.cxx
@@ -16,14 +16,13 @@
 //
 // 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>
+#ifdef HAVE_CONFIG_H
+#  include <simgear_config.h>
+#endif
 
 #include "route.hxx"
 
@@ -38,33 +37,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;
 }