simgear/math/polar3d.hxx

   1 // polar.hxx -- routines to deal with polar math and transformations
   2 //
   3 // Written by Curtis Olson, started June 1997.
   4 //
   5 // Copyright (C) 1997  Curtis L. Olson  - curt@infoplane.com
   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 #ifndef _POLAR_HXX
  25 #define _POLAR_HXX
  26
  27
  28 #ifndef __cplusplus
  29 # error This library requires C++
  30 #endif
  31
  32
  33 #include <simgear/constants.h>
  34 #include <simgear/math/point3d.hxx>
  35
  36
  37 // Find the Altitude above the Ellipsoid (WGS84) given the Earth
  38 // Centered Cartesian coordinate vector Distances are specified in
  39 // meters.
  40 double fgGeodAltFromCart(const Point3D& cp);
  41
  42
  43 // Convert a polar coordinate to a cartesian coordinate.  Lon and Lat
  44 // must be specified in radians.  The FG convention is for distances
  45 // to be specified in meters
  46 inline Point3D fgPolarToCart3d(const Point3D& p) {
  47     double tmp = cos( p.lat() ) * p.radius();
  48
  49     return Point3D( cos( p.lon() ) * tmp,
  50                     sin( p.lon() ) * tmp,
  51                     sin( p.lat() ) * p.radius() );
  52 }
  53
  54
  55 // Convert a cartesian coordinate to polar coordinates (lon/lat
  56 // specified in radians.  Distances are specified in meters.
  57 inline Point3D fgCartToPolar3d(const Point3D& cp) {
  58     return Point3D( atan2( cp.y(), cp.x() ),
  59                     FG_PI_2 -
  60                     atan2( sqrt(cp.x()*cp.x() + cp.y()*cp.y()), cp.z() ),
  61                     sqrt(cp.x()*cp.x() + cp.y()*cp.y() + cp.z()*cp.z()) );
  62 }
  63
  64
  65 // calc new lon/lat given starting lon/lat, and offset radial, and
  66 // distance.  NOTE: starting point is specifed in radians, distance is
  67 // specified in meters (and converted internally to radians)
  68 // ... assumes a spherical world
  69 inline Point3D calc_lon_lat( const Point3D& orig, double course, double dist ) {
  70     Point3D result;
  71
  72     // lat=asin(sin(lat1)*cos(d)+cos(lat1)*sin(d)*cos(tc))
  73     // IF (cos(lat)=0)
  74     //   lon=lon1      // endpoint a pole
  75     // ELSE
  76     //   lon=mod(lon1-asin(sin(tc)*sin(d)/cos(lat))+pi,2*pi)-pi
  77     // ENDIF
  78
  79     // printf("calc_lon_lat()  offset.theta = %.2f offset.dist = %.2f\n",
  80     //        offset.theta, offset.dist);
  81
  82     dist *= METER_TO_NM * NM_TO_RAD;
  83
  84     result.sety( asin( sin(orig.y()) * cos(dist) +
  85                        cos(orig.y()) * sin(dist) * cos(course) ) );
  86
  87     if ( cos(result.y()) < FG_EPSILON ) {
  88         result.setx( orig.x() );      // endpoint a pole
  89     } else {
  90         result.setx(
  91             fmod(orig.x() - asin( sin(course) * sin(dist) /
  92                                   cos(result.y()) ) + FG_PI, FG_2PI) - FG_PI );
  93     }
  94
  95     return result;
  96 }
  97
  98
  99 #endif // _POLAR_HXX
 100
 101