1 // terrainsampler.cxx --
\r
3 // Written by Torsten Dreyer, started July 2010
\r
4 // Based on local weather implementation in nasal from
\r
7 // Copyright (C) 2010 Curtis Olson
\r
9 // This program is free software; you can redistribute it and/or
\r
10 // modify it under the terms of the GNU General Public License as
\r
11 // published by the Free Software Foundation; either version 2 of the
\r
12 // License, or (at your option) any later version.
\r
14 // This program is distributed in the hope that it will be useful, but
\r
15 // WITHOUT ANY WARRANTY; without even the implied warranty of
\r
16 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
\r
17 // General Public License for more details.
\r
19 // You should have received a copy of the GNU General Public License
\r
20 // along with this program; if not, write to the Free Software
\r
21 // Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
\r
23 #ifdef HAVE_CONFIG_H
\r
24 # include <config.h>
\r
27 #include <Main/fg_props.hxx>
\r
28 #include <simgear/math/sg_random.h>
\r
29 #include <Scenery/scenery.hxx>
\r
32 #include "terrainsampler.hxx"
\r
33 using simgear::PropertyList;
\r
35 #include "tiedpropertylist.hxx"
\r
37 namespace Environment {
\r
39 * @brief Class for presampling the terrain roughness
\r
41 class AreaSampler : public SGSubsystem {
\r
43 AreaSampler( SGPropertyNode_ptr rootNode );
\r
44 virtual ~AreaSampler();
\r
45 void update( double dt );
\r
49 double getOrientationDeg() const { return _orientation_rad * SG_RADIANS_TO_DEGREES; }
\r
50 void setOrientationDeg( double value ) { _orientation_rad = value * SG_DEGREES_TO_RADIANS; }
\r
52 int getElevationHistogramStep() const { return _elevationHistogramStep; }
\r
53 void setElevationHistograpStep( int value ) {
\r
54 _elevationHistogramStep = value > 0 ? value : 500;
\r
55 _elevationHistogramCount = _elevationHistogramMax / _elevationHistogramStep;
\r
58 int getElevationHistogramMax() const { return _elevationHistogramMax; }
\r
59 void setElevationHistograpMax( int value ) {
\r
60 _elevationHistogramMax = value > 0 ? value : 10000;
\r
61 _elevationHistogramCount = _elevationHistogramMax / _elevationHistogramStep;
\r
64 int getElevationHistogramCount() const { return _elevationHistogramCount; }
\r
69 SGPropertyNode_ptr _rootNode;
\r
72 bool _useAircraftPosition;
\r
73 double _latitude_deg;
\r
74 double _longitude_deg;
\r
75 double _orientation_rad;
\r
77 int _samples_per_frame;
\r
78 int _max_samples; // keep xx samples in queue for analysis
\r
79 int _analyze_every; // Run analysis every xx samples
\r
80 int _elevationHistogramMax;
\r
81 int _elevationHistogramStep;
\r
82 int _elevationHistogramCount;
\r
90 SGPropertyNode_ptr _positionLatitudeNode;
\r
91 SGPropertyNode_ptr _positionLongitudeNode;
\r
93 deque<double> elevations;
\r
94 TiedPropertyList _tiedProperties;
\r
97 AreaSampler::AreaSampler( SGPropertyNode_ptr rootNode ) :
\r
98 _rootNode(rootNode),
\r
100 _useAircraftPosition(false),
\r
101 _latitude_deg(0.0),
\r
102 _longitude_deg(0.0),
\r
103 _orientation_rad(0.0),
\r
105 _samples_per_frame(5),
\r
106 _max_samples(1000),
\r
107 _analyze_every(200),
\r
108 _elevationHistogramMax(10000),
\r
109 _elevationHistogramStep(500),
\r
110 _elevationHistogramCount(_elevationHistogramMax/_elevationHistogramStep),
\r
117 _positionLatitudeNode = fgGetNode( "/position/latitude-deg", true );
\r
118 _positionLongitudeNode = fgGetNode( "/position/longitude-deg", true );
\r
121 AreaSampler::~AreaSampler()
\r
126 void AreaSampler::bind()
\r
128 _tiedProperties.setRoot( _rootNode );
\r
129 _tiedProperties.Tie( "enabled", &_enabled );
\r
131 _tiedProperties.setRoot( _rootNode->getNode( "input", true ) );
\r
132 _tiedProperties.Tie( "use-aircraft-position", &_useAircraftPosition );
\r
133 _tiedProperties.Tie( "latitude-deg", &_latitude_deg );
\r
134 _tiedProperties.Tie( "latitude-deg", &_latitude_deg );
\r
135 _tiedProperties.Tie( "longitude-deg", &_longitude_deg );
\r
136 _tiedProperties.Tie( "orientation-deg", this, &AreaSampler::getOrientationDeg, &AreaSampler::setOrientationDeg );
\r
137 _tiedProperties.Tie( "radius-m", &_radius );
\r
138 _tiedProperties.Tie( "max-samples-per-frame", &_samples_per_frame );
\r
139 _tiedProperties.Tie( "max-samples", &_max_samples );
\r
140 _tiedProperties.Tie( "analyse-every", &_analyze_every );
\r
141 _tiedProperties.Tie( "elevation-histogram-max-ft", this, &AreaSampler::getElevationHistogramMax, &AreaSampler::setElevationHistograpMax );
\r
142 _tiedProperties.Tie( "elevation-histogram-step-ft", this, &AreaSampler::getElevationHistogramStep, &AreaSampler::setElevationHistograpStep );
\r
143 _tiedProperties.Tie( "elevation-histogram-count", this, &AreaSampler::getElevationHistogramCount );
\r
145 _tiedProperties.setRoot( _rootNode->getNode( "output", true ) );
\r
146 _tiedProperties.Tie( "alt-offset-ft", &_altOffset );
\r
147 _tiedProperties.Tie( "alt-median-ft", &_altMedian );
\r
148 _tiedProperties.Tie( "alt-min-ft", &_altMin );
\r
149 _tiedProperties.Tie( "alt-layered-ft", &_altLayered );
\r
150 _tiedProperties.Tie( "alt-mean-ft", &_altMean );
\r
153 void AreaSampler::unbind()
\r
155 _tiedProperties.Untie();
\r
158 void AreaSampler::update( double dt )
\r
160 if( !(_enabled && dt > SGLimitsd::min()) )
\r
163 if( _useAircraftPosition ) {
\r
164 _longitude_deg = _positionLongitudeNode->getDoubleValue();
\r
165 _latitude_deg = _positionLatitudeNode->getDoubleValue();
\r
168 SGGeoc center = SGGeoc::fromGeod( SGGeod::fromDegM( _longitude_deg, _latitude_deg, SG_MAX_ELEVATION_M ) );
\r
170 FGScenery * scenery = globals->get_scenery();
\r
172 i < _samples_per_frame;
\r
175 double distance = sg_random() * _radius;
\r
176 double course = sg_random() * 2.0 * SG_PI;
\r
177 SGGeod probe = SGGeod::fromGeoc(center.advanceRadM( course, distance ));
\r
178 double elevation_m = 0.0;
\r
179 if (scenery->get_elevation_m( probe, elevation_m, NULL ))
\r
180 elevations.push_front(elevation_m *= SG_METER_TO_FEET);
\r
182 if( elevations.size() >= (deque<unsigned>::size_type)_max_samples ) {
\r
184 elevations.resize( _max_samples - _analyze_every );
\r
189 void AreaSampler::analyse()
\r
193 vector<int> histogram(_elevationHistogramCount,0);
\r
195 for( deque<double>::size_type i = 0; i < elevations.size(); i++ ) {
\r
196 int idx = SGMisc<int>::clip( (int)(elevations[i]/_elevationHistogramStep), 0, histogram.size()-1 );
\r
202 for( vector<int>::size_type i = 0; i < histogram.size(); i++ ) {
\r
203 sum += histogram[i];
\r
204 if( sum > 0.5 * elevations.size() ) {
\r
205 _altMedian = i * _elevationHistogramStep;
\r
212 for( vector<int>::size_type i = 0; i < histogram.size(); i++ ) {
\r
213 sum += histogram[i];
\r
214 if( sum > 0.3 * elevations.size() ) {
\r
215 _altOffset = i * _elevationHistogramStep;
\r
221 for( vector<int>::size_type i = 0; i < histogram.size(); i++ ) {
\r
222 _altMean += histogram[i] * i;
\r
224 _altMean *= _elevationHistogramStep;
\r
225 if( elevations.size() != 0.0 ) _altMean /= elevations.size();
\r
228 for( vector<int>::size_type i = 0; i < histogram.size(); i++ ) {
\r
229 if( histogram[i] > 0 ) {
\r
230 _altMin = i * _elevationHistogramStep;
\r
235 double alt_low_min = 0.0;
\r
236 double n_max = 0.0;
\r
238 for( vector<int>::size_type i = 0; i < histogram.size()-1; i++ ) {
\r
239 sum += histogram[i];
\r
240 if( histogram[i] > n_max ) n_max = histogram[i];
\r
241 if( n_max > histogram[i+1] && sum > 0.3*elevations.size()) {
\r
242 alt_low_min = i * _elevationHistogramStep;
\r
247 _altLayered = 0.5 * (_altMin + _altOffset);
\r
250 SG_LOG( SG_ALL, SG_ALERT, "TerrainPresampler - alalysis results:" <<
\r
251 " total:" << elevations.size() <<
\r
252 " mean:" << _altMean <<
\r
253 " median:" << _altMedian <<
\r
254 " min:" << _altMin <<
\r
255 " alt_20:" << _altOffset );
\r
258 append(alt_50_array, alt_med);
\r
262 /* --------------------- End of AreaSampler implementation ------------- */
\r
264 /* --------------------- TerrainSamplerImplementation -------------------------- */
\r
266 class TerrainSamplerImplementation : public TerrainSampler
\r
269 TerrainSamplerImplementation ( SGPropertyNode_ptr rootNode );
\r
270 virtual ~TerrainSamplerImplementation ();
\r
272 virtual void init ();
\r
273 virtual void reinit ();
\r
274 virtual void bind();
\r
275 virtual void unbind();
\r
276 virtual void update (double delta_time_sec);
\r
278 inline string areaSubsystemName( unsigned i ) {
\r
279 ostringstream name;
\r
280 name << "area" << i;
\r
284 SGPropertyNode_ptr _rootNode;
\r
286 TiedPropertyList _tiedProperties;
\r
289 TerrainSamplerImplementation::TerrainSamplerImplementation( SGPropertyNode_ptr rootNode ) :
\r
290 _rootNode( rootNode ),
\r
295 TerrainSamplerImplementation::~TerrainSamplerImplementation()
\r
299 void TerrainSamplerImplementation::init()
\r
301 PropertyList areaNodes = _rootNode->getChildren( "area" );
\r
303 for( PropertyList::size_type i = 0; i < areaNodes.size(); i++ )
\r
304 set_subsystem( areaSubsystemName(i), new AreaSampler( areaNodes[i] ) );
\r
306 SGSubsystemGroup::bind();// bind the subsystems before the get init()ed
\r
307 SGSubsystemGroup::init();
\r
310 void TerrainSamplerImplementation::reinit()
\r
312 for( unsigned i = 0;; i++ ) {
\r
313 string subsystemName = areaSubsystemName(i);
\r
314 SGSubsystem * subsys = get_subsystem( subsystemName );
\r
315 if( subsys == NULL )
\r
317 remove_subsystem( subsystemName );
\r
323 void TerrainSamplerImplementation::bind()
\r
325 SGSubsystemGroup::bind();
\r
326 _tiedProperties.Tie( _rootNode->getNode("enabled",true), &_enabled );
\r
329 void TerrainSamplerImplementation::unbind()
\r
331 _tiedProperties.Untie();
\r
332 SGSubsystemGroup::unbind();
\r
335 void TerrainSamplerImplementation::update( double dt )
\r
337 if( !(_enabled && dt > SGLimitsd::min()) )
\r
339 SGSubsystemGroup::update(dt);
\r
342 /* ----------------------------------------------------------------------- */
\r
344 /* implementation of the TerrainSampler factory to hide the implementation
\r
346 TerrainSampler::~TerrainSampler ()
\r
350 TerrainSampler * TerrainSampler::createInstance( SGPropertyNode_ptr rootNode )
\r
352 return new TerrainSamplerImplementation( rootNode );
\r