]> git.mxchange.org Git - flightgear.git/blob - src/FDM/JSBSim/FGLocation.h
Sync. w. JSBSim CVS
[flightgear.git] / 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