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 library is free software; you can redistribute it and/or
   8 // modify it under the terms of the GNU Library General Public
   9 // License as published by the Free Software Foundation; either
  10 // version 2 of the License, or (at your option) any later version.
  11 //
  12 // This library is distributed in the hope that it will be useful,
  13 // but WITHOUT ANY WARRANTY; without even the implied warranty of
  14 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
  15 // Library General Public License for more details.
  16 //
  17 // You should have received a copy of the GNU Library General Public
  18 // License along with this library; if not, write to the
  19 // Free Software Foundation, Inc., 59 Temple Place - Suite 330,
  20 // Boston, MA  02111-1307, USA.
  21 //
  22 // $Id$
  23
  24
  25 #ifndef _POLAR_HXX
  26 #define _POLAR_HXX
  27
  28
  29 #ifndef __cplusplus
  30 # error This library requires C++
  31 #endif
  32
  33
  34 #include <math.h>
  35
  36 #include <simgear/constants.h>
  37 #include <simgear/math/point3d.hxx>
  38
  39
  40 // Find the Altitude above the Ellipsoid (WGS84) given the Earth
  41 // Centered Cartesian coordinate vector Distances are specified in
  42 // meters.
  43 double fgGeodAltFromCart(const Point3D& cp);
  44
  45
  46 // Convert a polar coordinate to a cartesian coordinate.  Lon and Lat
  47 // must be specified in radians.  The FG convention is for distances
  48 // to be specified in meters
  49 inline Point3D fgPolarToCart3d(const Point3D& p) {
  50     double tmp = cos( p.lat() ) * p.radius();
  51
  52     return Point3D( cos( p.lon() ) * tmp,
  53                     sin( p.lon() ) * tmp,
  54                     sin( p.lat() ) * p.radius() );
  55 }
  56
  57
  58 // Convert a cartesian coordinate to polar coordinates (lon/lat
  59 // specified in radians.  Distances are specified in meters.
  60 inline Point3D fgCartToPolar3d(const Point3D& cp) {
  61     return Point3D( atan2( cp.y(), cp.x() ),
  62                     FG_PI_2 -
  63                     atan2( sqrt(cp.x()*cp.x() + cp.y()*cp.y()), cp.z() ),
  64                     sqrt(cp.x()*cp.x() + cp.y()*cp.y() + cp.z()*cp.z()) );
  65 }
  66
  67
  68 // calc new lon/lat given starting lon/lat, and offset radial, and
  69 // distance.  NOTE: starting point is specifed in radians, distance is
  70 // specified in meters (and converted internally to radians)
  71 // ... assumes a spherical world
  72 inline Point3D calc_gc_lon_lat( const Point3D& orig, double course,
  73                                 double dist ) {
  74     Point3D result;
  75
  76     // lat=asin(sin(lat1)*cos(d)+cos(lat1)*sin(d)*cos(tc))
  77     // IF (cos(lat)=0)
  78     //   lon=lon1      // endpoint a pole
  79     // ELSE
  80     //   lon=mod(lon1-asin(sin(tc)*sin(d)/cos(lat))+pi,2*pi)-pi
  81     // ENDIF
  82
  83     // printf("calc_lon_lat()  offset.theta = %.2f offset.dist = %.2f\n",
  84     //        offset.theta, offset.dist);
  85
  86     dist *= METER_TO_NM * NM_TO_RAD;
  87
  88     result.sety( asin( sin(orig.y()) * cos(dist) +
  89                        cos(orig.y()) * sin(dist) * cos(course) ) );
  90
  91     if ( cos(result.y()) < FG_EPSILON ) {
  92         result.setx( orig.x() );      // endpoint a pole
  93     } else {
  94         result.setx(
  95             fmod(orig.x() - asin( sin(course) * sin(dist) /
  96                                   cos(result.y()) ) + FG_PI, FG_2PI) - FG_PI );
  97     }
  98
  99     return result;
 100 }
 101
 102
 103 // calc course/dist
 104 inline void calc_gc_course_dist( const Point3D& start, const Point3D& dest,
 105                                  double *course, double *dist ) {
 106     // d = 2*asin(sqrt((sin((lat1-lat2)/2))^2 +
 107     //            cos(lat1)*cos(lat2)*(sin((lon1-lon2)/2))^2))
 108     double tmp1 = sin( (start.y() - dest.y()) / 2 );
 109     double tmp2 = sin( (start.x() - dest.x()) / 2 );
 110     double d = 2.0 * asin( sqrt( tmp1 * tmp1 +
 111                                  cos(start.y()) * cos(dest.y()) * tmp2 * tmp2));
 112
 113     // We obtain the initial course, tc1, (at point 1) from point 1 to
 114     // point 2 by the following. The formula fails if the initial
 115     // point is a pole. We can special case this with:
 116     //
 117     // IF (cos(lat1) < EPS)   // EPS a small number ~ machine precision
 118     //   IF (lat1 > 0)
 119     //     tc1= pi        //  starting from N pole
 120     //   ELSE
 121     //     tc1= 0         //  starting from S pole
 122     //   ENDIF
 123     // ENDIF
 124     //
 125     // For starting points other than the poles:
 126     //
 127     // IF sin(lon2-lon1)<0
 128     //   tc1=acos((sin(lat2)-sin(lat1)*cos(d))/(sin(d)*cos(lat1)))
 129     // ELSE
 130     //   tc1=2*pi-acos((sin(lat2)-sin(lat1)*cos(d))/(sin(d)*cos(lat1)))
 131     // ENDIF
 132
 133     double tc1;
 134
 135     if ( cos(start.y()) < FG_EPSILON ) {
 136         // EPS a small number ~ machine precision
 137         if ( start.y() > 0 ) {
 138             tc1 = FG_PI;        // starting from N pole
 139         } else {
 140             tc1 = 0;            // starting from S pole
 141         }
 142     }
 143
 144     // For starting points other than the poles:
 145
 146     double tmp3 = sin(d)*cos(start.y());
 147     double tmp4 = sin(dest.y())-sin(start.y())*cos(d);
 148     double tmp5 = acos(tmp4/tmp3);
 149     if ( sin( dest.x() - start.x() ) < 0 ) {
 150          tc1 = tmp5;
 151     } else {
 152          tc1 = 2 * FG_PI - tmp5;
 153     }
 154
 155     *course = tc1;
 156     *dist = d * RAD_TO_NM * NM_TO_METER;
 157 }
 158
 159 #endif // _POLAR_HXX