src/FDM/JSBSim/math/FGLocation.cpp

   1 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
   2
   3  Module:       FGLocation.cpp
   4  Author:       Jon S. Berndt
   5  Date started: 04/04/2004
   6  Purpose:      Store an arbitrary location on the globe
   7
   8  ------- Copyright (C) 1999  Jon S. Berndt (jsb@hal-pc.org) ------------------
   9  -------           (C) 2004  Mathias Froehlich (Mathias.Froehlich@web.de) ----
  10
  11  This program is free software; you can redistribute it and/or modify it under
  12  the terms of the GNU Lesser 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 Lesser General Public License for more
  19  details.
  20
  21  You should have received a copy of the GNU Lesser 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 Lesser 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 encapsulates an arbitrary position in the globe with its accessors.
  31 It has vector properties, so you can add multiply ....
  32
  33 HISTORY
  34 ------------------------------------------------------------------------------
  35 04/04/2004   MF    Created
  36
  37 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  38 INCLUDES
  39 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
  40
  41 #include <cmath>
  42
  43 #include "FGLocation.h"
  44 #include <input_output/FGPropertyManager.h>
  45
  46 namespace JSBSim {
  47
  48 static const char *IdSrc = "$Id$";
  49 static const char *IdHdr = ID_LOCATION;
  50
  51 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  52 CLASS IMPLEMENTATION
  53 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
  54
  55 FGLocation::FGLocation(void)
  56 {
  57   mCacheValid = false;
  58   initial_longitude = 0.0;
  59   a = 0.0;
  60   b = 0.0;
  61   a2 = 0.0;
  62   b2 = 0.0;
  63   e2 = 1.0;
  64   e = 1.0;
  65   eps2 = -1.0;
  66   f = 1.0;
  67 }
  68
  69 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  70
  71 FGLocation::FGLocation(double lon, double lat, double radius)
  72 {
  73   mCacheValid = false;
  74
  75   double sinLat = sin(lat);
  76   double cosLat = cos(lat);
  77   double sinLon = sin(lon);
  78   double cosLon = cos(lon);
  79   initial_longitude = lon;
  80   a = 0.0;
  81   b = 0.0;
  82   a2 = 0.0;
  83   b2 = 0.0;
  84   e2 = 1.0;
  85   e = 1.0;
  86   eps2 = -1.0;
  87   f = 1.0;
  88   mECLoc = FGColumnVector3( radius*cosLat*cosLon,
  89                             radius*cosLat*sinLon,
  90                             radius*sinLat );
  91 }
  92
  93 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  94
  95 void FGLocation::SetLongitude(double longitude)
  96 {
  97   double rtmp = mECLoc.Magnitude(eX, eY);
  98   // Check if we have zero radius.
  99   // If so set it to 1, so that we can set a position
 100   if (0.0 == mECLoc.Magnitude())
 101     rtmp = 1.0;
 102
 103   // Fast return if we are on the north or south pole ...
 104   if (rtmp == 0.0)
 105     return;
 106
 107   mCacheValid = false;
 108
 109   // Need to figure out how to set the initial_longitude here
 110
 111   mECLoc(eX) = rtmp*cos(longitude);
 112   mECLoc(eY) = rtmp*sin(longitude);
 113 }
 114
 115 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 116
 117 void FGLocation::SetLatitude(double latitude)
 118 {
 119   mCacheValid = false;
 120
 121   double r = mECLoc.Magnitude();
 122   if (r == 0.0) {
 123     mECLoc(eX) = 1.0;
 124     r = 1.0;
 125   }
 126
 127   double rtmp = mECLoc.Magnitude(eX, eY);
 128   if (rtmp != 0.0) {
 129     double fac = r/rtmp*cos(latitude);
 130     mECLoc(eX) *= fac;
 131     mECLoc(eY) *= fac;
 132   } else {
 133     mECLoc(eX) = r*cos(latitude);
 134     mECLoc(eY) = 0.0;
 135   }
 136   mECLoc(eZ) = r*sin(latitude);
 137 }
 138
 139 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 140
 141 void FGLocation::SetRadius(double radius)
 142 {
 143   mCacheValid = false;
 144
 145   double rold = mECLoc.Magnitude();
 146   if (rold == 0.0)
 147     mECLoc(eX) = radius;
 148   else
 149     mECLoc *= radius/rold;
 150 }
 151
 152 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 153
 154 void FGLocation::SetPosition(double lon, double lat, double radius)
 155 {
 156   mCacheValid = false;
 157
 158   double sinLat = sin(lat);
 159   double cosLat = cos(lat);
 160   double sinLon = sin(lon);
 161   double cosLon = cos(lon);
 162   initial_longitude = lon;
 163   mECLoc = FGColumnVector3( radius*cosLat*cosLon,
 164                             radius*cosLat*sinLon,
 165                             radius*sinLat );
 166   ComputeDerived();
 167 }
 168
 169 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 170
 171 void FGLocation::SetEllipse(double semimajor, double semiminor)
 172 {
 173   mCacheValid = false;
 174
 175   a = semimajor;
 176   b = semiminor;
 177   a2 = a*a;
 178   b2 = b*b;
 179   e2 = 1.0 - b2/a2;
 180   e = sqrt(e2);
 181   eps2 = a2/b2 - 1.0;
 182   f = 1.0 - b/a;
 183 }
 184
 185 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 186 // Compute the ECEF to ECI transformation matrix using Stevens and Lewis "Aircraft
 187 // Control and Simulation", second edition, eqn. 1.4-12, pg. 39
 188
 189 const FGMatrix33& FGLocation::GetTec2i(double epa)
 190 {
 191   double mu = epa - initial_longitude;
 192   mTec2i = FGMatrix33( cos(mu), -sin(mu), 0.0,
 193                        sin(mu),  cos(mu), 0.0,
 194                             0.0,     0.0, 1.0 );
 195   return mTec2i;
 196 }
 197
 198 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 199
 200 const FGMatrix33& FGLocation::GetTi2ec(double epa)
 201 {
 202   double mu = epa - initial_longitude;
 203   mTi2ec = FGMatrix33( cos(mu), sin(mu), 0.0,
 204                       -sin(mu), cos(mu), 0.0,
 205                            0.0,      0.0, 1.0 );
 206   return mTi2ec;
 207 }
 208
 209 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 210
 211 void FGLocation::ComputeDerivedUnconditional(void) const
 212 {
 213   // The radius is just the Euclidean norm of the vector.
 214   mRadius = mECLoc.Magnitude();
 215
 216   // The distance of the location to the Z-axis, which is the axis
 217   // through the poles.
 218   double rxy = sqrt(mECLoc(eX)*mECLoc(eX) + mECLoc(eY)*mECLoc(eY));
 219
 220   // Compute the sin/cos values of the longitude
 221   double sinLon, cosLon;
 222   if (rxy == 0.0) {
 223     sinLon = 0.0;
 224     cosLon = 1.0;
 225   } else {
 226     sinLon = mECLoc(eY)/rxy;
 227     cosLon = mECLoc(eX)/rxy;
 228   }
 229
 230   // Compute the sin/cos values of the latitude
 231   double sinLat, cosLat;
 232   if (mRadius == 0.0)  {
 233     sinLat = 0.0;
 234     cosLat = 1.0;
 235   } else {
 236     sinLat = mECLoc(eZ)/mRadius;
 237     cosLat = rxy/mRadius;
 238   }
 239
 240   // Compute the longitude and latitude itself
 241   if ( mECLoc( eX ) == 0.0 && mECLoc( eY ) == 0.0 )
 242     mLon = 0.0;
 243   else
 244     mLon = atan2( mECLoc( eY ), mECLoc( eX ) );
 245
 246   if ( rxy == 0.0 && mECLoc( eZ ) == 0.0 )
 247     mLat = 0.0;
 248   else
 249     mLat = atan2( mECLoc(eZ), rxy );
 250
 251   // Compute the transform matrices from and to the earth centered frame.
 252   // See Stevens and Lewis, "Aircraft Control and Simulation", Second Edition,
 253   // Eqn. 1.4-13, page 40.
 254   mTec2l = FGMatrix33( -cosLon*sinLat, -sinLon*sinLat,  cosLat,
 255                            -sinLon   ,     cosLon    ,    0.0 ,
 256                        -cosLon*cosLat, -sinLon*cosLat, -sinLat  );
 257
 258   mTl2ec = mTec2l.Transposed();
 259
 260   // Calculate the geodetic latitude base on AIAA Journal of Guidance and Control paper,
 261   // "Improved Method for Calculating Exact Geodetic Latitude and Altitude", and
 262   // "Improved Method for Calculating Exact Geodetic Latitude and Altitude, Revisited",
 263   // author: I. Sofair
 264
 265   if (a != 0.0 && b != 0.0) {
 266     double c, p, q, s, t, u, v, w, z, p2, u2, r02, r0;
 267     double Ne, P, Q0, Q, signz0, sqrt_q;
 268     p  = fabs(mECLoc(eZ))/eps2;
 269     r02 = mECLoc(eX)*mECLoc(eX) + mECLoc(eY)*mECLoc(eY);
 270     s  = r02/(e2*eps2);
 271     p2 = p*p;
 272     q  = p2 - b2 + s;
 273     sqrt_q = sqrt(q);
 274     if (q>0)
 275     {
 276       u  = p/sqrt_q;
 277       u2 = p2/q;
 278       v  = b2*u2/q;
 279       P  = 27.0*v*s/q;
 280       Q0 = sqrt(P+1) + sqrt(P);
 281       Q  = pow(Q0, 0.66666666667);
 282       t  = (1.0 + Q + 1.0/Q)/6.0;
 283       c  = sqrt(u2 - 1 + 2.0*t);
 284       w  = (c - u)/2.0;
 285       signz0 = mECLoc(eZ)>=0?1.0:-1.0;
 286       z  = signz0*sqrt_q*(w+sqrt(sqrt(t*t+v)-u*w-0.5*t-0.25));
 287       Ne = a*sqrt(1+eps2*z*z/b2);
 288       mGeodLat = asin((eps2+1.0)*(z/Ne));
 289       r0 = sqrt(r02);
 290       GeodeticAltitude = r0*cos(mGeodLat) + mECLoc(eZ)*sin(mGeodLat) - a2/Ne;
 291     }
 292   }
 293
 294   // Mark the cached values as valid
 295   mCacheValid = true;
 296 }
 297
 298 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 299
 300 } // namespace JSBSim