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 /*******************************************************************************
  61 ************************************ CODE **************************************
  62 *******************************************************************************/
  63
  64 FGUtility::FGUtility()
  65 {
  66   // Move constant stuff to here (if any) so it won't take CPU time
  67   // in the methods below.
  68   SeaLevelR   = EARTHRAD * ECCENT;
  69 }
  70
  71
  72 FGUtility::~FGUtility()
  73 {
  74 }
  75
  76
  77 float FGUtility::ToGeodetic()
  78 {
  79   float Latitude, Radius, Altitude;
  80   float tanLat, xAlpha, muAlpha, sinmuAlpha, denom, rhoAlpha, dMu;
  81   float lPoint, lambdaSL, sinlambdaSL, dLambda, rAlpha;
  82
  83   Latitude = State->Getlatitude();
  84   Radius = State->Geth() + EARTHRAD;
  85
  86   if (( M_PI_2 - Latitude < ONESECOND) ||
  87       ( M_PI_2 + Latitude < ONESECOND)) { // Near a pole
  88   } else {
  89     tanLat = tan(Latitude);
  90     xAlpha = ECCENT*EARTHRAD /
  91                                 sqrt(tanLat*tanLat + ECCENTSQRD);
  92     muAlpha = atan2(sqrt(EARTHRADSQRD - xAlpha*xAlpha), ECCENT*xAlpha);
  93
  94     if (Latitude < 0.0) muAlpha = -muAlpha;
  95
  96     sinmuAlpha  = sin(muAlpha);
  97     dLambda     = muAlpha - Latitude;
  98     rAlpha      = xAlpha / cos(Latitude);
  99     lPoint      = Radius - rAlpha;
 100     Altitude    = lPoint*cos(dLambda);
 101     denom       = sqrt(1-EPS*EPS*sinmuAlpha*sinmuAlpha);
 102     rhoAlpha    = EARTHRAD*(1.0 - EPS) / (denom*denom*denom);
 103     dMu         = atan2(lPoint*sin(dLambda),rhoAlpha + Altitude);
 104     State->SetGeodeticLat(muAlpha - dMu);
 105     lambdaSL    = atan(ECCENTSQRD*tan(muAlpha - dMu));
 106     sinlambdaSL = sin(lambdaSL);
 107     SeaLevelR   = sqrt(EARTHRADSQRD / (1 + INVECCENTSQRDM1* sinlambdaSL*sinlambdaSL));
 108   }
 109   return 0.0;
 110 }
 111
 112
 113 float FGUtility:: FromGeodetic()
 114 {
 115   float lambdaSL, sinlambdaSL, coslambdaSL, sinMu, cosMu, py, px;
 116   float Altitude, SeaLevelR, Radius;
 117
 118   Radius = State->Geth() + EARTHRAD;
 119   lambdaSL = atan(ECCENTSQRD*tan(State->GetGeodeticLat()));
 120   sinlambdaSL = sin(lambdaSL);
 121   coslambdaSL = cos(lambdaSL);
 122   sinMu = sin(State->GetGeodeticLat());
 123   cosMu = cos(State->GetGeodeticLat());
 124   SeaLevelR = sqrt(EARTHRADSQRD /
 125              (1 + INVECCENTSQRDM1*sinlambdaSL*sinlambdaSL));
 126   Altitude  = Radius - SeaLevelR;
 127   px = SeaLevelR*coslambdaSL + Altitude*cosMu;
 128   py = SeaLevelR*sinlambdaSL + Altitude*sinMu;
 129   State->Setlatitude(atan2(py,px));
 130   return 0.0;
 131 }
 132