- FG_LOG(FG_FLIGHT,FG_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]);
- FG_LOG(FG_FLIGHT,FG_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]);
- FG_LOG(FG_FLIGHT,FG_INFO,"f_body_total_v[3]: " << f_body_total_v[0] << ", " << f_body_total_v[1] << ", " << f_body_total_v[2]);
- FG_LOG(FG_FLIGHT,FG_INFO,"f_local_total_v[3]: " << f_local_total_v[0] << ", " << f_local_total_v[1] << ", " << f_local_total_v[2]);
- FG_LOG(FG_FLIGHT,FG_INFO,"f_aero_v[3]: " << f_aero_v[0] << ", " << f_aero_v[1] << ", " << f_aero_v[2]);
- FG_LOG(FG_FLIGHT,FG_INFO,"f_engine_v[3]: " << f_engine_v[0] << ", " << f_engine_v[1] << ", " << f_engine_v[2]);
- FG_LOG(FG_FLIGHT,FG_INFO,"f_gear_v[3]: " << f_gear_v[0] << ", " << f_gear_v[1] << ", " << f_gear_v[2]);
- FG_LOG(FG_FLIGHT,FG_INFO,"m_total_rp_v[3]: " << m_total_rp_v[0] << ", " << m_total_rp_v[1] << ", " << m_total_rp_v[2]);
- FG_LOG(FG_FLIGHT,FG_INFO,"m_total_cg_v[3]: " << m_total_cg_v[0] << ", " << m_total_cg_v[1] << ", " << m_total_cg_v[2]);
- FG_LOG(FG_FLIGHT,FG_INFO,"m_aero_v[3]: " << m_aero_v[0] << ", " << m_aero_v[1] << ", " << m_aero_v[2]);
- FG_LOG(FG_FLIGHT,FG_INFO,"m_engine_v[3]: " << m_engine_v[0] << ", " << m_engine_v[1] << ", " << m_engine_v[2]);
- FG_LOG(FG_FLIGHT,FG_INFO,"m_gear_v[3]: " << m_gear_v[0] << ", " << m_gear_v[1] << ", " << m_gear_v[2]);
- FG_LOG(FG_FLIGHT,FG_INFO,"v_dot_local_v[3]: " << v_dot_local_v[0] << ", " << v_dot_local_v[1] << ", " << v_dot_local_v[2]);
- FG_LOG(FG_FLIGHT,FG_INFO,"v_dot_body_v[3]: " << v_dot_body_v[0] << ", " << v_dot_body_v[1] << ", " << v_dot_body_v[2]);
- FG_LOG(FG_FLIGHT,FG_INFO,"a_cg_body_v[3]: " << a_cg_body_v[0] << ", " << a_cg_body_v[1] << ", " << a_cg_body_v[2]);
- FG_LOG(FG_FLIGHT,FG_INFO,"a_pilot_body_v[3]: " << a_pilot_body_v[0] << ", " << a_pilot_body_v[1] << ", " << a_pilot_body_v[2]);
- FG_LOG(FG_FLIGHT,FG_INFO,"n_cg_body_v[3]: " << n_cg_body_v[0] << ", " << n_cg_body_v[1] << ", " << n_cg_body_v[2]);
- FG_LOG(FG_FLIGHT,FG_INFO,"n_pilot_body_v[3]: " << n_pilot_body_v[0] << ", " << n_pilot_body_v[1] << ", " << n_pilot_body_v[2]);
- FG_LOG(FG_FLIGHT,FG_INFO,"omega_dot_body_v[3]: " << omega_dot_body_v[0] << ", " << omega_dot_body_v[1] << ", " << omega_dot_body_v[2]);
- FG_LOG(FG_FLIGHT,FG_INFO,"v_local_v[3]: " << v_local_v[0] << ", " << v_local_v[1] << ", " << v_local_v[2]);
- FG_LOG(FG_FLIGHT,FG_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]);
- FG_LOG(FG_FLIGHT,FG_INFO,"v_local_airmass_v[3]: " << v_local_airmass_v[0] << ", " << v_local_airmass_v[1] << ", " << v_local_airmass_v[2]);
- FG_LOG(FG_FLIGHT,FG_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]);
- FG_LOG(FG_FLIGHT,FG_INFO,"v_local_gust_v[3]: " << v_local_gust_v[0] << ", " << v_local_gust_v[1] << ", " << v_local_gust_v[2]);
- FG_LOG(FG_FLIGHT,FG_INFO,"v_wind_body_v[3]: " << v_wind_body_v[0] << ", " << v_wind_body_v[1] << ", " << v_wind_body_v[2]);
- FG_LOG(FG_FLIGHT,FG_INFO,"omega_body_v[3]: " << omega_body_v[0] << ", " << omega_body_v[1] << ", " << omega_body_v[2]);
- FG_LOG(FG_FLIGHT,FG_INFO,"omega_local_v[3]: " << omega_local_v[0] << ", " << omega_local_v[1] << ", " << omega_local_v[2]);
- FG_LOG(FG_FLIGHT,FG_INFO,"omega_total_v[3]: " << omega_total_v[0] << ", " << omega_total_v[1] << ", " << omega_total_v[2]);
- FG_LOG(FG_FLIGHT,FG_INFO,"euler_rates_v[3]: " << euler_rates_v[0] << ", " << euler_rates_v[1] << ", " << euler_rates_v[2]);
- FG_LOG(FG_FLIGHT,FG_INFO,"geocentric_rates_v[3]: " << geocentric_rates_v[0] << ", " << geocentric_rates_v[1] << ", " << geocentric_rates_v[2]);
- FG_LOG(FG_FLIGHT,FG_INFO,"geocentric_position_v[3]: " << geocentric_position_v[0] << ", " << geocentric_position_v[1] << ", " << geocentric_position_v[2]);
- FG_LOG(FG_FLIGHT,FG_INFO,"geodetic_position_v[3]: " << geodetic_position_v[0] << ", " << geodetic_position_v[1] << ", " << geodetic_position_v[2]);
- FG_LOG(FG_FLIGHT,FG_INFO,"euler_angles_v[3]: " << euler_angles_v[0] << ", " << euler_angles_v[1] << ", " << euler_angles_v[2]);
- FG_LOG(FG_FLIGHT,FG_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]);
- FG_LOG(FG_FLIGHT,FG_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]);
- FG_LOG(FG_FLIGHT,FG_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]);
- FG_LOG(FG_FLIGHT,FG_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,"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,"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_ground_speed: " << v_ground_speed );
+ SG_LOG(SG_FLIGHT,SG_INFO,"v_equiv_kts: " << v_equiv_kts );
+ SG_LOG(SG_FLIGHT,SG_INFO,"v_calibrated_kts: " << v_calibrated_kts );
+ SG_LOG(SG_FLIGHT,SG_INFO,"alpha: " << alpha );
+ SG_LOG(SG_FLIGHT,SG_INFO,"beta: " << beta );
+ SG_LOG(SG_FLIGHT,SG_INFO,"gamma_vert_rad: " << gamma_vert_rad );
+ SG_LOG(SG_FLIGHT,SG_INFO,"density: " << density );
+ 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,"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,"climb_rate: " << climb_rate );
+ SG_LOG(SG_FLIGHT,SG_INFO,"altitude_agl: " << altitude_agl );
+}
+
+bool
+FGInterface::prepare_ground_cache_m(double startSimTime, double endSimTime,
+ const double pt[3], double rad)
+{
+ return ground_cache.prepare_ground_cache(startSimTime, endSimTime,
+ SGVec3d(pt), rad);
+}
+
+bool
+FGInterface::prepare_ground_cache_ft(double startSimTime, double endSimTime,
+ const double pt[3], double rad)
+{
+ // Convert units and do the real work.
+ SGVec3d pt_ft = SG_FEET_TO_METER*SGVec3d(pt);
+ return ground_cache.prepare_ground_cache(startSimTime, endSimTime,
+ pt_ft, rad*SG_FEET_TO_METER);
+}
+
+bool
+FGInterface::is_valid_m(double *ref_time, double pt[3], double *rad)
+{
+ SGVec3d _pt;
+ bool valid = ground_cache.is_valid(*ref_time, _pt, *rad);
+ assign(pt, _pt);
+ return valid;
+}
+
+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);
+ assign(pt, SG_METER_TO_FEET*_pt);
+ *rad *= SG_METER_TO_FEET;
+ return found_ground;
+}
+
+double
+FGInterface::get_cat_m(double t, const double pt[3],
+ double end[2][3], double vel[2][3])
+{
+ SGVec3d _end[2], _vel[2];
+ double dist = ground_cache.get_cat(t, SGVec3d(pt), _end, _vel);
+ for (int k=0; k<2; ++k) {
+ assign( end[k], _end[k] );
+ assign( vel[k], _vel[k] );
+ }
+ return dist;
+}
+
+double
+FGInterface::get_cat_ft(double t, const double pt[3],
+ double end[2][3], double vel[2][3])
+{
+ // Convert units and do the real work.
+ SGVec3d pt_m = SG_FEET_TO_METER*SGVec3d(pt);
+ SGVec3d _end[2], _vel[2];
+ double dist = ground_cache.get_cat(t, pt_m, _end, _vel);
+ for (int k=0; k<2; ++k) {
+ assign( end[k], SG_METER_TO_FEET*_end[k] );
+ assign( vel[k], SG_METER_TO_FEET*_vel[k] );
+ }
+ return dist*SG_METER_TO_FEET;
+}
+
+bool
+FGInterface::get_body_m(double t, simgear::BVHNode::Id id,
+ double bodyToWorld[16], double linearVel[3],
+ double angularVel[3])
+{
+ SGMatrixd _bodyToWorld;
+ SGVec3d _linearVel, _angularVel;
+ if (!ground_cache.get_body(t, _bodyToWorld, _linearVel, _angularVel, id))
+ return false;
+
+ assign(linearVel, _linearVel);
+ assign(angularVel, _angularVel);
+ for (unsigned i = 0; i < 16; ++i)
+ bodyToWorld[i] = _bodyToWorld.data()[i];
+
+ return true;
+}
+
+bool
+FGInterface::get_agl_m(double t, const double pt[3], double max_altoff,
+ double contact[3], double normal[3],
+ double linearVel[3], double angularVel[3],
+ SGMaterial const*& material, simgear::BVHNode::Id& id)
+{
+ SGVec3d pt_m = SGVec3d(pt) - max_altoff*ground_cache.get_down();
+ SGVec3d _contact, _normal, _linearVel, _angularVel;
+ material = 0;
+ bool ret = ground_cache.get_agl(t, pt_m, _contact, _normal, _linearVel,
+ _angularVel, id, material);
+ // correct the linear velocity, since the line intersector delivers
+ // values for the start point and the get_agl function should
+ // traditionally deliver for the contact point
+ _linearVel += cross(_angularVel, _contact - pt_m);
+
+ assign(contact, _contact);
+ assign(normal, _normal);
+ assign(linearVel, _linearVel);
+ assign(angularVel, _angularVel);
+ return ret;
+}
+
+bool
+FGInterface::get_agl_ft(double t, const double pt[3], double max_altoff,
+ double contact[3], double normal[3],
+ double linearVel[3], double angularVel[3],
+ SGMaterial const*& material, simgear::BVHNode::Id& id)
+{
+ // Convert units and do the real work.
+ SGVec3d pt_m = SGVec3d(pt) - max_altoff*ground_cache.get_down();
+ pt_m *= SG_FEET_TO_METER;
+ SGVec3d _contact, _normal, _linearVel, _angularVel;
+ material = 0;
+ bool ret = ground_cache.get_agl(t, pt_m, _contact, _normal, _linearVel,
+ _angularVel, id, material);
+ // correct the linear velocity, since the line intersector delivers
+ // values for the start point and the get_agl function should
+ // traditionally deliver for the contact point
+ _linearVel += cross(_angularVel, _contact - pt_m);
+
+ // Convert units back ...
+ assign( contact, SG_METER_TO_FEET*_contact );
+ assign( normal, _normal );
+ assign( linearVel, SG_METER_TO_FEET*_linearVel );
+ assign( angularVel, _angularVel );
+ return ret;
+}
+
+bool
+FGInterface::get_nearest_m(double t, const double pt[3], double maxDist,
+ double contact[3], double normal[3],
+ double linearVel[3], double angularVel[3],
+ SGMaterial const*& material,
+ simgear::BVHNode::Id& id)
+{
+ SGVec3d _contact, _linearVel, _angularVel;
+ if (!ground_cache.get_nearest(t, SGVec3d(pt), maxDist, _contact, _linearVel,
+ _angularVel, id, material))
+ return false;
+
+ assign(contact, _contact);
+ assign(linearVel, _linearVel);
+ assign(angularVel, _angularVel);
+ return true;
+}
+
+bool
+FGInterface::get_nearest_ft(double t, const double pt[3], double maxDist,
+ double contact[3], double normal[3],
+ double linearVel[3], double angularVel[3],
+ SGMaterial const*& material,
+ simgear::BVHNode::Id& id)
+{
+ SGVec3d _contact, _linearVel, _angularVel;
+ if (!ground_cache.get_nearest(t, SG_FEET_TO_METER*SGVec3d(pt),
+ SG_FEET_TO_METER*maxDist, _contact, _linearVel,
+ _angularVel, id, material))
+ return false;
+
+ assign(contact, SG_METER_TO_FEET*_contact);
+ assign(linearVel, SG_METER_TO_FEET*_linearVel);
+ assign(angularVel, _angularVel);
+ return true;
+}
+
+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(geod);
+
+ // FIXME: how to handle t - ref_time differences ???
+ SGVec3d cpos;
+ double ref_time = 0, radius;
+ // Prepare the ground cache for that position.
+ if (!is_valid_m(&ref_time, cpos.data(), &radius)) {
+ double startTime = ref_time;
+ double endTime = startTime + 1;
+ bool ok = prepare_ground_cache_m(startTime, endTime, pos.data(), 10);
+ /// This is most likely the case when the given altitude is
+ /// 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::fromGeodM(geod, 10000));
+ /// If there is still no ground, return sea level radius
+ if (!prepare_ground_cache_m(startTime, endTime, pos.data(), 10))
+ return 0;
+ }
+ } else if (radius*radius <= distSqr(pos, cpos)) {
+ double startTime = ref_time;
+ double endTime = startTime + 1;
+
+ /// We reuse the old radius value, but only if it is at least 10 Meters ..
+ if (!(10 < radius)) // Well this strange compare is nan safe
+ radius = 10;
+
+ bool ok = prepare_ground_cache_m(startTime, endTime, pos.data(), radius);
+ /// This is most likely the case when the given altitude is
+ /// 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::fromGeodM(geod, 10000));
+ /// If there is still no ground, return sea level radius
+ if (!prepare_ground_cache_m(startTime, endTime, pos.data(), radius))
+ return 0;
+ }
+ }