simgear/math/polar3d.cxx

   1 // polar.cxx -- 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  - http://www.flightgear.org/~curt
   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 #ifdef HAVE_CONFIG_H
  25 #  include <simgear_config.h>
  26 #endif
  27
  28 #include <math.h>
  29
  30 #include <simgear/constants.h>
  31
  32 #include "polar3d.hxx"
  33
  34 /**
  35  * Calculate new lon/lat given starting lon/lat, and offset radial, and
  36  * distance.  NOTE: starting point is specifed in radians, distance is
  37  * specified in meters (and converted internally to radians)
  38  * ... assumes a spherical world.
  39  * @param orig specified in polar coordinates
  40  * @param course offset radial
  41  * @param dist offset distance
  42  * @return destination point in polar coordinates
  43  */
  44 Point3D calc_gc_lon_lat( const Point3D& orig, double course,
  45                                 double dist ) {
  46     Point3D result;
  47
  48     // lat=asin(sin(lat1)*cos(d)+cos(lat1)*sin(d)*cos(tc))
  49     // IF (cos(lat)=0)
  50     //   lon=lon1      // endpoint a pole
  51     // ELSE
  52     //   lon=mod(lon1-asin(sin(tc)*sin(d)/cos(lat))+pi,2*pi)-pi
  53     // ENDIF
  54
  55     // printf("calc_lon_lat()  offset.theta = %.2f offset.dist = %.2f\n",
  56     //        offset.theta, offset.dist);
  57
  58     dist *= SG_METER_TO_NM * SG_NM_TO_RAD;
  59
  60     result.sety( asin( sin(orig.y()) * cos(dist) +
  61                        cos(orig.y()) * sin(dist) * cos(course) ) );
  62
  63     if ( cos(result.y()) < SG_EPSILON ) {
  64         result.setx( orig.x() );      // endpoint a pole
  65     } else {
  66         result.setx(
  67             fmod(orig.x() - asin( sin(course) * sin(dist) /
  68                                   cos(result.y()) )
  69                  + SGD_PI, SGD_2PI) - SGD_PI );
  70     }
  71
  72     return result;
  73 }
  74
  75 /**
  76  * Calculate course/dist given two spherical points.
  77  * @param start starting point
  78  * @param dest ending point
  79  * @param course resulting course
  80  * @param dist resulting distance
  81  */
  82 void calc_gc_course_dist( const Point3D& start, const Point3D& dest,
  83                                  double *course, double *dist )
  84 {
  85     if ( start == dest) {
  86             *dist=0;
  87             *course=0;
  88             return;
  89     }
  90     // d = 2*asin(sqrt((sin((lat1-lat2)/2))^2 +
  91     //            cos(lat1)*cos(lat2)*(sin((lon1-lon2)/2))^2))
  92     double cos_start_y = cos( start.y() );
  93     double tmp1 = sin( (start.y() - dest.y()) * 0.5 );
  94     double tmp2 = sin( (start.x() - dest.x()) * 0.5 );
  95     double d = 2.0 * asin( sqrt( tmp1 * tmp1 +
  96                                  cos_start_y * cos(dest.y()) * tmp2 * tmp2));
  97
  98     *dist = d * SG_RAD_TO_NM * SG_NM_TO_METER;
  99
 100 #if 1
 101     double c1 = atan2(
 102                 cos(dest.y())*sin(dest.x()-start.x()),
 103                 cos(start.y())*sin(dest.y())-
 104                 sin(start.y())*cos(dest.y())*cos(dest.x()-start.x()));
 105     if (c1 >= 0)
 106       *course = SGD_2PI-c1;
 107     else
 108       *course = -c1;
 109 #else
 110     // We obtain the initial course, tc1, (at point 1) from point 1 to
 111     // point 2 by the following. The formula fails if the initial
 112     // point is a pole. We can special case this with:
 113     //
 114     // IF (cos(lat1) < EPS)   // EPS a small number ~ machine precision
 115     //   IF (lat1 > 0)
 116     //     tc1= pi        //  starting from N pole
 117     //   ELSE
 118     //     tc1= 0         //  starting from S pole
 119     //   ENDIF
 120     // ENDIF
 121     //
 122     // For starting points other than the poles:
 123     //
 124     // IF sin(lon2-lon1)<0
 125     //   tc1=acos((sin(lat2)-sin(lat1)*cos(d))/(sin(d)*cos(lat1)))
 126     // ELSE
 127     //   tc1=2*pi-acos((sin(lat2)-sin(lat1)*cos(d))/(sin(d)*cos(lat1)))
 128     // ENDIF
 129
 130     // if ( cos(start.y()) < SG_EPSILON ) {
 131     // doing it this way saves a transcendental call
 132     double sin_start_y = sin( start.y() );
 133     if ( fabs(1.0-sin_start_y) < SG_EPSILON ) {
 134         // EPS a small number ~ machine precision
 135         if ( start.y() > 0 ) {
 136             *course = SGD_PI;   // starting from N pole
 137         } else {
 138             *course = 0;        // starting from S pole
 139         }
 140     } else {
 141         // For starting points other than the poles:
 142         // double tmp3 = sin(d)*cos_start_y);
 143         // double tmp4 = sin(dest.y())-sin(start.y())*cos(d);
 144         // double tmp5 = acos(tmp4/tmp3);
 145
 146         // Doing this way gaurentees that the temps are
 147         // not stored into memory
 148         double tmp5 = acos( (sin(dest.y()) - sin_start_y * cos(d)) /
 149                             (sin(d) * cos_start_y) );
 150
 151         // if ( sin( dest.x() - start.x() ) < 0 ) {
 152         // the sin of the negative angle is just the opposite sign
 153         // of the sin of the angle  so tmp2 will have the opposite
 154         // sign of sin( dest.x() - start.x() )
 155         if ( tmp2 >= 0 ) {
 156             *course = tmp5;
 157         } else {
 158             *course = SGD_2PI - tmp5;
 159         }
 160     }
 161 #endif
 162 }
 163
 164
 165 #if 0
 166 /**
 167  * Calculate course/dist given two spherical points.
 168  * @param start starting point
 169  * @param dest ending point
 170  * @param course resulting course
 171  * @param dist resulting distance
 172  */
 173 void calc_gc_course_dist( const Point3D& start, const Point3D& dest,
 174                                  double *course, double *dist ) {
 175     // d = 2*asin(sqrt((sin((lat1-lat2)/2))^2 +
 176     //            cos(lat1)*cos(lat2)*(sin((lon1-lon2)/2))^2))
 177     double tmp1 = sin( (start.y() - dest.y()) / 2 );
 178     double tmp2 = sin( (start.x() - dest.x()) / 2 );
 179     double d = 2.0 * asin( sqrt( tmp1 * tmp1 +
 180                                  cos(start.y()) * cos(dest.y()) * tmp2 * tmp2));
 181     // We obtain the initial course, tc1, (at point 1) from point 1 to
 182     // point 2 by the following. The formula fails if the initial
 183     // point is a pole. We can special case this with:
 184     //
 185     // IF (cos(lat1) < EPS)   // EPS a small number ~ machine precision
 186     //   IF (lat1 > 0)
 187     //     tc1= pi        //  starting from N pole
 188     //   ELSE
 189     //     tc1= 0         //  starting from S pole
 190     //   ENDIF
 191     // ENDIF
 192     //
 193     // For starting points other than the poles:
 194     //
 195     // IF sin(lon2-lon1)<0
 196     //   tc1=acos((sin(lat2)-sin(lat1)*cos(d))/(sin(d)*cos(lat1)))
 197     // ELSE
 198     //   tc1=2*pi-acos((sin(lat2)-sin(lat1)*cos(d))/(sin(d)*cos(lat1)))
 199     // ENDIF
 200
 201     double tc1;
 202
 203     if ( cos(start.y()) < SG_EPSILON ) {
 204         // EPS a small number ~ machine precision
 205         if ( start.y() > 0 ) {
 206             tc1 = SGD_PI;        // starting from N pole
 207         } else {
 208             tc1 = 0;            // starting from S pole
 209         }
 210     }
 211
 212     // For starting points other than the poles:
 213
 214     double tmp3 = sin(d)*cos(start.y());
 215     double tmp4 = sin(dest.y())-sin(start.y())*cos(d);
 216     double tmp5 = acos(tmp4/tmp3);
 217     if ( sin( dest.x() - start.x() ) < 0 ) {
 218          tc1 = tmp5;
 219     } else {
 220          tc1 = SGD_2PI - tmp5;
 221     }
 222
 223     *course = tc1;
 224     *dist = d * SG_RAD_TO_NM * SG_NM_TO_METER;
 225 }
 226 #endif // 0