From 17262df51cc5a9a542a35d6126b55f08cb210f7e Mon Sep 17 00:00:00 2001 From: frohlich Date: Sun, 29 Jul 2007 10:21:22 +0000 Subject: [PATCH] Modified Files: src/FDM/flight.cxx src/FDM/flight.hxx src/FDM/LaRCsim/LaRCsim.cxx src/FDM/SP/ADA.cxx src/Scenery/scenery.cxx src/Scenery/scenery.hxx: Remove obviously unused variables from FGInterface, make use of SGMath functions. No longer use plib math functions in FGInterface. --- src/FDM/LaRCsim/LaRCsim.cxx | 10 +- src/FDM/SP/ADA.cxx | 4 - src/FDM/flight.cxx | 390 ++++++++--------------- src/FDM/flight.hxx | 598 ++++-------------------------------- src/Scenery/scenery.cxx | 8 +- src/Scenery/scenery.hxx | 2 + 6 files changed, 200 insertions(+), 812 deletions(-) diff --git a/src/FDM/LaRCsim/LaRCsim.cxx b/src/FDM/LaRCsim/LaRCsim.cxx index 268cabe8b..f1da8e847 100644 --- a/src/FDM/LaRCsim/LaRCsim.cxx +++ b/src/FDM/LaRCsim/LaRCsim.cxx @@ -696,7 +696,6 @@ bool FGLaRCsim::copy_from_LaRCsim() { _set_Altitude_AGL( Altitude - Runway_altitude ); // Miscellaneous quantities - _set_T_Local_to_Body(T_local_to_body_m); // set_Gravity( Gravity ); // set_Centrifugal_relief( Centrifugal_relief ); @@ -710,9 +709,9 @@ bool FGLaRCsim::copy_from_LaRCsim() { // set_Cos_beta( Cos_beta ); // set_Sin_beta( Sin_beta ); - _set_Cos_phi( Cos_phi ); + // _set_Cos_phi( Cos_phi ); // set_Sin_phi( Sin_phi ); - _set_Cos_theta( Cos_theta ); + // _set_Cos_theta( Cos_theta ); // set_Sin_theta( Sin_theta ); // set_Cos_psi( Cos_psi ); // set_Sin_psi( Sin_psi ); @@ -748,11 +747,6 @@ bool FGLaRCsim::copy_from_LaRCsim() { // D_pilot_above_rwy ); // set_Pilot_Rwy_Rwy( X_pilot_rwy, Y_pilot_rwy, H_pilot_rwy ); - _set_sin_lat_geocentric(Lat_geocentric); - _set_cos_lat_geocentric(Lat_geocentric); - _set_sin_cos_longitude(Longitude); - _set_sin_cos_latitude(Latitude); - // printf("sin_lat_geo %f cos_lat_geo %f\n", sin_Lat_geoc, cos_Lat_geoc); // printf("sin_lat %f cos_lat %f\n", // get_sin_latitude(), get_cos_latitude()); diff --git a/src/FDM/SP/ADA.cxx b/src/FDM/SP/ADA.cxx index f303704fc..b93fbc711 100644 --- a/src/FDM/SP/ADA.cxx +++ b/src/FDM/SP/ADA.cxx @@ -251,10 +251,6 @@ bool FGADA::copy_from_FGADA() { _set_Geocentric_Rates( Latitude_dot, Longitude_dot, Radius_dot ); _set_Earth_position_angle( Earth_position_angle ); - _set_sin_lat_geocentric(Lat_geocentric); - _set_cos_lat_geocentric(Lat_geocentric); - _set_sin_cos_longitude(Longitude); - _set_sin_cos_latitude(Latitude); // Velocities and accelerations for the pitch ladder and velocity vector _set_Accels_Local( U_dot_local, V_dot_local, W_dot_local ); diff --git a/src/FDM/flight.cxx b/src/FDM/flight.cxx index 1f4a6acfc..637649b97 100644 --- a/src/FDM/flight.cxx +++ b/src/FDM/flight.cxx @@ -24,14 +24,11 @@ # include "config.h" #endif -#include - -#include +#include "flight.hxx" #include #include -#include -#include +#include #include #include @@ -40,8 +37,13 @@ #include
#include -#include "flight.hxx" +static inline void assign(double* ptr, const SGVec3d& vec) +{ + ptr[0] = vec[0]; + ptr[1] = vec[1]; + ptr[2] = vec[2]; +} // base_fdm_state is the internal state that is updated in integer // multiples of "dt". This leads to "jitter" with respect to the real @@ -51,10 +53,6 @@ FGInterface *cur_fdm_state = 0; FGInterface base_fdm_state; -inline void init_vec(FG_VECTOR_3 vec) { - vec[0] = 0.0; vec[1] = 0.0; vec[2] = 0.0; -} - // Constructor FGInterface::FGInterface() : remainder(0) @@ -114,70 +112,38 @@ FGInterface::_setup () inited = false; bound = false; - init_vec( d_pilot_rp_body_v ); - init_vec( d_cg_rp_body_v ); - init_vec( f_body_total_v ); - init_vec( f_local_total_v ); - init_vec( f_aero_v ); - init_vec( f_engine_v ); - init_vec( f_gear_v ); - init_vec( m_total_rp_v ); - init_vec( m_total_cg_v ); - init_vec( m_aero_v ); - init_vec( m_engine_v ); - init_vec( m_gear_v ); - init_vec( v_dot_local_v ); - init_vec( v_dot_body_v ); - init_vec( a_cg_body_v ); - init_vec( a_pilot_body_v ); - init_vec( n_cg_body_v ); - init_vec( n_pilot_body_v ); - init_vec( omega_dot_body_v ); - init_vec( v_local_v ); - init_vec( v_local_rel_ground_v ); - init_vec( v_local_airmass_v ); - init_vec( v_local_rel_airmass_v ); - init_vec( v_local_gust_v ); - init_vec( v_wind_body_v ); - init_vec( omega_body_v ); - init_vec( omega_local_v ); - init_vec( omega_total_v ); - init_vec( euler_rates_v ); - init_vec( geocentric_rates_v ); - init_vec( geocentric_position_v ); - init_vec( geodetic_position_v ); - init_vec( euler_angles_v ); - init_vec( d_cg_rwy_local_v ); - init_vec( d_cg_rwy_rwy_v ); - init_vec( d_pilot_rwy_local_v ); - init_vec( d_pilot_rwy_rwy_v ); - init_vec( t_local_to_body_m[0] ); - init_vec( t_local_to_body_m[1] ); - init_vec( t_local_to_body_m[2] ); - + d_cg_rp_body_v = SGVec3d::zeros(); + v_dot_local_v = SGVec3d::zeros(); + v_dot_body_v = SGVec3d::zeros(); + a_cg_body_v = SGVec3d::zeros(); + a_pilot_body_v = SGVec3d::zeros(); + n_cg_body_v = SGVec3d::zeros(); + v_local_v = SGVec3d::zeros(); + v_local_rel_ground_v = SGVec3d::zeros(); + v_local_airmass_v = SGVec3d::zeros(); + v_wind_body_v = SGVec3d::zeros(); + omega_body_v = SGVec3d::zeros(); + euler_rates_v = SGVec3d::zeros(); + geocentric_rates_v = SGVec3d::zeros(); + geodetic_position_v = SGGeod::fromRadM(0, 0, 0); + cartesian_position_v = SGVec3d::fromGeod(geodetic_position_v); + geocentric_position_v = SGGeoc::fromCart(cartesian_position_v); + euler_angles_v = SGVec3d::zeros(); + mass=i_xx=i_yy=i_zz=i_xz=0; nlf=0; - v_rel_wind=v_true_kts=v_rel_ground=v_inertial=0; - v_ground_speed=v_equiv=v_equiv_kts=0; - v_calibrated=v_calibrated_kts=0; - gravity=0; - centrifugal_relief=0; - alpha=beta=alpha_dot=beta_dot=0; - cos_alpha=sin_alpha=cos_beta=sin_beta=0; - cos_phi=sin_phi=cos_theta=sin_theta=cos_psi=sin_psi=0; - gamma_vert_rad=gamma_horiz_rad=0; - sigma=density=v_sound=mach_number=0; - static_pressure=total_pressure=impact_pressure=0; + v_rel_wind=v_true_kts=0; + v_ground_speed=v_equiv_kts=0; + v_calibrated_kts=0; + alpha=beta=0; + gamma_vert_rad=0; + density=mach_number=0; + static_pressure=total_pressure=0; dynamic_pressure=0; static_temperature=total_temperature=0; sea_level_radius=earth_position_angle=0; - runway_altitude=runway_latitude=runway_longitude=0; - runway_heading=0; - radius_to_rwy=0; + runway_altitude=0; climb_rate=0; - sin_lat_geocentric=cos_lat_geocentric=0; - sin_latitude=cos_latitude=0; - sin_longitude=cos_longitude=0; altitude_agl=0; } @@ -237,13 +203,8 @@ FGInterface::common_init () SG_LOG( SG_FLIGHT, SG_INFO, " lat = " << fgGetDouble("/sim/presets/latitude-deg") << " alt = " << get_Altitude() ); - double sea_level_radius_meters; - double lat_geoc; - sgGeodToGeoc( fgGetDouble("/sim/presets/latitude-deg") - * SGD_DEGREES_TO_RADIANS, - get_Altitude() * SG_FEET_TO_METER, - &sea_level_radius_meters, &lat_geoc ); - _set_Sea_level_radius( sea_level_radius_meters * SG_METER_TO_FEET ); + double slr = SGGeodesy::SGGeodToSeaLevelRadius(geodetic_position_v); + _set_Sea_level_radius( slr * SG_METER_TO_FEET ); // Set initial velocities SG_LOG( SG_FLIGHT, SG_INFO, "...initializing velocities..." ); @@ -519,122 +480,66 @@ FGInterface::update (double dt) } -void FGInterface::_updateGeodeticPosition( double lat, double lon, double alt ) +void FGInterface::_updatePositionM(const SGVec3d& cartPos) { - double lat_geoc, sl_radius; - - // cout << "starting sea level rad = " << get_Sea_level_radius() << endl; - - sgGeodToGeoc( lat, alt * SG_FEET_TO_METER, &sl_radius, &lat_geoc ); - - SG_LOG( SG_FLIGHT, SG_DEBUG, "lon = " << lon - << " lat_geod = " << lat - << " lat_geoc = " << lat_geoc - << " alt = " << alt - << " sl_radius = " << sl_radius * SG_METER_TO_FEET - << " Equator = " << SG_EQUATORIAL_RADIUS_FT ); - - _set_Geocentric_Position( lat_geoc, lon, - sl_radius * SG_METER_TO_FEET + alt ); - - _set_Geodetic_Position( lat, lon, alt ); - - _set_Sea_level_radius( sl_radius * SG_METER_TO_FEET ); + cartesian_position_v = cartPos; + geodetic_position_v = SGGeod::fromCart(cartesian_position_v); + geocentric_position_v = SGGeoc::fromCart(cartesian_position_v); + _set_Sea_level_radius( SGGeodesy::SGGeodToSeaLevelRadius(geodetic_position_v)*SG_METER_TO_FEET ); _update_ground_elev_at_pos(); - - _set_sin_lat_geocentric( lat_geoc ); - _set_cos_lat_geocentric( lat_geoc ); - - _set_sin_cos_longitude( lon ); - - _set_sin_cos_latitude( lat ); } -void FGInterface::_updateGeocentricPosition( double lat_geoc, double lon, - double alt ) +void FGInterface::_updatePosition(const SGGeod& geod) { - double lat_geod, tmp_alt, sl_radius1, sl_radius2, tmp_lat_geoc; - - // cout << "starting sea level rad = " << get_Sea_level_radius() << endl; + geodetic_position_v = geod; + cartesian_position_v = SGVec3d::fromGeod(geodetic_position_v); + geocentric_position_v = SGGeoc::fromCart(cartesian_position_v); - sgGeocToGeod( lat_geoc, ( get_Sea_level_radius() + alt ) * SG_FEET_TO_METER, - &lat_geod, &tmp_alt, &sl_radius1 ); - sgGeodToGeoc( lat_geod, alt * SG_FEET_TO_METER, &sl_radius2, &tmp_lat_geoc ); + _set_Sea_level_radius( SGGeodesy::SGGeodToSeaLevelRadius(geodetic_position_v)*SG_METER_TO_FEET ); + _update_ground_elev_at_pos(); +} - SG_LOG( SG_FLIGHT, SG_DEBUG, "lon = " << lon - << " lat_geod = " << lat_geod - << " lat_geoc = " << lat_geoc - << " alt = " << alt - << " tmp_alt = " << tmp_alt * SG_METER_TO_FEET - << " sl_radius1 = " << sl_radius1 * SG_METER_TO_FEET - << " sl_radius2 = " << sl_radius2 * SG_METER_TO_FEET - << " Equator = " << SG_EQUATORIAL_RADIUS_FT ); - _set_Geocentric_Position( lat_geoc, lon, - sl_radius2 * SG_METER_TO_FEET + alt ); - - _set_Geodetic_Position( lat_geod, lon, alt ); +void FGInterface::_updatePosition(const SGGeoc& geoc) +{ + geocentric_position_v = geoc; + cartesian_position_v = SGVec3d::fromGeoc(geocentric_position_v); + geodetic_position_v = SGGeod::fromCart(cartesian_position_v); - _set_Sea_level_radius( sl_radius2 * SG_METER_TO_FEET ); + _set_Sea_level_radius( SGGeodesy::SGGeodToSeaLevelRadius(geodetic_position_v)*SG_METER_TO_FEET ); _update_ground_elev_at_pos(); +} + - _set_sin_lat_geocentric( lat_geoc ); - _set_cos_lat_geocentric( lat_geoc ); +void FGInterface::_updateGeodeticPosition( double lat, double lon, double alt ) +{ + _updatePosition(SGGeod::fromRadFt(lon, lat, alt)); +} - _set_sin_cos_longitude( lon ); - _set_sin_cos_latitude( lat_geod ); +void FGInterface::_updateGeocentricPosition( double lat, double lon, + double alt ) +{ + _updatePosition(SGGeoc::fromRadFt(lon, lat, get_Sea_level_radius() + alt)); } void FGInterface::_update_ground_elev_at_pos( void ) { - double lat = get_Latitude(); - double lon = get_Longitude(); - double alt_m = get_Altitude()*SG_FEET_TO_METER; - double groundlevel_m = get_groundlevel_m(lat, lon, alt_m); + double groundlevel_m = get_groundlevel_m(geodetic_position_v); _set_Runway_altitude( groundlevel_m * SG_METER_TO_FEET ); } -// Extrapolate fdm based on time_offset (in usec) -void FGInterface::extrapolate( int time_offset ) { - double dt = time_offset / 1000000.0; - - // -dw- metrowerks complains about ambiguous access, not critical - // to keep this ;) -#ifndef __MWERKS__ - SG_LOG(SG_FLIGHT, SG_INFO, "extrapolating FDM by dt = " << dt); -#endif - - double lat = geodetic_position_v[0] + geocentric_rates_v[0] * dt; - double lat_geoc = geocentric_position_v[0] + geocentric_rates_v[0] * dt; - - double lon = geodetic_position_v[1] + geocentric_rates_v[1] * dt; - double lon_geoc = geocentric_position_v[1] + geocentric_rates_v[1] * dt; - - double alt = geodetic_position_v[2] + geocentric_rates_v[2] * dt; - double radius = geocentric_position_v[2] + geocentric_rates_v[2] * dt; - - geodetic_position_v[0] = lat; - geocentric_position_v[0] = lat_geoc; - - geodetic_position_v[1] = lon; - geocentric_position_v[1] = lon_geoc; - - geodetic_position_v[2] = alt; - geocentric_position_v[2] = radius; -} - // Positions void FGInterface::set_Latitude(double lat) { - geodetic_position_v[0] = lat; + geodetic_position_v.setLatitudeRad(lat); } void FGInterface::set_Longitude(double lon) { - geodetic_position_v[1] = lon; + geodetic_position_v.setLongitudeRad(lon); } void FGInterface::set_Altitude(double alt) { - geodetic_position_v[2] = alt; + geodetic_position_v.setElevationFt(alt); } void FGInterface::set_AltitudeAGL(double altagl) { @@ -699,47 +604,22 @@ void FGInterface::set_Velocities_Local_Airmass (double wnorth, void FGInterface::_busdump(void) { - SG_LOG(SG_FLIGHT,SG_INFO,"d_pilot_rp_body_v[3]: " << d_pilot_rp_body_v[0] << ", " << d_pilot_rp_body_v[1] << ", " << d_pilot_rp_body_v[2]); - SG_LOG(SG_FLIGHT,SG_INFO,"d_cg_rp_body_v[3]: " << d_cg_rp_body_v[0] << ", " << d_cg_rp_body_v[1] << ", " << d_cg_rp_body_v[2]); - SG_LOG(SG_FLIGHT,SG_INFO,"f_body_total_v[3]: " << f_body_total_v[0] << ", " << f_body_total_v[1] << ", " << f_body_total_v[2]); - SG_LOG(SG_FLIGHT,SG_INFO,"f_local_total_v[3]: " << f_local_total_v[0] << ", " << f_local_total_v[1] << ", " << f_local_total_v[2]); - SG_LOG(SG_FLIGHT,SG_INFO,"f_aero_v[3]: " << f_aero_v[0] << ", " << f_aero_v[1] << ", " << f_aero_v[2]); - SG_LOG(SG_FLIGHT,SG_INFO,"f_engine_v[3]: " << f_engine_v[0] << ", " << f_engine_v[1] << ", " << f_engine_v[2]); - SG_LOG(SG_FLIGHT,SG_INFO,"f_gear_v[3]: " << f_gear_v[0] << ", " << f_gear_v[1] << ", " << f_gear_v[2]); - SG_LOG(SG_FLIGHT,SG_INFO,"m_total_rp_v[3]: " << m_total_rp_v[0] << ", " << m_total_rp_v[1] << ", " << m_total_rp_v[2]); - SG_LOG(SG_FLIGHT,SG_INFO,"m_total_cg_v[3]: " << m_total_cg_v[0] << ", " << m_total_cg_v[1] << ", " << m_total_cg_v[2]); - SG_LOG(SG_FLIGHT,SG_INFO,"m_aero_v[3]: " << m_aero_v[0] << ", " << m_aero_v[1] << ", " << m_aero_v[2]); - SG_LOG(SG_FLIGHT,SG_INFO,"m_engine_v[3]: " << m_engine_v[0] << ", " << m_engine_v[1] << ", " << m_engine_v[2]); - SG_LOG(SG_FLIGHT,SG_INFO,"m_gear_v[3]: " << m_gear_v[0] << ", " << m_gear_v[1] << ", " << m_gear_v[2]); - SG_LOG(SG_FLIGHT,SG_INFO,"v_dot_local_v[3]: " << v_dot_local_v[0] << ", " << v_dot_local_v[1] << ", " << v_dot_local_v[2]); - SG_LOG(SG_FLIGHT,SG_INFO,"v_dot_body_v[3]: " << v_dot_body_v[0] << ", " << v_dot_body_v[1] << ", " << v_dot_body_v[2]); - SG_LOG(SG_FLIGHT,SG_INFO,"a_cg_body_v[3]: " << a_cg_body_v[0] << ", " << a_cg_body_v[1] << ", " << a_cg_body_v[2]); - SG_LOG(SG_FLIGHT,SG_INFO,"a_pilot_body_v[3]: " << a_pilot_body_v[0] << ", " << a_pilot_body_v[1] << ", " << a_pilot_body_v[2]); - SG_LOG(SG_FLIGHT,SG_INFO,"n_cg_body_v[3]: " << n_cg_body_v[0] << ", " << n_cg_body_v[1] << ", " << n_cg_body_v[2]); - SG_LOG(SG_FLIGHT,SG_INFO,"n_pilot_body_v[3]: " << n_pilot_body_v[0] << ", " << n_pilot_body_v[1] << ", " << n_pilot_body_v[2]); - SG_LOG(SG_FLIGHT,SG_INFO,"omega_dot_body_v[3]: " << omega_dot_body_v[0] << ", " << omega_dot_body_v[1] << ", " << omega_dot_body_v[2]); - SG_LOG(SG_FLIGHT,SG_INFO,"v_local_v[3]: " << v_local_v[0] << ", " << v_local_v[1] << ", " << v_local_v[2]); - SG_LOG(SG_FLIGHT,SG_INFO,"v_local_rel_ground_v[3]: " << v_local_rel_ground_v[0] << ", " << v_local_rel_ground_v[1] << ", " << v_local_rel_ground_v[2]); - SG_LOG(SG_FLIGHT,SG_INFO,"v_local_airmass_v[3]: " << v_local_airmass_v[0] << ", " << v_local_airmass_v[1] << ", " << v_local_airmass_v[2]); - SG_LOG(SG_FLIGHT,SG_INFO,"v_local_rel_airmass_v[3]: " << v_local_rel_airmass_v[0] << ", " << v_local_rel_airmass_v[1] << ", " << v_local_rel_airmass_v[2]); - SG_LOG(SG_FLIGHT,SG_INFO,"v_local_gust_v[3]: " << v_local_gust_v[0] << ", " << v_local_gust_v[1] << ", " << v_local_gust_v[2]); - SG_LOG(SG_FLIGHT,SG_INFO,"v_wind_body_v[3]: " << v_wind_body_v[0] << ", " << v_wind_body_v[1] << ", " << v_wind_body_v[2]); - SG_LOG(SG_FLIGHT,SG_INFO,"omega_body_v[3]: " << omega_body_v[0] << ", " << omega_body_v[1] << ", " << omega_body_v[2]); - SG_LOG(SG_FLIGHT,SG_INFO,"omega_local_v[3]: " << omega_local_v[0] << ", " << omega_local_v[1] << ", " << omega_local_v[2]); - SG_LOG(SG_FLIGHT,SG_INFO,"omega_total_v[3]: " << omega_total_v[0] << ", " << omega_total_v[1] << ", " << omega_total_v[2]); - SG_LOG(SG_FLIGHT,SG_INFO,"euler_rates_v[3]: " << euler_rates_v[0] << ", " << euler_rates_v[1] << ", " << euler_rates_v[2]); - SG_LOG(SG_FLIGHT,SG_INFO,"geocentric_rates_v[3]: " << geocentric_rates_v[0] << ", " << geocentric_rates_v[1] << ", " << geocentric_rates_v[2]); - SG_LOG(SG_FLIGHT,SG_INFO,"geocentric_position_v[3]: " << geocentric_position_v[0] << ", " << geocentric_position_v[1] << ", " << geocentric_position_v[2]); - SG_LOG(SG_FLIGHT,SG_INFO,"geodetic_position_v[3]: " << geodetic_position_v[0] << ", " << geodetic_position_v[1] << ", " << geodetic_position_v[2]); - SG_LOG(SG_FLIGHT,SG_INFO,"euler_angles_v[3]: " << euler_angles_v[0] << ", " << euler_angles_v[1] << ", " << euler_angles_v[2]); - SG_LOG(SG_FLIGHT,SG_INFO,"d_cg_rwy_local_v[3]: " << d_cg_rwy_local_v[0] << ", " << d_cg_rwy_local_v[1] << ", " << d_cg_rwy_local_v[2]); - SG_LOG(SG_FLIGHT,SG_INFO,"d_cg_rwy_rwy_v[3]: " << d_cg_rwy_rwy_v[0] << ", " << d_cg_rwy_rwy_v[1] << ", " << d_cg_rwy_rwy_v[2]); - SG_LOG(SG_FLIGHT,SG_INFO,"d_pilot_rwy_local_v[3]: " << d_pilot_rwy_local_v[0] << ", " << d_pilot_rwy_local_v[1] << ", " << d_pilot_rwy_local_v[2]); - SG_LOG(SG_FLIGHT,SG_INFO,"d_pilot_rwy_rwy_v[3]: " << d_pilot_rwy_rwy_v[0] << ", " << d_pilot_rwy_rwy_v[1] << ", " << d_pilot_rwy_rwy_v[2]); - - SG_LOG(SG_FLIGHT,SG_INFO,"t_local_to_body_m[0][3]: " << t_local_to_body_m[0][0] << ", " << t_local_to_body_m[0][1] << ", " << t_local_to_body_m[0][2]); - SG_LOG(SG_FLIGHT,SG_INFO,"t_local_to_body_m[1][3]: " << t_local_to_body_m[1][0] << ", " << t_local_to_body_m[1][1] << ", " << t_local_to_body_m[1][2]); - SG_LOG(SG_FLIGHT,SG_INFO,"t_local_to_body_m[2][3]: " << t_local_to_body_m[2][0] << ", " << t_local_to_body_m[2][1] << ", " << t_local_to_body_m[2][2]); + SG_LOG(SG_FLIGHT,SG_INFO,"d_cg_rp_body_v: " << d_cg_rp_body_v); + SG_LOG(SG_FLIGHT,SG_INFO,"v_dot_local_v: " << v_dot_local_v); + SG_LOG(SG_FLIGHT,SG_INFO,"v_dot_body_v: " << v_dot_body_v); + SG_LOG(SG_FLIGHT,SG_INFO,"a_cg_body_v: " << a_cg_body_v); + SG_LOG(SG_FLIGHT,SG_INFO,"a_pilot_body_v: " << a_pilot_body_v); + SG_LOG(SG_FLIGHT,SG_INFO,"n_cg_body_v: " << n_cg_body_v); + SG_LOG(SG_FLIGHT,SG_INFO,"v_local_v: " << v_local_v); + SG_LOG(SG_FLIGHT,SG_INFO,"v_local_rel_ground_v: " << v_local_rel_ground_v); + SG_LOG(SG_FLIGHT,SG_INFO,"v_local_airmass_v: " << v_local_airmass_v); + SG_LOG(SG_FLIGHT,SG_INFO,"v_wind_body_v: " << v_wind_body_v); + SG_LOG(SG_FLIGHT,SG_INFO,"omega_body_v: " << omega_body_v); + SG_LOG(SG_FLIGHT,SG_INFO,"euler_rates_v: " << euler_rates_v); + SG_LOG(SG_FLIGHT,SG_INFO,"geocentric_rates_v: " << geocentric_rates_v); + SG_LOG(SG_FLIGHT,SG_INFO,"geocentric_position_v: " << geocentric_position_v); + SG_LOG(SG_FLIGHT,SG_INFO,"geodetic_position_v: " << geodetic_position_v); + SG_LOG(SG_FLIGHT,SG_INFO,"euler_angles_v: " << euler_angles_v); SG_LOG(SG_FLIGHT,SG_INFO,"mass: " << mass ); SG_LOG(SG_FLIGHT,SG_INFO,"i_xx: " << i_xx ); @@ -749,55 +629,23 @@ void FGInterface::_busdump(void) { SG_LOG(SG_FLIGHT,SG_INFO,"nlf: " << nlf ); SG_LOG(SG_FLIGHT,SG_INFO,"v_rel_wind: " << v_rel_wind ); SG_LOG(SG_FLIGHT,SG_INFO,"v_true_kts: " << v_true_kts ); - SG_LOG(SG_FLIGHT,SG_INFO,"v_rel_ground: " << v_rel_ground ); - SG_LOG(SG_FLIGHT,SG_INFO,"v_inertial: " << v_inertial ); SG_LOG(SG_FLIGHT,SG_INFO,"v_ground_speed: " << v_ground_speed ); - SG_LOG(SG_FLIGHT,SG_INFO,"v_equiv: " << v_equiv ); SG_LOG(SG_FLIGHT,SG_INFO,"v_equiv_kts: " << v_equiv_kts ); - SG_LOG(SG_FLIGHT,SG_INFO,"v_calibrated: " << v_calibrated ); SG_LOG(SG_FLIGHT,SG_INFO,"v_calibrated_kts: " << v_calibrated_kts ); - SG_LOG(SG_FLIGHT,SG_INFO,"gravity: " << gravity ); - SG_LOG(SG_FLIGHT,SG_INFO,"centrifugal_relief: " << centrifugal_relief ); SG_LOG(SG_FLIGHT,SG_INFO,"alpha: " << alpha ); SG_LOG(SG_FLIGHT,SG_INFO,"beta: " << beta ); - SG_LOG(SG_FLIGHT,SG_INFO,"alpha_dot: " << alpha_dot ); - SG_LOG(SG_FLIGHT,SG_INFO,"beta_dot: " << beta_dot ); - SG_LOG(SG_FLIGHT,SG_INFO,"cos_alpha: " << cos_alpha ); - SG_LOG(SG_FLIGHT,SG_INFO,"sin_alpha: " << sin_alpha ); - SG_LOG(SG_FLIGHT,SG_INFO,"cos_beta: " << cos_beta ); - SG_LOG(SG_FLIGHT,SG_INFO,"sin_beta: " << sin_beta ); - SG_LOG(SG_FLIGHT,SG_INFO,"cos_phi: " << cos_phi ); - SG_LOG(SG_FLIGHT,SG_INFO,"sin_phi: " << sin_phi ); - SG_LOG(SG_FLIGHT,SG_INFO,"cos_theta: " << cos_theta ); - SG_LOG(SG_FLIGHT,SG_INFO,"sin_theta: " << sin_theta ); - SG_LOG(SG_FLIGHT,SG_INFO,"cos_psi: " << cos_psi ); - SG_LOG(SG_FLIGHT,SG_INFO,"sin_psi: " << sin_psi ); SG_LOG(SG_FLIGHT,SG_INFO,"gamma_vert_rad: " << gamma_vert_rad ); - SG_LOG(SG_FLIGHT,SG_INFO,"gamma_horiz_rad: " << gamma_horiz_rad ); - SG_LOG(SG_FLIGHT,SG_INFO,"sigma: " << sigma ); SG_LOG(SG_FLIGHT,SG_INFO,"density: " << density ); - SG_LOG(SG_FLIGHT,SG_INFO,"v_sound: " << v_sound ); SG_LOG(SG_FLIGHT,SG_INFO,"mach_number: " << mach_number ); SG_LOG(SG_FLIGHT,SG_INFO,"static_pressure: " << static_pressure ); SG_LOG(SG_FLIGHT,SG_INFO,"total_pressure: " << total_pressure ); - SG_LOG(SG_FLIGHT,SG_INFO,"impact_pressure: " << impact_pressure ); SG_LOG(SG_FLIGHT,SG_INFO,"dynamic_pressure: " << dynamic_pressure ); SG_LOG(SG_FLIGHT,SG_INFO,"static_temperature: " << static_temperature ); SG_LOG(SG_FLIGHT,SG_INFO,"total_temperature: " << total_temperature ); SG_LOG(SG_FLIGHT,SG_INFO,"sea_level_radius: " << sea_level_radius ); SG_LOG(SG_FLIGHT,SG_INFO,"earth_position_angle: " << earth_position_angle ); SG_LOG(SG_FLIGHT,SG_INFO,"runway_altitude: " << runway_altitude ); - SG_LOG(SG_FLIGHT,SG_INFO,"runway_latitude: " << runway_latitude ); - SG_LOG(SG_FLIGHT,SG_INFO,"runway_longitude: " << runway_longitude ); - SG_LOG(SG_FLIGHT,SG_INFO,"runway_heading: " << runway_heading ); - SG_LOG(SG_FLIGHT,SG_INFO,"radius_to_rwy: " << radius_to_rwy ); SG_LOG(SG_FLIGHT,SG_INFO,"climb_rate: " << climb_rate ); - SG_LOG(SG_FLIGHT,SG_INFO,"sin_lat_geocentric: " << sin_lat_geocentric ); - SG_LOG(SG_FLIGHT,SG_INFO,"cos_lat_geocentric: " << cos_lat_geocentric ); - SG_LOG(SG_FLIGHT,SG_INFO,"sin_longitude: " << sin_longitude ); - SG_LOG(SG_FLIGHT,SG_INFO,"cos_longitude: " << cos_longitude ); - SG_LOG(SG_FLIGHT,SG_INFO,"sin_latitude: " << sin_latitude ); - SG_LOG(SG_FLIGHT,SG_INFO,"cos_latitude: " << cos_latitude ); SG_LOG(SG_FLIGHT,SG_INFO,"altitude_agl: " << altitude_agl ); } @@ -821,7 +669,7 @@ FGInterface::is_valid_m(double *ref_time, double pt[3], double *rad) { SGVec3d _pt; bool valid = ground_cache.is_valid(*ref_time, _pt, *rad); - sgdCopyVec3(pt, _pt.data()); + assign(pt, _pt); return valid; } @@ -830,7 +678,7 @@ bool FGInterface::is_valid_ft(double *ref_time, double pt[3], double *rad) // Convert units and do the real work. SGVec3d _pt; bool found_ground = ground_cache.is_valid(*ref_time, _pt, *rad); - sgdScaleVec3(pt, _pt.data(), SG_METER_TO_FEET); + assign(pt, SG_METER_TO_FEET*_pt); *rad *= SG_METER_TO_FEET; return found_ground; } @@ -842,8 +690,8 @@ FGInterface::get_cat_m(double t, const double pt[3], SGVec3d _end[2], _vel[2]; double dist = ground_cache.get_cat(t, SGVec3d(pt), _end, _vel); for (int k=0; k<2; ++k) { - sgdCopyVec3( end[k], _end[k].data() ); - sgdCopyVec3( vel[k], _vel[k].data() ); + assign( end[k], _end[k] ); + assign( vel[k], _vel[k] ); } return dist; } @@ -857,8 +705,8 @@ FGInterface::get_cat_ft(double t, const double pt[3], SGVec3d _end[2], _vel[2]; double dist = ground_cache.get_cat(t, pt_m, _end, _vel); for (int k=0; k<2; ++k) { - sgdScaleVec3( end[k], _end[k].data(), SG_METER_TO_FEET ); - sgdScaleVec3( vel[k], _vel[k].data(), SG_METER_TO_FEET ); + assign( end[k], SG_METER_TO_FEET*_end[k] ); + assign( vel[k], SG_METER_TO_FEET*_vel[k] ); } return dist*SG_METER_TO_FEET; } @@ -874,9 +722,9 @@ FGInterface::get_agl_m(double t, const double pt[3], SGVec3d _contact, _normal, _vel; bool ret = ground_cache.get_agl(t, SGVec3d(pt), 2.0, _contact, _normal, _vel, type, &material, agl); - sgdCopyVec3(contact, _contact.data()); - sgdCopyVec3(normal, _normal.data()); - sgdCopyVec3(vel, _vel.data()); + assign(contact, _contact); + assign(normal, _normal); + assign(vel, _vel); if (material) { *loadCapacity = material->get_load_resistance(); *frictionFactor = material->get_friction_factor(); @@ -896,9 +744,9 @@ FGInterface::get_agl_m(double t, const double pt[3], SGVec3d _contact, _normal, _vel; bool ret = ground_cache.get_agl(t, SGVec3d(pt), 2.0, _contact, _normal, _vel, type, material, agl); - sgdCopyVec3(contact, _contact.data()); - sgdCopyVec3(normal, _normal.data()); - sgdCopyVec3(vel, _vel.data()); + assign(contact, _contact); + assign(normal, _normal); + assign(vel, _vel); return ret; } @@ -917,9 +765,9 @@ FGInterface::get_agl_ft(double t, const double pt[3], bool ret = ground_cache.get_agl(t, pt_m, 2.0, _contact, _normal, _vel, type, &material, agl); // Convert units back ... - sgdScaleVec3( contact, _contact.data(), SG_METER_TO_FEET ); - sgdScaleVec3( vel, _vel.data(), SG_METER_TO_FEET ); - sgdCopyVec3( normal, _normal.data() ); + assign( contact, SG_METER_TO_FEET*_contact ); + assign( vel, SG_METER_TO_FEET*_vel ); + assign( normal, _normal ); *agl *= SG_METER_TO_FEET; // return material properties if available @@ -942,9 +790,9 @@ FGInterface::get_agl_m(double t, const double pt[3], double max_altoff, SGVec3d _contact, _normal, _vel; bool found = ground_cache.get_agl(t, SGVec3d(pt), max_altoff, _contact, _normal, _vel, type, material, agl); - sgdCopyVec3(contact, _contact.data()); - sgdCopyVec3(normal, _normal.data()); - sgdCopyVec3(vel, _vel.data()); + assign(contact, _contact); + assign(normal, _normal); + assign(vel, _vel); return found; } @@ -960,19 +808,24 @@ FGInterface::get_agl_ft(double t, const double pt[3], double max_altoff, _contact, _normal, _vel, type, material, agl); // Convert units back ... - sgdScaleVec3( contact, _contact.data(), SG_METER_TO_FEET ); - sgdScaleVec3( vel, _vel.data(), SG_METER_TO_FEET ); - sgdCopyVec3( normal, _normal.data() ); + assign( contact, SG_METER_TO_FEET*_contact ); + assign( vel, SG_METER_TO_FEET*_vel ); + assign( normal, _normal ); *agl *= SG_METER_TO_FEET; return ret; } - double FGInterface::get_groundlevel_m(double lat, double lon, double alt) +{ + return get_groundlevel_m(SGGeod::fromRadM(lon, lat, alt)); +} + +double +FGInterface::get_groundlevel_m(const SGGeod& geod) { // Compute the cartesian position of the given lat/lon/alt. - SGVec3d pos = SGVec3d::fromGeod(SGGeod::fromRadM(lon, lat, alt)); + SGVec3d pos = SGVec3d::fromGeod(geod); // FIXME: how to handle t - ref_time differences ??? SGVec3d cpos; @@ -984,7 +837,7 @@ FGInterface::get_groundlevel_m(double lat, double lon, double alt) /// too low, try with a new altitude of 10000m, that should be /// sufficient to find a ground level below everywhere on our planet if (!ok) { - pos = SGVec3d::fromGeod(SGGeod::fromRadM(lon, lat, 10000)); + pos = SGVec3d::fromGeod(SGGeod::fromRadM(geod.getLongitudeRad(), geod.getLatitudeRad(), 10000)); /// If there is still no ground, return sea level radius if (!prepare_ground_cache_m(ref_time, pos.data(), 10)) return 0; @@ -999,7 +852,7 @@ FGInterface::get_groundlevel_m(double lat, double lon, double alt) /// too low, try with a new altitude of 10000m, that should be /// sufficient to find a ground level below everywhere on our planet if (!ok) { - pos = SGVec3d::fromGeod(SGGeod::fromRadM(lon, lat, 10000)); + pos = SGVec3d::fromGeod(SGGeod::fromRadM(geod.getLongitudeRad(), geod.getLatitudeRad(), 10000)); /// If there is still no ground, return sea level radius if (!prepare_ground_cache_m(ref_time, pos.data(), radius)) return 0; @@ -1013,8 +866,7 @@ FGInterface::get_groundlevel_m(double lat, double lon, double alt) // computations below the groundcache. The contact point is still something // valid, the normals and the other returns just contain some defaults. get_agl_m(ref_time, pos.data(), 2.0, contact, normal, vel, &type, 0, &agl); - SGGeod geod = SGGeod::fromCart(SGVec3d(contact)); - return geod.getElevationM(); + return SGGeod::fromCart(SGVec3d(contact)).getElevationM(); } bool @@ -1022,7 +874,7 @@ FGInterface::caught_wire_m(double t, const double pt[4][3]) { SGVec3d pt_m[4]; for (int i=0; i<4; ++i) - sgdCopyVec3(pt_m[i].data(), pt[i]); + pt_m[i] = SGVec3d(pt[i]); return ground_cache.caught_wire(t, pt_m); } @@ -1033,7 +885,7 @@ FGInterface::caught_wire_ft(double t, const double pt[4][3]) // Convert units and do the real work. SGVec3d pt_m[4]; for (int i=0; i<4; ++i) - sgdScaleVec3(pt_m[i].data(), pt[i], SG_FEET_TO_METER); + pt_m[i] = SG_FEET_TO_METER*SGVec3d(pt[i]); return ground_cache.caught_wire(t, pt_m); } @@ -1044,8 +896,8 @@ FGInterface::get_wire_ends_m(double t, double end[2][3], double vel[2][3]) SGVec3d _end[2], _vel[2]; bool ret = ground_cache.get_wire_ends(t, _end, _vel); for (int k=0; k<2; ++k) { - sgdCopyVec3( end[k], _end[k].data() ); - sgdCopyVec3( vel[k], _vel[k].data() ); + assign( end[k], _end[k] ); + assign( vel[k], _vel[k] ); } return ret; } @@ -1057,8 +909,8 @@ FGInterface::get_wire_ends_ft(double t, double end[2][3], double vel[2][3]) SGVec3d _end[2], _vel[2]; bool ret = ground_cache.get_wire_ends(t, _end, _vel); for (int k=0; k<2; ++k) { - sgdScaleVec3( end[k], _end[k].data(), SG_METER_TO_FEET ); - sgdScaleVec3( vel[k], _vel[k].data(), SG_METER_TO_FEET ); + assign( end[k], SG_METER_TO_FEET*_end[k] ); + assign( vel[k], SG_METER_TO_FEET*_vel[k] ); } return ret; } diff --git a/src/FDM/flight.hxx b/src/FDM/flight.hxx index 3fdfbd1b2..4a47eeac3 100644 --- a/src/FDM/flight.hxx +++ b/src/FDM/flight.hxx @@ -74,8 +74,6 @@ `FGInterface::get_Dy_cg ()' `FGInterface::get_Dz_cg ()' - `FGInterface::get_T_local_to_body_11 ()' ... `FGInterface::get_T_local_to_body_33 ()' - `FGInterface::get_Radius_to_vehicle ()' */ @@ -96,8 +94,6 @@ SG_USING_STD(list); SG_USING_STD(vector); SG_USING_STD(string); -typedef double FG_VECTOR_3[3]; - // This is based heavily on LaRCsim/ls_generic.h class FGInterface : public SGSubsystem { @@ -121,65 +117,34 @@ private: // next elapsed time. This yields a small amount of temporal // jitter ( < dt ) but in practice seems to work well. -// double delta_t; // delta "t" -// SGTimeStamp time_stamp; // time stamp of last run -// long elapsed; // time elapsed since last run double remainder; // remainder time from last run -// int multi_loop; // number of iterations of "delta_t" to run - - // Pilot location rel to ref pt - FG_VECTOR_3 d_pilot_rp_body_v; // CG position w.r.t. ref. point - FG_VECTOR_3 d_cg_rp_body_v; - - // Forces - FG_VECTOR_3 f_body_total_v; - FG_VECTOR_3 f_local_total_v; - FG_VECTOR_3 f_aero_v; - FG_VECTOR_3 f_engine_v; - FG_VECTOR_3 f_gear_v; - - // Moments - FG_VECTOR_3 m_total_rp_v; - FG_VECTOR_3 m_total_cg_v; - FG_VECTOR_3 m_aero_v; - FG_VECTOR_3 m_engine_v; - FG_VECTOR_3 m_gear_v; + SGVec3d d_cg_rp_body_v; // Accelerations - FG_VECTOR_3 v_dot_local_v; - FG_VECTOR_3 v_dot_body_v; - FG_VECTOR_3 a_cg_body_v; - FG_VECTOR_3 a_pilot_body_v; - FG_VECTOR_3 n_cg_body_v; - FG_VECTOR_3 n_pilot_body_v; - FG_VECTOR_3 omega_dot_body_v; + SGVec3d v_dot_local_v; + SGVec3d v_dot_body_v; + SGVec3d a_cg_body_v; + SGVec3d a_pilot_body_v; + SGVec3d n_cg_body_v; + SGVec3d omega_dot_body_v; // Velocities - FG_VECTOR_3 v_local_v; - FG_VECTOR_3 v_local_rel_ground_v; // V rel w.r.t. earth surface - FG_VECTOR_3 v_local_airmass_v; // velocity of airmass (steady winds) - FG_VECTOR_3 v_local_rel_airmass_v; // velocity of veh. relative to airmass - FG_VECTOR_3 v_local_gust_v; // linear turbulence components, L frame - FG_VECTOR_3 v_wind_body_v; // Wind-relative velocities in body axis - - FG_VECTOR_3 omega_body_v; // Angular B rates - FG_VECTOR_3 omega_local_v; // Angular L rates - FG_VECTOR_3 omega_total_v; // Diff btw B & L - FG_VECTOR_3 euler_rates_v; - FG_VECTOR_3 geocentric_rates_v; // Geocentric linear velocities + SGVec3d v_local_v; + SGVec3d v_local_rel_ground_v; // V rel w.r.t. earth surface + SGVec3d v_local_airmass_v; // velocity of airmass (steady winds) + SGVec3d v_wind_body_v; // Wind-relative velocities in body axis - // Positions - FG_VECTOR_3 geocentric_position_v; - FG_VECTOR_3 geodetic_position_v; - FG_VECTOR_3 euler_angles_v; + SGVec3d omega_body_v; // Angular B rates + SGVec3d euler_rates_v; + SGVec3d geocentric_rates_v; // Geocentric linear velocities - // Miscellaneous Quantities - FG_VECTOR_3 d_cg_rwy_local_v; // CG rel. to rwy in local coords - FG_VECTOR_3 d_cg_rwy_rwy_v; // CG relative to rwy, in rwy coordinates - FG_VECTOR_3 d_pilot_rwy_local_v; // pilot rel. to rwy in local coords - FG_VECTOR_3 d_pilot_rwy_rwy_v; // pilot rel. to rwy, in rwy coords. + // Positions + SGGeod geodetic_position_v; + SGVec3d cartesian_position_v; + SGGeoc geocentric_position_v; + SGVec3d euler_angles_v; // Inertias double mass, i_xx, i_yy, i_zz, i_xz; @@ -188,39 +153,26 @@ private: double nlf; // Velocities - double v_rel_wind, v_true_kts, v_rel_ground, v_inertial; - double v_ground_speed, v_equiv, v_equiv_kts; - double v_calibrated, v_calibrated_kts; + double v_rel_wind, v_true_kts; + double v_ground_speed, v_equiv_kts; + double v_calibrated_kts; // Miscellaneious Quantities - double t_local_to_body_m[3][3]; // Transformation matrix L to B - double gravity; // Local acceleration due to G - double centrifugal_relief; // load factor reduction due to speed - double alpha, beta, alpha_dot, beta_dot; // in radians - double cos_alpha, sin_alpha, cos_beta, sin_beta; - double cos_phi, sin_phi, cos_theta, sin_theta, cos_psi, sin_psi; - double gamma_vert_rad, gamma_horiz_rad; // Flight path angles - double sigma, density, v_sound, mach_number; - double static_pressure, total_pressure, impact_pressure; + double alpha, beta; // in radians + double gamma_vert_rad; // Flight path angles + double density, mach_number; + double static_pressure, total_pressure; double dynamic_pressure; double static_temperature, total_temperature; double sea_level_radius, earth_position_angle; - double runway_altitude, runway_latitude, runway_longitude; - double runway_heading; - double radius_to_rwy; + double runway_altitude; double climb_rate; // in feet per second - double sin_lat_geocentric, cos_lat_geocentric; - double sin_longitude, cos_longitude; - double sin_latitude, cos_latitude; double altitude_agl; double daux[16]; // auxilliary doubles float faux[16]; // auxilliary floats int iaux[16]; // auxilliary ints - // SGTimeStamp valid_stamp; // time this record is valid - // SGTimeStamp next_stamp; // time this record is valid - // the ground cache object itself. FGGroundCache ground_cache; @@ -236,10 +188,16 @@ public: void _setup(); void _busdump(void); + void _updatePositionM(const SGVec3d& cartPos); + void _updatePositionFt(const SGVec3d& cartPos) { + _updatePositionM(SG_FEET_TO_METER*cartPos); + } + void _updatePosition(const SGGeod& geod); + void _updatePosition(const SGGeoc& geoc); + void _updateGeodeticPosition( double lat, double lon, double alt ); void _updateGeocentricPosition( double lat_geoc, double lon, double alt ); void _update_ground_elev_at_pos( void ); - void _updateWeather( void ); inline void _set_Inertias( double m, double xx, double yy, double zz, double xz) @@ -322,49 +280,37 @@ public: geocentric_rates_v[1] = lon; geocentric_rates_v[2] = rad; } -#if 0 - inline void _set_Radius_to_vehicle(double radius) { - geocentric_position_v[2] = radius; - } -#endif inline void _set_Geocentric_Position( double lat, double lon, double rad ) { - geocentric_position_v[0] = lat; - geocentric_position_v[1] = lon; - geocentric_position_v[2] = rad; + geocentric_position_v.setLatitudeRad(lat); + geocentric_position_v.setLongitudeRad(lon); + geocentric_position_v.setRadiusFt(rad); + } + inline void _set_Latitude(double lat) { + geodetic_position_v.setLatitudeRad(lat); + } + inline void _set_Longitude(double lon) { + geodetic_position_v.setLongitudeRad(lon); } - inline void _set_Latitude(double lat) { geodetic_position_v[0] = lat; } - inline void _set_Longitude(double lon) { geodetic_position_v[1] = lon; } inline void _set_Altitude(double altitude) { - geodetic_position_v[2] = altitude; + geodetic_position_v.setElevationFt(altitude); } inline void _set_Altitude_AGL(double agl) { altitude_agl = agl; } inline void _set_Geodetic_Position( double lat, double lon, double alt ) { - geodetic_position_v[0] = lat; - geodetic_position_v[1] = lon; - geodetic_position_v[2] = alt; + geodetic_position_v.setLatitudeRad(lat); + geodetic_position_v.setLongitudeRad(lon); + geodetic_position_v.setElevationFt(alt); } inline void _set_Euler_Angles( double phi, double theta, double psi ) { euler_angles_v[0] = phi; euler_angles_v[1] = theta; euler_angles_v[2] = psi; } - inline void _set_T_Local_to_Body( int i, int j, double value) { - t_local_to_body_m[i-1][j-1] = value; - } - inline void _set_T_Local_to_Body( double m[3][3] ) { - int i, j; - for ( i = 0; i < 3; i++ ) { - for ( j = 0; j < 3; j++ ) { - t_local_to_body_m[i][j] = m[i][j]; - } - } - } + // FIXME, for compatibility with JSBSim + inline void _set_T_Local_to_Body( int i, int j, double value) { } inline void _set_Alpha( double a ) { alpha = a; } inline void _set_Beta( double b ) { beta = b; } - inline void _set_Cos_phi( double cp ) { cos_phi = cp; } - inline void _set_Cos_theta( double ct ) { cos_theta = ct; } inline void _set_Gamma_vert_rad( double gv ) { gamma_vert_rad = gv; } inline void _set_Density( double d ) { density = d; } inline void _set_Mach_number( double m ) { mach_number = m; } @@ -375,20 +321,6 @@ public: inline void _set_Earth_position_angle(double a) { earth_position_angle = a; } inline void _set_Runway_altitude( double alt ) { runway_altitude = alt; } inline void _set_Climb_Rate(double rate) { climb_rate = rate; } - inline void _set_sin_lat_geocentric(double parm) { - sin_lat_geocentric = sin(parm); - } - inline void _set_cos_lat_geocentric(double parm) { - cos_lat_geocentric = cos(parm); - } - inline void _set_sin_cos_longitude(double parm) { - sin_longitude = sin(parm); - cos_longitude = cos(parm); - } - inline void _set_sin_cos_latitude(double parm) { - sin_latitude = sin(parm); - cos_latitude = cos(parm); - } inline void _set_daux( int n, double value ) { daux[n] = value; } inline void _set_faux( int n, float value ) { faux[n] = value; } @@ -445,19 +377,6 @@ public: //perform initializion that is common to all FDM's void common_init(); - // time and update management values -// inline double get_delta_t() const { return delta_t; } -// inline void set_delta_t( double dt ) { delta_t = dt; } -// inline SGTimeStamp get_time_stamp() const { return time_stamp; } -// inline void set_time_stamp( SGTimeStamp s ) { time_stamp = s; } -// inline void stamp() { time_stamp.stamp(); } -// inline long get_elapsed() const { return elapsed; } -// inline void set_elapsed( long e ) { elapsed = e; } -// inline long get_remainder() const { return remainder; } -// inline void set_remainder( long r ) { remainder = r; } -// inline int get_multi_loop() const { return multi_loop; } -// inline void set_multi_loop( int ml ) { multi_loop = ml; } - // Positions virtual void set_Latitude(double lat); // geocentric virtual void set_Longitude(double lon); @@ -538,182 +457,37 @@ public: inline double get_I_zz() const { return i_zz; } inline double get_I_xz() const { return i_xz; } - // Pilot location rel to ref pt - // inline double * get_D_pilot_rp_body_v() { - // return d_pilot_rp_body_v; - // } - // inline double get_Dx_pilot() const { return d_pilot_rp_body_v[0]; } - // inline double get_Dy_pilot() const { return d_pilot_rp_body_v[1]; } - // inline double get_Dz_pilot() const { return d_pilot_rp_body_v[2]; } - /* inline void set_Pilot_Location( double dx, double dy, double dz ) { - d_pilot_rp_body_v[0] = dx; - d_pilot_rp_body_v[1] = dy; - d_pilot_rp_body_v[2] = dz; - } */ - // CG position w.r.t. ref. point - // inline double * get_D_cg_rp_body_v() { return d_cg_rp_body_v; } inline double get_Dx_cg() const { return d_cg_rp_body_v[0]; } inline double get_Dy_cg() const { return d_cg_rp_body_v[1]; } inline double get_Dz_cg() const { return d_cg_rp_body_v[2]; } - // ========== Forces ========== - - // inline double * get_F_body_total_v() { return f_body_total_v; } - // inline double get_F_X() const { return f_body_total_v[0]; } - // inline double get_F_Y() const { return f_body_total_v[1]; } - // inline double get_F_Z() const { return f_body_total_v[2]; } - /* inline void set_Forces_Body_Total( double x, double y, double z ) { - f_body_total_v[0] = x; - f_body_total_v[1] = y; - f_body_total_v[2] = z; - } */ - - // inline double * get_F_local_total_v() { return f_local_total_v; } - // inline double get_F_north() const { return f_local_total_v[0]; } - // inline double get_F_east() const { return f_local_total_v[1]; } - // inline double get_F_down() const { return f_local_total_v[2]; } - /* inline void set_Forces_Local_Total( double x, double y, double z ) { - f_local_total_v[0] = x; - f_local_total_v[1] = y; - f_local_total_v[2] = z; - } */ - - // inline double * get_F_aero_v() { return f_aero_v; } - // inline double get_F_X_aero() const { return f_aero_v[0]; } - // inline double get_F_Y_aero() const { return f_aero_v[1]; } - // inline double get_F_Z_aero() const { return f_aero_v[2]; } - /* inline void set_Forces_Aero( double x, double y, double z ) { - f_aero_v[0] = x; - f_aero_v[1] = y; - f_aero_v[2] = z; - } */ - - // inline double * get_F_engine_v() { return f_engine_v; } - // inline double get_F_X_engine() const { return f_engine_v[0]; } - // inline double get_F_Y_engine() const { return f_engine_v[1]; } - // inline double get_F_Z_engine() const { return f_engine_v[2]; } - /* inline void set_Forces_Engine( double x, double y, double z ) { - f_engine_v[0] = x; - f_engine_v[1] = y; - f_engine_v[2] = z; - } */ - - // inline double * get_F_gear_v() { return f_gear_v; } - // inline double get_F_X_gear() const { return f_gear_v[0]; } - // inline double get_F_Y_gear() const { return f_gear_v[1]; } - // inline double get_F_Z_gear() const { return f_gear_v[2]; } - /* inline void set_Forces_Gear( double x, double y, double z ) { - f_gear_v[0] = x; - f_gear_v[1] = y; - f_gear_v[2] = z; - } */ - - // ========== Moments ========== - - // inline double * get_M_total_rp_v() { return m_total_rp_v; } - // inline double get_M_l_rp() const { return m_total_rp_v[0]; } - // inline double get_M_m_rp() const { return m_total_rp_v[1]; } - // inline double get_M_n_rp() const { return m_total_rp_v[2]; } - /* inline void set_Moments_Total_RP( double l, double m, double n ) { - m_total_rp_v[0] = l; - m_total_rp_v[1] = m; - m_total_rp_v[2] = n; - } */ - - // inline double * get_M_total_cg_v() { return m_total_cg_v; } - // inline double get_M_l_cg() const { return m_total_cg_v[0]; } - // inline double get_M_m_cg() const { return m_total_cg_v[1]; } - // inline double get_M_n_cg() const { return m_total_cg_v[2]; } - /* inline void set_Moments_Total_CG( double l, double m, double n ) { - m_total_cg_v[0] = l; - m_total_cg_v[1] = m; - m_total_cg_v[2] = n; - } */ - - // inline double * get_M_aero_v() { return m_aero_v; } - // inline double get_M_l_aero() const { return m_aero_v[0]; } - // inline double get_M_m_aero() const { return m_aero_v[1]; } - // inline double get_M_n_aero() const { return m_aero_v[2]; } - /* inline void set_Moments_Aero( double l, double m, double n ) { - m_aero_v[0] = l; - m_aero_v[1] = m; - m_aero_v[2] = n; - } */ - - // inline double * get_M_engine_v() { return m_engine_v; } - // inline double get_M_l_engine() const { return m_engine_v[0]; } - // inline double get_M_m_engine() const { return m_engine_v[1]; } - // inline double get_M_n_engine() const { return m_engine_v[2]; } - /* inline void set_Moments_Engine( double l, double m, double n ) { - m_engine_v[0] = l; - m_engine_v[1] = m; - m_engine_v[2] = n; - } */ - - // inline double * get_M_gear_v() { return m_gear_v; } - // inline double get_M_l_gear() const { return m_gear_v[0]; } - // inline double get_M_m_gear() const { return m_gear_v[1]; } - // inline double get_M_n_gear() const { return m_gear_v[2]; } - /* inline void set_Moments_Gear( double l, double m, double n ) { - m_gear_v[0] = l; - m_gear_v[1] = m; - m_gear_v[2] = n; - } */ - // ========== Accelerations ========== - // inline double * get_V_dot_local_v() { return v_dot_local_v; } inline double get_V_dot_north() const { return v_dot_local_v[0]; } inline double get_V_dot_east() const { return v_dot_local_v[1]; } inline double get_V_dot_down() const { return v_dot_local_v[2]; } - // inline double * get_V_dot_body_v() { return v_dot_body_v; } inline double get_U_dot_body() const { return v_dot_body_v[0]; } inline double get_V_dot_body() const { return v_dot_body_v[1]; } inline double get_W_dot_body() const { return v_dot_body_v[2]; } - // inline double * get_A_cg_body_v() { return a_cg_body_v; } inline double get_A_X_cg() const { return a_cg_body_v[0]; } inline double get_A_Y_cg() const { return a_cg_body_v[1]; } inline double get_A_Z_cg() const { return a_cg_body_v[2]; } - // inline double * get_A_pilot_body_v() { return a_pilot_body_v; } inline double get_A_X_pilot() const { return a_pilot_body_v[0]; } inline double get_A_Y_pilot() const { return a_pilot_body_v[1]; } inline double get_A_Z_pilot() const { return a_pilot_body_v[2]; } - // inline double * get_N_cg_body_v() { return n_cg_body_v; } inline double get_N_X_cg() const { return n_cg_body_v[0]; } inline double get_N_Y_cg() const { return n_cg_body_v[1]; } inline double get_N_Z_cg() const { return n_cg_body_v[2]; } - // inline double * get_N_pilot_body_v() { return n_pilot_body_v; } - // inline double get_N_X_pilot() const { return n_pilot_body_v[0]; } - // inline double get_N_Y_pilot() const { return n_pilot_body_v[1]; } - // inline double get_N_Z_pilot() const { return n_pilot_body_v[2]; } - // inline void set_Accels_Pilot_Body_N( double x, double y, double z ) { - // n_pilot_body_v[0] = x; - // n_pilot_body_v[1] = y; - // n_pilot_body_v[2] = z; - // } - inline double get_Nlf(void) const { return nlf; } - // inline double * get_Omega_dot_body_v() { return omega_dot_body_v; } - // inline double get_P_dot_body() const { return omega_dot_body_v[0]; } - // inline double get_Q_dot_body() const { return omega_dot_body_v[1]; } - // inline double get_R_dot_body() const { return omega_dot_body_v[2]; } - /* inline void set_Accels_Omega( double p, double q, double r ) { - omega_dot_body_v[0] = p; - omega_dot_body_v[1] = q; - omega_dot_body_v[2] = r; - } */ - - // ========== Velocities ========== - // inline double * get_V_local_v() { return v_local_v; } inline double get_V_north() const { return v_local_v[0]; } inline double get_V_east() const { return v_local_v[1]; } inline double get_V_down() const { return v_local_v[2]; } @@ -723,9 +497,6 @@ public: // Please dont comment these out. fdm=ada uses these (see // cockpit.cxx) ---> - inline double * get_V_local_rel_ground_v() { - return v_local_rel_ground_v; - } inline double get_V_north_rel_ground() const { return v_local_rel_ground_v[0]; } @@ -737,99 +508,29 @@ public: } // <--- fdm=ada uses these (see cockpit.cxx) - // inline double * get_V_local_airmass_v() { return v_local_airmass_v; } inline double get_V_north_airmass() const { return v_local_airmass_v[0]; } inline double get_V_east_airmass() const { return v_local_airmass_v[1]; } inline double get_V_down_airmass() const { return v_local_airmass_v[2]; } - // airmass - // inline double * get_V_local_rel_airmass_v() { - // return v_local_rel_airmass_v; - // } - // inline double get_V_north_rel_airmass() const { - // return v_local_rel_airmass_v[0]; - // } - // inline double get_V_east_rel_airmass() const { - // return v_local_rel_airmass_v[1]; - // } - // inline double get_V_down_rel_airmass() const { - // return v_local_rel_airmass_v[2]; - // } - /* inline void set_Velocities_Local_Rel_Airmass( double north, double east, - double down) - { - v_local_rel_airmass_v[0] = north; - v_local_rel_airmass_v[1] = east; - v_local_rel_airmass_v[2] = down; - } */ - - // inline double * get_V_local_gust_v() { return v_local_gust_v; } - // inline double get_U_gust() const { return v_local_gust_v[0]; } - // inline double get_V_gust() const { return v_local_gust_v[1]; } - // inline double get_W_gust() const { return v_local_gust_v[2]; } - /* inline void set_Velocities_Gust( double u, double v, double w) - { - v_local_gust_v[0] = u; - v_local_gust_v[1] = v; - v_local_gust_v[2] = w; - } */ - - // inline double * get_V_wind_body_v() { return v_wind_body_v; } inline double get_U_body() const { return v_wind_body_v[0]; } inline double get_V_body() const { return v_wind_body_v[1]; } inline double get_W_body() const { return v_wind_body_v[2]; } inline double get_V_rel_wind() const { return v_rel_wind; } - // inline void set_V_rel_wind(double wind) { v_rel_wind = wind; } inline double get_V_true_kts() const { return v_true_kts; } - // inline void set_V_true_kts(double kts) { v_true_kts = kts; } - - // inline double get_V_rel_ground() const { return v_rel_ground; } - // inline void set_V_rel_ground( double v ) { v_rel_ground = v; } - - // inline double get_V_inertial() const { return v_inertial; } - // inline void set_V_inertial(double v) { v_inertial = v; } inline double get_V_ground_speed() const { return v_ground_speed; } inline double get_V_ground_speed_kt() const { return v_ground_speed * SG_FEET_TO_METER * 3600 * SG_METER_TO_NM; } - // inline double get_V_equiv() const { return v_equiv; } - // inline void set_V_equiv( double v ) { v_equiv = v; } - inline double get_V_equiv_kts() const { return v_equiv_kts; } - //inline double get_V_calibrated() const { return v_calibrated; } - //inline void set_V_calibrated( double v ) { v_calibrated = v; } - inline double get_V_calibrated_kts() const { return v_calibrated_kts; } - // inline double * get_Omega_body_v() { return omega_body_v; } inline double get_P_body() const { return omega_body_v[0]; } inline double get_Q_body() const { return omega_body_v[1]; } inline double get_R_body() const { return omega_body_v[2]; } - // inline double * get_Omega_local_v() { return omega_local_v; } - // inline double get_P_local() const { return omega_local_v[0]; } - // inline double get_Q_local() const { return omega_local_v[1]; } - // inline double get_R_local() const { return omega_local_v[2]; } - /* inline void set_Omega_Local( double p, double q, double r ) { - omega_local_v[0] = p; - omega_local_v[1] = q; - omega_local_v[2] = r; - } */ - - // inline double * get_Omega_total_v() { return omega_total_v; } - // inline double get_P_total() const { return omega_total_v[0]; } - // inline double get_Q_total() const { return omega_total_v[1]; } - // inline double get_R_total() const { return omega_total_v[2]; } - /* inline void set_Omega_Total( double p, double q, double r ) { - omega_total_v[0] = p; - omega_total_v[1] = q; - omega_total_v[2] = r; - } */ - - // inline double * get_Euler_rates_v() { return euler_rates_v; } inline double get_Phi_dot() const { return euler_rates_v[0]; } inline double get_Theta_dot() const { return euler_rates_v[1]; } inline double get_Psi_dot() const { return euler_rates_v[2]; } @@ -837,40 +538,44 @@ public: inline double get_Theta_dot_degps() const { return euler_rates_v[1] * SGD_RADIANS_TO_DEGREES; } inline double get_Psi_dot_degps() const { return euler_rates_v[2] * SGD_RADIANS_TO_DEGREES; } - // inline double * get_Geocentric_rates_v() { return geocentric_rates_v; } inline double get_Latitude_dot() const { return geocentric_rates_v[0]; } inline double get_Longitude_dot() const { return geocentric_rates_v[1]; } inline double get_Radius_dot() const { return geocentric_rates_v[2]; } // ========== Positions ========== - // inline double * get_Geocentric_position_v() { - // return geocentric_position_v; - // } inline double get_Lat_geocentric() const { - return geocentric_position_v[0]; + return geocentric_position_v.getLatitudeRad(); } inline double get_Lon_geocentric() const { - return geocentric_position_v[1]; + return geocentric_position_v.getLongitudeRad(); } inline double get_Radius_to_vehicle() const { - return geocentric_position_v[2]; + return geocentric_position_v.getRadiusFt(); } - // inline double * get_Geodetic_position_v() { return geodetic_position_v; } - inline double get_Latitude() const { return geodetic_position_v[0]; } - inline double get_Longitude() const { return geodetic_position_v[1]; } - inline double get_Altitude() const { return geodetic_position_v[2]; } + const SGGeod& getPosition() const { return geodetic_position_v; } + const SGGeoc& getGeocPosition() const { return geocentric_position_v; } + const SGVec3d& getCartPosition() const { return cartesian_position_v; } + + inline double get_Latitude() const { + return geodetic_position_v.getLatitudeRad(); + } + inline double get_Longitude() const { + return geodetic_position_v.getLongitudeRad(); + } + inline double get_Altitude() const { + return geodetic_position_v.getElevationFt(); + } inline double get_Altitude_AGL(void) const { return altitude_agl; } inline double get_Latitude_deg () const { - return get_Latitude() * SGD_RADIANS_TO_DEGREES; + return geodetic_position_v.getLatitudeDeg(); } inline double get_Longitude_deg () const { - return get_Longitude() * SGD_RADIANS_TO_DEGREES; + return geodetic_position_v.getLongitudeDeg(); } - // inline double * get_Euler_angles_v() { return euler_angles_v; } inline double get_Phi() const { return euler_angles_v[0]; } inline double get_Theta() const { return euler_angles_v[1]; } inline double get_Psi() const { return euler_angles_v[2]; } @@ -881,96 +586,21 @@ public: // ========== Miscellaneous quantities ========== - // inline double * get_T_local_to_body_m() { return t_local_to_body_m; } - inline double get_T_local_to_body_11() const { - return t_local_to_body_m[0][0]; - } - inline double get_T_local_to_body_12() const { - return t_local_to_body_m[0][1]; - } - inline double get_T_local_to_body_13() const { - return t_local_to_body_m[0][2]; - } - inline double get_T_local_to_body_21() const { - return t_local_to_body_m[1][0]; - } - inline double get_T_local_to_body_22() const { - return t_local_to_body_m[1][1]; - } - inline double get_T_local_to_body_23() const { - return t_local_to_body_m[1][2]; - } - inline double get_T_local_to_body_31() const { - return t_local_to_body_m[2][0]; - } - inline double get_T_local_to_body_32() const { - return t_local_to_body_m[2][1]; - } - inline double get_T_local_to_body_33() const { - return t_local_to_body_m[2][2]; - } - - // inline double get_Gravity() const { return gravity; } - // inline void set_Gravity(double g) { gravity = g; } - - // inline double get_Centrifugal_relief() const { - // return centrifugal_relief; - // } - // inline void set_Centrifugal_relief(double cr) { - // centrifugal_relief = cr; - // } - inline double get_Alpha() const { return alpha; } inline double get_Alpha_deg() const { return alpha * SGD_RADIANS_TO_DEGREES; } inline double get_Beta() const { return beta; } inline double get_Beta_deg() const { return beta * SGD_RADIANS_TO_DEGREES; } - inline double get_Alpha_dot() const { return alpha_dot; } - // inline void set_Alpha_dot( double ad ) { alpha_dot = ad; } - inline double get_Beta_dot() const { return beta_dot; } - // inline void set_Beta_dot( double bd ) { beta_dot = bd; } - - // inline double get_Cos_alpha() const { return cos_alpha; } - // inline void set_Cos_alpha( double ca ) { cos_alpha = ca; } - // inline double get_Sin_alpha() const { return sin_alpha; } - // inline void set_Sin_alpha( double sa ) { sin_alpha = sa; } - // inline double get_Cos_beta() const { return cos_beta; } - // inline void set_Cos_beta( double cb ) { cos_beta = cb; } - // inline double get_Sin_beta() const { return sin_beta; } - // inline void set_Sin_beta( double sb ) { sin_beta = sb; } - - inline double get_Cos_phi() const { return cos_phi; } - // inline double get_Sin_phi() const { return sin_phi; } - // inline void set_Sin_phi( double sp ) { sin_phi = sp; } - inline double get_Cos_theta() const { return cos_theta; } - // inline double get_Sin_theta() const { return sin_theta; } - // inline void set_Sin_theta( double st ) { sin_theta = st; } - // inline double get_Cos_psi() const { return cos_psi; } - // inline void set_Cos_psi( double cp ) { cos_psi = cp; } - // inline double get_Sin_psi() const { return sin_psi; } - // inline void set_Sin_psi( double sp ) { sin_psi = sp; } - inline double get_Gamma_vert_rad() const { return gamma_vert_rad; } - // inline double get_Gamma_horiz_rad() const { return gamma_horiz_rad; } - // inline void set_Gamma_horiz_rad( double gh ) { gamma_horiz_rad = gh; } - // inline double get_Sigma() const { return sigma; } - // inline void set_Sigma( double s ) { sigma = s; } inline double get_Density() const { return density; } - // inline double get_V_sound() const { return v_sound; } - // inline void set_V_sound( double v ) { v_sound = v; } inline double get_Mach_number() const { return mach_number; } inline double get_Static_pressure() const { return static_pressure; } inline double get_Total_pressure() const { return total_pressure; } - // inline void set_Total_pressure( double tp ) { total_pressure = tp; } - // inline double get_Impact_pressure() const { return impact_pressure; } - // inline void set_Impact_pressure( double ip ) { impact_pressure = ip; } inline double get_Dynamic_pressure() const { return dynamic_pressure; } - // inline void set_Dynamic_pressure( double dp ) { dynamic_pressure = dp; } inline double get_Static_temperature() const { return static_temperature; } inline double get_Total_temperature() const { return total_temperature; } - // inline void set_Total_temperature( double t ) { total_temperature = t; } inline double get_Sea_level_radius() const { return sea_level_radius; } inline double get_Earth_position_angle() const { @@ -979,102 +609,9 @@ public: inline double get_Runway_altitude() const { return runway_altitude; } inline double get_Runway_altitude_m() const { return SG_FEET_TO_METER * runway_altitude; } - // inline double get_Runway_latitude() const { return runway_latitude; } - // inline void set_Runway_latitude( double lat ) { runway_latitude = lat; } - // inline double get_Runway_longitude() const { return runway_longitude; } - // inline void set_Runway_longitude( double lon ) { - // runway_longitude = lon; - // } - // inline double get_Runway_heading() const { return runway_heading; } - // inline void set_Runway_heading( double h ) { runway_heading = h; } - - // inline double get_Radius_to_rwy() const { return radius_to_rwy; } - // inline void set_Radius_to_rwy( double r ) { radius_to_rwy = r; } - - // inline double * get_D_cg_rwy_local_v() { return d_cg_rwy_local_v; } - // inline double get_D_cg_north_of_rwy() const { - // return d_cg_rwy_local_v[0]; - // } - // inline double get_D_cg_east_of_rwy() const { - // return d_cg_rwy_local_v[1]; - // } - // inline double get_D_cg_above_rwy() const { return d_cg_rwy_local_v[2]; } - /* inline void set_CG_Rwy_Local( double north, double east, double above ) - { - d_cg_rwy_local_v[0] = north; - d_cg_rwy_local_v[1] = east; - d_cg_rwy_local_v[2] = above; - } */ - - // inline double * get_D_cg_rwy_rwy_v() { return d_cg_rwy_rwy_v; } - // inline double get_X_cg_rwy() const { return d_cg_rwy_rwy_v[0]; } - // inline double get_Y_cg_rwy() const { return d_cg_rwy_rwy_v[1]; } - // inline double get_H_cg_rwy() const { return d_cg_rwy_rwy_v[2]; } - /* inline void set_CG_Rwy_Rwy( double x, double y, double h ) - { - d_cg_rwy_rwy_v[0] = x; - d_cg_rwy_rwy_v[1] = y; - d_cg_rwy_rwy_v[2] = h; - } */ - - // inline double * get_D_pilot_rwy_local_v() { return d_pilot_rwy_local_v; } - // inline double get_D_pilot_north_of_rwy() const { - // return d_pilot_rwy_local_v[0]; - // } - // inline double get_D_pilot_east_of_rwy() const { - // return d_pilot_rwy_local_v[1]; - // } - // inline double get_D_pilot_above_rwy() const { - // return d_pilot_rwy_local_v[2]; - // } - /* inline void set_Pilot_Rwy_Local( double north, double east, double above ) - { - d_pilot_rwy_local_v[0] = north; - d_pilot_rwy_local_v[1] = east; - d_pilot_rwy_local_v[2] = above; - } */ - - // inline double * get_D_pilot_rwy_rwy_v() { return d_pilot_rwy_rwy_v; } - // inline double get_X_pilot_rwy() const { return d_pilot_rwy_rwy_v[0]; } - // inline double get_Y_pilot_rwy() const { return d_pilot_rwy_rwy_v[1]; } - // inline double get_H_pilot_rwy() const { return d_pilot_rwy_rwy_v[2]; } - /* inline void set_Pilot_Rwy_Rwy( double x, double y, double h ) - { - d_pilot_rwy_rwy_v[0] = x; - d_pilot_rwy_rwy_v[1] = y; - d_pilot_rwy_rwy_v[2] = h; - } */ inline double get_Climb_Rate() const { return climb_rate; } - // inline SGTimeStamp get_time_stamp() const { return valid_stamp; } - // inline void stamp_time() { valid_stamp = next_stamp; next_stamp.stamp(); } - - // Extrapolate FDM based on time_offset (in usec) - void extrapolate( int time_offset ); - - // sin/cos lat_geocentric - inline double get_sin_lat_geocentric(void) const { - return sin_lat_geocentric; - } - inline double get_cos_lat_geocentric(void) const { - return cos_lat_geocentric; - } - - inline double get_sin_longitude(void) const { - return sin_longitude; - } - inline double get_cos_longitude(void) const { - return cos_longitude; - } - - inline double get_sin_latitude(void) const { - return sin_latitude; - } - inline double get_cos_latitude(void) const { - return cos_latitude; - } - // Auxilliary variables inline double get_daux( int n ) const { return daux[n]; } inline float get_faux( int n ) const { return faux[n]; } @@ -1148,6 +685,7 @@ public: double contact[3], double normal[3], double vel[3], int *type, const SGMaterial** material, double *agl); double get_groundlevel_m(double lat, double lon, double alt); + double get_groundlevel_m(const SGGeod& geod); // Return 1 if the hook intersects with a wire. diff --git a/src/Scenery/scenery.cxx b/src/Scenery/scenery.cxx index b24131196..e7d33aba8 100644 --- a/src/Scenery/scenery.cxx +++ b/src/Scenery/scenery.cxx @@ -118,7 +118,13 @@ bool FGScenery::get_elevation_m(double lat, double lon, double max_alt, double& alt, const SGMaterial** material) { - SGGeod geod = SGGeod::fromDegM(lon, lat, max_alt); + return get_elevation_m(SGGeod::fromDegM(lon, lat, max_alt), alt, material); +} + +bool +FGScenery::get_elevation_m(const SGGeod& geod, + double& alt, const SGMaterial** material) +{ SGVec3d pos = SGVec3d::fromGeod(geod); return get_cart_elevation_m(pos, 0, alt, material); } diff --git a/src/Scenery/scenery.hxx b/src/Scenery/scenery.hxx index 55dca7a90..75512b3aa 100644 --- a/src/Scenery/scenery.hxx +++ b/src/Scenery/scenery.hxx @@ -70,6 +70,8 @@ public: /// All values are meant to be in meters or degrees. bool get_elevation_m(double lat, double lon, double max_alt, double& alt, const SGMaterial** material); + bool get_elevation_m(const SGGeod& geod, double& alt, + const SGMaterial** material); /// Compute the elevation of the scenery beow the cartesian point pos. /// you the returned scenery altitude is not higher than the position -- 2.39.5