diff --git a/src/input_output/FGGroundCallback.cpp b/src/input_output/FGGroundCallback.cpp index 3a983b91aa..6002f2ae3a 100644 --- a/src/input_output/FGGroundCallback.cpp +++ b/src/input_output/FGGroundCallback.cpp @@ -56,6 +56,13 @@ double FGDefaultGroundCallback::GetAGLevel(double t, const FGLocation& loc, return l.GetGeodAltitude() - mTerrainElevation; } +double FGDefaultGroundCallback::GetAGLevel(const FGLocation& loc) const +{ + FGLocation l = loc; + l.SetEllipse(a, b); + return l.GetGeodAltitude() - mTerrainElevation; +} + //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% } // namespace JSBSim diff --git a/src/input_output/FGGroundCallback.h b/src/input_output/FGGroundCallback.h index bc5cc5e35d..4702b7ea71 100644 --- a/src/input_output/FGGroundCallback.h +++ b/src/input_output/FGGroundCallback.h @@ -95,6 +95,13 @@ class JSBSIM_API FGGroundCallback FGColumnVector3& w) const { return GetAGLevel(time, location, contact, normal, v, w); } + virtual double GetAGLevel(const FGLocation& location) const + { + FGLocation lDummy; + FGColumnVector3 vDummy; + return GetAGLevel(location, lDummy, vDummy, vDummy, vDummy); + } + /** Set the terrain elevation. Only needs to be implemented if JSBSim should be allowed to modify the local terrain radius (see the default implementation) @@ -133,6 +140,8 @@ class JSBSIM_API FGDefaultGroundCallback : public FGGroundCallback FGColumnVector3& normal, FGColumnVector3& v, FGColumnVector3& w) const override; + double GetAGLevel(const FGLocation& location) const override; + void SetTerrainElevation(double h) override { mTerrainElevation = h; } diff --git a/src/math/FGColumnVector3.cpp b/src/math/FGColumnVector3.cpp index 8ee499e7e9..f67a7a0142 100644 --- a/src/math/FGColumnVector3.cpp +++ b/src/math/FGColumnVector3.cpp @@ -42,7 +42,6 @@ INCLUDES #include #include #include -#include using namespace std; @@ -106,13 +105,6 @@ FGColumnVector3& FGColumnVector3::operator/=(const double scalar) //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% -double FGColumnVector3::Magnitude(void) const -{ - return sqrt( data[0]*data[0] + data[1]*data[1] + data[2]*data[2] ); -} - -//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% - FGColumnVector3& FGColumnVector3::Normalize(void) { double Mag = Magnitude(); diff --git a/src/math/FGColumnVector3.h b/src/math/FGColumnVector3.h index 4b6886a99a..20afeee0ae 100644 --- a/src/math/FGColumnVector3.h +++ b/src/math/FGColumnVector3.h @@ -39,6 +39,7 @@ SENTRY INCLUDES %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/ +#include #include #include @@ -236,7 +237,11 @@ class JSBSIM_API FGColumnVector3 /** Length of the vector. Compute and return the euclidean norm of this vector. */ - double Magnitude(void) const; + double Magnitude(void) const { return std::sqrt(SqrMagnitude()); } + + /** Square of the length of the vector. + Compute and return the square of the euclidean norm of this vector. */ + double SqrMagnitude(void) const { return data[0]*data[0] + data[1]*data[1] + data[2]*data[2]; } /** Length of the vector in a coordinate axis plane. Compute and return the euclidean norm of this vector projected into diff --git a/src/math/FGLocation.cpp b/src/math/FGLocation.cpp index 0e9ad65145..677f2b35d5 100644 --- a/src/math/FGLocation.cpp +++ b/src/math/FGLocation.cpp @@ -106,6 +106,20 @@ FGLocation::FGLocation(const FGColumnVector3& lv) //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +FGLocation::FGLocation(const FGColumnVector3& lv, double semimajor, double semiminor) + : mECLoc(lv), mCacheValid(false) +{ + SetEllipse(semimajor, semiminor); + + mLon = mLat = mRadius = 0.0; + mGeodLat = GeodeticAltitude = 0.0; + + mTl2ec.InitMatrix(); + mTec2l.InitMatrix(); +} + +//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + FGLocation::FGLocation(const FGLocation& l) : mECLoc(l.mECLoc), mCacheValid(l.mCacheValid) { diff --git a/src/math/FGLocation.h b/src/math/FGLocation.h index c53e5e743d..4087f69eda 100644 --- a/src/math/FGLocation.h +++ b/src/math/FGLocation.h @@ -165,7 +165,16 @@ class JSBSIM_API FGLocation : public FGJSBBase (X,Y,Z) contained in the input FGColumnVector3. Distances are in feet, the position is expressed in the ECEF frame. @param lv vector that contain the cartesian coordinates*/ - FGLocation(const FGColumnVector3& lv); + explicit FGLocation(const FGColumnVector3& lv); + + /** Constructor to initialize the location with the cartesian coordinates + (X,Y,Z) contained in the input FGColumnVector3. Distances are in feet, + the position is expressed in the ECEF frame. Sets the semimajor and + semiminor axis lengths for this planet as if SetEllipse() was called. + @param lv vector that contain the cartesian coordinates + @param semimajor planet semi-major axis in ft. + @param semiminor planet semi-minor axis in ft.*/ + FGLocation(const FGColumnVector3& lv, double semimajor, double semiminor); /** Copy constructor. */ FGLocation(const FGLocation& l); @@ -324,7 +333,7 @@ class JSBSIM_API FGLocation : public FGJSBBase @param lvec Vector in the local horizontal coordinate frame @return The location in the earth centered and fixed frame */ FGLocation LocalToLocation(const FGColumnVector3& lvec) const { - ComputeDerived(); return mTl2ec*lvec + mECLoc; + ComputeDerived(); return FGLocation(mTl2ec*lvec + mECLoc); } /** Conversion from a location in the earth centered and fixed frame @@ -416,6 +425,12 @@ class JSBSIM_API FGLocation : public FGJSBBase return *this; } + const FGLocation& operator+=(const FGColumnVector3& l) { + mCacheValid = false; + mECLoc += l; + return *this; + } + /** This operator substracts the ECEF position vectors. The cartesian coordinates of the supplied vector (right side) are substracted from the ECEF position vector on the left side of the @@ -426,6 +441,12 @@ class JSBSIM_API FGLocation : public FGJSBBase return *this; } + const FGLocation& operator-=(const FGColumnVector3& l) { + mCacheValid = false; + mECLoc -= l; + return *this; + } + /** This operator scales the ECEF position vector. The cartesian coordinates of the ECEF position vector on the left side of the equality are scaled by the supplied value (right side), and a diff --git a/src/models/FGAtmosphere.cpp b/src/models/FGAtmosphere.cpp index 4ce21592bd..af8fd2ee24 100644 --- a/src/models/FGAtmosphere.cpp +++ b/src/models/FGAtmosphere.cpp @@ -137,6 +137,16 @@ double FGAtmosphere::ValidateTemperature(double t, const string& msg, bool quiet //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +void FGAtmosphere::GetAltitudeDependentValues(AltitudeDependentValues& values, double altitude) const +{ + values.Temperature = GetTemperature(altitude); + values.Pressure = GetPressure(altitude); + values.Density = (values.Pressure / (Reng * values.Temperature)); + values.SoundSpeed = sqrt(SHRatio * Reng * values.Temperature); +} + +//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + void FGAtmosphere::Calculate(double altitude) { FGPropertyNode* node = PropertyManager->GetNode(); diff --git a/src/models/FGAtmosphere.h b/src/models/FGAtmosphere.h index c943b70aa0..1fe95fb19a 100644 --- a/src/models/FGAtmosphere.h +++ b/src/models/FGAtmosphere.h @@ -82,6 +82,13 @@ class JSBSIM_API FGAtmosphere : public FGModel { /// Enums for specifying pressure units. enum ePressure {eNoPressUnit=0, ePSF, eMillibars, ePascals, eInchesHg}; + typedef struct AltitudeDependentValues { + double Temperature = 0.0; + double Pressure = 0.0; + double Density = 0.0; + double SoundSpeed = 0.0; + } AltitudeDependentValues; + /// Constructor FGAtmosphere(FGFDMExec*); @@ -206,6 +213,8 @@ class JSBSIM_API FGAtmosphere : public FGModel { virtual double GetPressureAltitude() const {return PressureAltitude;} + virtual void GetAltitudeDependentValues(AltitudeDependentValues& values, double altitude) const; + struct Inputs { double altitudeASL; double GeodLatitudeDeg; diff --git a/src/models/FGInertial.h b/src/models/FGInertial.h index 3140f4462f..89023d3bfb 100644 --- a/src/models/FGInertial.h +++ b/src/models/FGInertial.h @@ -113,10 +113,7 @@ class JSBSIM_API FGInertial : public FGModel { @param location Location at which the AGL is evaluated. @see SetGroundCallback */ double GetAltitudeAGL(const FGLocation& location) const { - FGLocation lDummy; - FGColumnVector3 vDummy; - return GroundCallback->GetAGLevel(location, lDummy, vDummy, vDummy, - vDummy); + return GroundCallback->GetAGLevel(location); } /** Set the altitude above ground level. diff --git a/tests/unit_tests/FGAtmosphereTest.h b/tests/unit_tests/FGAtmosphereTest.h index 25ada7598f..d305a546d4 100644 --- a/tests/unit_tests/FGAtmosphereTest.h +++ b/tests/unit_tests/FGAtmosphereTest.h @@ -150,8 +150,24 @@ class FGAtmosphereTest : public CxxTest::TestSuite const double nu0 = mu0/rho0; for(double h=-1000.0; h<10000; h+= 1000) { - double T = T0 + 0.1*h; - double P = P0 + 1.0*h; + const double T = T0 + 0.1*h; + const double P = P0 + 1.0*h; + const double rho = P/(R*T); + const double a = sqrt(gama*R*T); + + FGAtmosphere::AltitudeDependentValues adv; + atm.GetAltitudeDependentValues(adv, h); + TS_ASSERT_DELTA(adv.Temperature, T, epsilon); + TS_ASSERT_DELTA(adv.Pressure, P, epsilon); + TS_ASSERT_DELTA(adv.Density, rho, epsilon); + TS_ASSERT_DELTA(adv.SoundSpeed, a, epsilon); + + FGAtmosphere::AltitudeDependentValues adv_0; + atm.GetAltitudeDependentValues(adv_0, 0.0); + TS_ASSERT_EQUALS(adv_0.Temperature, T0); + TS_ASSERT_EQUALS(adv_0.Pressure, P0); + TS_ASSERT_DELTA(adv_0.Density, rho0, epsilon); + TS_ASSERT_DELTA(adv_0.SoundSpeed, a0, epsilon); TS_ASSERT_DELTA(atm.GetTemperature(h), T, epsilon); TS_ASSERT_EQUALS(atm.GetTemperature(0.0), T0); @@ -160,11 +176,9 @@ class FGAtmosphereTest : public CxxTest::TestSuite TS_ASSERT_DELTA(atm.GetPressure(h), P, epsilon); TS_ASSERT_EQUALS(atm.GetPressure(0.0), P0); - double rho = P/(R*T); TS_ASSERT_DELTA(atm.GetDensity(h), rho, epsilon); TS_ASSERT_DELTA(atm.GetDensity(0.0), rho0, epsilon); - double a = sqrt(gama*R*T); TS_ASSERT_DELTA(atm.GetSoundSpeed(h), a, epsilon); TS_ASSERT_DELTA(atm.GetSoundSpeed(0.0), a0, epsilon); diff --git a/tests/unit_tests/FGColumnVector3Test.h b/tests/unit_tests/FGColumnVector3Test.h index 5a092ce697..974013accc 100644 --- a/tests/unit_tests/FGColumnVector3Test.h +++ b/tests/unit_tests/FGColumnVector3Test.h @@ -577,6 +577,24 @@ class FGColumnVector3Test : public CxxTest::TestSuite TS_ASSERT_EQUALS(Z(3), 1.0); } + void testSqrMagnitude(void) { + JSBSim::FGColumnVector3 v0; + JSBSim::FGColumnVector3 v(3.0, 4.0, 0.0); + + TS_ASSERT_EQUALS(v0.SqrMagnitude(), 0.0); + TS_ASSERT_EQUALS(v.SqrMagnitude(), 25.0); + TS_ASSERT_EQUALS(DotProduct(v,v), v.SqrMagnitude()); + + // Verify that the operands are not modified + TS_ASSERT_EQUALS(v0(1), 0.0); + TS_ASSERT_EQUALS(v0(2), 0.0); + TS_ASSERT_EQUALS(v0(3), 0.0); + + TS_ASSERT_EQUALS(v(1), 3.0); + TS_ASSERT_EQUALS(v(2), 4.0); + TS_ASSERT_EQUALS(v(3), 0.0); + } + void testMagnitude(void) { JSBSim::FGColumnVector3 v0; JSBSim::FGColumnVector3 v(3.0, 4.0, 0.0); diff --git a/tests/unit_tests/FGGroundCallbackTest.h b/tests/unit_tests/FGGroundCallbackTest.h index e532802fb1..3030025928 100644 --- a/tests/unit_tests/FGGroundCallbackTest.h +++ b/tests/unit_tests/FGGroundCallbackTest.h @@ -55,6 +55,8 @@ class FGGroundCallbackTest : public CxxTest::TestSuite double lat_rad = lat*M_PI/180.; loc = FGLocation(lon_rad, lat_rad, RadiusReference); double agl = cb->GetAGLevel(loc, contact, normal, v, w); + double agl_2 = cb->GetAGLevel(loc); + TS_ASSERT_EQUALS(agl, agl_2); TS_ASSERT_DELTA(0.0, agl, 1e-8); TS_ASSERT_VECTOR_EQUALS(v, zero); TS_ASSERT_VECTOR_EQUALS(w, zero); @@ -88,6 +90,8 @@ class FGGroundCallbackTest : public CxxTest::TestSuite double lat_rad = lat*M_PI/180.; loc = FGLocation(lon_rad, lat_rad, RadiusReference+h); double agl = cb->GetAGLevel(loc, contact, normal, v, w); + double agl_2 = cb->GetAGLevel(loc); + TS_ASSERT_EQUALS(agl, agl_2); TS_ASSERT_DELTA(h/agl, 1.0, epsilon*100.); TS_ASSERT_VECTOR_EQUALS(v, zero); TS_ASSERT_VECTOR_EQUALS(w, zero); @@ -129,6 +133,8 @@ class FGGroundCallbackTest : public CxxTest::TestSuite double lat_rad = lat*M_PI/180.; loc = FGLocation(lon_rad, lat_rad, RadiusReference+h); double agl = cb->GetAGLevel(loc, contact, normal, v, w); + double agl_2 = cb->GetAGLevel(loc); + TS_ASSERT_EQUALS(agl, agl_2); TS_ASSERT_DELTA((h-elevation)/agl, 1.0, epsilon*100.); TS_ASSERT_VECTOR_EQUALS(v, zero); TS_ASSERT_VECTOR_EQUALS(w, zero); @@ -195,13 +201,15 @@ class FGGroundCallbackTest : public CxxTest::TestSuite double lat_rad = lat*M_PI/180.; loc.SetPositionGeodetic(lon_rad, lat_rad, h); double agl = cb->GetAGLevel(loc, contact, normal, v, w); + double agl_2 = cb->GetAGLevel(loc); + TS_ASSERT_EQUALS(agl, agl_2); TS_ASSERT_DELTA(h, agl, 1e-8); TS_ASSERT_VECTOR_EQUALS(v, zero); TS_ASSERT_VECTOR_EQUALS(w, zero); TS_ASSERT_DELTA(normal(1), cos(lat_rad)*cos(lon_rad), epsilon); TS_ASSERT_DELTA(normal(2), cos(lat_rad)*sin(lon_rad), epsilon); TS_ASSERT_DELTA(normal(3), sin(lat_rad), epsilon); - FGColumnVector3 vLoc = loc-h*normal; + FGColumnVector3 vLoc = FGColumnVector3(loc)-h*normal; FGColumnVector3 vContact = contact; TS_ASSERT_DELTA(vLoc(1), vContact(1), 1e-7); TS_ASSERT_DELTA(vLoc(2), vContact(2), 1e-7); @@ -235,7 +243,7 @@ class FGGroundCallbackTest : public CxxTest::TestSuite TS_ASSERT_DELTA(normal(1), cos(lat_rad)*cos(lon_rad), epsilon); TS_ASSERT_DELTA(normal(2), cos(lat_rad)*sin(lon_rad), epsilon); TS_ASSERT_DELTA(normal(3), sin(lat_rad), epsilon); - FGColumnVector3 vLoc = loc-(h-elevation)*normal; + FGColumnVector3 vLoc = FGColumnVector3(loc)-(h-elevation)*normal; FGColumnVector3 vContact = contact; TS_ASSERT_DELTA(vLoc(1), vContact(1), 1e-7); TS_ASSERT_DELTA(vLoc(2), vContact(2), 1e-7); diff --git a/tests/unit_tests/FGLocationTest.h b/tests/unit_tests/FGLocationTest.h index 44ddd30793..221049e3e4 100644 --- a/tests/unit_tests/FGLocationTest.h +++ b/tests/unit_tests/FGLocationTest.h @@ -115,6 +115,9 @@ class FGLocationTest : public CxxTest::TestSuite lv3.SetEllipse(1., 1.); CheckLocation(lv3, v); + + JSBSim::FGLocation lv4(v, 1., 1.); + CheckLocation(lv3, v); } void testCopyConstructor() {