//
// You should have received a copy of the GNU General Public License
// along with this program; if not, write to the Free Software
-// Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+// Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
//
// $Id$
+#ifdef HAVE_CONFIG_H
+# include "config.h"
+#endif
#include <stdio.h>
#include <simgear/debug/logstream.hxx>
#include <simgear/math/sg_geodesy.hxx>
#include <simgear/scene/model/placement.hxx>
+#include <simgear/scene/material/mat.hxx>
#include <simgear/timing/timestamp.hxx>
#include <Scenery/scenery.hxx>
// ... ok, two times the roundoff to have enough room.
int multiloop = int(floor(ml * (1.0 + 2.0*DBL_EPSILON)));
remainder = (ml - multiloop) / hz;
+
+ // If we artificially inflate ml above by a tiny amount to get the
+ // closest integer, then subtract the integer from the original
+ // slightly smaller value, we can get a negative remainder.
+ // Logically this should never happen, and we definitely don't want
+ // to carry a negative remainder over to the next iteration, so
+ // never let the remainder go below zero.
+ //
+ // Note: this fixes a problem where we run 1, 3, 1, 3, 1, 3... loops
+ // of the FDM when in fact we want to run 2, 2, 2, 2, 2...
+ if ( remainder < 0 ) { remainder = 0; }
+
return (multiloop * speedup);
}
double alt_m = alt_ft * SG_FEET_TO_METER;
set_Longitude( lon );
set_Latitude( lat );
+ SG_LOG( SG_FLIGHT, SG_INFO, "Checking for lon = "
+ << lon*SGD_RADIANS_TO_DEGREES << "deg, lat = "
+ << lat*SGD_RADIANS_TO_DEGREES << "deg, alt = "
+ << alt_ft << "ft");
double ground_elev_m = get_groundlevel_m(lat, lon, alt_m);
double ground_elev_ft = ground_elev_m * SG_METER_TO_FEET;
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)
{
- return ground_cache.get_agl(t, pt, 2.0, contact, normal, vel, type,
- loadCapacity, frictionFactor, agl);
+ const SGMaterial* material;
+ bool ret = ground_cache.get_agl(t, pt, 2.0, contact, normal, vel, type,
+ &material, agl);
+ if (material) {
+ *loadCapacity = material->get_load_resistence();
+ *frictionFactor = material->get_friction_factor();
+
+ } else {
+ *loadCapacity = DBL_MAX;
+ *frictionFactor = 1.0;
+ }
+ 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],
// Convert units and do the real work.
sgdVec3 pt_m;
sgdScaleVec3( pt_m, pt, SG_FEET_TO_METER );
+
+ const SGMaterial* material;
bool ret = ground_cache.get_agl(t, pt_m, 2.0, contact, normal, vel,
- type, loadCapacity, frictionFactor, agl);
+ type, &material, agl);
// Convert units back ...
sgdScaleVec3( contact, SG_METER_TO_FEET );
sgdScaleVec3( vel, SG_METER_TO_FEET );
*agl *= SG_METER_TO_FEET;
- // FIXME: scale the load limit to something in the english unit system.
- // Be careful with the DBL_MAX which is returned by default.
+
+ // return material properties if available
+ if (material) {
+ // FIXME: convert units?? now pascal to lbf/ft^2
+ *loadCapacity = 0.020885434*material->get_load_resistence();
+ *frictionFactor = material->get_friction_factor();
+ } else {
+ *loadCapacity = DBL_MAX;
+ *frictionFactor = 1.0;
+ }
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, double *loadCapacity,
- double *frictionFactor, double *agl)
+ int *type, const SGMaterial** material, double *agl)
{
return ground_cache.get_agl(t, pt, max_altoff, contact, normal, vel, type,
- loadCapacity, frictionFactor, agl);
+ material, agl);
}
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, double *loadCapacity,
- double *frictionFactor, double *agl)
+ int *type, const SGMaterial** material, double *agl)
{
// Convert units and do the real work.
sgdVec3 pt_m;
sgdScaleVec3( pt_m, pt, SG_FEET_TO_METER );
bool ret = ground_cache.get_agl(t, pt_m, SG_FEET_TO_METER * max_altoff,
contact, normal, vel,
- type, loadCapacity, frictionFactor, agl);
+ type, material, agl);
// Convert units back ...
sgdScaleVec3( contact, SG_METER_TO_FEET );
sgdScaleVec3( vel, SG_METER_TO_FEET );
*agl *= SG_METER_TO_FEET;
- // FIXME: scale the load limit to something in the english unit system.
- // Be careful with the DBL_MAX which is returned by default.
return ret;
}
double
FGInterface::get_groundlevel_m(double lat, double lon, double alt)
{
- // First compute the sea level radius,
sgdVec3 pos, cpos;
- sgGeodToCart(lat, lon, 0, pos);
- double slr = sgdLengthVec3(pos);
- // .. then the cartesian position of the given lat/lon/alt.
+ // Compute the cartesian position of the given lat/lon/alt.
sgGeodToCart(lat, lon, alt, pos);
// FIXME: how to handle t - ref_time differences ???
double ref_time, radius;
// Prepare the ground cache for that position.
- if (!is_valid_m(&ref_time, cpos, &radius))
- prepare_ground_cache_m(ref_time, pos, 10);
- else if (radius*radius <= sgdDistanceSquaredVec3(pos, cpos))
- prepare_ground_cache_m(ref_time, pos, radius);
+ if (!is_valid_m(&ref_time, cpos, &radius)) {
+ bool ok = prepare_ground_cache_m(ref_time, pos, 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) {
+ sgGeodToCart(lat, lon, 10000, pos);
+ /// If there is still no ground, return sea level radius
+ if (!prepare_ground_cache_m(ref_time, pos, 10))
+ return 0;
+ }
+ } else if (radius*radius <= sgdDistanceSquaredVec3(pos, cpos)) {
+ /// 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, 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) {
+ sgGeodToCart(lat, lon, 10000, pos);
+ /// If there is still no ground, return sea level radius
+ if (!prepare_ground_cache_m(ref_time, pos, radius))
+ return 0;
+ }
+ }
- double contact[3], normal[3], vel[3], lc, ff, agl;
+ double contact[3], normal[3], vel[3], agl;
int type;
- get_agl_m(ref_time, pos, 2.0, contact, normal, vel, &type, &lc, &ff, &agl);
-
- return sgdLengthVec3(contact) - slr;
+ // 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, 2.0, contact, normal, vel, &type, 0, &agl);
+ Point3D geodPos = sgCartToGeod(Point3D(contact[0], contact[1], contact[2]));
+ return geodPos.elev();
}
bool
projects / flightgear.git / blobdiff