src/FDM/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 HISTORY
  34 --------------------------------------------------------------------------------
  35 01/09/99   JSB   Created
  36
  37 ********************************************************************************
  38 DEFINES
  39 *******************************************************************************/
  40
  41 /*******************************************************************************
  42 INCLUDES
  43 *******************************************************************************/
  44
  45 #ifdef FGFS
  46 #  include <Include/compiler.h>
  47 #  ifdef FG_HAVE_STD_INCLUDES
  48 #    include <cmath>
  49 #  else
  50 #    include <math.h>
  51 #  endif
  52 #else
  53 #  include <cmath>
  54 #endif
  55
  56 #include "FGUtility.h"
  57 #include "FGState.h"
  58 #include "FGFDMExec.h"
  59
  60 #ifndef M_PI_2
  61 /* get a definition for pi */
  62 #include <Include/fg_constants.h>
  63 #define M_PI_2 FG_PI_2
  64 #endif
  65
  66 /*******************************************************************************
  67 ************************************ CODE **************************************
  68 *******************************************************************************/
  69
  70 FGUtility::FGUtility()
  71 {
  72   // Move constant stuff to here (if any) so it won't take CPU time
  73   // in the methods below.
  74   SeaLevelR   = EARTHRAD * ECCENT;
  75 }
  76
  77
  78 FGUtility::~FGUtility()
  79 {
  80 }
  81
  82
  83 float FGUtility::ToGeodetic()
  84 {
  85   float Latitude, Radius, Altitude;
  86   float tanLat, xAlpha, muAlpha, sinmuAlpha, denom, rhoAlpha, dMu;
  87   float lPoint, lambdaSL, sinlambdaSL, dLambda, rAlpha;
  88
  89   Latitude = State->Getlatitude();
  90   Radius = State->Geth() + EARTHRAD;
  91
  92   if (( M_PI_2 - Latitude < ONESECOND) ||
  93       ( M_PI_2 + Latitude < ONESECOND)) { // Near a pole
  94   } else {
  95     tanLat = tan(Latitude);
  96     xAlpha = ECCENT*EARTHRAD /
  97                                 sqrt(tanLat*tanLat + ECCENTSQRD);
  98     muAlpha = atan2(sqrt(EARTHRADSQRD - xAlpha*xAlpha), ECCENT*xAlpha);
  99
 100     if (Latitude < 0.0) muAlpha = -muAlpha;
 101
 102     sinmuAlpha  = sin(muAlpha);
 103     dLambda     = muAlpha - Latitude;
 104     rAlpha      = xAlpha / cos(Latitude);
 105     lPoint      = Radius - rAlpha;
 106     Altitude    = lPoint*cos(dLambda);
 107     denom       = sqrt(1-EPS*EPS*sinmuAlpha*sinmuAlpha);
 108     rhoAlpha    = EARTHRAD*(1.0 - EPS) / (denom*denom*denom);
 109     dMu         = atan2(lPoint*sin(dLambda),rhoAlpha + Altitude);
 110     State->SetGeodeticLat(muAlpha - dMu);
 111     lambdaSL    = atan(ECCENTSQRD*tan(muAlpha - dMu));
 112     sinlambdaSL = sin(lambdaSL);
 113     SeaLevelR   = sqrt(EARTHRADSQRD / (1 + INVECCENTSQRDM1* sinlambdaSL*sinlambdaSL));
 114   }
 115   return 0.0;
 116 }
 117
 118
 119 float FGUtility:: FromGeodetic()
 120 {
 121   float lambdaSL, sinlambdaSL, coslambdaSL, sinMu, cosMu, py, px;
 122   float Altitude, SeaLevelR, Radius;
 123
 124   Radius = State->Geth() + EARTHRAD;
 125   lambdaSL = atan(ECCENTSQRD*tan(State->GetGeodeticLat()));
 126   sinlambdaSL = sin(lambdaSL);
 127   coslambdaSL = cos(lambdaSL);
 128   sinMu = sin(State->GetGeodeticLat());
 129   cosMu = cos(State->GetGeodeticLat());
 130   SeaLevelR = sqrt(EARTHRADSQRD /
 131              (1 + INVECCENTSQRDM1*sinlambdaSL*sinlambdaSL));
 132   Altitude  = Radius - SeaLevelR;
 133   px = SeaLevelR*coslambdaSL + Altitude*cosMu;
 134   py = SeaLevelR*sinlambdaSL + Altitude*sinMu;
 135   State->Setlatitude(atan2(py,px));
 136   return 0.0;
 137 }
 138