Math/polar3d.cxx

   1 /**************************************************************************
   2  * polar.cxx -- routines to deal with polar math and transformations
   3  *
   4  * Written by Curtis Olson, started June 1997.
   5  *
   6  * Copyright (C) 1997  Curtis L. Olson  - curt@infoplane.com
   7  *
   8  * This program is free software; you can redistribute it and/or
   9  * modify it under the terms of the GNU General Public License as
  10  * published by the Free Software Foundation; either version 2 of the
  11  * License, or (at your option) any later version.
  12  *
  13  * This program is distributed in the hope that it will be useful, but
  14  * WITHOUT ANY WARRANTY; without even the implied warranty of
  15  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
  16  * General Public License for more details.
  17  *
  18  * You should have received a copy of the GNU General Public License
  19  * along with this program; if not, write to the Free Software
  20  * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
  21  *
  22  * $Id$
  23  * (Log is kept at end of this file)
  24  **************************************************************************/
  25
  26
  27 #include <math.h>
  28 #include <stdio.h>
  29
  30 #include <Include/fg_constants.h>
  31
  32 #include "polar3d.hxx"
  33
  34
  35 /* Convert a polar coordinate to a cartesian coordinate.  Lon and Lat
  36  * must be specified in radians.  The FG convention is for distances
  37  * to be specified in meters */
  38 Point3D fgPolarToCart3d(const Point3D& p) {
  39     Point3D pnew;
  40     double tmp;
  41
  42     tmp = cos( p.lat() ) * p.radius();
  43
  44     pnew.setvals( cos( p.lon() ) * tmp,
  45                   sin( p.lon() ) * tmp,
  46                   sin( p.lat() ) * p.radius() );
  47
  48     return(pnew);
  49 }
  50
  51
  52 /* Convert a cartesian coordinate to polar coordinates (lon/lat
  53  * specified in radians.  Distances are specified in meters. */
  54 Point3D fgCartToPolar3d(const Point3D& cp) {
  55     Point3D pp;
  56
  57     pp.setvals( atan2( cp.y(), cp.x() ),
  58                 FG_PI_2 - atan2( sqrt(cp.x()*cp.x() + cp.y()*cp.y()), cp.z() ),
  59                 sqrt(cp.x()*cp.x() + cp.y()*cp.y() + cp.z()*cp.z()) );
  60
  61     /* printf("lon = %.2f  lat = %.2f  radius = %.2f\n",
  62               pp.lon, pp.lat, pp.radius); */
  63
  64     return(pp);
  65 }
  66
  67
  68 /* Find the Altitude above the Ellipsoid (WGS84) given the Earth
  69  * Centered Cartesian coordinate vector Distances are specified in
  70  * meters. */
  71 double fgGeodAltFromCart(const Point3D& cp)
  72 {
  73     double t_lat, x_alpha, mu_alpha;
  74     double lat_geoc, radius;
  75     double result;
  76
  77     lat_geoc = FG_PI_2 - atan2( sqrt(cp.x()*cp.x() + cp.y()*cp.y()), cp.z() );
  78     radius = sqrt( cp.x()*cp.x() + cp.y()*cp.y() + cp.z()*cp.z() );
  79
  80     if( ( (FG_PI_2 - lat_geoc) < ONE_SECOND )    /* near North pole */
  81         || ( (FG_PI_2 + lat_geoc) < ONE_SECOND ) )   /* near South pole */
  82     {
  83         result = radius - EQUATORIAL_RADIUS_M*E;
  84     } else {
  85         t_lat = tan(lat_geoc);
  86         x_alpha = E*EQUATORIAL_RADIUS_M/sqrt(t_lat*t_lat + E*E);
  87         mu_alpha = atan2(sqrt(RESQ_M - x_alpha*x_alpha),E*x_alpha);
  88         if (lat_geoc < 0) {
  89             mu_alpha = - mu_alpha;
  90         }
  91         result = (radius - x_alpha/cos(lat_geoc))*cos(mu_alpha - lat_geoc);
  92     }
  93
  94     return(result);
  95 }
  96
  97
  98 /* $Log$
  99 /* Revision 1.3  1998/10/16 00:50:29  curt
 100 /* Added point3d.hxx to replace cheezy fgPoint3d struct.
 101 /*
 102  * Revision 1.2  1998/08/24 20:04:11  curt
 103  * Various "inline" code optimizations contributed by Norman Vine.
 104  *
 105  * Revision 1.1  1998/07/08 14:40:08  curt
 106  * polar3d.[ch] renamed to polar3d.[ch]xx, vector.[ch] renamed to vector.[ch]xx
 107  * Updated fg_geodesy comments to reflect that routines expect and produce
 108  *   meters.
 109  *
 110  * Revision 1.2  1998/05/03 00:45:49  curt
 111  * Commented out a debugging printf.
 112  *
 113  * Revision 1.1  1998/05/02 01:50:11  curt
 114  * polar.[ch] renamed to polar3d.[ch]
 115  *
 116  * Revision 1.6  1998/04/25 22:06:23  curt
 117  * Edited cvs log messages in source files ... bad bad bad!
 118  *
 119  * Revision 1.5  1998/01/27 00:48:00  curt
 120  * Incorporated Paul Bleisch's <pbleisch@acm.org> new debug message
 121  * system and commandline/config file processing code.
 122  *
 123  * Revision 1.4  1998/01/19 19:27:12  curt
 124  * Merged in make system changes from Bob Kuehne <rpk@sgi.com>
 125  * This should simplify things tremendously.
 126  *
 127  * Revision 1.3  1997/12/15 23:54:54  curt
 128  * Add xgl wrappers for debugging.
 129  * Generate terrain normals on the fly.
 130  *
 131  * Revision 1.2  1997/07/31 22:52:27  curt
 132  * Working on redoing internal coordinate systems & scenery transformations.
 133  *
 134  * Revision 1.1  1997/07/07 21:02:36  curt
 135  * Initial revision.
 136  * */