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