src/FDM/JSBSim/math/FGLocation.h

   1 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
   2
   3  Header:       FGLocation.h
   4  Author:       Jon S. Berndt, Mathias Froehlich
   5  Date started: 04/04/2004
   6
   7  ------- Copyright (C) 1999  Jon S. Berndt (jsb@hal-pc.org) ------------------
   8  -------           (C) 2004  Mathias Froehlich (Mathias.Froehlich@web.de) ----
   9
  10  This program is free software; you can redistribute it and/or modify it under
  11  the terms of the GNU Lesser General Public License as published by the Free Software
  12  Foundation; either version 2 of the License, or (at your option) any later
  13  version.
  14
  15  This program is distributed in the hope that it will be useful, but WITHOUT
  16  ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
  17  FOR A PARTICULAR PURPOSE.  See the GNU Lesser General Public License for more
  18  details.
  19
  20  You should have received a copy of the GNU Lesser General Public License along with
  21  this program; if not, write to the Free Software Foundation, Inc., 59 Temple
  22  Place - Suite 330, Boston, MA  02111-1307, USA.
  23
  24  Further information about the GNU Lesser General Public License can also be found on
  25  the world wide web at http://www.gnu.org.
  26
  27 HISTORY
  28 -------------------------------------------------------------------------------
  29 04/04/2004   MF   Created from code previously in the old positions class.
  30
  31 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  32 SENTRY
  33 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
  34
  35 #ifndef FGLOCATION_H
  36 #define FGLOCATION_H
  37
  38 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  39 INCLUDES
  40 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
  41
  42 #include <FGJSBBase.h>
  43 #include <input_output/FGPropertyManager.h>
  44 #include "FGColumnVector3.h"
  45 #include "FGMatrix33.h"
  46
  47 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  48 DEFINITIONS
  49 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
  50
  51 #define ID_LOCATION "$Id$"
  52
  53 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  54 FORWARD DECLARATIONS
  55 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
  56
  57 namespace JSBSim {
  58
  59 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  60 CLASS DOCUMENTATION
  61 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
  62
  63 /** Holds an arbitrary location in the earth centered reference frame.
  64     This coordinate frame has its center in the middle of the earth.
  65     Its x-axis points from the center of the earth towards a location
  66     with zero latitude and longitude on the earths surface. The y-axis
  67     points from the center of the earth towards a location with zero
  68     latitude and 90deg longitude on the earths surface. The z-axis
  69     points from the earths center to the geographic north pole.
  70
  71     This class provides access functions to set and get the location as
  72     either the simple x, y and z values in ft or longitude/latitude and
  73     the radial distance of the location from the earth center.
  74
  75     It is common to associate a parent frame with a location. This
  76     frame is usually called the local horizontal frame or simply the local
  77     frame. This frame has its x/y plane parallel to the surface of the earth
  78     (with the assumption of a spherical earth). The x-axis points
  79     towards north, the y-axis points towards east and the z-axis
  80     points to the center of the earth.
  81
  82     Since this frame is determined by the location, this class also
  83     provides the rotation matrices required to transform from the
  84     earth centered frame to the local horizontal frame and back. There
  85     are also conversion functions for conversion of position vectors
  86     given in the one frame to positions in the other frame.
  87
  88     The earth centered reference frame is *NOT* an inertial frame
  89     since it rotates with the earth.
  90
  91     The coordinates in the earth centered frame are the master values.
  92     All other values are computed from these master values and are
  93     cached as long as the location is changed by access through a
  94     non-const member function. Values are cached to improve performance.
  95     It is best practice to work with a natural set of master values.
  96     Other parameters that are derived from these master values are calculated
  97     only when needed, and IF they are needed and calculated, then they are
  98     cached (stored and remembered) so they do not need to be re-calculated
  99     until the master values they are derived from are themselves changed
 100     (and become stale).
 101
 102     Accuracy and round off:
 103
 104     Given that we model a vehicle near the earth, the earths surface
 105     radius is about 2*10^7, ft and that we use double values for the
 106     representation of the location, we have an accuracy of about
 107     1e-16*2e7ft/1=2e-9ft left. This should be sufficient for our needs.
 108     Note that this is the same relative accuracy we would have when we
 109     compute directly with lon/lat/radius. For the radius value this
 110     is clear. For the lon/lat pair this is easy to see. Take for
 111     example KSFO located at about 37.61deg north 122.35deg west, which
 112     corresponds to 0.65642rad north and 2.13541rad west. Both values
 113     are of magnitude of about 1. But 1ft corresponds to about
 114     1/(2e7*2*pi)=7.9577e-09rad. So the left accuracy with this
 115     representation is also about 1*1e-16/7.9577e-09=1.2566e-08 which
 116     is of the same magnitude as the representation chosen here.
 117
 118     The advantage of this representation is that it is a linear space
 119     without singularities. The singularities are the north and south
 120     pole and most notably the non-steady jump at -pi to pi. It is
 121     harder to track this jump correctly especially when we need to
 122     work with error norms and derivatives of the equations of motion
 123     within the time-stepping code. Also, the rate of change is of the
 124     same magnitude for all components in this representation which is
 125     an advantage for numerical stability in implicit time-stepping too.
 126
 127     Note: The latitude is a GEOCENTRIC value. FlightGear
 128     converts latitude to a geodetic value and uses that. In order to get best
 129     matching relative to a map, geocentric latitude must be converted to geodetic.
 130
 131     @see Stevens and Lewis, "Aircraft Control and Simulation", Second edition
 132     @see W. C. Durham "Aircraft Dynamics & Control", section 2.2
 133
 134     @author Mathias Froehlich
 135     @version $Id$
 136   */
 137
 138 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 139 CLASS DECLARATION
 140 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
 141
 142 class FGLocation : virtual FGJSBBase
 143 {
 144 public:
 145   /** Default constructor. */
 146   FGLocation(void);
 147
 148   /** Constructor to set the longitude, latitude and the distance
 149       from the center of the earth.
 150       @param lon longitude
 151       @param lat GEOCENTRIC latitude
 152       @param radius distance from center of earth to vehicle in feet*/
 153   FGLocation(double lon, double lat, double radius);
 154
 155   /** Column constructor. */
 156   FGLocation(const FGColumnVector3& lv) : mECLoc(lv), mCacheValid(false)
 157   {
 158     a = 0.0;
 159     b = 0.0;
 160     a2 = 0.0;
 161     b2 = 0.0;
 162     e2 = 1.0;
 163     e = 1.0;
 164     eps2 = -1.0;
 165     f = 1.0;
 166   }
 167
 168   /** Copy constructor. */
 169   FGLocation(const FGLocation& l)
 170     : mECLoc(l.mECLoc), mCacheValid(l.mCacheValid)
 171   {
 172 //    if (!mCacheValid) return; // This doesn't seem right.
 173
 174     mLon = l.mLon;
 175     mLat = l.mLat;
 176     mRadius = l.mRadius;
 177
 178     mTl2ec = l.mTl2ec;
 179     mTec2l = l.mTec2l;
 180
 181     a = l.a;
 182     b = l.b;
 183     a2 = l.a2;
 184     b2 = l.b2;
 185     e2 = l.e2;
 186     e = l.e;
 187     eps2 = l.eps2;
 188     f = l.f;
 189
 190     initial_longitude = l.initial_longitude;
 191   }
 192
 193   /** Set the longitude.
 194       @param longitude Longitude in rad to set.
 195       Sets the longitude of the location represented with this class
 196       instance to the value of the given argument. The value is meant
 197       to be in rad. The latitude and the radius value are preserved
 198       with this call with the exception of radius being equal to
 199       zero. If the radius is previously set to zero it is changed to be
 200       equal to 1.0 past this call. Longitude is positive east and negative west. */
 201   void SetLongitude(double longitude);
 202
 203   /** Set the latitude.
 204       @param latitude Latitude in rad to set.
 205       Sets the latitude of the location represented with this class
 206       instance to the value of the given argument. The value is meant
 207       to be in rad. The longitude and the radius value are preserved
 208       with this call with the exception of radius being equal to
 209       zero. If the radius is previously set to zero it is changed to be
 210       equal to 1.0 past this call.
 211       Latitude is positive north and negative south.
 212       The arguments should be within the bounds of -pi/2 <= lat <= pi/2.
 213       The behavior of this function with arguments outside this range is
 214       left as an exercise to the gentle reader ... */
 215   void SetLatitude(double latitude);
 216
 217   /** Set the distance from the center of the earth.
 218       @param radius Radius in ft to set.
 219       Sets the radius of the location represented with this class
 220       instance to the value of the given argument. The value is meant
 221       to be in ft. The latitude and longitude values are preserved
 222       with this call with the exception of radius being equal to
 223       zero. If the radius is previously set to zero, latitude and
 224       longitude is set equal to zero past this call.
 225       The argument should be positive.
 226       The behavior of this function called with a negative argument is
 227       left as an exercise to the gentle reader ... */
 228   void SetRadius(double radius);
 229
 230   /** Sets the longitude, latitude and the distance from the center of the earth.
 231       @param lon longitude in radians
 232       @param lat GEOCENTRIC latitude in radians
 233       @param radius distance from center of earth to vehicle in feet*/
 234   void SetPosition(double lon, double lat, double radius);
 235
 236   /** Sets the semimajor and semiminor axis lengths for this planet.
 237       The eccentricity and flattening are calculated from the semimajor
 238       and semiminor axis lengths */
 239   void SetEllipse(double semimajor, double semiminor);
 240
 241   /** Get the longitude.
 242       @return the longitude in rad of the location represented with this
 243       class instance. The returned values are in the range between
 244       -pi <= lon <= pi. Longitude is positive east and negative west. */
 245   double GetLongitude() const { ComputeDerived(); return mLon; }
 246
 247   /** Get the longitude.
 248       @return the longitude in deg of the location represented with this
 249       class instance. The returned values are in the range between
 250       -180 <= lon <= 180.  Longitude is positive east and negative west. */
 251   double GetLongitudeDeg() const { ComputeDerived(); return radtodeg*mLon; }
 252
 253   /** Get the sine of Longitude. */
 254   double GetSinLongitude() const { ComputeDerived(); return -mTec2l(2,1); }
 255
 256   /** Get the cosine of Longitude. */
 257   double GetCosLongitude() const { ComputeDerived(); return mTec2l(2,2); }
 258
 259   /** Get the latitude.
 260       @return the latitude in rad of the location represented with this
 261       class instance. The returned values are in the range between
 262       -pi/2 <= lon <= pi/2. Latitude is positive north and negative south. */
 263   double GetLatitude() const { ComputeDerived(); return mLat; }
 264
 265   /** Get the geodetic latitude.
 266       @return the geodetic latitude in rad of the location represented with this
 267       class instance. The returned values are in the range between
 268       -pi/2 <= lon <= pi/2. Latitude is positive north and negative south. */
 269   double GetGeodLatitudeRad(void) const { ComputeDerived(); return mGeodLat; }
 270
 271   /** Get the latitude.
 272       @return the latitude in deg of the location represented with this
 273       class instance. The returned value is in the range between
 274       -90 <= lon <= 90. Latitude is positive north and negative south. */
 275   double GetLatitudeDeg() const { ComputeDerived(); return radtodeg*mLat; }
 276
 277   /** Get the geodetic latitude in degrees.
 278       @return the geodetic latitude in degrees of the location represented by
 279       this class instance. The returned value is in the range between
 280       -90 <= lon <= 90. Latitude is positive north and negative south. */
 281   double GetGeodLatitudeDeg(void) const { ComputeDerived(); return radtodeg*mGeodLat; }
 282
 283   /** Gets the geodetic altitude in feet. */
 284   double GetGeodAltitude(void) const { return GeodeticAltitude;}
 285
 286   /** Get the sine of Latitude. */
 287   double GetSinLatitude() const { ComputeDerived(); return -mTec2l(3,3); }
 288
 289   /** Get the cosine of Latitude. */
 290   double GetCosLatitude() const { ComputeDerived(); return mTec2l(1,3); }
 291
 292   /** Get the cosine of Latitude. */
 293   double GetTanLatitude() const {
 294     ComputeDerived();
 295     double cLat = mTec2l(1,3);
 296     if (cLat == 0.0)
 297       return 0.0;
 298     else
 299       return -mTec2l(3,3)/cLat;
 300   }
 301
 302   /** Get the distance from the center of the earth.
 303       @return the distance of the location represented with this class
 304       instance to the center of the earth in ft. The radius value is
 305       always positive. */
 306   double GetRadius() const { ComputeDerived(); return mRadius; }
 307
 308   /** Transform matrix from local horizontal to earth centered frame.
 309       Returns a const reference to the rotation matrix of the transform from
 310       the local horizontal frame to the earth centered frame. */
 311   const FGMatrix33& GetTl2ec(void) const { ComputeDerived(); return mTl2ec; }
 312
 313   /** Transform matrix from the earth centered to local horizontal frame.
 314       Returns a const reference to the rotation matrix of the transform from
 315       the earth centered frame to the local horizontal frame. */
 316   const FGMatrix33& GetTec2l(void) const { ComputeDerived(); return mTec2l; }
 317
 318   /** Transform matrix from inertial to earth centered frame.
 319       Returns a const reference to the rotation matrix of the transform from
 320       the inertial frame to the earth centered frame (ECI to ECEF). */
 321   const FGMatrix33& GetTi2ec(double epa);
 322
 323   /** Transform matrix from the earth centered to inertial frame.
 324       Returns a const reference to the rotation matrix of the transform from
 325       the earth centered frame to the inertial frame (ECEF to ECI). */
 326   const FGMatrix33& GetTec2i(double epa);
 327
 328   /** Conversion from Local frame coordinates to a location in the
 329       earth centered and fixed frame.
 330       @param lvec Vector in the local horizontal coordinate frame
 331       @return The location in the earth centered and fixed frame */
 332   FGLocation LocalToLocation(const FGColumnVector3& lvec) const {
 333     ComputeDerived(); return mTl2ec*lvec + mECLoc;
 334   }
 335
 336   /** Conversion from a location in the earth centered and fixed frame
 337       to local horizontal frame coordinates.
 338       @param ecvec Vector in the earth centered and fixed frame
 339       @return The vector in the local horizontal coordinate frame */
 340   FGColumnVector3 LocationToLocal(const FGColumnVector3& ecvec) const {
 341     ComputeDerived(); return mTec2l*(ecvec - mECLoc);
 342   }
 343
 344   // For time-stepping, locations have vector properties...
 345
 346   /** Read access the entries of the vector.
 347       @param idx the component index.
 348       Return the value of the matrix entry at the given index.
 349       Indices are counted starting with 1.
 350       Note that the index given in the argument is unchecked. */
 351   double operator()(unsigned int idx) const { return Entry(idx); }
 352
 353   /** Write access the entries of the vector.
 354       @param idx the component index.
 355       @return a reference to the vector entry at the given index.
 356       Indices are counted starting with 1.
 357       Note that the index given in the argument is unchecked. */
 358   double& operator()(unsigned int idx) { return Entry(idx); }
 359
 360   /** Read access the entries of the vector.
 361       @param idx the component index.
 362       @return the value of the matrix entry at the given index.
 363       Indices are counted starting with 1.
 364       This function is just a shortcut for the <tt>double
 365       operator()(unsigned int idx) const</tt> function. It is
 366       used internally to access the elements in a more convenient way.
 367       Note that the index given in the argument is unchecked. */
 368   double Entry(unsigned int idx) const { return mECLoc.Entry(idx); }
 369
 370   /** Write access the entries of the vector.
 371       @param idx the component index.
 372       @return a reference to the vector entry at the given index.
 373       Indices are counted starting with 1.
 374       This function is just a shortcut for the double&
 375       operator()(unsigned int idx) function. It is
 376       used internally to access the elements in a more convenient way.
 377       Note that the index given in the argument is unchecked. */
 378   double& Entry(unsigned int idx) {
 379     mCacheValid = false; return mECLoc.Entry(idx);
 380   }
 381
 382   const FGLocation& operator=(const FGColumnVector3& v)
 383   {
 384     mECLoc(eX) = v(eX);
 385     mECLoc(eY) = v(eY);
 386     mECLoc(eZ) = v(eZ);
 387     mCacheValid = false;
 388     ComputeDerived();
 389     return *this;
 390   }
 391
 392   const FGLocation& operator=(const FGLocation& l)
 393   {
 394     mECLoc = l.mECLoc;
 395     mCacheValid = l.mCacheValid;
 396
 397 //    if (!mCacheValid) return *this; // Why is this here for an assignment operator?
 398
 399     mLon = l.mLon;
 400     mLat = l.mLat;
 401     mRadius = l.mRadius;
 402
 403     mTl2ec = l.mTl2ec;
 404     mTec2l = l.mTec2l;
 405
 406     a = l.a;
 407     b = l.b;
 408     a2 = l.a2;
 409     b2 = l.b2;
 410     e2 = l.e2;
 411     e = l.e;
 412     eps2 = l.eps2;
 413     f = l.f;
 414
 415     initial_longitude = l.initial_longitude;
 416
 417     return *this;
 418   }
 419   bool operator==(const FGLocation& l) const {
 420     return mECLoc == l.mECLoc;
 421   }
 422   bool operator!=(const FGLocation& l) const { return ! operator==(l); }
 423   const FGLocation& operator+=(const FGLocation &l) {
 424     mCacheValid = false;
 425     mECLoc += l.mECLoc;
 426     return *this;
 427   }
 428   const FGLocation& operator-=(const FGLocation &l) {
 429     mCacheValid = false;
 430     mECLoc -= l.mECLoc;
 431     return *this;
 432   }
 433   const FGLocation& operator*=(double scalar) {
 434     mCacheValid = false;
 435     mECLoc *= scalar;
 436     return *this;
 437   }
 438   const FGLocation& operator/=(double scalar) {
 439     return operator*=(1.0/scalar);
 440   }
 441   FGLocation operator+(const FGLocation& l) const {
 442     return FGLocation(mECLoc + l.mECLoc);
 443   }
 444   FGLocation operator-(const FGLocation& l) const {
 445     return FGLocation(mECLoc - l.mECLoc);
 446   }
 447
 448   FGLocation operator*(double scalar) const {
 449     return FGLocation(scalar*mECLoc);
 450   }
 451
 452   /** Cast to a simple 3d vector */
 453   operator const FGColumnVector3&() const {
 454     return mECLoc;
 455   }
 456
 457 private:
 458   /** Computation of derived values.
 459       This function re-computes the derived values like lat/lon and
 460       transformation matrices. It does this unconditionally. */
 461   void ComputeDerivedUnconditional(void) const;
 462
 463   /** Computation of derived values.
 464       This function checks if the derived values like lat/lon and
 465       transformation matrices are already computed. If so, it
 466       returns. If they need to be computed this is done here. */
 467   void ComputeDerived(void) const {
 468     if (!mCacheValid)
 469       ComputeDerivedUnconditional();
 470   }
 471
 472   /** The coordinates in the earth centered frame. This is the master copy.
 473       The coordinate frame has its center in the middle of the earth.
 474       Its x-axis points from the center of the earth towards a
 475       location with zero latitude and longitude on the earths
 476       surface. The y-axis points from the center of the earth towards a
 477       location with zero latitude and 90deg longitude on the earths
 478       surface. The z-axis points from the earths center to the
 479       geographic north pole.
 480       @see W. C. Durham "Aircraft Dynamics & Control", section 2.2 */
 481   FGColumnVector3 mECLoc;
 482
 483   /** The cached lon/lat/radius values. */
 484   mutable double mLon;
 485   mutable double mLat;
 486   mutable double mRadius;
 487   mutable double mGeodLat;
 488   mutable double GeodeticAltitude;
 489
 490   double initial_longitude;
 491
 492   /** The cached rotation matrices from and to the associated frames. */
 493   mutable FGMatrix33 mTl2ec;
 494   mutable FGMatrix33 mTec2l;
 495   mutable FGMatrix33 mTi2ec;
 496   mutable FGMatrix33 mTec2i;
 497
 498   /* Terms for geodetic latitude calculation. Values are from WGS84 model */
 499   double a;    // Earth semimajor axis in feet (6,378,137.0 meters)
 500   double b;    // Earth semiminor axis in feet (6,356,752.3142 meters)
 501   double a2;
 502   double b2;
 503   double e;    // Earth eccentricity
 504   double e2;   // Earth eccentricity squared
 505   double eps2; //
 506   double f;    // Flattening
 507
 508   /** A data validity flag.
 509       This class implements caching of the derived values like the
 510       orthogonal rotation matrices or the lon/lat/radius values. For caching we
 511       carry a flag which signals if the values are valid or not.
 512       The C++ keyword "mutable" tells the compiler that the data member is
 513       allowed to change during a const member function. */
 514   mutable bool mCacheValid;
 515 };
 516
 517 /** Scalar multiplication.
 518
 519     @param scalar scalar value to multiply with.
 520     @param l Vector to multiply.
 521
 522     Multiply the Vector with a scalar value. */
 523 inline FGLocation operator*(double scalar, const FGLocation& l)
 524 {
 525   return l.operator*(scalar);
 526 }
 527
 528 } // namespace JSBSim
 529
 530 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 531 #endif