src/FDM/JSBSim/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 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 General Public License for more
  18  details.
  19
  20  You should have received a copy of the GNU 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 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 "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 W. C. Durham "Aircraft Dynamics & Control", section 2.2
 132
 133     @author Mathias Froehlich
 134     @version $Id$
 135   */
 136
 137 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 138 CLASS DECLARATION
 139 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
 140
 141 class FGLocation : virtual FGJSBBase
 142 {
 143 public:
 144   /** Default constructor. */
 145   FGLocation() { mCacheValid = false; }
 146
 147   /** Constructor to set the longitude, latitude and the distance
 148       from the center of the earth.
 149       @param lon longitude
 150       @param lat GEOCENTRIC latitude
 151       @param distance from center of earth to vehicle in feet*/
 152   FGLocation(double lon, double lat, double radius);
 153
 154   /** Copy constructor. */
 155   FGLocation(const FGColumnVector3& lv)
 156     : mECLoc(lv), mCacheValid(false) {}
 157
 158   /** Copy constructor. */
 159   FGLocation(const FGLocation& l)
 160     : mECLoc(l.mECLoc), mCacheValid(l.mCacheValid) {
 161     if (!mCacheValid)
 162       return;
 163
 164     mLon = l.mLon;
 165     mLat = l.mLat;
 166     mRadius = l.mRadius;
 167
 168     mTl2ec = l.mTl2ec;
 169     mTec2l = l.mTec2l;
 170   }
 171
 172   /** Get the longitude.
 173       @return the longitude in rad of the location represented with this
 174       class instance. The returned values are in the range between
 175       -pi <= lon <= pi. Longitude is positive east and negative west. */
 176   double GetLongitude() const { ComputeDerived(); return mLon; }
 177
 178   /** Get the longitude.
 179       @return the longitude in deg of the location represented with this
 180       class instance. The returned values are in the range between
 181       -180 <= lon <= 180.  Longitude is positive east and negative west. */
 182   double GetLongitudeDeg() const { ComputeDerived(); return radtodeg*mLon; }
 183
 184   /** Set the longitude.
 185       @param longitude Longitude in rad to set.
 186       Sets the longitude of the location represented with this class
 187       instance to the value of the given argument. The value is meant
 188       to be in rad. The latitude and the radius value are preserved
 189       with this call with the exception of radius being equal to
 190       zero. If the radius is previously set to zero it is changed to be
 191       equal to 1.0 past this call. Longitude is positive east and negative west. */
 192   void SetLongitude(double longitude);
 193
 194   /** Get the sine of Longitude. */
 195   double GetSinLongitude() const { ComputeDerived(); return -mTec2l(2,1); }
 196
 197   /** Get the cosine of Longitude. */
 198   double GetCosLongitude() const { ComputeDerived(); return mTec2l(2,2); }
 199
 200   /** Get the latitude.
 201       @return the latitude in rad of the location represented with this
 202       class instance. The returned values are in the range between
 203       -pi/2 <= lon <= pi/2. Latitude is positive north and negative south. */
 204   double GetLatitude() const { ComputeDerived(); return mLat; }
 205
 206   /** Get the latitude.
 207       @return the latitude in deg of the location represented with this
 208       class instance. The returned values are in the range between
 209       -90 <= lon <= 90. Latitude is positive north and negative south. */
 210   double GetLatitudeDeg() const { ComputeDerived(); return radtodeg*mLat; }
 211
 212   /** Set the latitude.
 213       @param latitude Latitude in rad to set.
 214       Sets the latitude of the location represented with this class
 215       instance to the value of the given argument. The value is meant
 216       to be in rad. The longitude and the radius value are preserved
 217       with this call with the exception of radius being equal to
 218       zero. If the radius is previously set to zero it is changed to be
 219       equal to 1.0 past this call.
 220       Latitude is positive north and negative south.
 221       The arguments should be within the bounds of -pi/2 <= lat <= pi/2.
 222       The behavior of this function with arguments outside this range is
 223       left as an exercise to the gentle reader ... */
 224   void SetLatitude(double latitude);
 225
 226   /** Get the sine of Latitude. */
 227   double GetSinLatitude() const { ComputeDerived(); return -mTec2l(3,3); }
 228
 229   /** Get the cosine of Latitude. */
 230   double GetCosLatitude() const { ComputeDerived(); return mTec2l(1,3); }
 231
 232   /** Get the cosine of Latitude. */
 233   double GetTanLatitude() const {
 234     ComputeDerived();
 235     double cLat = mTec2l(1,3);
 236     if (cLat == 0.0)
 237       return 0.0;
 238     else
 239       return -mTec2l(3,3)/cLat;
 240   }
 241
 242   /** Get the distance from the center of the earth.
 243       @return the distance of the location represented with this class
 244       instance to the center of the earth in ft. The radius value is
 245       always positive. */
 246   double GetRadius() const { ComputeDerived(); return mRadius; }
 247
 248   /** Set the distance from the center of the earth.
 249       @param radius Radius in ft to set.
 250       Sets the radius of the location represented with this class
 251       instance to the value of the given argument. The value is meant
 252       to be in ft. The latitude and longitude values are preserved
 253       with this call with the exception of radius being equal to
 254       zero. If the radius is previously set to zero, latitude and
 255       longitude is set equal to zero past this call.
 256       The argument should be positive.
 257       The behavior of this function called with a negative argument is
 258       left as an exercise to the gentle reader ... */
 259   void SetRadius(double radius);
 260
 261   /** Transform matrix from local horizontal to earth centered frame.
 262       Returns a const reference to the rotation matrix of the transform from
 263       the local horizontal frame to the earth centered frame. */
 264   const FGMatrix33& GetTl2ec(void) const { ComputeDerived(); return mTl2ec; }
 265
 266   /** Transform matrix from the earth centered to local horizontal frame.
 267       Returns a const reference to the rotation matrix of the transform from
 268       the earth centered frame to the local horizontal frame. */
 269   const FGMatrix33& GetTec2l(void) const { ComputeDerived(); return mTec2l; }
 270
 271   /** Conversion from Local frame coordinates to a location in the
 272       earth centered and fixed frame.
 273       @parm lvec Vector in the local horizontal coordinate frame
 274       @return The location in the earth centered and fixed frame */
 275   FGLocation LocalToLocation(const FGColumnVector3& lvec) const {
 276     ComputeDerived(); return mTl2ec*lvec + mECLoc;
 277   }
 278
 279   /** Conversion from a location in the earth centered and fixed frame
 280       to local horizontal frame coordinates.
 281       @parm ecvec Vector in the earth centered and fixed frame
 282       @return The vector in the local horizontal coordinate frame */
 283   FGColumnVector3 LocationToLocal(const FGColumnVector3& ecvec) const {
 284     ComputeDerived(); return mTec2l*(ecvec - mECLoc);
 285   }
 286
 287   // For time-stepping, locations have vector properties...
 288
 289   /** Read access the entries of the vector.
 290       @param idx the component index.
 291       Return the value of the matrix entry at the given index.
 292       Indices are counted starting with 1.
 293       Note that the index given in the argument is unchecked. */
 294   double operator()(unsigned int idx) const { return Entry(idx); }
 295
 296   /** Write access the entries of the vector.
 297       @param idx the component index.
 298       @return a reference to the vector entry at the given index.
 299       Indices are counted starting with 1.
 300       Note that the index given in the argument is unchecked. */
 301   double& operator()(unsigned int idx) { return Entry(idx); }
 302
 303   /** Read access the entries of the vector.
 304       @param idx the component index.
 305       @return the value of the matrix entry at the given index.
 306       Indices are counted starting with 1.
 307       This function is just a shortcut for the @ref double
 308       operator()(unsigned int idx) const function. It is
 309       used internally to access the elements in a more convenient way.
 310       Note that the index given in the argument is unchecked. */
 311   double Entry(unsigned int idx) const { return mECLoc.Entry(idx); }
 312
 313   /** Write access the entries of the vector.
 314       @param idx the component index.
 315       @return a reference to the vector entry at the given index.
 316       Indices are counted starting with 1.
 317       This function is just a shortcut for the double&
 318       operator()(unsigned int idx) function. It is
 319       used internally to access the elements in a more convenient way.
 320       Note that the index given in the argument is unchecked. */
 321   double& Entry(unsigned int idx) {
 322     mCacheValid = false; return mECLoc.Entry(idx);
 323   }
 324
 325   const FGLocation& operator=(const FGLocation& l) {
 326     mECLoc = l.mECLoc;
 327     mCacheValid = l.mCacheValid;
 328     if (!mCacheValid)
 329       return *this;
 330
 331     mLon = l.mLon;
 332     mLat = l.mLat;
 333     mRadius = l.mRadius;
 334
 335     mTl2ec = l.mTl2ec;
 336     mTec2l = l.mTec2l;
 337
 338     return *this;
 339   }
 340   bool operator==(const FGLocation& l) const {
 341     return mECLoc == l.mECLoc;
 342   }
 343   bool operator!=(const FGLocation& l) const { return ! operator==(l); }
 344   const FGLocation& operator+=(const FGLocation &l) {
 345     mCacheValid = false;
 346     mECLoc += l.mECLoc;
 347     return *this;
 348   }
 349   const FGLocation& operator-=(const FGLocation &l) {
 350     mCacheValid = false;
 351     mECLoc -= l.mECLoc;
 352     return *this;
 353   }
 354   const FGLocation& operator*=(double scalar) {
 355     mCacheValid = false;
 356     mECLoc *= scalar;
 357     return *this;
 358   }
 359   const FGLocation& operator/=(double scalar) {
 360     return operator*=(1.0/scalar);
 361   }
 362   FGLocation operator+(const FGLocation& l) const {
 363     return FGLocation(mECLoc + l.mECLoc);
 364   }
 365   FGLocation operator-(const FGLocation& l) const {
 366     return FGLocation(mECLoc - l.mECLoc);
 367   }
 368
 369   FGLocation operator*(double scalar) const {
 370     return FGLocation(scalar*mECLoc);
 371   }
 372
 373   /** Cast to a simple 3d vector */
 374   operator const FGColumnVector3&() const {
 375     return mECLoc;
 376   }
 377
 378   /** Ties into the property tree.
 379       Ties the variables represented by this class into the property tree. */
 380   void bind(FGPropertyManager*, const string&) const;
 381
 382   /** Remove from property tree.
 383       Unties the variables represented by this class into the property tree. */
 384   void unbind(FGPropertyManager*, const string&) const;
 385
 386 private:
 387   /** Computation of derived values.
 388       This function re-computes the derived values like lat/lon and
 389       transformation matrices. It does this unconditionally. */
 390   void ComputeDerivedUnconditional(void) const;
 391
 392   /** Computation of derived values.
 393       This function checks if the derived values like lat/lon and
 394       transformation matrices are already computed. If so, it
 395       returns. If they need to be computed this is done here. */
 396   void ComputeDerived(void) const {
 397     if (!mCacheValid)
 398       ComputeDerivedUnconditional();
 399   }
 400
 401   /** The coordinates in the earth centered frame. This is the master copy.
 402       The coordinate frame has its center in the middle of the earth.
 403       Its x-axis points from the center of the earth towards a
 404       location with zero latitude and longitude on the earths
 405       surface. The y-axis points from the center of the earth towards a
 406       location with zero latitude and 90deg longitude on the earths
 407       surface. The z-axis points from the earths center to the
 408       geographic north pole.
 409       @see W. C. Durham "Aircraft Dynamics & Control", section 2.2 */
 410   FGColumnVector3 mECLoc;
 411
 412   /** The cached lon/lat/radius values. */
 413   mutable double mLon;
 414   mutable double mLat;
 415   mutable double mRadius;
 416
 417   /** The cached rotation matrices from and to the associated frames. */
 418   mutable FGMatrix33 mTl2ec;
 419   mutable FGMatrix33 mTec2l;
 420
 421   /** A data validity flag.
 422       This class implements caching of the derived values like the
 423       orthogonal rotation matrices or the lon/lat/radius values. For caching we
 424       carry a flag which signals if the values are valid or not.
 425       The C++ keyword "mutable" tells the compiler that the data member is
 426       allowed to change during a const member function. */
 427   mutable bool mCacheValid;
 428 };
 429
 430 /** Scalar multiplication.
 431
 432     @param scalar scalar value to multiply with.
 433     @param l Vector to multiply.
 434
 435     Multiply the Vector with a scalar value. */
 436 inline FGLocation operator*(double scalar, const FGLocation& l)
 437 {
 438   return l.operator*(scalar);
 439 }
 440
 441 } // namespace JSBSim
 442
 443 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 444 #endif