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$"
53 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
55 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
59 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
61 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
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.
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.
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.
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.
88 The earth centered reference frame is *NOT* an inertial frame
89 since it rotates with the earth.
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
102 Accuracy and round off:
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.
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.
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.
131 @see Stevens and Lewis, "Aircraft Control and Simulation", Second edition
132 @see W. C. Durham "Aircraft Dynamics & Control", section 2.2
134 @author Mathias Froehlich
138 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
140 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
142 class FGLocation : virtual FGJSBBase
145 /** Default constructor. */
148 /** Constructor to set the longitude, latitude and the distance
149 from the center of the earth.
151 @param lat GEOCENTRIC latitude
152 @param radius distance from center of earth to vehicle in feet*/
153 FGLocation(double lon, double lat, double radius);
155 /** Column constructor. */
156 FGLocation(const FGColumnVector3& lv) : mECLoc(lv), mCacheValid(false)
168 /** Copy constructor. */
169 FGLocation(const FGLocation& l)
170 : mECLoc(l.mECLoc), mCacheValid(l.mCacheValid)
172 // if (!mCacheValid) return; // This doesn't seem right.
190 initial_longitude = l.initial_longitude;
193 /** Set the longitude.
194 @param longitude Longitude in rad to set.
195 Sets the longitude of the location represented with this class
196 instance to the value of the given argument. The value is meant
197 to be in rad. The latitude and the radius value are preserved
198 with this call with the exception of radius being equal to
199 zero. If the radius is previously set to zero it is changed to be
200 equal to 1.0 past this call. Longitude is positive east and negative west. */
201 void SetLongitude(double longitude);
203 /** Set the latitude.
204 @param latitude Latitude in rad to set.
205 Sets the latitude of the location represented with this class
206 instance to the value of the given argument. The value is meant
207 to be in rad. The longitude and the radius value are preserved
208 with this call with the exception of radius being equal to
209 zero. If the radius is previously set to zero it is changed to be
210 equal to 1.0 past this call.
211 Latitude is positive north and negative south.
212 The arguments should be within the bounds of -pi/2 <= lat <= pi/2.
213 The behavior of this function with arguments outside this range is
214 left as an exercise to the gentle reader ... */
215 void SetLatitude(double latitude);
217 /** Set the distance from the center of the earth.
218 @param radius Radius in ft to set.
219 Sets the radius of the location represented with this class
220 instance to the value of the given argument. The value is meant
221 to be in ft. The latitude and longitude values are preserved
222 with this call with the exception of radius being equal to
223 zero. If the radius is previously set to zero, latitude and
224 longitude is set equal to zero past this call.
225 The argument should be positive.
226 The behavior of this function called with a negative argument is
227 left as an exercise to the gentle reader ... */
228 void SetRadius(double radius);
230 /** Sets the longitude, latitude and the distance from the center of the earth.
231 @param lon longitude in radians
232 @param lat GEOCENTRIC latitude in radians
233 @param radius distance from center of earth to vehicle in feet*/
234 void SetPosition(double lon, double lat, double radius);
236 /** Sets the longitude, latitude and the distance above the reference ellipsoid.
237 @param lon longitude in radians
238 @param lat GEODETIC latitude in radians
239 @param height distance above the reference ellipsoid to vehicle in feet*/
240 void SetPositionGeodetic(double lon, double lat, double height);
242 /** Sets the semimajor and semiminor axis lengths for this planet.
243 The eccentricity and flattening are calculated from the semimajor
244 and semiminor axis lengths */
245 void SetEllipse(double semimajor, double semiminor);
247 /** Get the longitude.
248 @return the longitude in rad of the location represented with this
249 class instance. The returned values are in the range between
250 -pi <= lon <= pi. Longitude is positive east and negative west. */
251 double GetLongitude() const { ComputeDerived(); return mLon; }
253 /** Get the longitude.
254 @return the longitude in deg of the location represented with this
255 class instance. The returned values are in the range between
256 -180 <= lon <= 180. Longitude is positive east and negative west. */
257 double GetLongitudeDeg() const { ComputeDerived(); return radtodeg*mLon; }
259 /** Get the sine of Longitude. */
260 double GetSinLongitude() const { ComputeDerived(); return -mTec2l(2,1); }
262 /** Get the cosine of Longitude. */
263 double GetCosLongitude() const { ComputeDerived(); return mTec2l(2,2); }
265 /** Get the latitude.
266 @return the latitude in rad of the location represented with this
267 class instance. The returned values are in the range between
268 -pi/2 <= lon <= pi/2. Latitude is positive north and negative south. */
269 double GetLatitude() const { ComputeDerived(); return mLat; }
271 /** Get the geodetic latitude.
272 @return the geodetic latitude in rad of the location represented with this
273 class instance. The returned values are in the range between
274 -pi/2 <= lon <= pi/2. Latitude is positive north and negative south. */
275 double GetGeodLatitudeRad(void) const { ComputeDerived(); return mGeodLat; }
277 /** Get the latitude.
278 @return the latitude in deg of the location represented with this
279 class instance. The returned value is in the range between
280 -90 <= lon <= 90. Latitude is positive north and negative south. */
281 double GetLatitudeDeg() const { ComputeDerived(); return radtodeg*mLat; }
283 /** Get the geodetic latitude in degrees.
284 @return the geodetic latitude in degrees of the location represented by
285 this class instance. The returned value is in the range between
286 -90 <= lon <= 90. Latitude is positive north and negative south. */
287 double GetGeodLatitudeDeg(void) const { ComputeDerived(); return radtodeg*mGeodLat; }
289 /** Gets the geodetic altitude in feet. */
290 double GetGeodAltitude(void) const { return GeodeticAltitude;}
292 /** Get the sine of Latitude. */
293 double GetSinLatitude() const { ComputeDerived(); return -mTec2l(3,3); }
295 /** Get the cosine of Latitude. */
296 double GetCosLatitude() const { ComputeDerived(); return mTec2l(1,3); }
298 /** Get the cosine of Latitude. */
299 double GetTanLatitude() const {
301 double cLat = mTec2l(1,3);
305 return -mTec2l(3,3)/cLat;
308 /** Get the distance from the center of the earth.
309 @return the distance of the location represented with this class
310 instance to the center of the earth in ft. The radius value is
312 double GetRadius() const { ComputeDerived(); return mRadius; }
314 /** Transform matrix from local horizontal to earth centered frame.
315 Returns a const reference to the rotation matrix of the transform from
316 the local horizontal frame to the earth centered frame. */
317 const FGMatrix33& GetTl2ec(void) const { ComputeDerived(); return mTl2ec; }
319 /** Transform matrix from the earth centered to local horizontal frame.
320 Returns a const reference to the rotation matrix of the transform from
321 the earth centered frame to the local horizontal frame. */
322 const FGMatrix33& GetTec2l(void) const { ComputeDerived(); return mTec2l; }
324 /** Transform matrix from inertial to earth centered frame.
325 Returns a const reference to the rotation matrix of the transform from
326 the inertial frame to the earth centered frame (ECI to ECEF). */
327 const FGMatrix33& GetTi2ec(double epa);
329 /** Transform matrix from the earth centered to inertial frame.
330 Returns a const reference to the rotation matrix of the transform from
331 the earth centered frame to the inertial frame (ECEF to ECI). */
332 const FGMatrix33& GetTec2i(double epa);
334 /** Conversion from Local frame coordinates to a location in the
335 earth centered and fixed frame.
336 @param lvec Vector in the local horizontal coordinate frame
337 @return The location in the earth centered and fixed frame */
338 FGLocation LocalToLocation(const FGColumnVector3& lvec) const {
339 ComputeDerived(); return mTl2ec*lvec + mECLoc;
342 /** Conversion from a location in the earth centered and fixed frame
343 to local horizontal frame coordinates.
344 @param ecvec Vector in the earth centered and fixed frame
345 @return The vector in the local horizontal coordinate frame */
346 FGColumnVector3 LocationToLocal(const FGColumnVector3& ecvec) const {
347 ComputeDerived(); return mTec2l*(ecvec - mECLoc);
350 // For time-stepping, locations have vector properties...
352 /** Read access the entries of the vector.
353 @param idx the component index.
354 Return the value of the matrix entry at the given index.
355 Indices are counted starting with 1.
356 Note that the index given in the argument is unchecked. */
357 double operator()(unsigned int idx) const { return Entry(idx); }
359 /** Write access the entries of the vector.
360 @param idx the component index.
361 @return a reference to the vector entry at the given index.
362 Indices are counted starting with 1.
363 Note that the index given in the argument is unchecked. */
364 double& operator()(unsigned int idx) { return Entry(idx); }
366 /** Read access the entries of the vector.
367 @param idx the component index.
368 @return the value of the matrix entry at the given index.
369 Indices are counted starting with 1.
370 This function is just a shortcut for the <tt>double
371 operator()(unsigned int idx) const</tt> function. It is
372 used internally to access the elements in a more convenient way.
373 Note that the index given in the argument is unchecked. */
374 double Entry(unsigned int idx) const { return mECLoc.Entry(idx); }
376 /** Write access the entries of the vector.
377 @param idx the component index.
378 @return a reference to the vector entry at the given index.
379 Indices are counted starting with 1.
380 This function is just a shortcut for the double&
381 operator()(unsigned int idx) function. It is
382 used internally to access the elements in a more convenient way.
383 Note that the index given in the argument is unchecked. */
384 double& Entry(unsigned int idx) {
385 mCacheValid = false; return mECLoc.Entry(idx);
388 const FGLocation& operator=(const FGColumnVector3& v)
398 const FGLocation& operator=(const FGLocation& l)
401 mCacheValid = l.mCacheValid;
403 // if (!mCacheValid) return *this; // Why is this here for an assignment operator?
421 initial_longitude = l.initial_longitude;
425 bool operator==(const FGLocation& l) const {
426 return mECLoc == l.mECLoc;
428 bool operator!=(const FGLocation& l) const { return ! operator==(l); }
429 const FGLocation& operator+=(const FGLocation &l) {
434 const FGLocation& operator-=(const FGLocation &l) {
439 const FGLocation& operator*=(double scalar) {
444 const FGLocation& operator/=(double scalar) {
445 return operator*=(1.0/scalar);
447 FGLocation operator+(const FGLocation& l) const {
448 return FGLocation(mECLoc + l.mECLoc);
450 FGLocation operator-(const FGLocation& l) const {
451 return FGLocation(mECLoc - l.mECLoc);
454 FGLocation operator*(double scalar) const {
455 return FGLocation(scalar*mECLoc);
458 /** Cast to a simple 3d vector */
459 operator const FGColumnVector3&() const {
464 /** Computation of derived values.
465 This function re-computes the derived values like lat/lon and
466 transformation matrices. It does this unconditionally. */
467 void ComputeDerivedUnconditional(void) const;
469 /** Computation of derived values.
470 This function checks if the derived values like lat/lon and
471 transformation matrices are already computed. If so, it
472 returns. If they need to be computed this is done here. */
473 void ComputeDerived(void) const {
475 ComputeDerivedUnconditional();
478 /** The coordinates in the earth centered frame. This is the master copy.
479 The coordinate frame has its center in the middle of the earth.
480 Its x-axis points from the center of the earth towards a
481 location with zero latitude and longitude on the earths
482 surface. The y-axis points from the center of the earth towards a
483 location with zero latitude and 90deg longitude on the earths
484 surface. The z-axis points from the earths center to the
485 geographic north pole.
486 @see W. C. Durham "Aircraft Dynamics & Control", section 2.2 */
487 FGColumnVector3 mECLoc;
489 /** The cached lon/lat/radius values. */
492 mutable double mRadius;
493 mutable double mGeodLat;
494 mutable double GeodeticAltitude;
496 double initial_longitude;
498 /** The cached rotation matrices from and to the associated frames. */
499 mutable FGMatrix33 mTl2ec;
500 mutable FGMatrix33 mTec2l;
501 mutable FGMatrix33 mTi2ec;
502 mutable FGMatrix33 mTec2i;
504 /* Terms for geodetic latitude calculation. Values are from WGS84 model */
505 double a; // Earth semimajor axis in feet (6,378,137.0 meters)
506 double b; // Earth semiminor axis in feet (6,356,752.3142 meters)
509 double e; // Earth eccentricity
510 double e2; // Earth eccentricity squared
512 double f; // Flattening
514 /** A data validity flag.
515 This class implements caching of the derived values like the
516 orthogonal rotation matrices or the lon/lat/radius values. For caching we
517 carry a flag which signals if the values are valid or not.
518 The C++ keyword "mutable" tells the compiler that the data member is
519 allowed to change during a const member function. */
520 mutable bool mCacheValid;
523 /** Scalar multiplication.
525 @param scalar scalar value to multiply with.
526 @param l Vector to multiply.
528 Multiply the Vector with a scalar value. */
529 inline FGLocation operator*(double scalar, const FGLocation& l)
531 return l.operator*(scalar);
534 } // namespace JSBSim
536 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%