simgear/math/polar3d.hxx

   1 /**
   2  * \file polar3d.hxx
   3  * Routines to deal with polar math and transformations.
   4  */
   5
   6 // Written by Curtis Olson, started June 1997.
   7 //
   8 // Copyright (C) 1997  Curtis L. Olson  - http://www.flightgear.org/~curt
   9 //
  10 // This library is free software; you can redistribute it and/or
  11 // modify it under the terms of the GNU Library General Public
  12 // License as published by the Free Software Foundation; either
  13 // version 2 of the License, or (at your option) any later version.
  14 //
  15 // This library is distributed in the hope that it will be useful,
  16 // but WITHOUT ANY WARRANTY; without even the implied warranty of
  17 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
  18 // Library General Public License for more details.
  19 //
  20 // You should have received a copy of the GNU Library General Public
  21 // License along with this library; if not, write to the
  22 // Free Software Foundation, Inc., 59 Temple Place - Suite 330,
  23 // Boston, MA  02111-1307, USA.
  24 //
  25 // $Id$
  26
  27
  28 #ifndef _POLAR3D_HXX
  29 #define _POLAR3D_HXX
  30
  31
  32 #ifndef __cplusplus
  33 # error This library requires C++
  34 #endif
  35
  36
  37 #include <math.h>
  38
  39 #include <simgear/constants.h>
  40 #include <simgear/math/point3d.hxx>
  41
  42
  43 /**
  44  * Find the Altitude above the Ellipsoid (WGS84) given the Earth
  45  * Centered Cartesian coordinate vector Distances are specified in
  46  * meters.
  47  * @param cp point specified in cartesian coordinates
  48  * @return altitude above the (wgs84) earth in meters
  49  */
  50 double sgGeodAltFromCart(const Point3D& cp);
  51
  52
  53 /**
  54  * Convert a polar coordinate to a cartesian coordinate.  Lon and Lat
  55  * must be specified in radians.  The SG convention is for distances
  56  * to be specified in meters
  57  * @param p point specified in polar coordinates
  58  * @return the same point in cartesian coordinates
  59  */
  60 inline Point3D sgPolarToCart3d(const Point3D& p) {
  61     double tmp = cos( p.lat() ) * p.radius();
  62
  63     return Point3D( cos( p.lon() ) * tmp,
  64                     sin( p.lon() ) * tmp,
  65                     sin( p.lat() ) * p.radius() );
  66 }
  67
  68
  69 /**
  70  * Convert a cartesian coordinate to polar coordinates (lon/lat
  71  * specified in radians.  Distances are specified in meters.
  72  * @param cp point specified in cartesian coordinates
  73  * @return the same point in polar coordinates
  74  */
  75 inline Point3D sgCartToPolar3d(const Point3D& cp) {
  76     return Point3D( atan2( cp.y(), cp.x() ),
  77                     SGD_PI_2 -
  78                     atan2( sqrt(cp.x()*cp.x() + cp.y()*cp.y()), cp.z() ),
  79                     sqrt(cp.x()*cp.x() + cp.y()*cp.y() + cp.z()*cp.z()) );
  80 }
  81
  82
  83 /**
  84  * Calculate new lon/lat given starting lon/lat, and offset radial, and
  85  * distance.  NOTE: starting point is specifed in radians, distance is
  86  * specified in meters (and converted internally to radians)
  87  * ... assumes a spherical world.
  88  * @param orig specified in polar coordinates
  89  * @param course offset radial
  90  * @param dist offset distance
  91  * @return destination point in polar coordinates
  92  */
  93 inline Point3D calc_gc_lon_lat( const Point3D& orig, double course,
  94                                 double dist ) {
  95     Point3D result;
  96
  97     // lat=asin(sin(lat1)*cos(d)+cos(lat1)*sin(d)*cos(tc))
  98     // IF (cos(lat)=0)
  99     //   lon=lon1      // endpoint a pole
 100     // ELSE
 101     //   lon=mod(lon1-asin(sin(tc)*sin(d)/cos(lat))+pi,2*pi)-pi
 102     // ENDIF
 103
 104     // printf("calc_lon_lat()  offset.theta = %.2f offset.dist = %.2f\n",
 105     //        offset.theta, offset.dist);
 106
 107     dist *= SG_METER_TO_NM * SG_NM_TO_RAD;
 108
 109     result.sety( asin( sin(orig.y()) * cos(dist) +
 110                        cos(orig.y()) * sin(dist) * cos(course) ) );
 111
 112     if ( cos(result.y()) < SG_EPSILON ) {
 113         result.setx( orig.x() );      // endpoint a pole
 114     } else {
 115         result.setx(
 116             fmod(orig.x() - asin( sin(course) * sin(dist) /
 117                                   cos(result.y()) )
 118                  + SGD_PI, SGD_2PI) - SGD_PI );
 119     }
 120
 121     return result;
 122 }
 123
 124
 125 /**
 126  * Calculate course/dist given two spherical points.
 127  * @param start starting point
 128  * @param dest ending point
 129  * @param course resulting course
 130  * @param dist resulting distance
 131  */
 132 inline void calc_gc_course_dist( const Point3D& start, const Point3D& dest,
 133                                  double *course, double *dist )
 134 {
 135     // d = 2*asin(sqrt((sin((lat1-lat2)/2))^2 +
 136     //            cos(lat1)*cos(lat2)*(sin((lon1-lon2)/2))^2))
 137     double cos_start_y = cos( start.y() );
 138     volatile double tmp1 = sin( (start.y() - dest.y()) * 0.5 );
 139     volatile double tmp2 = sin( (start.x() - dest.x()) * 0.5 );
 140     double d = 2.0 * asin( sqrt( tmp1 * tmp1 +
 141                                  cos_start_y * cos(dest.y()) * tmp2 * tmp2));
 142
 143     *dist = d * SG_RAD_TO_NM * SG_NM_TO_METER;
 144
 145     // We obtain the initial course, tc1, (at point 1) from point 1 to
 146     // point 2 by the following. The formula fails if the initial
 147     // point is a pole. We can special case this with:
 148     //
 149     // IF (cos(lat1) < EPS)   // EPS a small number ~ machine precision
 150     //   IF (lat1 > 0)
 151     //     tc1= pi        //  starting from N pole
 152     //   ELSE
 153     //     tc1= 0         //  starting from S pole
 154     //   ENDIF
 155     // ENDIF
 156     //
 157     // For starting points other than the poles:
 158     //
 159     // IF sin(lon2-lon1)<0
 160     //   tc1=acos((sin(lat2)-sin(lat1)*cos(d))/(sin(d)*cos(lat1)))
 161     // ELSE
 162     //   tc1=2*pi-acos((sin(lat2)-sin(lat1)*cos(d))/(sin(d)*cos(lat1)))
 163     // ENDIF
 164
 165     // if ( cos(start.y()) < SG_EPSILON ) {
 166     // doing it this way saves a transcendental call
 167     double sin_start_y = sin( start.y() );
 168     if ( fabs(1.0-sin_start_y) < SG_EPSILON ) {
 169         // EPS a small number ~ machine precision
 170         if ( start.y() > 0 ) {
 171             *course = SGD_PI;   // starting from N pole
 172         } else {
 173             *course = 0;        // starting from S pole
 174         }
 175     } else {
 176         // For starting points other than the poles:
 177         // double tmp3 = sin(d)*cos_start_y);
 178         // double tmp4 = sin(dest.y())-sin(start.y())*cos(d);
 179         // double tmp5 = acos(tmp4/tmp3);
 180
 181         // Doing this way gaurentees that the temps are
 182         // not stored into memory
 183         double tmp5 = acos( (sin(dest.y()) - sin_start_y * cos(d)) /
 184                             (sin(d) * cos_start_y) );
 185
 186         // if ( sin( dest.x() - start.x() ) < 0 ) {
 187         // the sin of the negative angle is just the opposite sign
 188         // of the sin of the angle  so tmp2 will have the opposite
 189         // sign of sin( dest.x() - start.x() )
 190         if ( tmp2 >= 0 ) {
 191             *course = tmp5;
 192         } else {
 193             *course = 2 * SGD_PI - tmp5;
 194         }
 195     }
 196 }
 197
 198 #if 0
 199 /**
 200  * Calculate course/dist given two spherical points.
 201  * @param start starting point
 202  * @param dest ending point
 203  * @param course resulting course
 204  * @param dist resulting distance
 205  */
 206 inline void calc_gc_course_dist( const Point3D& start, const Point3D& dest,
 207                                  double *course, double *dist ) {
 208     // d = 2*asin(sqrt((sin((lat1-lat2)/2))^2 +
 209     //            cos(lat1)*cos(lat2)*(sin((lon1-lon2)/2))^2))
 210     double tmp1 = sin( (start.y() - dest.y()) / 2 );
 211     double tmp2 = sin( (start.x() - dest.x()) / 2 );
 212     double d = 2.0 * asin( sqrt( tmp1 * tmp1 +
 213                                  cos(start.y()) * cos(dest.y()) * tmp2 * tmp2));
 214
 215     // We obtain the initial course, tc1, (at point 1) from point 1 to
 216     // point 2 by the following. The formula fails if the initial
 217     // point is a pole. We can special case this with:
 218     //
 219     // IF (cos(lat1) < EPS)   // EPS a small number ~ machine precision
 220     //   IF (lat1 > 0)
 221     //     tc1= pi        //  starting from N pole
 222     //   ELSE
 223     //     tc1= 0         //  starting from S pole
 224     //   ENDIF
 225     // ENDIF
 226     //
 227     // For starting points other than the poles:
 228     //
 229     // IF sin(lon2-lon1)<0
 230     //   tc1=acos((sin(lat2)-sin(lat1)*cos(d))/(sin(d)*cos(lat1)))
 231     // ELSE
 232     //   tc1=2*pi-acos((sin(lat2)-sin(lat1)*cos(d))/(sin(d)*cos(lat1)))
 233     // ENDIF
 234
 235     double tc1;
 236
 237     if ( cos(start.y()) < SG_EPSILON ) {
 238         // EPS a small number ~ machine precision
 239         if ( start.y() > 0 ) {
 240             tc1 = SGD_PI;        // starting from N pole
 241         } else {
 242             tc1 = 0;            // starting from S pole
 243         }
 244     }
 245
 246     // For starting points other than the poles:
 247
 248     double tmp3 = sin(d)*cos(start.y());
 249     double tmp4 = sin(dest.y())-sin(start.y())*cos(d);
 250     double tmp5 = acos(tmp4/tmp3);
 251     if ( sin( dest.x() - start.x() ) < 0 ) {
 252          tc1 = tmp5;
 253     } else {
 254          tc1 = 2 * SGD_PI - tmp5;
 255     }
 256
 257     *course = tc1;
 258     *dist = d * SG_RAD_TO_NM * SG_NM_TO_METER;
 259 }
 260 #endif // 0
 261
 262 #endif // _POLAR3D_HXX