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   initial_longitude = 0.0;
  60   a = 0.0;
  61   b = 0.0;
  62   a2 = 0.0;
  63   b2 = 0.0;
  64   e2 = 1.0;
  65   e = 1.0;
  66   eps2 = -1.0;
  67   f = 1.0;
  68   epa = 0.0;
  69
  70   mLon = mLat = mRadius = mGeodLat = GeodeticAltitude = 0.0;
  71
  72 //  initial_longitude = 0.0;
  73
  74   mTl2ec.InitMatrix();
  75   mTec2l.InitMatrix();
  76   mTi2ec.InitMatrix();
  77   mTec2i.InitMatrix();
  78   mTi2l.InitMatrix();
  79   mTl2i.InitMatrix();
  80   mECLoc.InitMatrix();
  81 }
  82
  83 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  84
  85 FGLocation::FGLocation(double lon, double lat, double radius)
  86 {
  87   mCacheValid = false;
  88
  89   double sinLat = sin(lat);
  90   double cosLat = cos(lat);
  91   double sinLon = sin(lon);
  92   double cosLon = cos(lon);
  93
  94   a = 0.0;
  95   b = 0.0;
  96   a2 = 0.0;
  97   b2 = 0.0;
  98   e2 = 1.0;
  99   e = 1.0;
 100   eps2 = -1.0;
 101   f = 1.0;
 102   epa = 0.0;
 103   mECLoc = FGColumnVector3( radius*cosLat*cosLon,
 104                             radius*cosLat*sinLon,
 105                             radius*sinLat );
 106   mLon = mLat = mRadius = mGeodLat = GeodeticAltitude = 0.0;
 107 }
 108
 109 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 110
 111 void FGLocation::SetLongitude(double longitude)
 112 {
 113   double rtmp = mECLoc.Magnitude(eX, eY);
 114   // Check if we have zero radius.
 115   // If so set it to 1, so that we can set a position
 116   if (0.0 == mECLoc.Magnitude())
 117     rtmp = 1.0;
 118
 119   // Fast return if we are on the north or south pole ...
 120   if (rtmp == 0.0)
 121     return;
 122
 123   mCacheValid = false;
 124
 125   // Need to figure out how to set the initial_longitude here
 126
 127   mECLoc(eX) = rtmp*cos(longitude);
 128   mECLoc(eY) = rtmp*sin(longitude);
 129 }
 130
 131 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 132
 133 void FGLocation::SetLatitude(double latitude)
 134 {
 135   mCacheValid = false;
 136
 137   double r = mECLoc.Magnitude();
 138   if (r == 0.0) {
 139     mECLoc(eX) = 1.0;
 140     r = 1.0;
 141   }
 142
 143   double rtmp = mECLoc.Magnitude(eX, eY);
 144   if (rtmp != 0.0) {
 145     double fac = r/rtmp*cos(latitude);
 146     mECLoc(eX) *= fac;
 147     mECLoc(eY) *= fac;
 148   } else {
 149     mECLoc(eX) = r*cos(latitude);
 150     mECLoc(eY) = 0.0;
 151   }
 152   mECLoc(eZ) = r*sin(latitude);
 153 }
 154
 155 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 156
 157 void FGLocation::SetRadius(double radius)
 158 {
 159   mCacheValid = false;
 160
 161   double rold = mECLoc.Magnitude();
 162   if (rold == 0.0)
 163     mECLoc(eX) = radius;
 164   else
 165     mECLoc *= radius/rold;
 166 }
 167
 168 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 169
 170 void FGLocation::SetPosition(double lon, double lat, double radius)
 171 {
 172   mCacheValid = false;
 173
 174   double sinLat = sin(lat);
 175   double cosLat = cos(lat);
 176   double sinLon = sin(lon);
 177   double cosLon = cos(lon);
 178 //  initial_longitude = lon;
 179   mECLoc = FGColumnVector3( radius*cosLat*cosLon,
 180                             radius*cosLat*sinLon,
 181                             radius*sinLat );
 182 }
 183
 184 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 185
 186 void FGLocation::SetPositionGeodetic(double lon, double lat, double height)
 187 {
 188   mCacheValid = false;
 189
 190   mGeodLat = lat;
 191   mLon = lon;
 192   GeodeticAltitude = height;
 193
 194 //  initial_longitude = mLon;
 195
 196   double RN = a / sqrt(1.0 - e2*sin(mGeodLat)*sin(mGeodLat));
 197
 198   mECLoc(eX) = (RN + GeodeticAltitude)*cos(mGeodLat)*cos(mLon);
 199   mECLoc(eY) = (RN + GeodeticAltitude)*cos(mGeodLat)*sin(mLon);
 200   mECLoc(eZ) = ((1 - e2)*RN + GeodeticAltitude)*sin(mGeodLat);
 201 }
 202
 203 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 204
 205 void FGLocation::SetEllipse(double semimajor, double semiminor)
 206 {
 207   mCacheValid = false;
 208
 209   a = semimajor;
 210   b = semiminor;
 211   a2 = a*a;
 212   b2 = b*b;
 213   e2 = 1.0 - b2/a2;
 214   e = sqrt(e2);
 215   eps2 = a2/b2 - 1.0;
 216   f = 1.0 - b/a;
 217 }
 218
 219 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 220 // Compute the ECEF to ECI transformation matrix using Stevens and Lewis "Aircraft
 221 // Control and Simulation", second edition, eqn. 1.4-12, pg. 39. In Stevens and Lewis
 222 // notation, this is C_i/e, a transformation from ECEF to ECI.
 223
 224 const FGMatrix33& FGLocation::GetTec2i(void)
 225 {
 226   ComputeDerived();
 227   return mTec2i;
 228 }
 229
 230 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 231 // This is given in Stevens and Lewis "Aircraft
 232 // Control and Simulation", second edition, eqn. 1.4-12, pg. 39
 233 // The notation in Stevens and Lewis is: C_e/i. This represents a transformation
 234 // from ECI to ECEF - and the orientation of the ECEF frame relative to the ECI
 235 // frame.
 236
 237 const FGMatrix33& FGLocation::GetTi2ec(void)
 238 {
 239   ComputeDerived();
 240   return mTi2ec;
 241 }
 242
 243 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 244
 245 void FGLocation::ComputeDerivedUnconditional(void) const
 246 {
 247   // The radius is just the Euclidean norm of the vector.
 248   mRadius = mECLoc.Magnitude();
 249
 250   // The distance of the location to the Z-axis, which is the axis
 251   // through the poles.
 252   double r02 = mECLoc(eX)*mECLoc(eX) + mECLoc(eY)*mECLoc(eY);
 253   double rxy = sqrt(r02);
 254
 255   // Compute the sin/cos values of the longitude
 256   double sinLon, cosLon;
 257   if (rxy == 0.0) {
 258     sinLon = 0.0;
 259     cosLon = 1.0;
 260   } else {
 261     sinLon = mECLoc(eY)/rxy;
 262     cosLon = mECLoc(eX)/rxy;
 263   }
 264
 265   // Compute the sin/cos values of the latitude
 266   double sinLat, cosLat;
 267   if (mRadius == 0.0)  {
 268     sinLat = 0.0;
 269     cosLat = 1.0;
 270   } else {
 271     sinLat = mECLoc(eZ)/mRadius;
 272     cosLat = rxy/mRadius;
 273   }
 274
 275   // Compute the longitude and latitude itself
 276   if ( mECLoc( eX ) == 0.0 && mECLoc( eY ) == 0.0 )
 277     mLon = 0.0;
 278   else
 279     mLon = atan2( mECLoc( eY ), mECLoc( eX ) );
 280
 281   if ( rxy == 0.0 && mECLoc( eZ ) == 0.0 )
 282     mLat = 0.0;
 283   else
 284     mLat = atan2( mECLoc(eZ), rxy );
 285
 286   // Compute the transform matrices from and to the earth centered frame.
 287   // See Stevens and Lewis, "Aircraft Control and Simulation", Second Edition,
 288   // Eqn. 1.4-13, page 40. In Stevens and Lewis notation, this is C_n/e - the
 289   // orientation of the navigation (local) frame relative to the ECEF frame,
 290   // and a transformation from ECEF to nav (local) frame.
 291
 292   mTec2l = FGMatrix33( -cosLon*sinLat, -sinLon*sinLat,  cosLat,
 293                            -sinLon   ,     cosLon    ,    0.0 ,
 294                        -cosLon*cosLat, -sinLon*cosLat, -sinLat  );
 295
 296   // In Stevens and Lewis notation, this is C_e/n - the
 297   // orientation of the ECEF frame relative to the nav (local) frame,
 298   // and a transformation from nav (local) to ECEF frame.
 299
 300   mTl2ec = mTec2l.Transposed();
 301
 302   // Calculate the inertial to ECEF and transpose matrices
 303   double cos_epa = cos(epa);
 304   double sin_epa = sin(epa);
 305   mTi2ec = FGMatrix33( cos_epa, sin_epa, 0.0,
 306                       -sin_epa, cos_epa, 0.0,
 307                            0.0,      0.0, 1.0 );
 308   mTec2i = mTi2ec.Transposed();
 309
 310   // Now calculate the local (or nav, or ned) frame to inertial transform matrix,
 311   // and the inverse.
 312   mTl2i = mTec2i * mTl2ec;
 313   mTi2l = mTl2i.Transposed();
 314
 315   // Calculate the geodetic latitude base on AIAA Journal of Guidance and Control paper,
 316   // "Improved Method for Calculating Exact Geodetic Latitude and Altitude", and
 317   // "Improved Method for Calculating Exact Geodetic Latitude and Altitude, Revisited",
 318   // author: I. Sofair
 319
 320   if (a != 0.0 && b != 0.0) {
 321     double c, p, q, s, t, u, v, w, z, p2, u2, r0;
 322     double Ne, P, Q0, Q, signz0, sqrt_q, z_term;
 323     p  = fabs(mECLoc(eZ))/eps2;
 324     s  = r02/(e2*eps2);
 325     p2 = p*p;
 326     q  = p2 - b2 + s;
 327     sqrt_q = sqrt(q);
 328     if (q>0)
 329     {
 330       u  = p/sqrt_q;
 331       u2 = p2/q;
 332       v  = b2*u2/q;
 333       P  = 27.0*v*s/q;
 334       Q0 = sqrt(P+1) + sqrt(P);
 335       Q  = pow(Q0, 0.66666666667);
 336       t  = (1.0 + Q + 1.0/Q)/6.0;
 337       c  = sqrt(u2 - 1 + 2.0*t);
 338       w  = (c - u)/2.0;
 339       signz0 = mECLoc(eZ)>=0?1.0:-1.0;
 340       z_term = sqrt(t*t+v)-u*w-0.5*t-0.25;
 341       if (z_term < 0.0) {
 342         z = 0.0;
 343       } else {
 344         z  = signz0*sqrt_q*(w+sqrt(z_term));
 345       }
 346       Ne = a*sqrt(1+eps2*z*z/b2);
 347       mGeodLat = asin((eps2+1.0)*(z/Ne));
 348       r0 = rxy;
 349       GeodeticAltitude = r0*cos(mGeodLat) + mECLoc(eZ)*sin(mGeodLat) - a2/Ne;
 350     }
 351   }
 352
 353   // Mark the cached values as valid
 354   mCacheValid = true;
 355 }
 356
 357 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 358
 359 } // namespace JSBSim