1 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
5 Date started: 04/04/2004
6 Purpose: Store an arbitrary location on the globe
8 ------- Copyright (C) 1999 Jon S. Berndt (jon@jsbsim.org) ------------------
9 ------- (C) 2004 Mathias Froehlich (Mathias.Froehlich@web.de) ----
11 This program is free software; you can redistribute it and/or modify it under
12 the terms of the GNU Lesser General Public License as published by the Free Software
13 Foundation; either version 2 of the License, or (at your option) any later
16 This program is distributed in the hope that it will be useful, but WITHOUT
17 ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
18 FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License for more
21 You should have received a copy of the GNU Lesser General Public License along with
22 this program; if not, write to the Free Software Foundation, Inc., 59 Temple
23 Place - Suite 330, Boston, MA 02111-1307, USA.
25 Further information about the GNU Lesser General Public License can also be found on
26 the world wide web at http://www.gnu.org.
28 FUNCTIONAL DESCRIPTION
29 ------------------------------------------------------------------------------
30 This class encapsulates an arbitrary position in the globe with its accessors.
31 It has vector properties, so you can add multiply ....
34 ------------------------------------------------------------------------------
37 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
39 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
43 #include "FGLocation.h"
44 #include "input_output/FGPropertyManager.h"
48 static const char *IdSrc = "$Id: FGLocation.cpp,v 1.21 2010/07/02 01:48:12 jberndt Exp $";
49 static const char *IdHdr = ID_LOCATION;
51 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
53 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
55 FGLocation::FGLocation(void)
58 initial_longitude = 0.0;
69 mLon = mLat = mRadius = mGeodLat = GeodeticAltitude = 0.0;
71 // initial_longitude = 0.0;
82 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
84 FGLocation::FGLocation(double lon, double lat, double radius)
88 double sinLat = sin(lat);
89 double cosLat = cos(lat);
90 double sinLon = sin(lon);
91 double cosLon = cos(lon);
102 mECLoc = FGColumnVector3( radius*cosLat*cosLon,
103 radius*cosLat*sinLon,
105 mLon = mLat = mRadius = mGeodLat = GeodeticAltitude = 0.0;
107 // initial_longitude = 0.0
112 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
114 void FGLocation::SetLongitude(double longitude)
116 double rtmp = mECLoc.Magnitude(eX, eY);
117 // Check if we have zero radius.
118 // If so set it to 1, so that we can set a position
119 if (0.0 == mECLoc.Magnitude())
122 // Fast return if we are on the north or south pole ...
128 // Need to figure out how to set the initial_longitude here
130 mECLoc(eX) = rtmp*cos(longitude);
131 mECLoc(eY) = rtmp*sin(longitude);
134 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
136 void FGLocation::SetLatitude(double latitude)
140 double r = mECLoc.Magnitude();
146 double rtmp = mECLoc.Magnitude(eX, eY);
148 double fac = r/rtmp*cos(latitude);
152 mECLoc(eX) = r*cos(latitude);
155 mECLoc(eZ) = r*sin(latitude);
158 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
160 void FGLocation::SetRadius(double radius)
164 double rold = mECLoc.Magnitude();
168 mECLoc *= radius/rold;
171 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
173 void FGLocation::SetPosition(double lon, double lat, double radius)
177 double sinLat = sin(lat);
178 double cosLat = cos(lat);
179 double sinLon = sin(lon);
180 double cosLon = cos(lon);
181 // initial_longitude = lon;
182 mECLoc = FGColumnVector3( radius*cosLat*cosLon,
183 radius*cosLat*sinLon,
188 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
190 void FGLocation::SetPositionGeodetic(double lon, double lat, double height)
196 GeodeticAltitude = height;
198 // initial_longitude = mLon;
200 double RN = a / sqrt(1.0 - e2*sin(mGeodLat)*sin(mGeodLat));
202 mECLoc(eX) = (RN + GeodeticAltitude)*cos(mGeodLat)*cos(mLon);
203 mECLoc(eY) = (RN + GeodeticAltitude)*cos(mGeodLat)*sin(mLon);
204 mECLoc(eZ) = ((1 - e2)*RN + GeodeticAltitude)*sin(mGeodLat);
208 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
210 void FGLocation::SetEllipse(double semimajor, double semiminor)
224 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
225 // Compute the ECEF to ECI transformation matrix using Stevens and Lewis "Aircraft
226 // Control and Simulation", second edition, eqn. 1.4-12, pg. 39. In Stevens and Lewis
227 // notation, this is C_i/e, a transformation from ECEF to ECI.
229 const FGMatrix33& FGLocation::GetTec2i(void)
234 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
235 // This is given in Stevens and Lewis "Aircraft
236 // Control and Simulation", second edition, eqn. 1.4-12, pg. 39
237 // The notation in Stevens and Lewis is: C_e/i. This represents a transformation
238 // from ECI to ECEF - and the orientation of the ECEF frame relative to the ECI
241 const FGMatrix33& FGLocation::GetTi2ec(void)
246 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
248 void FGLocation::ComputeDerivedUnconditional(void) const
250 // The radius is just the Euclidean norm of the vector.
251 mRadius = mECLoc.Magnitude();
253 // The distance of the location to the Z-axis, which is the axis
254 // through the poles.
255 double r02 = mECLoc(eX)*mECLoc(eX) + mECLoc(eY)*mECLoc(eY);
256 double rxy = sqrt(r02);
258 // Compute the sin/cos values of the longitude
259 double sinLon, cosLon;
264 sinLon = mECLoc(eY)/rxy;
265 cosLon = mECLoc(eX)/rxy;
268 // Compute the sin/cos values of the latitude
269 double sinLat, cosLat;
270 if (mRadius == 0.0) {
274 sinLat = mECLoc(eZ)/mRadius;
275 cosLat = rxy/mRadius;
278 // Compute the longitude and latitude itself
279 if ( mECLoc( eX ) == 0.0 && mECLoc( eY ) == 0.0 )
282 mLon = atan2( mECLoc( eY ), mECLoc( eX ) );
284 if ( rxy == 0.0 && mECLoc( eZ ) == 0.0 )
287 mLat = atan2( mECLoc(eZ), rxy );
289 // Compute the transform matrices from and to the earth centered frame.
290 // See Stevens and Lewis, "Aircraft Control and Simulation", Second Edition,
291 // Eqn. 1.4-13, page 40. In Stevens and Lewis notation, this is C_n/e - the
292 // orientation of the navigation (local) frame relative to the ECEF frame,
293 // and a transformation from ECEF to nav (local) frame.
295 mTec2l = FGMatrix33( -cosLon*sinLat, -sinLon*sinLat, cosLat,
296 -sinLon , cosLon , 0.0 ,
297 -cosLon*cosLat, -sinLon*cosLat, -sinLat );
299 // In Stevens and Lewis notation, this is C_e/n - the
300 // orientation of the ECEF frame relative to the nav (local) frame,
301 // and a transformation from nav (local) to ECEF frame.
303 mTl2ec = mTec2l.Transposed();
305 // Calculate the inertial to ECEF and transpose matrices
306 double cos_epa = cos(epa);
307 double sin_epa = sin(epa);
308 mTi2ec = FGMatrix33( cos_epa, sin_epa, 0.0,
309 -sin_epa, cos_epa, 0.0,
311 mTec2i = mTi2ec.Transposed();
313 // Now calculate the local (or nav, or ned) frame to inertial transform matrix,
315 mTl2i = mTec2i * mTl2ec;
316 mTi2l = mTl2i.Transposed();
318 // Calculate the geodetic latitude base on AIAA Journal of Guidance and Control paper,
319 // "Improved Method for Calculating Exact Geodetic Latitude and Altitude", and
320 // "Improved Method for Calculating Exact Geodetic Latitude and Altitude, Revisited",
323 if (a != 0.0 && b != 0.0) {
324 double c, p, q, s, t, u, v, w, z, p2, u2, r0;
325 double Ne, P, Q0, Q, signz0, sqrt_q;
326 p = fabs(mECLoc(eZ))/eps2;
337 Q0 = sqrt(P+1) + sqrt(P);
338 Q = pow(Q0, 0.66666666667);
339 t = (1.0 + Q + 1.0/Q)/6.0;
340 c = sqrt(u2 - 1 + 2.0*t);
342 signz0 = mECLoc(eZ)>=0?1.0:-1.0;
343 z = signz0*sqrt_q*(w+sqrt(sqrt(t*t+v)-u*w-0.5*t-0.25));
344 Ne = a*sqrt(1+eps2*z*z/b2);
345 mGeodLat = asin((eps2+1.0)*(z/Ne));
347 GeodeticAltitude = r0*cos(mGeodLat) + mECLoc(eZ)*sin(mGeodLat) - a2/Ne;
351 // Mark the cached values as valid
355 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
357 } // namespace JSBSim