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