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 (jon@jsbsim.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 longitude, latitude and the distance above the reference ellipsoid.
 237       @param lon longitude in radians
 238       @param lat GEODETIC latitude in radians
 239       @param height distance above the reference ellipsoid to vehicle in feet*/
 240   void SetPositionGeodetic(double lon, double lat, double height);
 241
 242   /** Sets the semimajor and semiminor axis lengths for this planet.
 243       The eccentricity and flattening are calculated from the semimajor
 244       and semiminor axis lengths */
 245   void SetEllipse(double semimajor, double semiminor);
 246
 247   /** Get the longitude.
 248       @return the longitude in rad of the location represented with this
 249       class instance. The returned values are in the range between
 250       -pi <= lon <= pi. Longitude is positive east and negative west. */
 251   double GetLongitude() const { ComputeDerived(); return mLon; }
 252
 253   /** Get the longitude.
 254       @return the longitude in deg of the location represented with this
 255       class instance. The returned values are in the range between
 256       -180 <= lon <= 180.  Longitude is positive east and negative west. */
 257   double GetLongitudeDeg() const { ComputeDerived(); return radtodeg*mLon; }
 258
 259   /** Get the sine of Longitude. */
 260   double GetSinLongitude() const { ComputeDerived(); return -mTec2l(2,1); }
 261
 262   /** Get the cosine of Longitude. */
 263   double GetCosLongitude() const { ComputeDerived(); return mTec2l(2,2); }
 264
 265   /** Get the latitude.
 266       @return the 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 GetLatitude() const { ComputeDerived(); return mLat; }
 270
 271   /** Get the geodetic latitude.
 272       @return the geodetic latitude in rad of the location represented with this
 273       class instance. The returned values are in the range between
 274       -pi/2 <= lon <= pi/2. Latitude is positive north and negative south. */
 275   double GetGeodLatitudeRad(void) const { ComputeDerived(); return mGeodLat; }
 276
 277   /** Get the latitude.
 278       @return the latitude in deg of the location represented with this
 279       class instance. The returned value is in the range between
 280       -90 <= lon <= 90. Latitude is positive north and negative south. */
 281   double GetLatitudeDeg() const { ComputeDerived(); return radtodeg*mLat; }
 282
 283   /** Get the geodetic latitude in degrees.
 284       @return the geodetic latitude in degrees of the location represented by
 285       this class instance. The returned value is in the range between
 286       -90 <= lon <= 90. Latitude is positive north and negative south. */
 287   double GetGeodLatitudeDeg(void) const { ComputeDerived(); return radtodeg*mGeodLat; }
 288
 289   /** Gets the geodetic altitude in feet. */
 290   double GetGeodAltitude(void) const { return GeodeticAltitude;}
 291
 292   /** Get the sine of Latitude. */
 293   double GetSinLatitude() const { ComputeDerived(); return -mTec2l(3,3); }
 294
 295   /** Get the cosine of Latitude. */
 296   double GetCosLatitude() const { ComputeDerived(); return mTec2l(1,3); }
 297
 298   /** Get the cosine of Latitude. */
 299   double GetTanLatitude() const {
 300     ComputeDerived();
 301     double cLat = mTec2l(1,3);
 302     if (cLat == 0.0)
 303       return 0.0;
 304     else
 305       return -mTec2l(3,3)/cLat;
 306   }
 307
 308   /** Get the distance from the center of the earth.
 309       @return the distance of the location represented with this class
 310       instance to the center of the earth in ft. The radius value is
 311       always positive. */
 312   double GetRadius() const { ComputeDerived(); return mRadius; }
 313
 314   /** Transform matrix from local horizontal to earth centered frame.
 315       Returns a const reference to the rotation matrix of the transform from
 316       the local horizontal frame to the earth centered frame. */
 317   const FGMatrix33& GetTl2ec(void) const { ComputeDerived(); return mTl2ec; }
 318
 319   /** Transform matrix from the earth centered to local horizontal frame.
 320       Returns a const reference to the rotation matrix of the transform from
 321       the earth centered frame to the local horizontal frame. */
 322   const FGMatrix33& GetTec2l(void) const { ComputeDerived(); return mTec2l; }
 323
 324   /** Transform matrix from inertial to earth centered frame.
 325       Returns a const reference to the rotation matrix of the transform from
 326       the inertial frame to the earth centered frame (ECI to ECEF). */
 327   const FGMatrix33& GetTi2ec(double epa);
 328
 329   /** Transform matrix from the earth centered to inertial frame.
 330       Returns a const reference to the rotation matrix of the transform from
 331       the earth centered frame to the inertial frame (ECEF to ECI). */
 332   const FGMatrix33& GetTec2i(double epa);
 333
 334   /** Conversion from Local frame coordinates to a location in the
 335       earth centered and fixed frame.
 336       @param lvec Vector in the local horizontal coordinate frame
 337       @return The location in the earth centered and fixed frame */
 338   FGLocation LocalToLocation(const FGColumnVector3& lvec) const {
 339     ComputeDerived(); return mTl2ec*lvec + mECLoc;
 340   }
 341
 342   /** Conversion from a location in the earth centered and fixed frame
 343       to local horizontal frame coordinates.
 344       @param ecvec Vector in the earth centered and fixed frame
 345       @return The vector in the local horizontal coordinate frame */
 346   FGColumnVector3 LocationToLocal(const FGColumnVector3& ecvec) const {
 347     ComputeDerived(); return mTec2l*(ecvec - mECLoc);
 348   }
 349
 350   // For time-stepping, locations have vector properties...
 351
 352   /** Read access the entries of the vector.
 353       @param idx the component index.
 354       Return the value of the matrix entry at the given index.
 355       Indices are counted starting with 1.
 356       Note that the index given in the argument is unchecked. */
 357   double operator()(unsigned int idx) const { return Entry(idx); }
 358
 359   /** Write access the entries of the vector.
 360       @param idx the component index.
 361       @return a reference to the vector entry at the given index.
 362       Indices are counted starting with 1.
 363       Note that the index given in the argument is unchecked. */
 364   double& operator()(unsigned int idx) { return Entry(idx); }
 365
 366   /** Read access the entries of the vector.
 367       @param idx the component index.
 368       @return the value of the matrix entry at the given index.
 369       Indices are counted starting with 1.
 370       This function is just a shortcut for the <tt>double
 371       operator()(unsigned int idx) const</tt> function. It is
 372       used internally to access the elements in a more convenient way.
 373       Note that the index given in the argument is unchecked. */
 374   double Entry(unsigned int idx) const { return mECLoc.Entry(idx); }
 375
 376   /** Write access the entries of the vector.
 377       @param idx the component index.
 378       @return a reference to the vector entry at the given index.
 379       Indices are counted starting with 1.
 380       This function is just a shortcut for the double&
 381       operator()(unsigned int idx) function. It is
 382       used internally to access the elements in a more convenient way.
 383       Note that the index given in the argument is unchecked. */
 384   double& Entry(unsigned int idx) {
 385     mCacheValid = false; return mECLoc.Entry(idx);
 386   }
 387
 388   const FGLocation& operator=(const FGColumnVector3& v)
 389   {
 390     mECLoc(eX) = v(eX);
 391     mECLoc(eY) = v(eY);
 392     mECLoc(eZ) = v(eZ);
 393     mCacheValid = false;
 394     ComputeDerived();
 395     return *this;
 396   }
 397
 398   const FGLocation& operator=(const FGLocation& l)
 399   {
 400     mECLoc = l.mECLoc;
 401     mCacheValid = l.mCacheValid;
 402
 403 //    if (!mCacheValid) return *this; // Why is this here for an assignment operator?
 404
 405     mLon = l.mLon;
 406     mLat = l.mLat;
 407     mRadius = l.mRadius;
 408
 409     mTl2ec = l.mTl2ec;
 410     mTec2l = l.mTec2l;
 411
 412     a = l.a;
 413     b = l.b;
 414     a2 = l.a2;
 415     b2 = l.b2;
 416     e2 = l.e2;
 417     e = l.e;
 418     eps2 = l.eps2;
 419     f = l.f;
 420
 421     initial_longitude = l.initial_longitude;
 422
 423     return *this;
 424   }
 425   bool operator==(const FGLocation& l) const {
 426     return mECLoc == l.mECLoc;
 427   }
 428   bool operator!=(const FGLocation& l) const { return ! operator==(l); }
 429   const FGLocation& operator+=(const FGLocation &l) {
 430     mCacheValid = false;
 431     mECLoc += l.mECLoc;
 432     return *this;
 433   }
 434   const FGLocation& operator-=(const FGLocation &l) {
 435     mCacheValid = false;
 436     mECLoc -= l.mECLoc;
 437     return *this;
 438   }
 439   const FGLocation& operator*=(double scalar) {
 440     mCacheValid = false;
 441     mECLoc *= scalar;
 442     return *this;
 443   }
 444   const FGLocation& operator/=(double scalar) {
 445     return operator*=(1.0/scalar);
 446   }
 447   FGLocation operator+(const FGLocation& l) const {
 448     return FGLocation(mECLoc + l.mECLoc);
 449   }
 450   FGLocation operator-(const FGLocation& l) const {
 451     return FGLocation(mECLoc - l.mECLoc);
 452   }
 453
 454   FGLocation operator*(double scalar) const {
 455     return FGLocation(scalar*mECLoc);
 456   }
 457
 458   /** Cast to a simple 3d vector */
 459   operator const FGColumnVector3&() const {
 460     return mECLoc;
 461   }
 462
 463 private:
 464   /** Computation of derived values.
 465       This function re-computes the derived values like lat/lon and
 466       transformation matrices. It does this unconditionally. */
 467   void ComputeDerivedUnconditional(void) const;
 468
 469   /** Computation of derived values.
 470       This function checks if the derived values like lat/lon and
 471       transformation matrices are already computed. If so, it
 472       returns. If they need to be computed this is done here. */
 473   void ComputeDerived(void) const {
 474     if (!mCacheValid)
 475       ComputeDerivedUnconditional();
 476   }
 477
 478   /** The coordinates in the earth centered frame. This is the master copy.
 479       The coordinate frame has its center in the middle of the earth.
 480       Its x-axis points from the center of the earth towards a
 481       location with zero latitude and longitude on the earths
 482       surface. The y-axis points from the center of the earth towards a
 483       location with zero latitude and 90deg longitude on the earths
 484       surface. The z-axis points from the earths center to the
 485       geographic north pole.
 486       @see W. C. Durham "Aircraft Dynamics & Control", section 2.2 */
 487   FGColumnVector3 mECLoc;
 488
 489   /** The cached lon/lat/radius values. */
 490   mutable double mLon;
 491   mutable double mLat;
 492   mutable double mRadius;
 493   mutable double mGeodLat;
 494   mutable double GeodeticAltitude;
 495
 496   double initial_longitude;
 497
 498   /** The cached rotation matrices from and to the associated frames. */
 499   mutable FGMatrix33 mTl2ec;
 500   mutable FGMatrix33 mTec2l;
 501   mutable FGMatrix33 mTi2ec;
 502   mutable FGMatrix33 mTec2i;
 503
 504   /* Terms for geodetic latitude calculation. Values are from WGS84 model */
 505   double a;    // Earth semimajor axis in feet (6,378,137.0 meters)
 506   double b;    // Earth semiminor axis in feet (6,356,752.3142 meters)
 507   double a2;
 508   double b2;
 509   double e;    // Earth eccentricity
 510   double e2;   // Earth eccentricity squared
 511   double eps2; //
 512   double f;    // Flattening
 513
 514   /** A data validity flag.
 515       This class implements caching of the derived values like the
 516       orthogonal rotation matrices or the lon/lat/radius values. For caching we
 517       carry a flag which signals if the values are valid or not.
 518       The C++ keyword "mutable" tells the compiler that the data member is
 519       allowed to change during a const member function. */
 520   mutable bool mCacheValid;
 521 };
 522
 523 /** Scalar multiplication.
 524
 525     @param scalar scalar value to multiply with.
 526     @param l Vector to multiply.
 527
 528     Multiply the Vector with a scalar value. */
 529 inline FGLocation operator*(double scalar, const FGLocation& l)
 530 {
 531   return l.operator*(scalar);
 532 }
 533
 534 } // namespace JSBSim
 535
 536 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 537 #endif