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