X-Git-Url: https://git.mxchange.org/?a=blobdiff_plain;f=src%2FEnvironment%2Fterrainsampler.cxx;h=744b3ef977b484a57d9c7ba63cbd32228d7c4c0c;hb=4a3b8c57654f633d8e6923f7217ea4b4d36938a1;hp=460f0336dcc23841e21d38f326541e44060b158f;hpb=f10f1263527661d14fd8dbd5152933e0138847e9;p=flightgear.git diff --git a/src/Environment/terrainsampler.cxx b/src/Environment/terrainsampler.cxx old mode 100755 new mode 100644 index 460f0336d..744b3ef97 --- a/src/Environment/terrainsampler.cxx +++ b/src/Environment/terrainsampler.cxx @@ -30,9 +30,14 @@ #include #include "terrainsampler.hxx" + using simgear::PropertyList; +using std::deque; +using std::vector; +using std::ostringstream; +using std::string; -#include "tiedpropertylist.hxx" +#include namespace Environment { /** @@ -40,14 +45,13 @@ namespace Environment { */ class AreaSampler : public SGSubsystem { public: - AreaSampler( SGPropertyNode_ptr rootNode ); - virtual ~AreaSampler(); - void update( double dt ); + AreaSampler( SGPropertyNode_ptr rootNode ); + virtual ~AreaSampler(); + void update( double dt ); void bind(); void unbind(); - - double getOrientationDeg() const { return _orientation_rad * SG_RADIANS_TO_DEGREES; } - void setOrientationDeg( double value ) { _orientation_rad = value * SG_DEGREES_TO_RADIANS; } + void init(); + void reinit(); int getElevationHistogramStep() const { return _elevationHistogramStep; } void setElevationHistograpStep( int value ) { @@ -70,41 +74,44 @@ private: bool _enabled; bool _useAircraftPosition; - double _latitude_deg; - double _longitude_deg; - double _orientation_rad; - int _radius; - int _samples_per_frame; + double _heading_deg; + double _speed_kt; + int _radius; + double _max_computation_time_norm; int _max_samples; // keep xx samples in queue for analysis - int _analyze_every; // Run analysis every xx samples + double _reuse_samples_norm; + double _recalc_distance_norm; int _elevationHistogramMax; int _elevationHistogramStep; int _elevationHistogramCount; + SGGeod _inputPosition; double _altOffset; double _altMedian; double _altMin; double _altLayered; double _altMean; + SGGeod _outputPosition; + SGPropertyNode_ptr _signalNode; SGPropertyNode_ptr _positionLatitudeNode; SGPropertyNode_ptr _positionLongitudeNode; - deque elevations; - TiedPropertyList _tiedProperties; + deque _elevations; + simgear::TiedPropertyList _tiedProperties; }; AreaSampler::AreaSampler( SGPropertyNode_ptr rootNode ) : _rootNode(rootNode), _enabled(true), _useAircraftPosition(false), - _latitude_deg(0.0), - _longitude_deg(0.0), - _orientation_rad(0.0), - _radius(40000.0), - _samples_per_frame(5), + _heading_deg(0.0), + _speed_kt(0.0), + _radius(40000.0), + _max_computation_time_norm(0.1), _max_samples(1000), - _analyze_every(200), + _reuse_samples_norm(0.8), + _recalc_distance_norm(0.1), _elevationHistogramMax(10000), _elevationHistogramStep(500), _elevationHistogramCount(_elevationHistogramMax/_elevationHistogramStep), @@ -112,10 +119,12 @@ AreaSampler::AreaSampler( SGPropertyNode_ptr rootNode ) : _altMedian(0), _altMin(0), _altLayered(0), - _altMean(0) + _altMean(0), + _signalNode(rootNode->getNode("output/valid", true )), + _positionLatitudeNode(fgGetNode( "/position/latitude-deg", true )), + _positionLongitudeNode(fgGetNode( "/position/longitude-deg", true )) { - _positionLatitudeNode = fgGetNode( "/position/latitude-deg", true ); - _positionLongitudeNode = fgGetNode( "/position/longitude-deg", true ); + _inputPosition.setElevationM( SG_MAX_ELEVATION_M ); } AreaSampler::~AreaSampler() @@ -130,14 +139,15 @@ void AreaSampler::bind() _tiedProperties.setRoot( _rootNode->getNode( "input", true ) ); _tiedProperties.Tie( "use-aircraft-position", &_useAircraftPosition ); - _tiedProperties.Tie( "latitude-deg", &_latitude_deg ); - _tiedProperties.Tie( "latitude-deg", &_latitude_deg ); - _tiedProperties.Tie( "longitude-deg", &_longitude_deg ); - _tiedProperties.Tie( "orientation-deg", this, &AreaSampler::getOrientationDeg, &AreaSampler::setOrientationDeg ); + _tiedProperties.Tie( "latitude-deg", &_inputPosition, &SGGeod::getLatitudeDeg, &SGGeod::setLatitudeDeg ); + _tiedProperties.Tie( "longitude-deg", &_inputPosition, &SGGeod::getLongitudeDeg, &SGGeod::setLongitudeDeg ); + _tiedProperties.Tie( "heading-deg", &_heading_deg ); + _tiedProperties.Tie( "speed-kt", &_speed_kt ); _tiedProperties.Tie( "radius-m", &_radius ); - _tiedProperties.Tie( "max-samples-per-frame", &_samples_per_frame ); + _tiedProperties.Tie( "max-computation-time-norm", &_max_computation_time_norm ); _tiedProperties.Tie( "max-samples", &_max_samples ); - _tiedProperties.Tie( "analyse-every", &_analyze_every ); + _tiedProperties.Tie( "reuse-samples-norm", &_reuse_samples_norm ); + _tiedProperties.Tie( "recalc-distance-norm", &_recalc_distance_norm ); _tiedProperties.Tie( "elevation-histogram-max-ft", this, &AreaSampler::getElevationHistogramMax, &AreaSampler::setElevationHistograpMax ); _tiedProperties.Tie( "elevation-histogram-step-ft", this, &AreaSampler::getElevationHistogramStep, &AreaSampler::setElevationHistograpStep ); _tiedProperties.Tie( "elevation-histogram-count", this, &AreaSampler::getElevationHistogramCount ); @@ -148,6 +158,9 @@ void AreaSampler::bind() _tiedProperties.Tie( "alt-min-ft", &_altMin ); _tiedProperties.Tie( "alt-layered-ft", &_altLayered ); _tiedProperties.Tie( "alt-mean-ft", &_altMean ); + _tiedProperties.Tie( "longitude-deg", &_outputPosition, &SGGeod::getLongitudeDeg ); + _tiedProperties.Tie( "latitude-deg", &_outputPosition, &SGGeod::getLatitudeDeg ); + } void AreaSampler::unbind() @@ -155,45 +168,90 @@ void AreaSampler::unbind() _tiedProperties.Untie(); } +void AreaSampler::init() +{ + _signalNode->setBoolValue(false); + _elevations.clear(); + _altOffset = 0.0; + _altMedian = 0.0; + _altMin = 0.0; + _altLayered = 0.0; + _altMean = 0.0; +} + +void AreaSampler::reinit() +{ + init(); +} + void AreaSampler::update( double dt ) { - if( !(_enabled && dt > SGLimitsd::min()) ) + // if not enabled or time has stalled, do nothing + if( !(_enabled && dt > SGLimitsd::min()) ) return; - if( _useAircraftPosition ) { - _longitude_deg = _positionLongitudeNode->getDoubleValue(); - _latitude_deg = _positionLatitudeNode->getDoubleValue(); + // get the aircraft's position if requested + if( _useAircraftPosition && _speed_kt < 0.5 ) { + _inputPosition = SGGeod::fromDegM( + _positionLongitudeNode->getDoubleValue(), + _positionLatitudeNode->getDoubleValue(), + SG_MAX_ELEVATION_M ); + } + + // need geocentric coordinates + SGGeoc center = SGGeoc::fromGeod( _inputPosition ); + + // if a speed is set, move the input position + if( _speed_kt >= 0.5 ) { + double distance_m = _speed_kt * dt * SG_NM_TO_METER; + center = center.advanceRadM( _heading_deg * SG_DEGREES_TO_RADIANS, distance_m ); + _inputPosition = SGGeod::fromGeoc( center ); + } + + if( _signalNode->getBoolValue() ) { + // if we had finished the iteration and moved more than 10% of the radius + // of the sampling area, drop the oldest samples and continue sampling + if( SGGeoc::distanceM( center, SGGeoc::fromGeod(_outputPosition ) ) >= _recalc_distance_norm * _radius ) { + _elevations.resize( _max_samples * _reuse_samples_norm ); + _signalNode->setBoolValue( false ); + } } - SGGeoc center = SGGeoc::fromGeod( SGGeod::fromDegM( _longitude_deg, _latitude_deg, SG_MAX_ELEVATION_M ) ); + if( _signalNode->getBoolValue() ) + return; // nothing to do. FGScenery * scenery = globals->get_scenery(); - for( int i = 0; - i < _samples_per_frame; - i++ ) { - - double distance = sg_random() * _radius; - double course = sg_random() * 2.0 * SG_PI; - SGGeod probe = SGGeod::fromGeoc(center.advanceRadM( course, distance )); - double elevation_m = 0.0; - if (scenery->get_elevation_m( probe, elevation_m, NULL )) - elevations.push_front(elevation_m *= SG_METER_TO_FEET); + + SGTimeStamp start = SGTimeStamp::now(); + while( (SGTimeStamp::now() - start).toSecs() < dt * _max_computation_time_norm ) { + // sample until we used up all our configured time + double distance = sg_random(); + distance = _radius * (1-distance*distance); + double course = sg_random() * 2.0 * SG_PI; + SGGeod probe = SGGeod::fromGeoc(center.advanceRadM( course, distance )); + double elevation_m = 0.0; + + if (scenery->get_elevation_m( probe, elevation_m, NULL )) + _elevations.push_front(elevation_m *= SG_METER_TO_FEET); - if( elevations.size() >= (deque::size_type)_max_samples ) { + if( _elevations.size() >= (deque::size_type)_max_samples ) { + // sampling complete? analyse(); - elevations.resize( _max_samples - _analyze_every ); + _outputPosition = _inputPosition; + _signalNode->setBoolValue( true ); + break; } - } + } } void AreaSampler::analyse() { double sum; - vector histogram(_elevationHistogramCount,0); + vector histogram(_elevationHistogramCount,0); - for( deque::size_type i = 0; i < elevations.size(); i++ ) { - int idx = SGMisc::clip( (int)(elevations[i]/_elevationHistogramStep), 0, histogram.size()-1 ); + for( deque::size_type i = 0; i < _elevations.size(); i++ ) { + int idx = SGMisc::clip( (int)(_elevations[i]/_elevationHistogramStep), 0, histogram.size()-1 ); histogram[idx]++; } @@ -201,7 +259,7 @@ void AreaSampler::analyse() sum = 0.0; for( vector::size_type i = 0; i < histogram.size(); i++ ) { sum += histogram[i]; - if( sum > 0.5 * elevations.size() ) { + if( sum > 0.5 * _elevations.size() ) { _altMedian = i * _elevationHistogramStep; break; } @@ -211,7 +269,7 @@ void AreaSampler::analyse() sum = 0.0; for( vector::size_type i = 0; i < histogram.size(); i++ ) { sum += histogram[i]; - if( sum > 0.3 * elevations.size() ) { + if( sum > 0.3 * _elevations.size() ) { _altOffset = i * _elevationHistogramStep; break; } @@ -222,7 +280,7 @@ void AreaSampler::analyse() _altMean += histogram[i] * i; } _altMean *= _elevationHistogramStep; - if( elevations.size() != 0.0 ) _altMean /= elevations.size(); + if( _elevations.size() != 0.0 ) _altMean /= _elevations.size(); _altMin = 0.0; for( vector::size_type i = 0; i < histogram.size(); i++ ) { @@ -232,28 +290,21 @@ void AreaSampler::analyse() } } +/* double alt_low_min = 0.0; double n_max = 0.0; sum = 0.0; for( vector::size_type i = 0; i < histogram.size()-1; i++ ) { sum += histogram[i]; if( histogram[i] > n_max ) n_max = histogram[i]; - if( n_max > histogram[i+1] && sum > 0.3*elevations.size()) { + if( n_max > histogram[i+1] && sum > 0.3*_elevations.size()) { alt_low_min = i * _elevationHistogramStep; break; } } - +*/ _altLayered = 0.5 * (_altMin + _altOffset); -#if 0 - SG_LOG( SG_ALL, SG_ALERT, "TerrainPresampler - alalysis results:" << - " total:" << elevations.size() << - " mean:" << _altMean << - " median:" << _altMedian << - " min:" << _altMin << - " alt_20:" << _altOffset ); -#endif #if 0 append(alt_50_array, alt_med); #endif @@ -266,14 +317,16 @@ append(alt_50_array, alt_med); class TerrainSamplerImplementation : public TerrainSampler { public: - TerrainSamplerImplementation ( SGPropertyNode_ptr rootNode ); - virtual ~TerrainSamplerImplementation (); - - virtual void init (); - virtual void reinit (); + TerrainSamplerImplementation ( SGPropertyNode_ptr rootNode ); + virtual ~TerrainSamplerImplementation (); + + virtual void init (); + virtual InitStatus incrementalInit (); + virtual void postinit(); + virtual void reinit (); virtual void bind(); virtual void unbind(); - virtual void update (double delta_time_sec); + virtual void update (double delta_time_sec); private: inline string areaSubsystemName( unsigned i ) { ostringstream name; @@ -281,9 +334,9 @@ private: return name.str(); } - SGPropertyNode_ptr _rootNode; + SGPropertyNode_ptr _rootNode; bool _enabled; - TiedPropertyList _tiedProperties; + simgear::TiedPropertyList _tiedProperties; }; TerrainSamplerImplementation::TerrainSamplerImplementation( SGPropertyNode_ptr rootNode ) : @@ -295,6 +348,12 @@ TerrainSamplerImplementation::TerrainSamplerImplementation( SGPropertyNode_ptr r TerrainSamplerImplementation::~TerrainSamplerImplementation() { } + +SGSubsystem::InitStatus TerrainSamplerImplementation::incrementalInit() +{ + init(); + return INIT_DONE; +} void TerrainSamplerImplementation::init() { @@ -303,10 +362,14 @@ void TerrainSamplerImplementation::init() for( PropertyList::size_type i = 0; i < areaNodes.size(); i++ ) set_subsystem( areaSubsystemName(i), new AreaSampler( areaNodes[i] ) ); - SGSubsystemGroup::bind();// bind the subsystems before the get init()ed SGSubsystemGroup::init(); } +void TerrainSamplerImplementation::postinit() +{ + SGSubsystemGroup::bind();// +} + void TerrainSamplerImplementation::reinit() { for( unsigned i = 0;; i++ ) { @@ -315,6 +378,8 @@ void TerrainSamplerImplementation::reinit() if( subsys == NULL ) break; remove_subsystem( subsystemName ); + subsys->unbind(); + delete subsys; } init(); @@ -334,8 +399,8 @@ void TerrainSamplerImplementation::unbind() void TerrainSamplerImplementation::update( double dt ) { - if( !(_enabled && dt > SGLimitsd::min()) ) - return; + if( !(_enabled && dt > SGLimitsd::min()) ) + return; SGSubsystemGroup::update(dt); } @@ -343,13 +408,9 @@ void TerrainSamplerImplementation::update( double dt ) /* implementation of the TerrainSampler factory to hide the implementation details */ -TerrainSampler::~TerrainSampler () -{ -} - TerrainSampler * TerrainSampler::createInstance( SGPropertyNode_ptr rootNode ) { - return new TerrainSamplerImplementation( rootNode ); + return new TerrainSamplerImplementation( rootNode ); } } // namespace