// $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, 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;
}
-SGWayPoint::SGWayPoint( const double _lon, const double _lat ) {
- SGWayPoint( _lon, _lat, LATLON );
+// Destructor
+SGWayPoint::~SGWayPoint() {
}
-SGWayPoint::SGWayPoint() {
- SGWayPoint( 0.0, 0.0, LATLON );
+// Calculate course and distances. For WGS84 and SPHERICAL
+// coordinates lat, lon, and course are in degrees, alt and distance
+// are 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,
+ 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 );
+ }
}
-
-// Destructor
-SGWayPoint::~SGWayPoint() {
+// 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 );
}