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