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  - 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 #include <math.h>
  26 #include <stdio.h>
  27
  28 #include <simgear/constants.h>
  29
  30 #include "polar3d.hxx"
  31
  32
  33 // Find the Altitude above the Ellipsoid (WGS84) given the Earth
  34 // Centered Cartesian coordinate vector Distances are specified in
  35 // meters.
  36 double fgGeodAltFromCart(const Point3D& cp)
  37 {
  38     double t_lat, x_alpha, mu_alpha;
  39     double lat_geoc, radius;
  40     double result;
  41
  42     lat_geoc = FG_PI_2 - atan2( sqrt(cp.x()*cp.x() + cp.y()*cp.y()), cp.z() );
  43     radius = sqrt( cp.x()*cp.x() + cp.y()*cp.y() + cp.z()*cp.z() );
  44
  45     if( ( (FG_PI_2 - lat_geoc) < ONE_SECOND )        // near North pole
  46         || ( (FG_PI_2 + lat_geoc) < ONE_SECOND ) )   // near South pole
  47     {
  48         result = radius - EQUATORIAL_RADIUS_M*E;
  49     } else {
  50         t_lat = tan(lat_geoc);
  51         x_alpha = E*EQUATORIAL_RADIUS_M/sqrt(t_lat*t_lat + E*E);
  52         mu_alpha = atan2(sqrt(RESQ_M - x_alpha*x_alpha),E*x_alpha);
  53         if (lat_geoc < 0) {
  54             mu_alpha = - mu_alpha;
  55         }
  56         result = (radius - x_alpha/cos(lat_geoc))*cos(mu_alpha - lat_geoc);
  57     }
  58
  59     return(result);
  60 }
  61
  62