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 (jsb@hal-pc.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 #ifdef FGFS
  42 #  include <simgear/compiler.h>
  43 #  ifdef SG_HAVE_STD_INCLUDES
  44 #    include <cmath>
  45 #  else
  46 #    include <math.h>
  47 #  endif
  48 #else
  49 #  if defined(sgi) && !defined(__GNUC__)
  50 #    include <math.h>
  51 #  else
  52 #    include <cmath>
  53 #  endif
  54 #endif
  55
  56 #include "FGLocation.h"
  57 #include <input_output/FGPropertyManager.h>
  58
  59 namespace JSBSim {
  60
  61 static const char *IdSrc = "$Id$";
  62 static const char *IdHdr = ID_LOCATION;
  63
  64 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  65 CLASS IMPLEMENTATION
  66 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
  67
  68 FGLocation::FGLocation(void)
  69 {
  70   mCacheValid = false;
  71   initial_longitude = 0.0;
  72   a = 0.0;
  73   b = 0.0;
  74   a2 = 0.0;
  75   b2 = 0.0;
  76   e2 = 1.0;
  77   e = 1.0;
  78   eps2 = -1.0;
  79   f = 1.0;
  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   initial_longitude = lon;
  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   mECLoc = FGColumnVector3( radius*cosLat*cosLon,
 102                             radius*cosLat*sinLon,
 103                             radius*sinLat );
 104 }
 105
 106 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 107
 108 void FGLocation::SetLongitude(double longitude)
 109 {
 110   double rtmp = mECLoc.Magnitude(eX, eY);
 111   // Check if we have zero radius.
 112   // If so set it to 1, so that we can set a position
 113   if (0.0 == mECLoc.Magnitude())
 114     rtmp = 1.0;
 115
 116   // Fast return if we are on the north or south pole ...
 117   if (rtmp == 0.0)
 118     return;
 119
 120   mCacheValid = false;
 121
 122   // Need to figure out how to set the initial_longitude here
 123
 124   mECLoc(eX) = rtmp*cos(longitude);
 125   mECLoc(eY) = rtmp*sin(longitude);
 126 }
 127
 128 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 129
 130 void FGLocation::SetLatitude(double latitude)
 131 {
 132   mCacheValid = false;
 133
 134   double r = mECLoc.Magnitude();
 135   if (r == 0.0) {
 136     mECLoc(eX) = 1.0;
 137     r = 1.0;
 138   }
 139
 140   double rtmp = mECLoc.Magnitude(eX, eY);
 141   if (rtmp != 0.0) {
 142     double fac = r/rtmp*cos(latitude);
 143     mECLoc(eX) *= fac;
 144     mECLoc(eY) *= fac;
 145   } else {
 146     mECLoc(eX) = r*cos(latitude);
 147     mECLoc(eY) = 0.0;
 148   }
 149   mECLoc(eZ) = r*sin(latitude);
 150 }
 151
 152 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 153
 154 void FGLocation::SetRadius(double radius)
 155 {
 156   mCacheValid = false;
 157
 158   double rold = mECLoc.Magnitude();
 159   if (rold == 0.0)
 160     mECLoc(eX) = radius;
 161   else
 162     mECLoc *= radius/rold;
 163 }
 164
 165 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 166
 167 void FGLocation::SetPosition(double lon, double lat, double radius)
 168 {
 169   mCacheValid = false;
 170
 171   double sinLat = sin(lat);
 172   double cosLat = cos(lat);
 173   double sinLon = sin(lon);
 174   double cosLon = cos(lon);
 175   initial_longitude = lon;
 176   mECLoc = FGColumnVector3( radius*cosLat*cosLon,
 177                             radius*cosLat*sinLon,
 178                             radius*sinLat );
 179   ComputeDerived();
 180 }
 181
 182 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 183
 184 void FGLocation::SetEllipse(double semimajor, double semiminor)
 185 {
 186   mCacheValid = false;
 187
 188   a = semimajor;
 189   b = semiminor;
 190   a2 = a*a;
 191   b2 = b*b;
 192   e2 = 1.0 - b2/a2;
 193   e = sqrt(e2);
 194   eps2 = a2/b2 - 1.0;
 195   f = 1.0 - b/a;
 196 }
 197
 198 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 199 // Compute the ECEF to ECI transformation matrix using Stevens and Lewis "Aircraft
 200 // Control and Simulation", second edition, eqn. 1.4-12, pg. 39
 201
 202 const FGMatrix33& FGLocation::GetTec2i(double epa)
 203 {
 204   double mu = epa - initial_longitude;
 205   mTec2i = FGMatrix33( cos(mu), -sin(mu), 0.0,
 206                        sin(mu),  cos(mu), 0.0,
 207                             0.0,     0.0, 1.0 );
 208   return mTec2i;
 209 }
 210
 211 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 212
 213 const FGMatrix33& FGLocation::GetTi2ec(double epa)
 214 {
 215   double mu = epa - initial_longitude;
 216   mTi2ec = FGMatrix33( cos(mu), sin(mu), 0.0,
 217                       -sin(mu), cos(mu), 0.0,
 218                            0.0,      0.0, 1.0 );
 219   return mTi2ec;
 220 }
 221
 222 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 223
 224 void FGLocation::ComputeDerivedUnconditional(void) const
 225 {
 226   // The radius is just the Euclidean norm of the vector.
 227   mRadius = mECLoc.Magnitude();
 228
 229   // The distance of the location to the Z-axis, which is the axis
 230   // through the poles.
 231   double rxy = sqrt(mECLoc(eX)*mECLoc(eX) + mECLoc(eY)*mECLoc(eY));
 232
 233   // Compute the sin/cos values of the longitude
 234   double sinLon, cosLon;
 235   if (rxy == 0.0) {
 236     sinLon = 0.0;
 237     cosLon = 1.0;
 238   } else {
 239     sinLon = mECLoc(eY)/rxy;
 240     cosLon = mECLoc(eX)/rxy;
 241   }
 242
 243   // Compute the sin/cos values of the latitude
 244   double sinLat, cosLat;
 245   if (mRadius == 0.0)  {
 246     sinLat = 0.0;
 247     cosLat = 1.0;
 248   } else {
 249     sinLat = mECLoc(eZ)/mRadius;
 250     cosLat = rxy/mRadius;
 251   }
 252
 253   // Compute the longitude and latitude itself
 254   if ( mECLoc( eX ) == 0.0 && mECLoc( eY ) == 0.0 )
 255     mLon = 0.0;
 256   else
 257     mLon = atan2( mECLoc( eY ), mECLoc( eX ) );
 258
 259   if ( rxy == 0.0 && mECLoc( eZ ) == 0.0 )
 260     mLat = 0.0;
 261   else
 262     mLat = atan2( mECLoc(eZ), rxy );
 263
 264   // Compute the transform matrices from and to the earth centered frame.
 265   // See Stevens and Lewis, "Aircraft Control and Simulation", Second Edition,
 266   // Eqn. 1.4-13, page 40.
 267   mTec2l = FGMatrix33( -cosLon*sinLat, -sinLon*sinLat,  cosLat,
 268                            -sinLon   ,     cosLon    ,    0.0 ,
 269                        -cosLon*cosLat, -sinLon*cosLat, -sinLat  );
 270
 271   mTl2ec = mTec2l.Transposed();
 272
 273   // Calculate the geodetic latitude base on AIAA Journal of Guidance and Control paper,
 274   // "Improved Method for Calculating Exact Geodetic Latitude and Altitude", and
 275   // "Improved Method for Calculating Exact Geodetic Latitude and Altitude, Revisited",
 276   // author: I. Sofair
 277
 278   if (a != 0.0 && b != 0.0) {
 279     double c, p, q, s, t, u, v, w, z, p2, u2, r02, r0;
 280     double Ne, P, Q0, Q, signz0, sqrt_q;
 281     p  = fabs(mECLoc(eZ))/eps2;
 282     r02 = mECLoc(eX)*mECLoc(eX) + mECLoc(eY)*mECLoc(eY);
 283     s  = r02/(e2*eps2);
 284     p2 = p*p;
 285     q  = p2 - b2 + s;
 286     sqrt_q = sqrt(q);
 287     if (q>0)
 288     {
 289       u  = p/sqrt_q;
 290       u2 = p2/q;
 291       v  = b2*u2/q;
 292       P  = 27.0*v*s/q;
 293       Q0 = sqrt(P+1) + sqrt(P);
 294       Q  = pow(Q0, 0.66666666667);
 295       t  = (1.0 + Q + 1.0/Q)/6.0;
 296       c  = sqrt(u2 - 1 + 2.0*t);
 297       w  = (c - u)/2.0;
 298       signz0 = mECLoc(eZ)>=0?1.0:-1.0;
 299       z  = signz0*sqrt_q*(w+sqrt(sqrt(t*t+v)-u*w-0.5*t-0.25));
 300       Ne = a*sqrt(1+eps2*z*z/b2);
 301       mGeodLat = asin((eps2+1.0)*(z/Ne));
 302       r0 = sqrt(r02);
 303       GeodeticAltitude = r0*cos(mGeodLat) + mECLoc(eZ)*sin(mGeodLat) - a2/Ne;
 304     }
 305   }
 306
 307   // Mark the cached values as valid
 308   mCacheValid = true;
 309 }
 310
 311 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 312
 313 } // namespace JSBSim