Date started: 04/04/2004
Purpose: Store an arbitrary location on the globe
- ------- Copyright (C) 1999 Jon S. Berndt (jsb@hal-pc.org) ------------------
+ ------- Copyright (C) 1999 Jon S. Berndt (jon@jsbsim.org) ------------------
------- (C) 2004 Mathias Froehlich (Mathias.Froehlich@web.de) ----
This program is free software; you can redistribute it and/or modify it under
INCLUDES
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
-#ifdef FGFS
-# include <simgear/compiler.h>
-# ifdef SG_HAVE_STD_INCLUDES
-# include <cmath>
-# else
-# include <math.h>
-# endif
-#else
-# if defined(sgi) && !defined(__GNUC__)
-# include <math.h>
-# else
-# include <cmath>
-# endif
-#endif
+#include <cmath>
#include "FGLocation.h"
-#include <input_output/FGPropertyManager.h>
+#include "input_output/FGPropertyManager.h"
namespace JSBSim {
CLASS IMPLEMENTATION
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
+FGLocation::FGLocation(void)
+{
+ mCacheValid = false;
+ initial_longitude = 0.0;
+ a = 0.0;
+ b = 0.0;
+ a2 = 0.0;
+ b2 = 0.0;
+ e2 = 1.0;
+ e = 1.0;
+ eps2 = -1.0;
+ f = 1.0;
+}
+
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
FGLocation::FGLocation(double lon, double lat, double radius)
{
mCacheValid = false;
double cosLat = cos(lat);
double sinLon = sin(lon);
double cosLon = cos(lon);
+ initial_longitude = lon;
+ a = 0.0;
+ b = 0.0;
+ a2 = 0.0;
+ b2 = 0.0;
+ e2 = 1.0;
+ e = 1.0;
+ eps2 = -1.0;
+ f = 1.0;
mECLoc = FGColumnVector3( radius*cosLat*cosLon,
radius*cosLat*sinLon,
radius*sinLat );
mCacheValid = false;
+ // Need to figure out how to set the initial_longitude here
+
mECLoc(eX) = rtmp*cos(longitude);
mECLoc(eY) = rtmp*sin(longitude);
}
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+void FGLocation::SetPosition(double lon, double lat, double radius)
+{
+ mCacheValid = false;
+
+ double sinLat = sin(lat);
+ double cosLat = cos(lat);
+ double sinLon = sin(lon);
+ double cosLon = cos(lon);
+ initial_longitude = lon;
+ mECLoc = FGColumnVector3( radius*cosLat*cosLon,
+ radius*cosLat*sinLon,
+ radius*sinLat );
+ ComputeDerived();
+}
+
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+void FGLocation::SetPositionGeodetic(double lon, double lat, double height)
+{
+ mCacheValid = false;
+
+ mGeodLat = lat;
+ mLon = lon;
+ GeodeticAltitude = height;
+
+ initial_longitude = mLon;
+
+ double RN = a / sqrt(1.0 - e2*sin(mGeodLat)*sin(mGeodLat));
+
+ mECLoc(eX) = (RN + GeodeticAltitude)*cos(mGeodLat)*cos(mLon);
+ mECLoc(eY) = (RN + GeodeticAltitude)*cos(mGeodLat)*sin(mLon);
+ mECLoc(eZ) = ((1 - e2)*RN + GeodeticAltitude)*sin(mGeodLat);
+
+}
+
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+void FGLocation::SetEllipse(double semimajor, double semiminor)
+{
+ mCacheValid = false;
+
+ a = semimajor;
+ b = semiminor;
+ a2 = a*a;
+ b2 = b*b;
+ e2 = 1.0 - b2/a2;
+ e = sqrt(e2);
+ eps2 = a2/b2 - 1.0;
+ f = 1.0 - b/a;
+}
+
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+// Compute the ECEF to ECI transformation matrix using Stevens and Lewis "Aircraft
+// Control and Simulation", second edition, eqn. 1.4-12, pg. 39
+
+const FGMatrix33& FGLocation::GetTec2i(double epa)
+{
+ double mu = epa - initial_longitude;
+ mTec2i = FGMatrix33( cos(mu), -sin(mu), 0.0,
+ sin(mu), cos(mu), 0.0,
+ 0.0, 0.0, 1.0 );
+ return mTec2i;
+}
+
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+const FGMatrix33& FGLocation::GetTi2ec(double epa)
+{
+ double mu = epa - initial_longitude;
+ mTi2ec = FGMatrix33( cos(mu), sin(mu), 0.0,
+ -sin(mu), cos(mu), 0.0,
+ 0.0, 0.0, 1.0 );
+ return mTi2ec;
+}
+
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
void FGLocation::ComputeDerivedUnconditional(void) const
{
// The radius is just the Euclidean norm of the vector.
mRadius = mECLoc.Magnitude();
- // The distance of the location to the y-axis, which is the axis
+ // The distance of the location to the Z-axis, which is the axis
// through the poles.
double rxy = sqrt(mECLoc(eX)*mECLoc(eX) + mECLoc(eY)*mECLoc(eY));
mLat = atan2( mECLoc(eZ), rxy );
// Compute the transform matrices from and to the earth centered frame.
- // see Durham Chapter 4, problem 1, page 52
+ // See Stevens and Lewis, "Aircraft Control and Simulation", Second Edition,
+ // Eqn. 1.4-13, page 40.
mTec2l = FGMatrix33( -cosLon*sinLat, -sinLon*sinLat, cosLat,
-sinLon , cosLon , 0.0 ,
-cosLon*cosLat, -sinLon*cosLat, -sinLat );
mTl2ec = mTec2l.Transposed();
+
+ // Calculate the geodetic latitude base on AIAA Journal of Guidance and Control paper,
+ // "Improved Method for Calculating Exact Geodetic Latitude and Altitude", and
+ // "Improved Method for Calculating Exact Geodetic Latitude and Altitude, Revisited",
+ // author: I. Sofair
+
+ if (a != 0.0 && b != 0.0) {
+ double c, p, q, s, t, u, v, w, z, p2, u2, r02, r0;
+ double Ne, P, Q0, Q, signz0, sqrt_q;
+ p = fabs(mECLoc(eZ))/eps2;
+ r02 = mECLoc(eX)*mECLoc(eX) + mECLoc(eY)*mECLoc(eY);
+ s = r02/(e2*eps2);
+ p2 = p*p;
+ q = p2 - b2 + s;
+ sqrt_q = sqrt(q);
+ if (q>0)
+ {
+ u = p/sqrt_q;
+ u2 = p2/q;
+ v = b2*u2/q;
+ P = 27.0*v*s/q;
+ Q0 = sqrt(P+1) + sqrt(P);
+ Q = pow(Q0, 0.66666666667);
+ t = (1.0 + Q + 1.0/Q)/6.0;
+ c = sqrt(u2 - 1 + 2.0*t);
+ w = (c - u)/2.0;
+ signz0 = mECLoc(eZ)>=0?1.0:-1.0;
+ z = signz0*sqrt_q*(w+sqrt(sqrt(t*t+v)-u*w-0.5*t-0.25));
+ Ne = a*sqrt(1+eps2*z*z/b2);
+ mGeodLat = asin((eps2+1.0)*(z/Ne));
+ r0 = sqrt(r02);
+ GeodeticAltitude = r0*cos(mGeodLat) + mECLoc(eZ)*sin(mGeodLat) - a2/Ne;
+ }
+ }
// Mark the cached values as valid
mCacheValid = true;
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-void FGLocation::bind(FGPropertyManager* PropertyManager, const string& prefix) const
-{
- PropertyManager->Tie(prefix + "lat-gc-rad", (FGLocation*)this,
- &FGLocation::GetLatitude);
- PropertyManager->Tie(prefix + "lat-gc-deg", (FGLocation*)this,
- &FGLocation::GetLatitudeDeg);
- PropertyManager->Tie(prefix + "long-gc-rad", (FGLocation*)this,
- &FGLocation::GetLongitude);
- PropertyManager->Tie(prefix + "long-gc-deg", (FGLocation*)this,
- &FGLocation::GetLongitudeDeg);
- PropertyManager->Tie(prefix + "radius-ft", (FGLocation*)this,
- &FGLocation::GetRadius);
-}
-
-//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-
-void FGLocation::unbind(FGPropertyManager* PropertyManager, const string& prefix) const
-{
- PropertyManager->Untie(prefix + "lat-gc-rad");
- PropertyManager->Untie(prefix + "lat-gc-deg");
- PropertyManager->Untie(prefix + "long-gc-rad");
- PropertyManager->Untie(prefix + "long-gc-deg");
- PropertyManager->Untie(prefix + "radius-ft");
-}
-
-//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-
} // namespace JSBSim
projects / flightgear.git / blobdiff