1 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
4 Author: Jon S. Berndt, Mathias Froehlich
5 Date started: 04/04/2004
7 ------- Copyright (C) 1999 Jon S. Berndt (jon@jsbsim.org) ------------------
8 ------- (C) 2004 Mathias Froehlich (Mathias.Froehlich@web.de) ----
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
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
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.
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.
28 -------------------------------------------------------------------------------
29 04/04/2004 MF Created from code previously in the old positions class.
31 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
33 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
38 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
40 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
42 #include "FGJSBBase.h"
43 #include "input_output/FGPropertyManager.h"
44 #include "FGColumnVector3.h"
45 #include "FGMatrix33.h"
47 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
49 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
51 #define ID_LOCATION "$Id: FGLocation.h,v 1.28 2011/08/04 12:46:32 jberndt Exp $"
53 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
55 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
59 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
61 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
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.
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.
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.
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.
94 The Earth centered reference frame is NOT an inertial frame since it rotates
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).
107 Accuracy and round off
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
115 we have an accuracy of about
117 1e-16*2e7ft/1 = 2e-9 ft
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.
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.
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.
141 @see Stevens and Lewis, "Aircraft Control and Simulation", Second edition
142 @see W. C. Durham "Aircraft Dynamics & Control", section 2.2
144 @author Mathias Froehlich
145 @version $Id: FGLocation.h,v 1.28 2011/08/04 12:46:32 jberndt Exp $
148 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
150 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
152 class FGLocation : public FGJSBBase
155 /** Default constructor. */
158 /** Constructor to set the longitude, latitude and the distance
159 from the center of the earth.
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);
165 /** Column constructor. */
166 FGLocation(const FGColumnVector3& lv);
168 /** Copy constructor. */
169 FGLocation(const FGLocation& l);
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);
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);
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);
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);
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);
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);
225 /** Sets the Earth position angle.
226 This is the relative orientation of the ECEF frame with respect to the
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;}
232 /** Increments the Earth position angle.
233 This is the relative orientation of the ECEF frame with respect to the
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;}
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; }
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; }
251 /** Get the sine of Longitude. */
252 double GetSinLongitude() const { ComputeDerived(); return -mTec2l(2,1); }
254 /** Get the cosine of Longitude. */
255 double GetCosLongitude() const { ComputeDerived(); return mTec2l(2,2); }
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; }
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; }
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; }
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; }
281 /** Gets the geodetic altitude in feet. */
282 double GetGeodAltitude(void) const {ComputeDerived(); return GeodeticAltitude;}
284 /** Get the sine of Latitude. */
285 double GetSinLatitude() const { ComputeDerived(); return -mTec2l(3,3); }
287 /** Get the cosine of Latitude. */
288 double GetCosLatitude() const { ComputeDerived(); return mTec2l(1,3); }
290 /** Get the cosine of Latitude. */
291 double GetTanLatitude() const {
293 double cLat = mTec2l(1,3);
297 return -mTec2l(3,3)/cLat;
300 double GetEPA() const {return epa;}
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
306 //double GetRadius() const { return mECLoc.Magnitude(); } // may not work with FlightGear
307 double GetRadius() const { ComputeDerived(); return mRadius; }
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; }
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; }
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);
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);
329 const FGMatrix33& GetTi2l(void) const {ComputeDerived(); return mTi2l;}
331 const FGMatrix33& GetTl2i(void) const {ComputeDerived(); return mTl2i;}
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;
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);
349 // For time-stepping, locations have vector properties...
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); }
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); }
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); }
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);
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)
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);
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;
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); }
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
422 const FGLocation& operator+=(const FGLocation &l) {
428 const FGLocation& operator-=(const FGLocation &l) {
434 const FGLocation& operator*=(double scalar) {
440 const FGLocation& operator/=(double scalar) {
441 return operator*=(1.0/scalar);
444 FGLocation operator+(const FGLocation& l) const {
445 return FGLocation(mECLoc + l.mECLoc);
448 FGLocation operator-(const FGLocation& l) const {
449 return FGLocation(mECLoc - l.mECLoc);
452 FGLocation operator*(double scalar) const {
453 return FGLocation(scalar*mECLoc);
456 /** Cast to a simple 3d vector */
457 operator const FGColumnVector3&() const {
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;
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 {
473 ComputeDerivedUnconditional();
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;
487 /** The cached lon/lat/radius values. */
490 mutable double mRadius;
491 mutable double mGeodLat;
492 mutable double GeodeticAltitude;
494 double initial_longitude;
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;
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)
511 double e; // Earth eccentricity
512 double e2; // Earth eccentricity squared
514 double f; // Flattening
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;
525 /** Scalar multiplication.
527 @param scalar scalar value to multiply with.
528 @param l Vector to multiply.
530 Multiply the Vector with a scalar value. */
531 inline FGLocation operator*(double scalar, const FGLocation& l)
533 return l.operator*(scalar);
536 } // namespace JSBSim
538 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%