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