From: jmt <jmt>
Date: Mon, 8 Jun 2009 23:18:39 +0000 (+0000)
Subject: Change SGWaypoint to use SGGeod internally. Remove some unused code, to
X-Git-Url: https://git.mxchange.org/?a=commitdiff_plain;h=03a7d72a62eb3f2a15242146b7b43ac54ab1fa44;p=simgear.git

Change SGWaypoint to use SGGeod internally. Remove some unused code, to
support cartesian waypoints and compute distance off a cartesian route.
Add a helper to access the total route distance.

Should not cause any visible functionality change.
---

diff --git a/simgear/route/route.cxx b/simgear/route/route.cxx
index c4e66a8d..431ceea1 100644
--- a/simgear/route/route.cxx
+++ b/simgear/route/route.cxx
@@ -38,37 +38,6 @@ 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)
 {
@@ -115,3 +84,11 @@ void SGRoute::delete_waypoint( int n ) {
     if ( n < size - 1 )
         update_distance( 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;
+}
diff --git a/simgear/route/route.hxx b/simgear/route/route.hxx
index 75fac7f0..5307acf4 100644
--- a/simgear/route/route.hxx
+++ b/simgear/route/route.hxx
@@ -142,14 +142,11 @@ public:
 
     /** Delete waypoint waypoint with index n  (last one if n < 0) */
     void delete_waypoint( int n = 0 );
-
+    
     /**
-     * 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 most accurate
-     * results, use with CARTESIAN way points.
+     * Helper, sum the distance members of each waypoint
      */
-    double distance_off_route( double x, double y ) const;
+    double total_distance() const;
 };
 
 
diff --git a/simgear/route/waypoint.cxx b/simgear/route/waypoint.cxx
index b56b25ba..697eb374 100644
--- a/simgear/route/waypoint.cxx
+++ b/simgear/route/waypoint.cxx
@@ -29,23 +29,43 @@
 
 #include "waypoint.hxx"
 
+using std::string;
 
 // Constructor
 SGWayPoint::SGWayPoint( const double lon, const double lat, const double alt,
-			const modetype m, const string& s, const string& n ) {
-    target_lon = lon;
-    target_lat = lat;
-    target_alt = alt;
-    mode = m;
-    id = s;
-    name = n;
+			const modetype m, const string& s, const string& n ) :
+  mode(m),
+  pos(SGGeod::fromDegM(lon, lat, alt)),
+  distance(0.0),
+  id(s),
+  name(n)
+{
 }
 
+SGWayPoint::SGWayPoint(const SGGeod& geod, const string& s, const string& n ) :
+  mode(WGS84),
+  pos(geod),
+  distance(0.0),
+  id(s),
+  name(n)
+{
+}
 
 // Destructor
 SGWayPoint::~SGWayPoint() {
 }
 
+void SGWayPoint::CourseAndDistance(const SGGeod& cur, double& course, double& dist ) const {
+  if ( mode == WGS84 ) {
+    double reverse;
+    SGGeodesy::inverse(cur, pos, course, reverse, dist);
+  } else if ( mode == SPHERICAL ) {
+    Point3D currentPoint(cur.getLongitudeRad(), cur.getLatitudeRad(), 0.0 );
+    Point3D targetPoint(pos.getLongitudeRad(), pos.getLatitudeRad(), 0.0 );
+    calc_gc_course_dist( currentPoint, targetPoint, &course, &dist );
+    course = 360.0 - course * SGD_RADIANS_TO_DEGREES;
+  }
+}
 
 // Calculate course and distances.  For WGS84 and SPHERICAL
 // coordinates lat, lon, and course are in degrees, alt and distance
@@ -55,34 +75,11 @@ void SGWayPoint::CourseAndDistance( const double cur_lon,
 				    const double cur_lat,
 				    const double cur_alt,
 				    double *course, double *dist ) const {
-    if ( mode == WGS84 ) {
-	double reverse;
-	geo_inverse_wgs_84( cur_alt, cur_lat, cur_lon, target_lat, target_lon,
-			    course, &reverse, dist );
-    } else if ( mode == SPHERICAL ) {
-	Point3D current( cur_lon * SGD_DEGREES_TO_RADIANS, cur_lat * SGD_DEGREES_TO_RADIANS, 0.0 );
-	Point3D target( target_lon * SGD_DEGREES_TO_RADIANS, target_lat * SGD_DEGREES_TO_RADIANS, 0.0 );
-	calc_gc_course_dist( current, target, course, dist );
-	*course = 360.0 - *course * SGD_RADIANS_TO_DEGREES;
-    } else if ( mode == CARTESIAN ) {
-	double dx = target_lon - cur_lon;
-	double dy = target_lat - cur_lat;
-	*course = -atan2( dy, dx ) * SGD_RADIANS_TO_DEGREES - 90;
-	while ( *course < 0 ) {
-	    *course += 360.0;
-	}
-	while ( *course > 360.0 ) {
-	    *course -= 360.0;
-	}
-	*dist = sqrt( dx * dx + dy * dy );
-    }
+  CourseAndDistance(SGGeod::fromDegM(cur_lon, cur_lat, cur_alt), *course, *dist);
 }
 
 // Calculate course and distances between two waypoints
 void SGWayPoint::CourseAndDistance( const SGWayPoint &wp,
 			double *course, double *dist ) const {
-    CourseAndDistance( wp.get_target_lon(),
-		       wp.get_target_lat(),
-		       wp.get_target_alt(),
-		       course, dist );
+    CourseAndDistance( wp.get_target(), course, dist );
 }
diff --git a/simgear/route/waypoint.hxx b/simgear/route/waypoint.hxx
index 206dcb3b..2141200c 100644
--- a/simgear/route/waypoint.hxx
+++ b/simgear/route/waypoint.hxx
@@ -35,9 +35,10 @@
 
 #include <simgear/compiler.h>
 
-#include <string>
+#include <simgear/math/SGMath.hxx>
+#include <simgear/math/SGGeod.hxx>
 
-using std::string;
+#include <string>
 
 
 /**
@@ -57,26 +58,21 @@ public:
      *      the world is a perfect sphere.  This is less compuntationally
      *      expensive than using wgs84 math and still a fairly good
      *      approximation of the real world, especially over shorter distances.
-     * <li> CARTESIAN requests all math be done assuming the coordinates specify
-     *      position in a Z = up world.
      */
     enum modetype { 
 	WGS84 = 0,
 	SPHERICAL = 1,
-	CARTESIAN = 2
     };
 
 private:
 
     modetype mode;
 
-    double target_lon;
-    double target_lat;
-    double target_alt;
+    SGGeod pos;
     double distance;
 
-    string id;
-    string name;
+    std::string id;
+    std::string name;
 
 public:
 
@@ -91,7 +87,12 @@ public:
      */
     SGWayPoint( const double lon = 0.0, const double lat = 0.0,
 		const double alt = 0.0, const modetype m = WGS84,
-		const string& s = "", const string& n = "" );
+		const std::string& s = "", const std::string& n = "" );
+  
+    /**
+     * Construct from a geodetic position, in WGS84 coordinates
+     */
+    SGWayPoint(const SGGeod& pos, const std::string& s = "", const std::string& n = "" );
 
     /** Destructor */
     ~SGWayPoint();
