X-Git-Url: https://git.mxchange.org/?a=blobdiff_plain;f=src%2FFDM%2Fflight.cxx;h=0e4ff912a3948a99ffce66c9a298993d110eae60;hb=a99ea1c7b5fb0ba69810bbad9b6aca8b4cdc116b;hp=637649b97381ac97970fe70cebbce8a6eff1968e;hpb=17262df51cc5a9a542a35d6126b55f08cb210f7e;p=flightgear.git diff --git a/src/FDM/flight.cxx b/src/FDM/flight.cxx index 637649b97..0e4ff912a 100644 --- a/src/FDM/flight.cxx +++ b/src/FDM/flight.cxx @@ -29,7 +29,6 @@ #include #include #include -#include #include #include @@ -130,7 +129,6 @@ FGInterface::_setup () 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=0; v_ground_speed=v_equiv_kts=0; @@ -165,6 +163,8 @@ FGInterface::common_init () set_inited( true ); + ground_cache.set_cache_time_offset(globals->get_sim_time_sec()); + // stamp(); // set_remainder( 0 ); @@ -621,11 +621,6 @@ void FGInterface::_busdump(void) { 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 ); - SG_LOG(SG_FLIGHT,SG_INFO,"i_yy: " << i_yy ); - SG_LOG(SG_FLIGHT,SG_INFO,"i_zz: " << i_zz ); - SG_LOG(SG_FLIGHT,SG_INFO,"i_xz: " << i_xz ); 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 ); @@ -650,18 +645,21 @@ void FGInterface::_busdump(void) { } bool -FGInterface::prepare_ground_cache_m(double ref_time, const double pt[3], - double rad) +FGInterface::prepare_ground_cache_m(double startSimTime, double endSimTime, + const double pt[3], double rad) { - return ground_cache.prepare_ground_cache(ref_time, SGVec3d(pt), rad); + return ground_cache.prepare_ground_cache(startSimTime, endSimTime, + SGVec3d(pt), rad); } -bool FGInterface::prepare_ground_cache_ft(double ref_time, const double pt[3], - double 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(ref_time, pt_ft, rad*SG_FEET_TO_METER); + return ground_cache.prepare_ground_cache(startSimTime, endSimTime, + pt_ft, rad*SG_FEET_TO_METER); } bool @@ -711,108 +709,108 @@ FGInterface::get_cat_ft(double t, const double pt[3], return dist*SG_METER_TO_FEET; } -// Legacy interface just kept because of JSBSim bool -FGInterface::get_agl_m(double t, const double pt[3], - double contact[3], double normal[3], double vel[3], - int *type, double *loadCapacity, - double *frictionFactor, double *agl) +FGInterface::get_body_m(double t, simgear::BVHNode::Id id, + double bodyToWorld[16], double linearVel[3], + double angularVel[3]) { - const SGMaterial* material; - SGVec3d _contact, _normal, _vel; - bool ret = ground_cache.get_agl(t, SGVec3d(pt), 2.0, _contact, _normal, - _vel, type, &material, agl); - assign(contact, _contact); - assign(normal, _normal); - assign(vel, _vel); - if (material) { - *loadCapacity = material->get_load_resistance(); - *frictionFactor = material->get_friction_factor(); - - } else { - *loadCapacity = DBL_MAX; - *frictionFactor = 1.0; - } - return ret; + 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 contact[3], double normal[3], double vel[3], - int *type, const SGMaterial **material, double *agl) -{ - SGVec3d _contact, _normal, _vel; - bool ret = ground_cache.get_agl(t, SGVec3d(pt), 2.0, _contact, _normal, - _vel, type, material, agl); +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(vel, _vel); + assign(linearVel, _linearVel); + assign(angularVel, _angularVel); return ret; } -// Legacy interface just kept because of JSBSim bool -FGInterface::get_agl_ft(double t, const double pt[3], - double contact[3], double normal[3], double vel[3], - int *type, double *loadCapacity, - double *frictionFactor, double *agl) +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 = SG_FEET_TO_METER*SGVec3d(pt); + 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); - const SGMaterial* material; - SGVec3d _contact, _normal, _vel; - bool ret = ground_cache.get_agl(t, pt_m, 2.0, _contact, _normal, _vel, - type, &material, agl); // Convert units back ... 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 - if (material) { - // FIXME: convert units?? now pascal to lbf/ft^2 - *loadCapacity = 0.020885434*material->get_load_resistance(); - *frictionFactor = material->get_friction_factor(); - } else { - *loadCapacity = DBL_MAX; - *frictionFactor = 1.0; - } + assign( linearVel, SG_METER_TO_FEET*_linearVel ); + assign( angularVel, _angularVel ); return ret; } bool -FGInterface::get_agl_m(double t, const double pt[3], double max_altoff, - double contact[3], double normal[3], double vel[3], - int *type, const SGMaterial** material, double *agl) +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, _normal, _vel; - bool found = ground_cache.get_agl(t, SGVec3d(pt), max_altoff, _contact, - _normal, _vel, type, material, agl); + SGVec3d _contact, _linearVel, _angularVel; + if (!ground_cache.get_nearest(t, SGVec3d(pt), maxDist, _contact, _linearVel, + _angularVel, id, material)) + return false; + assign(contact, _contact); - assign(normal, _normal); - assign(vel, _vel); - return found; + assign(linearVel, _linearVel); + assign(angularVel, _angularVel); + return true; } bool -FGInterface::get_agl_ft(double t, const double pt[3], double max_altoff, - double contact[3], double normal[3], double vel[3], - int *type, const SGMaterial** material, double *agl) +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) { - // Convert units and do the real work. - SGVec3d pt_m = SG_FEET_TO_METER*SGVec3d(pt); - SGVec3d _contact, _normal, _vel; - bool ret = ground_cache.get_agl(t, pt_m, SG_FEET_TO_METER * max_altoff, - _contact, _normal, _vel, - type, material, agl); - // Convert units back ... - assign( contact, SG_METER_TO_FEET*_contact ); - assign( vel, SG_METER_TO_FEET*_vel ); - assign( normal, _normal ); - *agl *= SG_METER_TO_FEET; - return ret; + 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 @@ -829,43 +827,50 @@ FGInterface::get_groundlevel_m(const SGGeod& geod) // FIXME: how to handle t - ref_time differences ??? SGVec3d cpos; - double ref_time, radius; + double ref_time = 0, radius; // Prepare the ground cache for that position. if (!is_valid_m(&ref_time, cpos.data(), &radius)) { - bool ok = prepare_ground_cache_m(ref_time, pos.data(), 10); + 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::fromRadM(geod.getLongitudeRad(), geod.getLatitudeRad(), 10000)); + pos = SGVec3d::fromGeod(SGGeod::fromGeodM(geod, 10000)); /// If there is still no ground, return sea level radius - if (!prepare_ground_cache_m(ref_time, pos.data(), 10)) + 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(ref_time, pos.data(), radius); + 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::fromRadM(geod.getLongitudeRad(), geod.getLatitudeRad(), 10000)); + pos = SGVec3d::fromGeod(SGGeod::fromGeodM(geod, 10000)); /// If there is still no ground, return sea level radius - if (!prepare_ground_cache_m(ref_time, pos.data(), radius)) + if (!prepare_ground_cache_m(startTime, endTime, pos.data(), radius)) return 0; } } - double contact[3], normal[3], vel[3], agl; - int type; + double contact[3], normal[3], vel[3], angvel[3]; + const SGMaterial* material; + simgear::BVHNode::Id id; // Ignore the return value here, since it just tells us if // the returns stem from the groundcache or from the coarse // 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); + get_agl_m(ref_time, pos.data(), 2.0, contact, normal, vel, angvel, + material, id); return SGGeod::fromCart(SGVec3d(contact)).getElevationM(); }