JSBsim/FGUtility.cpp

   1 /*******************************************************************************
   2
   3  Module:       FGUtility.cpp
   4  Author:       Jon Berndt
   5  Date started: 01/09/99
   6  Purpose:      Contains utility classes for the FG FDM
   7  Called by:    FGPosition, et. al.
   8
   9  ------------- Copyright (C) 1999  Jon S. Berndt (jsb@hal-pc.org) -------------
  10
  11  This program is free software; you can redistribute it and/or modify it under
  12  the terms of the GNU General Public License as published by the Free Software
  13  Foundation; either version 2 of the License, or (at your option) any later
  14  version.
  15
  16  This program is distributed in the hope that it will be useful, but WITHOUT
  17  ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
  18  FOR A PARTICULAR PURPOSE.  See the GNU General Public License for more
  19  details.
  20
  21  You should have received a copy of the GNU General Public License along with
  22  this program; if not, write to the Free Software Foundation, Inc., 59 Temple
  23  Place - Suite 330, Boston, MA  02111-1307, USA.
  24
  25  Further information about the GNU General Public License can also be found on
  26  the world wide web at http://www.gnu.org.
  27
  28 FUNCTIONAL DESCRIPTION
  29 --------------------------------------------------------------------------------
  30 This class is a container for all utility classes used by the flight dynamics
  31 model.
  32
  33 ARGUMENTS
  34 --------------------------------------------------------------------------------
  35
  36
  37 HISTORY
  38 --------------------------------------------------------------------------------
  39 01/09/99   JSB   Created
  40 *******************************************************************************/
  41
  42 /*******************************************************************************
  43 DEFINES
  44 *******************************************************************************/
  45
  46 /********************************************************************************
  47 INCLUDES
  48 *******************************************************************************/
  49
  50 #include "FGUtility.h"
  51 #include "FGState.h"
  52 #include <math.h>
  53
  54 #ifdef HAVE_NCURSES
  55   #include <ncurses.h>
  56 #endif
  57
  58 /*******************************************************************************
  59 ************************************ CODE **************************************
  60 *******************************************************************************/
  61
  62 const float EarthRadSqrd   = 437882827922500.0;
  63 const float OneSecond      = 4.848136811E-6;
  64 const float Eccentricity   = 0.996647186;
  65 const float EccentSqrd     = Eccentricity*Eccentricity;
  66 const float EPS            = 0.081819221;
  67
  68 FGUtility::FGUtility()
  69 {
  70 }
  71
  72
  73 FGUtility::~FGUtility()
  74 {
  75 }
  76
  77
  78 float FGUtility::ToGeodetic()
  79 {
  80   float GeodeticLat, Latitude, Radius, Altitude, SeaLevelR;
  81   float tanLat, xAlpha, muAlpha, sinmuAlpha, denom, rhoAlpha, dMu;
  82   float lPoint, lambdaSL, sinlambdaSL, dLambda, rAlpha;
  83
  84   Latitude = State->Getlatitude();
  85   Radius = State->Geth() + State->EarthRad;
  86
  87   if (( M_PI_2 - Latitude < OneSecond) ||
  88       ( M_PI_2 + Latitude < OneSecond)) { // Near a pole
  89     GeodeticLat = Latitude;
  90     SeaLevelR   = State->EarthRad * Eccentricity;
  91     Altitude    = Radius - SeaLevelR;
  92   } else {
  93     tanLat = tan(Latitude);
  94     xAlpha = Eccentricity*State->EarthRad /
  95                                 sqrt(tanLat*tanLat + EccentSqrd);
  96     muAlpha = atan2(sqrt(EarthRadSqrd - xAlpha*xAlpha), Eccentricity*xAlpha);
  97
  98     if (Latitude < 0.0) muAlpha = -muAlpha;
  99
 100     sinmuAlpha  = sin(muAlpha);
 101     dLambda     = muAlpha - Latitude;
 102     rAlpha      = xAlpha / cos(Latitude);
 103     lPoint      = Radius - rAlpha;
 104     Altitude    = lPoint*cos(dLambda);
 105     denom       = sqrt(1-EPS*EPS*sinmuAlpha*sinmuAlpha);
 106     rhoAlpha    = State->EarthRad*(1.0 - EPS) / (denom*denom*denom);
 107     dMu         = atan2(lPoint*sin(dLambda),rhoAlpha + Altitude);
 108     State->SetGeodeticLat(muAlpha - dMu);
 109     lambdaSL    = atan(EccentSqrd*tan(GeodeticLat));
 110     sinlambdaSL = sin(lambdaSL);
 111     SeaLevelR   = sqrt(EarthRadSqrd / (1 + (1/EccentSqrd - 1.0)* sinlambdaSL*sinlambdaSL));
 112   }
 113   return 0.0;
 114 }
 115
 116
 117 float FGUtility:: FromGeodetic()
 118 {
 119   float lambdaSL, sinlambdaSL, coslambdaSL, sinMu, cosMu, py, px;
 120   float Altitude, SeaLevelR;
 121
 122   lambdaSL = atan(EccentSqrd*tan(State->GetGeodeticLat()));
 123   sinlambdaSL = sin(lambdaSL);
 124   coslambdaSL = cos(lambdaSL);
 125   sinMu = sin(State->GetGeodeticLat());
 126   cosMu = cos(State->GetGeodeticLat());
 127   SeaLevelR = sqrt(EarthRadSqrd /
 128              (1 + ((1/EccentSqrd)-1)*sinlambdaSL*sinlambdaSL));
 129   px = SeaLevelR*coslambdaSL + Altitude*cosMu;
 130   py = SeaLevelR*sinlambdaSL + Altitude*sinMu;
 131   State->Setlatitude(atan2(py,px));
 132   return 0.0;
 133 }
 134