1 // terrainsampler.cxx --
3 // Written by Torsten Dreyer, started July 2010
4 // Based on local weather implementation in nasal from
7 // Copyright (C) 2010 Curtis Olson
9 // This program is free software; you can redistribute it and/or
10 // modify it under the terms of the GNU General Public License as
11 // published by the Free Software Foundation; either version 2 of the
12 // License, or (at your option) any later version.
14 // This program is distributed in the hope that it will be useful, but
15 // WITHOUT ANY WARRANTY; without even the implied warranty of
16 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
17 // General Public License for more details.
19 // You should have received a copy of the GNU General Public License
20 // along with this program; if not, write to the Free Software
21 // Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
27 #include <Main/fg_props.hxx>
28 #include <simgear/math/sg_random.h>
29 #include <Scenery/scenery.hxx>
32 #include "terrainsampler.hxx"
34 using simgear::PropertyList;
37 using std::ostringstream;
40 #include <simgear/props/tiedpropertylist.hxx>
42 namespace Environment {
44 * @brief Class for presampling the terrain roughness
46 class AreaSampler : public SGSubsystem {
48 AreaSampler( SGPropertyNode_ptr rootNode );
49 virtual ~AreaSampler();
50 void update( double dt );
56 int getElevationHistogramStep() const { return _elevationHistogramStep; }
57 void setElevationHistograpStep( int value ) {
58 _elevationHistogramStep = value > 0 ? value : 500;
59 _elevationHistogramCount = _elevationHistogramMax / _elevationHistogramStep;
62 int getElevationHistogramMax() const { return _elevationHistogramMax; }
63 void setElevationHistograpMax( int value ) {
64 _elevationHistogramMax = value > 0 ? value : 10000;
65 _elevationHistogramCount = _elevationHistogramMax / _elevationHistogramStep;
68 int getElevationHistogramCount() const { return _elevationHistogramCount; }
73 SGPropertyNode_ptr _rootNode;
76 bool _useAircraftPosition;
80 double _max_computation_time_norm;
81 int _max_samples; // keep xx samples in queue for analysis
82 double _reuse_samples_norm;
83 double _recalc_distance_norm;
84 int _elevationHistogramMax;
85 int _elevationHistogramStep;
86 int _elevationHistogramCount;
87 SGGeod _inputPosition;
94 SGGeod _outputPosition;
96 SGPropertyNode_ptr _signalNode;
97 SGPropertyNode_ptr _positionLatitudeNode;
98 SGPropertyNode_ptr _positionLongitudeNode;
100 deque<double> _elevations;
101 simgear::TiedPropertyList _tiedProperties;
104 AreaSampler::AreaSampler( SGPropertyNode_ptr rootNode ) :
107 _useAircraftPosition(false),
111 _max_computation_time_norm(0.1),
113 _reuse_samples_norm(0.8),
114 _recalc_distance_norm(0.1),
115 _elevationHistogramMax(10000),
116 _elevationHistogramStep(500),
117 _elevationHistogramCount(_elevationHistogramMax/_elevationHistogramStep),
123 _signalNode(rootNode->getNode("output/valid", true )),
124 _positionLatitudeNode(fgGetNode( "/position/latitude-deg", true )),
125 _positionLongitudeNode(fgGetNode( "/position/longitude-deg", true ))
127 _inputPosition.setElevationM( SG_MAX_ELEVATION_M );
130 AreaSampler::~AreaSampler()
135 void AreaSampler::bind()
137 _tiedProperties.setRoot( _rootNode );
138 _tiedProperties.Tie( "enabled", &_enabled );
140 _tiedProperties.setRoot( _rootNode->getNode( "input", true ) );
141 _tiedProperties.Tie( "use-aircraft-position", &_useAircraftPosition );
142 _tiedProperties.Tie( "latitude-deg", &_inputPosition, &SGGeod::getLatitudeDeg, &SGGeod::setLatitudeDeg );
143 _tiedProperties.Tie( "longitude-deg", &_inputPosition, &SGGeod::getLongitudeDeg, &SGGeod::setLongitudeDeg );
144 _tiedProperties.Tie( "heading-deg", &_heading_deg );
145 _tiedProperties.Tie( "speed-kt", &_speed_kt );
146 _tiedProperties.Tie( "radius-m", &_radius );
147 _tiedProperties.Tie( "max-computation-time-norm", &_max_computation_time_norm );
148 _tiedProperties.Tie( "max-samples", &_max_samples );
149 _tiedProperties.Tie( "reuse-samples-norm", &_reuse_samples_norm );
150 _tiedProperties.Tie( "recalc-distance-norm", &_recalc_distance_norm );
151 _tiedProperties.Tie( "elevation-histogram-max-ft", this, &AreaSampler::getElevationHistogramMax, &AreaSampler::setElevationHistograpMax );
152 _tiedProperties.Tie( "elevation-histogram-step-ft", this, &AreaSampler::getElevationHistogramStep, &AreaSampler::setElevationHistograpStep );
153 _tiedProperties.Tie( "elevation-histogram-count", this, &AreaSampler::getElevationHistogramCount );
155 _tiedProperties.setRoot( _rootNode->getNode( "output", true ) );
156 _tiedProperties.Tie( "alt-offset-ft", &_altOffset );
157 _tiedProperties.Tie( "alt-median-ft", &_altMedian );
158 _tiedProperties.Tie( "alt-min-ft", &_altMin );
159 _tiedProperties.Tie( "alt-layered-ft", &_altLayered );
160 _tiedProperties.Tie( "alt-mean-ft", &_altMean );
161 _tiedProperties.Tie( "longitude-deg", &_outputPosition, &SGGeod::getLongitudeDeg );
162 _tiedProperties.Tie( "latitude-deg", &_outputPosition, &SGGeod::getLatitudeDeg );
166 void AreaSampler::unbind()
168 _tiedProperties.Untie();
171 void AreaSampler::init()
173 _signalNode->setBoolValue(false);
182 void AreaSampler::reinit()
187 void AreaSampler::update( double dt )
189 // if not enabled or time has stalled, do nothing
190 if( !(_enabled && dt > SGLimitsd::min()) )
193 // get the aircraft's position if requested
194 if( _useAircraftPosition && _speed_kt < 0.5 ) {
195 _inputPosition = SGGeod::fromDegM(
196 _positionLongitudeNode->getDoubleValue(),
197 _positionLatitudeNode->getDoubleValue(),
198 SG_MAX_ELEVATION_M );
201 // need geocentric coordinates
202 SGGeoc center = SGGeoc::fromGeod( _inputPosition );
204 // if a speed is set, move the input position
205 if( _speed_kt >= 0.5 ) {
206 double distance_m = _speed_kt * dt * SG_NM_TO_METER;
207 center = center.advanceRadM( _heading_deg * SG_DEGREES_TO_RADIANS, distance_m );
208 _inputPosition = SGGeod::fromGeoc( center );
211 if( _signalNode->getBoolValue() ) {
212 // if we had finished the iteration and moved more than 10% of the radius
213 // of the sampling area, drop the oldest samples and continue sampling
214 if( SGGeoc::distanceM( center, SGGeoc::fromGeod(_outputPosition ) ) >= _recalc_distance_norm * _radius ) {
215 _elevations.resize( _max_samples * _reuse_samples_norm );
216 _signalNode->setBoolValue( false );
220 if( _signalNode->getBoolValue() )
221 return; // nothing to do.
223 FGScenery * scenery = globals->get_scenery();
225 SGTimeStamp start = SGTimeStamp::now();
226 while( (SGTimeStamp::now() - start).toSecs() < dt * _max_computation_time_norm ) {
227 // sample until we used up all our configured time
228 double distance = sg_random();
229 distance = _radius * (1-distance*distance);
230 double course = sg_random() * 2.0 * SG_PI;
231 SGGeod probe = SGGeod::fromGeoc(center.advanceRadM( course, distance ));
232 double elevation_m = 0.0;
234 if (scenery->get_elevation_m( probe, elevation_m, NULL ))
235 _elevations.push_front(elevation_m *= SG_METER_TO_FEET);
237 if( _elevations.size() >= (deque<unsigned>::size_type)_max_samples ) {
238 // sampling complete?
240 _outputPosition = _inputPosition;
241 _signalNode->setBoolValue( true );
247 void AreaSampler::analyse()
251 vector<int> histogram(_elevationHistogramCount,0);
253 for( deque<double>::size_type i = 0; i < _elevations.size(); i++ ) {
254 int idx = SGMisc<int>::clip( (int)(_elevations[i]/_elevationHistogramStep), 0, histogram.size()-1 );
260 for( vector<int>::size_type i = 0; i < histogram.size(); i++ ) {
262 if( sum > 0.5 * _elevations.size() ) {
263 _altMedian = i * _elevationHistogramStep;
270 for( vector<int>::size_type i = 0; i < histogram.size(); i++ ) {
272 if( sum > 0.3 * _elevations.size() ) {
273 _altOffset = i * _elevationHistogramStep;
279 for( vector<int>::size_type i = 0; i < histogram.size(); i++ ) {
280 _altMean += histogram[i] * i;
282 _altMean *= _elevationHistogramStep;
283 if( _elevations.size() != 0.0 ) _altMean /= _elevations.size();
286 for( vector<int>::size_type i = 0; i < histogram.size(); i++ ) {
287 if( histogram[i] > 0 ) {
288 _altMin = i * _elevationHistogramStep;
294 double alt_low_min = 0.0;
297 for( vector<int>::size_type i = 0; i < histogram.size()-1; i++ ) {
299 if( histogram[i] > n_max ) n_max = histogram[i];
300 if( n_max > histogram[i+1] && sum > 0.3*_elevations.size()) {
301 alt_low_min = i * _elevationHistogramStep;
306 _altLayered = 0.5 * (_altMin + _altOffset);
309 append(alt_50_array, alt_med);
313 /* --------------------- End of AreaSampler implementation ------------- */
315 /* --------------------- TerrainSamplerImplementation -------------------------- */
317 class TerrainSamplerImplementation : public TerrainSampler
320 TerrainSamplerImplementation ( SGPropertyNode_ptr rootNode );
321 virtual ~TerrainSamplerImplementation ();
323 virtual void init ();
324 virtual void postinit();
325 virtual void reinit ();
327 virtual void unbind();
328 virtual void update (double delta_time_sec);
330 inline string areaSubsystemName( unsigned i ) {
336 SGPropertyNode_ptr _rootNode;
338 simgear::TiedPropertyList _tiedProperties;
341 TerrainSamplerImplementation::TerrainSamplerImplementation( SGPropertyNode_ptr rootNode ) :
342 _rootNode( rootNode ),
347 TerrainSamplerImplementation::~TerrainSamplerImplementation()
351 void TerrainSamplerImplementation::init()
353 PropertyList areaNodes = _rootNode->getChildren( "area" );
355 for( PropertyList::size_type i = 0; i < areaNodes.size(); i++ )
356 set_subsystem( areaSubsystemName(i), new AreaSampler( areaNodes[i] ) );
358 SGSubsystemGroup::init();
361 void TerrainSamplerImplementation::postinit()
363 SGSubsystemGroup::bind();//
366 void TerrainSamplerImplementation::reinit()
368 for( unsigned i = 0;; i++ ) {
369 string subsystemName = areaSubsystemName(i);
370 SGSubsystem * subsys = get_subsystem( subsystemName );
373 remove_subsystem( subsystemName );
381 void TerrainSamplerImplementation::bind()
383 SGSubsystemGroup::bind();
384 _tiedProperties.Tie( _rootNode->getNode("enabled",true), &_enabled );
387 void TerrainSamplerImplementation::unbind()
389 _tiedProperties.Untie();
390 SGSubsystemGroup::unbind();
393 void TerrainSamplerImplementation::update( double dt )
395 if( !(_enabled && dt > SGLimitsd::min()) )
397 SGSubsystemGroup::update(dt);
400 /* ----------------------------------------------------------------------- */
402 /* implementation of the TerrainSampler factory to hide the implementation
404 TerrainSampler * TerrainSampler::createInstance( SGPropertyNode_ptr rootNode )
406 return new TerrainSamplerImplementation( rootNode );