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