@@ -112,6 +113,9 @@ public:
 			    const double cur_alt,
 			    double *course, double *dist ) const;
 
+    void CourseAndDistance(const SGGeod& current,
+			    double& course, double& dist ) const;
+
     /**
      * Calculate course and distances between a specified starting waypoint
      * and this waypoint.
@@ -122,17 +126,16 @@ public:
     void CourseAndDistance( const SGWayPoint &wp,
 			    double *course, double *dist ) const;
 
-    /** @return waypoint mode */
-    inline modetype get_mode() const { return mode; }
-
     /** @return waypoint longitude */
-    inline double get_target_lon() const { return target_lon; }
+    inline double get_target_lon() const { return pos.getLongitudeDeg(); }
 
     /** @return waypoint latitude */
-    inline double get_target_lat() const { return target_lat; }
+    inline double get_target_lat() const { return pos.getLatitudeDeg(); }
 
     /** @return waypoint altitude */
-    inline double get_target_alt() const { return target_alt; }
+    inline double get_target_alt() const { return pos.getElevationM(); }
+
+    inline const SGGeod& get_target() const { return pos; }
 
     /**
      * This value is not calculated by this class.  It is simply a
@@ -153,10 +156,10 @@ public:
     inline void set_distance( double d ) { distance = d; }
 
     /** @return waypoint id */
-    inline const string& get_id() const { return id; }
+    inline const std::string& get_id() const { return id; }
 
     /** @return waypoint name */
-    inline const string& get_name() const { return name; }
+    inline const std::string& get_name() const { return name; }
 
 };