First working revision.

diff --git a/simgear/route/Makefile.am b/simgear/route/Makefile.am
index c45639e59d53696897f91c9442146f81e2a17600..2f2b30b7ed977cf5f8c7b383e49a4e005ed88763 100644 (file)
--- a/simgear/route/Makefile.am
+++ b/simgear/route/Makefile.am
@@ -8,3 +8,12 @@ libsgroute_a_SOURCES = \
        waypoint.cxx
 
 INCLUDES += -I$(top_srcdir)
+
+noinst_PROGRAMS = waytest
+
+waytest_SOURCES = waytest.cxx
+
+waytest_LDADD = \
+       $(top_builddir)/simgear/route/libsgroute.a \
+       $(top_builddir)/simgear/math/libsgmath.a \
+       $(top_builddir)/simgear/debug/libsgdebug.a
\ No newline at end of file

diff --git a/simgear/route/waypoint.cxx b/simgear/route/waypoint.cxx
index 231b862c0a7e3d79fe820f58d4e10bfa4900d7b7..ec490744128b20ad6ff3fbf3021a7c430dff64e7 100644 (file)
--- a/simgear/route/waypoint.cxx
+++ b/simgear/route/waypoint.cxx
@@ -21,28 +21,57 @@
 // $Id$
 
 
+#include <simgear/math/polar3d.hxx>
+#include <simgear/math/sg_geodesy.hxx>
+
 #include "waypoint.hxx"
 
 
 // Constructor
-SGWayPoint::SGWayPoint( const double _lon, const double _lat,
-                       const modetype _mode ) {
-    lon = _lon;
-    lat = _lat;
-    mode = _mode;
-}
-
-
-SGWayPoint::SGWayPoint( const double _lon, const double _lat ) {
-    SGWayPoint( _lon, _lat, LATLON );
+SGWayPoint::SGWayPoint( const double lon, const double lat,
+                       const modetype m, const string s ) {
+    target_lon = lon;
+    target_lat = lat;
+    mode = m;
+    id = s;
 }
 
 
 SGWayPoint::SGWayPoint() {
-    SGWayPoint( 0.0, 0.0, LATLON );
+    SGWayPoint( 0.0, 0.0, WGS84, "" );
 }
 
 
 // Destructor
 SGWayPoint::~SGWayPoint() {
 }
+
+
+// Calculate course and distances.  For WGS84 and SPHERICAL
+// coordinates lat, lon, and course are in degrees and distance in
+// meters.  For CARTESIAN coordinates x = lon, y = lat.  Course is in
+// degrees and distance is in what ever units x and y are in.
+void SGWayPoint::CourseAndDistance( const double cur_lon, const double cur_lat,
+                                   double *course, double *distance ) {
+    if ( mode == WGS84 ) {
+       double reverse;
+       geo_inverse_wgs_84( 0.0, cur_lat, cur_lon, target_lat, target_lon,
+                           course, &reverse, distance );
+    } else if ( mode == SPHERICAL ) {
+       Point3D current( cur_lon * DEG_TO_RAD, cur_lat * DEG_TO_RAD, 0.0 );
+       Point3D target( target_lon * DEG_TO_RAD, target_lat * DEG_TO_RAD, 0.0 );
+       calc_gc_course_dist( current, target, course, distance );
+       *course = 360.0 - *course * RAD_TO_DEG;
+    } else if ( mode == CARTESIAN ) {
+       double dx = target_lon - cur_lon;
+       double dy = target_lat - cur_lat;
+       *course = -atan2( dy, dx ) * RAD_TO_DEG - 90;
+       while ( *course < 0 ) {
+           *course += 360.0;
+       }
+       while ( *course > 360.0 ) {
+           *course -= 360.0;
+       }
+       *distance = sqrt( dx * dx + dy * dy );
+    }
+}

diff --git a/simgear/route/waypoint.hxx b/simgear/route/waypoint.hxx
index 174e38ba8bd5f44f7b54c2260edc9d0d65876de6..46730878e9a8fed6eff0c2f306589821d854a242 100644 (file)
--- a/simgear/route/waypoint.hxx
+++ b/simgear/route/waypoint.hxx
@@ -34,33 +34,48 @@
 #  include <config.h>
 #endif
 
+#include <simgear/compiler.h>
+
+#include STL_STRING
+
+FG_USING_STD(string);
+
 
 class SGWayPoint {
 
 public:
 
     enum modetype { 
-       LATLON = 0,
-       XY = 1,
+       WGS84 = 0,
+       SPHERICAL = 1,
+       CARTESIAN = 2
     };
 
 private:
 
     modetype mode;
 
-    double lon;
-    double lat;
+    double target_lon;
+    double target_lat;
+
+    string id;
 
 public:
 
     SGWayPoint();
-    SGWayPoint( const double _lon, const double _lat );
-    SGWayPoint( const double _lon, const double _lat, const modetype _mode );
+    SGWayPoint( const double lon, const double lat,
+               const modetype m = WGS84, const string s = "" );
     ~SGWayPoint();
 
+    // Calculate course and distances.  Lat, lon, and azimuth are in
+    // degrees.  distance in meters
+    void CourseAndDistance( const double cur_lon, const double cur_lat,
+                           double *course, double *distance );
+
     inline modetype get_mode() const { return mode; }
-    inline double get_lon() const { return lon; }
-    inline double get_lat() const { return lat; }
+    inline double get_target_lon() const { return target_lon; }
+    inline double get_target_lat() const { return target_lat; }
+    inline string get_id() const { return id; }
 };