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