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