Branch data Line data Source code
1 : : /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
2 : :
3 : : Module: FGLocation.cpp
4 : : Author: Jon S. Berndt
5 : : Date started: 04/04/2004
6 : : Purpose: Store an arbitrary location on the globe
7 : :
8 : : ------- Copyright (C) 1999 Jon S. Berndt (jon@jsbsim.org) ------------------
9 : : ------- (C) 2004 Mathias Froehlich (Mathias.Froehlich@web.de) ----
10 : :
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
14 : : version.
15 : :
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
19 : : details.
20 : :
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.
24 : :
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.
27 : :
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 ....
32 : :
33 : : HISTORY
34 : : ------------------------------------------------------------------------------
35 : : 04/04/2004 MF Created
36 : :
37 : : %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
38 : : INCLUDES
39 : : %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
40 : :
41 : : #include <cmath>
42 : :
43 : : #include "FGLocation.h"
44 : : #include "input_output/FGPropertyManager.h"
45 : :
46 : : namespace JSBSim {
47 : :
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;
50 : :
51 : : /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
52 : : CLASS IMPLEMENTATION
53 : : %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
54 : :
55 : 3 : FGLocation::FGLocation(void)
56 : : {
57 : 3 : mCacheValid = false;
58 : 3 : initial_longitude = 0.0;
59 : 3 : a = 0.0;
60 : 3 : b = 0.0;
61 : 3 : a2 = 0.0;
62 : 3 : b2 = 0.0;
63 : 3 : e2 = 1.0;
64 : 3 : e = 1.0;
65 : 3 : eps2 = -1.0;
66 : 3 : f = 1.0;
67 : 3 : epa = 0.0;
68 : :
69 : 3 : mLon = mLat = mRadius = mGeodLat = GeodeticAltitude = 0.0;
70 : :
71 : : // initial_longitude = 0.0;
72 : :
73 : 3 : mTl2ec.InitMatrix();
74 : 3 : mTec2l.InitMatrix();
75 : 3 : mTi2ec.InitMatrix();
76 : 3 : mTec2i.InitMatrix();
77 : 3 : mTi2l.InitMatrix();
78 : 3 : mTl2i.InitMatrix();
79 : 3 : mECLoc.InitMatrix();
80 : 3 : }
81 : :
82 : : //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
83 : :
84 : 0 : FGLocation::FGLocation(double lon, double lat, double radius)
85 : : {
86 : 0 : mCacheValid = false;
87 : :
88 : 0 : double sinLat = sin(lat);
89 : 0 : double cosLat = cos(lat);
90 : 0 : double sinLon = sin(lon);
91 : 0 : double cosLon = cos(lon);
92 : :
93 : 0 : a = 0.0;
94 : 0 : b = 0.0;
95 : 0 : a2 = 0.0;
96 : 0 : b2 = 0.0;
97 : 0 : e2 = 1.0;
98 : 0 : e = 1.0;
99 : 0 : eps2 = -1.0;
100 : 0 : f = 1.0;
101 : 0 : epa = 0.0;
102 : : mECLoc = FGColumnVector3( radius*cosLat*cosLon,
103 : : radius*cosLat*sinLon,
104 : 0 : radius*sinLat );
105 : 0 : mLon = mLat = mRadius = mGeodLat = GeodeticAltitude = 0.0;
106 : :
107 : : // initial_longitude = 0.0
108 : :
109 : : ComputeDerived();
110 : 0 : }
111 : :
112 : : //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
113 : :
114 : 0 : void FGLocation::SetLongitude(double longitude)
115 : : {
116 : 0 : 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 [ # # ]: 0 : if (0.0 == mECLoc.Magnitude())
120 : 0 : rtmp = 1.0;
121 : :
122 : : // Fast return if we are on the north or south pole ...
123 [ # # ]: 0 : if (rtmp == 0.0)
124 : 0 : return;
125 : :
126 : 0 : mCacheValid = false;
127 : :
128 : : // Need to figure out how to set the initial_longitude here
129 : :
130 : 0 : mECLoc(eX) = rtmp*cos(longitude);
131 : 0 : mECLoc(eY) = rtmp*sin(longitude);
132 : : }
133 : :
134 : : //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
135 : :
136 : 0 : void FGLocation::SetLatitude(double latitude)
137 : : {
138 : 0 : mCacheValid = false;
139 : :
140 : 0 : double r = mECLoc.Magnitude();
141 [ # # ]: 0 : if (r == 0.0) {
142 : 0 : mECLoc(eX) = 1.0;
143 : 0 : r = 1.0;
144 : : }
145 : :
146 : 0 : double rtmp = mECLoc.Magnitude(eX, eY);
147 [ # # ]: 0 : if (rtmp != 0.0) {
148 : 0 : double fac = r/rtmp*cos(latitude);
149 : 0 : mECLoc(eX) *= fac;
150 : 0 : mECLoc(eY) *= fac;
151 : : } else {
152 : 0 : mECLoc(eX) = r*cos(latitude);
153 : 0 : mECLoc(eY) = 0.0;
154 : : }
155 : 0 : mECLoc(eZ) = r*sin(latitude);
156 : 0 : }
157 : :
158 : : //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
159 : :
160 : 3 : void FGLocation::SetRadius(double radius)
161 : : {
162 : 3 : mCacheValid = false;
163 : :
164 : 3 : double rold = mECLoc.Magnitude();
165 [ + + ]: 3 : if (rold == 0.0)
166 : 1 : mECLoc(eX) = radius;
167 : : else
168 : 2 : mECLoc *= radius/rold;
169 : 3 : }
170 : :
171 : : //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
172 : :
173 : 1 : void FGLocation::SetPosition(double lon, double lat, double radius)
174 : : {
175 : 1 : mCacheValid = false;
176 : :
177 : 1 : double sinLat = sin(lat);
178 : 1 : double cosLat = cos(lat);
179 : 1 : double sinLon = sin(lon);
180 : 1 : double cosLon = cos(lon);
181 : : // initial_longitude = lon;
182 : : mECLoc = FGColumnVector3( radius*cosLat*cosLon,
183 : : radius*cosLat*sinLon,
184 : 2 : radius*sinLat );
185 : : ComputeDerived();
186 : 1 : }
187 : :
188 : : //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
189 : :
190 : 0 : void FGLocation::SetPositionGeodetic(double lon, double lat, double height)
191 : : {
192 : 0 : mCacheValid = false;
193 : :
194 : 0 : mGeodLat = lat;
195 : 0 : mLon = lon;
196 : 0 : GeodeticAltitude = height;
197 : :
198 : : // initial_longitude = mLon;
199 : :
200 : 0 : double RN = a / sqrt(1.0 - e2*sin(mGeodLat)*sin(mGeodLat));
201 : :
202 : 0 : mECLoc(eX) = (RN + GeodeticAltitude)*cos(mGeodLat)*cos(mLon);
203 : 0 : mECLoc(eY) = (RN + GeodeticAltitude)*cos(mGeodLat)*sin(mLon);
204 : 0 : mECLoc(eZ) = ((1 - e2)*RN + GeodeticAltitude)*sin(mGeodLat);
205 : :
206 : 0 : }
207 : :
208 : : //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
209 : :
210 : 1 : void FGLocation::SetEllipse(double semimajor, double semiminor)
211 : : {
212 : 1 : mCacheValid = false;
213 : :
214 : 1 : a = semimajor;
215 : 1 : b = semiminor;
216 : 1 : a2 = a*a;
217 : 1 : b2 = b*b;
218 : 1 : e2 = 1.0 - b2/a2;
219 : 1 : e = sqrt(e2);
220 : 1 : eps2 = a2/b2 - 1.0;
221 : 1 : f = 1.0 - b/a;
222 : 1 : }
223 : :
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.
228 : :
229 : 0 : const FGMatrix33& FGLocation::GetTec2i(void)
230 : : {
231 : 0 : return mTec2i;
232 : : }
233 : :
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
239 : : // frame.
240 : :
241 : 54006 : const FGMatrix33& FGLocation::GetTi2ec(void)
242 : : {
243 : 54006 : return mTi2ec;
244 : : }
245 : :
246 : : //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
247 : :
248 : 216026 : void FGLocation::ComputeDerivedUnconditional(void) const
249 : : {
250 : : // The radius is just the Euclidean norm of the vector.
251 : 216026 : mRadius = mECLoc.Magnitude();
252 : :
253 : : // The distance of the location to the Z-axis, which is the axis
254 : : // through the poles.
255 : 1080130 : double r02 = mECLoc(eX)*mECLoc(eX) + mECLoc(eY)*mECLoc(eY);
256 : 216026 : double rxy = sqrt(r02);
257 : :
258 : : // Compute the sin/cos values of the longitude
259 : : double sinLon, cosLon;
260 [ - + ]: 216026 : if (rxy == 0.0) {
261 : 0 : sinLon = 0.0;
262 : 0 : cosLon = 1.0;
263 : : } else {
264 : 432052 : sinLon = mECLoc(eY)/rxy;
265 : 216026 : cosLon = mECLoc(eX)/rxy;
266 : : }
267 : :
268 : : // Compute the sin/cos values of the latitude
269 : : double sinLat, cosLat;
270 [ - + ]: 216026 : if (mRadius == 0.0) {
271 : 0 : sinLat = 0.0;
272 : 0 : cosLat = 1.0;
273 : : } else {
274 : 432052 : sinLat = mECLoc(eZ)/mRadius;
275 : 216026 : cosLat = rxy/mRadius;
276 : : }
277 : :
278 : : // Compute the longitude and latitude itself
279 [ - + ][ # # ]: 216026 : if ( mECLoc( eX ) == 0.0 && mECLoc( eY ) == 0.0 )
[ - + ]
280 : 0 : mLon = 0.0;
281 : : else
282 : 216026 : mLon = atan2( mECLoc( eY ), mECLoc( eX ) );
283 : :
284 [ - + ][ # # ]: 216026 : if ( rxy == 0.0 && mECLoc( eZ ) == 0.0 )
[ - + ]
285 : 0 : mLat = 0.0;
286 : : else
287 : 216026 : mLat = atan2( mECLoc(eZ), rxy );
288 : :
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.
294 : :
295 : : mTec2l = FGMatrix33( -cosLon*sinLat, -sinLon*sinLat, cosLat,
296 : : -sinLon , cosLon , 0.0 ,
297 : 216026 : -cosLon*cosLat, -sinLon*cosLat, -sinLat );
298 : :
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.
302 : :
303 : 216026 : mTl2ec = mTec2l.Transposed();
304 : :
305 : : // Calculate the inertial to ECEF and transpose matrices
306 : 216026 : double cos_epa = cos(epa);
307 : 216026 : double sin_epa = sin(epa);
308 : : mTi2ec = FGMatrix33( cos_epa, sin_epa, 0.0,
309 : : -sin_epa, cos_epa, 0.0,
310 : 216026 : 0.0, 0.0, 1.0 );
311 : 216026 : mTec2i = mTi2ec.Transposed();
312 : :
313 : : // Now calculate the local (or nav, or ned) frame to inertial transform matrix,
314 : : // and the inverse.
315 : 216026 : mTl2i = mTec2i * mTl2ec;
316 : 216026 : mTi2l = mTl2i.Transposed();
317 : :
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",
321 : : // author: I. Sofair
322 : :
323 [ + + ][ + - ]: 216026 : 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 : 324038 : p = fabs(mECLoc(eZ))/eps2;
327 : 162019 : s = r02/(e2*eps2);
328 : 162019 : p2 = p*p;
329 : 162019 : q = p2 - b2 + s;
330 : 162019 : sqrt_q = sqrt(q);
331 [ + - ]: 162019 : if (q>0)
332 : : {
333 : 162019 : u = p/sqrt_q;
334 : 162019 : u2 = p2/q;
335 : 162019 : v = b2*u2/q;
336 : 162019 : P = 27.0*v*s/q;
337 : 162019 : Q0 = sqrt(P+1) + sqrt(P);
338 : 162019 : Q = pow(Q0, 0.66666666667);
339 : 162019 : t = (1.0 + Q + 1.0/Q)/6.0;
340 : 162019 : c = sqrt(u2 - 1 + 2.0*t);
341 : 162019 : w = (c - u)/2.0;
342 [ + - ]: 162019 : signz0 = mECLoc(eZ)>=0?1.0:-1.0;
343 : 162019 : z = signz0*sqrt_q*(w+sqrt(sqrt(t*t+v)-u*w-0.5*t-0.25));
344 : 162019 : Ne = a*sqrt(1+eps2*z*z/b2);
345 : 162019 : mGeodLat = asin((eps2+1.0)*(z/Ne));
346 : 162019 : r0 = rxy;
347 : 162019 : GeodeticAltitude = r0*cos(mGeodLat) + mECLoc(eZ)*sin(mGeodLat) - a2/Ne;
348 : : }
349 : : }
350 : :
351 : : // Mark the cached values as valid
352 : 216026 : mCacheValid = true;
353 : 216026 : }
354 : :
355 : : //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
356 : :
357 [ + + ][ + - ]: 12 : } // namespace JSBSim
|
