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 (jon@jsbsim.org) ------------------
   9  -------           (C) 2004  Mathias Froehlich (Mathias.Froehlich@web.de) ----
  10  -------           (C) 2011  Ola Røer Thorsen (ola@silentwings.no) -----------
  11
  12  This program is free software; you can redistribute it and/or modify it under
  13  the terms of the GNU Lesser General Public License as published by the Free Software
  14  Foundation; either version 2 of the License, or (at your option) any later
  15  version.
  16
  17  This program is distributed in the hope that it will be useful, but WITHOUT
  18  ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
  19  FOR A PARTICULAR PURPOSE.  See the GNU Lesser General Public License for more
  20  details.
  21
  22  You should have received a copy of the GNU Lesser General Public License along with
  23  this program; if not, write to the Free Software Foundation, Inc., 59 Temple
  24  Place - Suite 330, Boston, MA  02111-1307, USA.
  25
  26  Further information about the GNU Lesser General Public License can also be found on
  27  the world wide web at http://www.gnu.org.
  28
  29 FUNCTIONAL DESCRIPTION
  30 ------------------------------------------------------------------------------
  31 This class encapsulates an arbitrary position in the globe with its accessors.
  32 It has vector properties, so you can add multiply ....
  33
  34 HISTORY
  35 ------------------------------------------------------------------------------
  36 04/04/2004   MF    Created
  37 11/01/2011   ORT   Encapsulated ground callback code in FGLocation and removed
  38                    it from FGFDMExec.
  39
  40 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  41 INCLUDES
  42 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
  43
  44 #include <cmath>
  45
  46 #include "FGLocation.h"
  47 #include "input_output/FGPropertyManager.h"
  48
  49 namespace JSBSim {
  50
  51 static const char *IdSrc = "$Id: FGLocation.cpp,v 1.26 2011/11/06 18:14:51 bcoconni Exp $";
  52 static const char *IdHdr = ID_LOCATION;
  53 using std::cerr;
  54 using std::endl;
  55
  56 // Set up the default ground callback object.
  57 FGGroundCallback_ptr FGLocation::GroundCallback = NULL;
  58
  59 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  60 CLASS IMPLEMENTATION
  61 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
  62
  63 FGLocation::FGLocation(void)
  64 {
  65   mCacheValid = false;
  66
  67   a = b = a2 = b2 = 0.0;
  68   e = e2 = f = 1.0;
  69   eps2 = -1.0;
  70   epa = 0.0;
  71
  72   mLon = mLat = mRadius = 0.0;
  73   mGeodLat = GeodeticAltitude = initial_longitude = 0.0;
  74
  75   mTl2ec.InitMatrix();
  76   mTec2l.InitMatrix();
  77   mTi2ec.InitMatrix();
  78   mTec2i.InitMatrix();
  79   mTi2l.InitMatrix();
  80   mTl2i.InitMatrix();
  81   mECLoc.InitMatrix();
  82 }
  83
  84 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  85
  86 FGLocation::FGLocation(double lon, double lat, double radius)
  87 {
  88
  89   a = b = a2 = b2 = 0.0;
  90   e = e2 = f = 1.0;
  91   eps2 = -1.0;
  92   epa = 0.0;
  93
  94   mLon = mLat = mRadius = 0.0;
  95   mGeodLat = GeodeticAltitude = initial_longitude = 0.0;
  96
  97   mTl2ec.InitMatrix();
  98   mTec2l.InitMatrix();
  99   mTi2ec.InitMatrix();
 100   mTec2i.InitMatrix();
 101   mTi2l.InitMatrix();
 102   mTl2i.InitMatrix();
 103
 104   double sinLat = sin(lat);
 105   double cosLat = cos(lat);
 106   double sinLon = sin(lon);
 107   double cosLon = cos(lon);
 108   mECLoc = FGColumnVector3( radius*cosLat*cosLon,
 109                             radius*cosLat*sinLon,
 110                             radius*sinLat );
 111 }
 112
 113 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 114
 115 FGLocation::FGLocation(const FGColumnVector3& lv) : mECLoc(lv), mCacheValid(false)
 116 {
 117   a = b = a2 = b2 = 0.0;
 118   e = e2 = f = 1.0;
 119   eps2 = -1.0;
 120   epa = 0.0;
 121
 122   mLon = mLat = mRadius = 0.0;
 123   mGeodLat = GeodeticAltitude = initial_longitude = 0.0;
 124
 125   mTl2ec.InitMatrix();
 126   mTec2l.InitMatrix();
 127   mTi2ec.InitMatrix();
 128   mTec2i.InitMatrix();
 129   mTi2l.InitMatrix();
 130   mTl2i.InitMatrix();
 131 }
 132
 133 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 134
 135 FGLocation::FGLocation(const FGLocation& l)
 136   : mECLoc(l.mECLoc), mCacheValid(l.mCacheValid)
 137 {
 138
 139   a = l.a;
 140   b = l.b;
 141   a2 = l.a2;
 142   b2 = l.b2;
 143   e2 = l.e2;
 144   e = l.e;
 145   eps2 = l.eps2;
 146   f = l.f;
 147   epa = l.epa;
 148
 149   /*ag
 150    * if the cache is not valid, all of the following values are unset.
 151    * They will be calculated once ComputeDerivedUnconditional is called.
 152    * If unset, they may possibly contain NaN and could thus trigger floating
 153    * point exceptions.
 154    */
 155   if (!mCacheValid) return;
 156
 157   mLon = l.mLon;
 158   mLat = l.mLat;
 159   mRadius = l.mRadius;
 160
 161   mTl2ec = l.mTl2ec;
 162   mTec2l = l.mTec2l;
 163   mTi2ec = l.mTi2ec;
 164   mTec2i = l.mTec2i;
 165   mTi2l = l.mTi2l;
 166   mTl2i = l.mTl2i;
 167
 168   initial_longitude = l.initial_longitude;
 169   mGeodLat = l.mGeodLat;
 170   GeodeticAltitude = l.GeodeticAltitude;
 171 }
 172
 173 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 174
 175 const FGLocation& FGLocation::operator=(const FGLocation& l)
 176 {
 177   mECLoc = l.mECLoc;
 178   mCacheValid = l.mCacheValid;
 179
 180   a = l.a;
 181   b = l.b;
 182   a2 = l.a2;
 183   b2 = l.b2;
 184   e2 = l.e2;
 185   e = l.e;
 186   eps2 = l.eps2;
 187   f = l.f;
 188   epa = l.epa;
 189
 190   //ag See comment in constructor above
 191   if (!mCacheValid) return *this;
 192
 193   mLon = l.mLon;
 194   mLat = l.mLat;
 195   mRadius = l.mRadius;
 196
 197   mTl2ec = l.mTl2ec;
 198   mTec2l = l.mTec2l;
 199   mTi2ec = l.mTi2ec;
 200   mTec2i = l.mTec2i;
 201   mTi2l = l.mTi2l;
 202   mTl2i = l.mTl2i;
 203
 204   initial_longitude = l.initial_longitude;
 205   mGeodLat = l.mGeodLat;
 206   GeodeticAltitude = l.GeodeticAltitude;
 207
 208   return *this;
 209 }
 210
 211 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 212
 213 void FGLocation::SetLongitude(double longitude)
 214 {
 215   double rtmp = mECLoc.Magnitude(eX, eY);
 216   // Check if we have zero radius.
 217   // If so set it to 1, so that we can set a position
 218   if (0.0 == mECLoc.Magnitude())
 219     rtmp = 1.0;
 220
 221   // Fast return if we are on the north or south pole ...
 222   if (rtmp == 0.0)
 223     return;
 224
 225   mCacheValid = false;
 226
 227   // Need to figure out how to set the initial_longitude here
 228
 229   mECLoc(eX) = rtmp*cos(longitude);
 230   mECLoc(eY) = rtmp*sin(longitude);
 231 }
 232
 233 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 234
 235 void FGLocation::SetLatitude(double latitude)
 236 {
 237   mCacheValid = false;
 238
 239   double r = mECLoc.Magnitude();
 240   if (r == 0.0) {
 241     mECLoc(eX) = 1.0;
 242     r = 1.0;
 243   }
 244
 245   double rtmp = mECLoc.Magnitude(eX, eY);
 246   if (rtmp != 0.0) {
 247     double fac = r/rtmp*cos(latitude);
 248     mECLoc(eX) *= fac;
 249     mECLoc(eY) *= fac;
 250   } else {
 251     mECLoc(eX) = r*cos(latitude);
 252     mECLoc(eY) = 0.0;
 253   }
 254   mECLoc(eZ) = r*sin(latitude);
 255 }
 256
 257 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 258
 259 void FGLocation::SetRadius(double radius)
 260 {
 261   mCacheValid = false;
 262
 263   double rold = mECLoc.Magnitude();
 264   if (rold == 0.0)
 265     mECLoc(eX) = radius;
 266   else
 267     mECLoc *= radius/rold;
 268 }
 269
 270 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 271
 272 void FGLocation::SetPosition(double lon, double lat, double radius)
 273 {
 274   mCacheValid = false;
 275
 276   double sinLat = sin(lat);
 277   double cosLat = cos(lat);
 278   double sinLon = sin(lon);
 279   double cosLon = cos(lon);
 280 //  initial_longitude = lon;
 281   mECLoc = FGColumnVector3( radius*cosLat*cosLon,
 282                             radius*cosLat*sinLon,
 283                             radius*sinLat );
 284 }
 285
 286 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 287
 288 void FGLocation::SetPositionGeodetic(double lon, double lat, double height)
 289 {
 290   mCacheValid = false;
 291
 292   mGeodLat = lat;
 293   mLon = lon;
 294   GeodeticAltitude = height;
 295
 296 //  initial_longitude = mLon;
 297
 298   double RN = a / sqrt(1.0 - e2*sin(mGeodLat)*sin(mGeodLat));
 299
 300   mECLoc(eX) = (RN + GeodeticAltitude)*cos(mGeodLat)*cos(mLon);
 301   mECLoc(eY) = (RN + GeodeticAltitude)*cos(mGeodLat)*sin(mLon);
 302   mECLoc(eZ) = ((1 - e2)*RN + GeodeticAltitude)*sin(mGeodLat);
 303 }
 304
 305 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 306
 307 void FGLocation::SetEllipse(double semimajor, double semiminor)
 308 {
 309   mCacheValid = false;
 310
 311   a = semimajor;
 312   b = semiminor;
 313   a2 = a*a;
 314   b2 = b*b;
 315   e2 = 1.0 - b2/a2;
 316   e = sqrt(e2);
 317   eps2 = a2/b2 - 1.0;
 318   f = 1.0 - b/a;
 319 }
 320
 321 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 322 // Compute the ECEF to ECI transformation matrix using Stevens and Lewis "Aircraft
 323 // Control and Simulation", second edition, eqn. 1.4-12, pg. 39. In Stevens and Lewis
 324 // notation, this is C_i/e, a transformation from ECEF to ECI.
 325
 326 const FGMatrix33& FGLocation::GetTec2i(void)
 327 {
 328   ComputeDerived();
 329   return mTec2i;
 330 }
 331
 332 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 333 // This is given in Stevens and Lewis "Aircraft
 334 // Control and Simulation", second edition, eqn. 1.4-12, pg. 39
 335 // The notation in Stevens and Lewis is: C_e/i. This represents a transformation
 336 // from ECI to ECEF - and the orientation of the ECEF frame relative to the ECI
 337 // frame.
 338
 339 const FGMatrix33& FGLocation::GetTi2ec(void)
 340 {
 341   ComputeDerived();
 342   return mTi2ec;
 343 }
 344
 345 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 346
 347 void FGLocation::ComputeDerivedUnconditional(void) const
 348 {
 349   // The radius is just the Euclidean norm of the vector.
 350   mRadius = mECLoc.Magnitude();
 351
 352   // The distance of the location to the Z-axis, which is the axis
 353   // through the poles.
 354   double r02 = mECLoc(eX)*mECLoc(eX) + mECLoc(eY)*mECLoc(eY);
 355   double rxy = sqrt(r02);
 356
 357   // Compute the sin/cos values of the longitude
 358   double sinLon, cosLon;
 359   if (rxy == 0.0) {
 360     sinLon = 0.0;
 361     cosLon = 1.0;
 362   } else {
 363     sinLon = mECLoc(eY)/rxy;
 364     cosLon = mECLoc(eX)/rxy;
 365   }
 366
 367   // Compute the sin/cos values of the latitude
 368   double sinLat, cosLat;
 369   if (mRadius == 0.0)  {
 370     sinLat = 0.0;
 371     cosLat = 1.0;
 372   } else {
 373     sinLat = mECLoc(eZ)/mRadius;
 374     cosLat = rxy/mRadius;
 375   }
 376
 377   // Compute the longitude and latitude itself
 378   if ( mECLoc( eX ) == 0.0 && mECLoc( eY ) == 0.0 )
 379     mLon = 0.0;
 380   else
 381     mLon = atan2( mECLoc( eY ), mECLoc( eX ) );
 382
 383   if ( rxy == 0.0 && mECLoc( eZ ) == 0.0 )
 384     mLat = 0.0;
 385   else
 386     mLat = atan2( mECLoc(eZ), rxy );
 387
 388   // Compute the transform matrices from and to the earth centered frame.
 389   // See Stevens and Lewis, "Aircraft Control and Simulation", Second Edition,
 390   // Eqn. 1.4-13, page 40. In Stevens and Lewis notation, this is C_n/e - the
 391   // orientation of the navigation (local) frame relative to the ECEF frame,
 392   // and a transformation from ECEF to nav (local) frame.
 393
 394   mTec2l = FGMatrix33( -cosLon*sinLat, -sinLon*sinLat,  cosLat,
 395                            -sinLon   ,     cosLon    ,    0.0 ,
 396                        -cosLon*cosLat, -sinLon*cosLat, -sinLat  );
 397
 398   // In Stevens and Lewis notation, this is C_e/n - the
 399   // orientation of the ECEF frame relative to the nav (local) frame,
 400   // and a transformation from nav (local) to ECEF frame.
 401
 402   mTl2ec = mTec2l.Transposed();
 403
 404   // Calculate the inertial to ECEF and transpose matrices
 405   double cos_epa = cos(epa);
 406   double sin_epa = sin(epa);
 407   mTi2ec = FGMatrix33( cos_epa, sin_epa, 0.0,
 408                       -sin_epa, cos_epa, 0.0,
 409                            0.0,      0.0, 1.0 );
 410   mTec2i = mTi2ec.Transposed();
 411
 412   // Now calculate the local (or nav, or ned) frame to inertial transform matrix,
 413   // and the inverse.
 414   mTl2i = mTec2i * mTl2ec;
 415   mTi2l = mTl2i.Transposed();
 416
 417   // Calculate the geodetic latitude base on AIAA Journal of Guidance and Control paper,
 418   // "Improved Method for Calculating Exact Geodetic Latitude and Altitude", and
 419   // "Improved Method for Calculating Exact Geodetic Latitude and Altitude, Revisited",
 420   // author: I. Sofair
 421
 422   if (a != 0.0 && b != 0.0) {
 423     double c, p, q, s, t, u, v, w, z, p2, u2, r0;
 424     double Ne, P, Q0, Q, signz0, sqrt_q, z_term;
 425     p  = fabs(mECLoc(eZ))/eps2;
 426     s  = r02/(e2*eps2);
 427     p2 = p*p;
 428     q  = p2 - b2 + s;
 429     if (q>0)
 430     {
 431       sqrt_q = sqrt(q);
 432       u  = p/sqrt_q;
 433       u2 = p2/q;
 434       v  = b2*u2/q;
 435       P  = 27.0*v*s/q;
 436       Q0 = sqrt(P+1) + sqrt(P);
 437       Q  = pow(Q0, 0.66666666667);
 438       t  = (1.0 + Q + 1.0/Q)/6.0;
 439       c  = sqrt(u2 - 1 + 2.0*t);
 440       w  = (c - u)/2.0;
 441       signz0 = mECLoc(eZ)>=0?1.0:-1.0;
 442       z_term = sqrt(t*t+v)-u*w-0.5*t-0.25;
 443       if (z_term < 0.0) {
 444         z = 0.0;
 445       } else {
 446         z  = signz0*sqrt_q*(w+sqrt(z_term));
 447       }
 448       Ne = a*sqrt(1+eps2*z*z/b2);
 449       mGeodLat = asin((eps2+1.0)*(z/Ne));
 450       r0 = rxy;
 451       GeodeticAltitude = r0*cos(mGeodLat) + mECLoc(eZ)*sin(mGeodLat) - a2/Ne;
 452     }
 453   }
 454
 455   // Mark the cached values as valid
 456   mCacheValid = true;
 457 }
 458
 459 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 460
 461 } // namespace JSBSim