simgear/route/waypoint.cxx

   1 // waypoint.cxx -- Class to hold data and return info relating to a waypoint
   2 //
   3 // Written by Curtis Olson, started September 2000.
   4 //
   5 // Copyright (C) 2000  Curtis L. Olson  - curt@hfrl.umn.edu
   6 //
   7 // This program is free software; you can redistribute it and/or
   8 // modify it under the terms of the GNU General Public License as
   9 // published by the Free Software Foundation; either version 2 of the
  10 // License, or (at your option) any later version.
  11 //
  12 // This program is distributed in the hope that it will be useful, but
  13 // WITHOUT ANY WARRANTY; without even the implied warranty of
  14 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
  15 // General Public License for more details.
  16 //
  17 // You should have received a copy of the GNU General Public License
  18 // along with this program; if not, write to the Free Software
  19 // Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
  20 //
  21 // $Id$
  22
  23
  24 #include <simgear/math/polar3d.hxx>
  25 #include <simgear/math/sg_geodesy.hxx>
  26
  27 #include "waypoint.hxx"
  28
  29
  30 // Constructor
  31 SGWayPoint::SGWayPoint( const double lon, const double lat, const double alt,
  32                         const modetype m, const string s, const string n ) {
  33     target_lon = lon;
  34     target_lat = lat;
  35     target_alt = alt;
  36     mode = m;
  37     id = s;
  38     name = n;
  39 }
  40
  41
  42 // Destructor
  43 SGWayPoint::~SGWayPoint() {
  44 }
  45
  46
  47 // Calculate course and distances.  For WGS84 and SPHERICAL
  48 // coordinates lat, lon, and course are in degrees, alt and distance
  49 // are in meters.  For CARTESIAN coordinates x = lon, y = lat.  Course
  50 // is in degrees and distance is in what ever units x and y are in.
  51 void SGWayPoint::CourseAndDistance( const double cur_lon,
  52                                     const double cur_lat,
  53                                     const double cur_alt,
  54                                     double *course, double *dist ) const {
  55     if ( mode == WGS84 ) {
  56         double reverse;
  57         geo_inverse_wgs_84( cur_alt, cur_lat, cur_lon, target_lat, target_lon,
  58                             course, &reverse, dist );
  59     } else if ( mode == SPHERICAL ) {
  60         Point3D current( cur_lon * SGD_DEGREES_TO_RADIANS, cur_lat * SGD_DEGREES_TO_RADIANS, 0.0 );
  61         Point3D target( target_lon * SGD_DEGREES_TO_RADIANS, target_lat * SGD_DEGREES_TO_RADIANS, 0.0 );
  62         calc_gc_course_dist( current, target, course, dist );
  63         *course = 360.0 - *course * SGD_RADIANS_TO_DEGREES;
  64     } else if ( mode == CARTESIAN ) {
  65         double dx = target_lon - cur_lon;
  66         double dy = target_lat - cur_lat;
  67         *course = -atan2( dy, dx ) * SGD_RADIANS_TO_DEGREES - 90;
  68         while ( *course < 0 ) {
  69             *course += 360.0;
  70         }
  71         while ( *course > 360.0 ) {
  72             *course -= 360.0;
  73         }
  74         *dist = sqrt( dx * dx + dy * dy );
  75     }
  76 }
  77
  78 // Calculate course and distances between two waypoints
  79 void SGWayPoint::CourseAndDistance( const SGWayPoint &wp,
  80                         double *course, double *dist ) const {
  81     CourseAndDistance( wp.get_target_lon(),
  82                        wp.get_target_lat(),
  83                        wp.get_target_alt(),
  84                        course, dist );
  85 }