src/Environment/terrainsampler.cxx

   1 // terrainsampler.cxx --
   2 //
   3 // Written by Torsten Dreyer, started July 2010
   4 // Based on local weather implementation in nasal from
   5 // Thorsten Renk
   6 //
   7 // Copyright (C) 2010  Curtis Olson
   8 //
   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.
  13 //
  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.
  18 //
  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.
  22 //
  23 #ifdef HAVE_CONFIG_H
  24 #  include <config.h>
  25 #endif
  26
  27 #include <Main/fg_props.hxx>
  28 #include <simgear/math/sg_random.h>
  29 #include <Scenery/scenery.hxx>
  30 #include <deque>
  31
  32 #include "terrainsampler.hxx"
  33
  34 using simgear::PropertyList;
  35 using std::deque;
  36 using std::vector;
  37 using std::ostringstream;
  38 using std::string;
  39
  40 #include <simgear/props/tiedpropertylist.hxx>
  41
  42 namespace Environment {
  43 /**
  44  * @brief Class for presampling the terrain roughness
  45  */
  46 class AreaSampler : public SGSubsystem {
  47 public:
  48     AreaSampler( SGPropertyNode_ptr rootNode );
  49     virtual ~AreaSampler();
  50     void update( double dt );
  51     void bind();
  52     void unbind();
  53     void init();
  54     void reinit();
  55
  56     int getElevationHistogramStep() const { return _elevationHistogramStep; }
  57     void setElevationHistograpStep( int value ) {
  58         _elevationHistogramStep = value > 0 ? value : 500;
  59         _elevationHistogramCount = _elevationHistogramMax / _elevationHistogramStep;
  60     }
  61
  62     int getElevationHistogramMax() const { return _elevationHistogramMax; }
  63     void setElevationHistograpMax( int value ) {
  64         _elevationHistogramMax = value > 0 ? value : 10000;
  65         _elevationHistogramCount = _elevationHistogramMax / _elevationHistogramStep;
  66     }
  67
  68     int getElevationHistogramCount() const { return _elevationHistogramCount; }
  69
  70 private:
  71     void analyse();
  72
  73     SGPropertyNode_ptr _rootNode;
  74
  75     bool _enabled;
  76     bool _useAircraftPosition;
  77     double _heading_deg;
  78     double _speed_kt;
  79     int _radius;
  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;
  88
  89     double _altOffset;
  90     double _altMedian;
  91     double _altMin;
  92     double _altLayered;
  93     double _altMean;
  94     SGGeod _outputPosition;
  95
  96     SGPropertyNode_ptr _signalNode;
  97     SGPropertyNode_ptr _positionLatitudeNode;
  98     SGPropertyNode_ptr _positionLongitudeNode;
  99
 100     deque<double> _elevations;
 101     simgear::TiedPropertyList _tiedProperties;
 102 };
 103
 104 AreaSampler::AreaSampler( SGPropertyNode_ptr rootNode ) :
 105     _rootNode(rootNode),
 106     _enabled(true),
 107     _useAircraftPosition(false),
 108     _heading_deg(0.0),
 109     _speed_kt(0.0),
 110     _radius(40000.0),
 111     _max_computation_time_norm(0.1),
 112     _max_samples(1000),
 113     _reuse_samples_norm(0.8),
 114     _recalc_distance_norm(0.1),
 115     _elevationHistogramMax(10000),
 116     _elevationHistogramStep(500),
 117     _elevationHistogramCount(_elevationHistogramMax/_elevationHistogramStep),
 118     _altOffset(0),
 119     _altMedian(0),
 120     _altMin(0),
 121     _altLayered(0),
 122     _altMean(0),
 123     _signalNode(rootNode->getNode("output/valid", true )),
 124     _positionLatitudeNode(fgGetNode( "/position/latitude-deg", true )),
 125     _positionLongitudeNode(fgGetNode( "/position/longitude-deg", true ))
 126 {
 127     _inputPosition.setElevationM( SG_MAX_ELEVATION_M );
 128 }
 129
 130 AreaSampler::~AreaSampler()
 131 {
 132 }
 133
 134
 135 void AreaSampler::bind()
 136 {
 137     _tiedProperties.setRoot( _rootNode );
 138     _tiedProperties.Tie( "enabled", &_enabled );
 139
 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 );
 154
 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 );
 163
 164 }
 165
 166 void AreaSampler::unbind()
 167 {
 168     _tiedProperties.Untie();
 169 }
 170
 171 void AreaSampler::init()
 172 {
 173    _signalNode->setBoolValue(false);
 174    _elevations.clear();
 175    _altOffset = 0.0;
 176    _altMedian = 0.0;
 177    _altMin = 0.0;
 178    _altLayered = 0.0;
 179    _altMean = 0.0;
 180 }
 181
 182 void AreaSampler::reinit()
 183 {
 184     init();
 185 }
 186
 187 void AreaSampler::update( double dt )
 188 {
 189     // if not enabled or time has stalled, do nothing
 190     if( !(_enabled && dt > SGLimitsd::min()) )
 191         return;
 192
 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 );
 199     }
 200
 201     // need geocentric coordinates
 202     SGGeoc center = SGGeoc::fromGeod( _inputPosition );
 203
 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 );
 209     }
 210
 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 );
 217         }
 218     }
 219
 220     if( _signalNode->getBoolValue() )
 221         return; // nothing to do.
 222
 223     FGScenery * scenery = globals->get_scenery();
 224
 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;
 233
 234         if (scenery->get_elevation_m( probe, elevation_m, NULL ))
 235             _elevations.push_front(elevation_m *= SG_METER_TO_FEET);
 236
 237         if( _elevations.size() >= (deque<unsigned>::size_type)_max_samples ) {
 238             // sampling complete?
 239             analyse();
 240             _outputPosition = _inputPosition;
 241             _signalNode->setBoolValue( true );
 242             break;
 243         }
 244     }
 245 }
 246
 247 void AreaSampler::analyse()
 248 {
 249     double sum;
 250
 251     vector<int> histogram(_elevationHistogramCount,0);
 252
 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 );
 255         histogram[idx]++;
 256     }
 257
 258     _altMedian = 0.0;
 259     sum = 0.0;
 260     for( vector<int>::size_type i = 0; i < histogram.size(); i++ ) {
 261         sum += histogram[i];
 262         if( sum > 0.5 * _elevations.size() ) {
 263             _altMedian = i * _elevationHistogramStep;
 264             break;
 265         }
 266     }
 267
 268     _altOffset = 0.0;
 269     sum = 0.0;
 270     for(  vector<int>::size_type i = 0; i < histogram.size(); i++ ) {
 271         sum += histogram[i];
 272         if( sum > 0.3 * _elevations.size() ) {
 273             _altOffset = i * _elevationHistogramStep;
 274             break;
 275         }
 276     }
 277
 278    _altMean = 0.0;
 279     for(  vector<int>::size_type i = 0; i < histogram.size(); i++ ) {
 280         _altMean += histogram[i] * i;
 281     }
 282     _altMean *= _elevationHistogramStep;
 283     if( _elevations.size() != 0.0 ) _altMean /= _elevations.size();
 284
 285     _altMin = 0.0;
 286     for(  vector<int>::size_type i = 0; i < histogram.size(); i++ ) {
 287         if( histogram[i] > 0 ) {
 288             _altMin = i * _elevationHistogramStep;
 289             break;
 290         }
 291     }
 292
 293 /*
 294     double alt_low_min = 0.0;
 295     double n_max = 0.0;
 296     sum = 0.0;
 297     for(  vector<int>::size_type i = 0; i < histogram.size()-1; i++ ) {
 298         sum += histogram[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;
 302             break;
 303         }
 304     }
 305 */
 306     _altLayered = 0.5 * (_altMin + _altOffset);
 307
 308 #if 0
 309 append(alt_50_array, alt_med);
 310 #endif
 311 }
 312
 313 /* --------------------- End of AreaSampler implementation ------------- */
 314
 315 /* --------------------- TerrainSamplerImplementation -------------------------- */
 316
 317 class TerrainSamplerImplementation : public TerrainSampler
 318 {
 319 public:
 320     TerrainSamplerImplementation ( SGPropertyNode_ptr rootNode );
 321     virtual ~TerrainSamplerImplementation ();
 322
 323     virtual void init ();
 324     virtual InitStatus incrementalInit ();
 325     virtual void postinit();
 326     virtual void reinit ();
 327     virtual void bind();
 328     virtual void unbind();
 329     virtual void update (double delta_time_sec);
 330 private:
 331     inline string areaSubsystemName( unsigned i ) {
 332       ostringstream name;
 333       name <<  "area" << i;
 334       return name.str();
 335     }
 336
 337     SGPropertyNode_ptr _rootNode;
 338     bool _enabled;
 339     simgear::TiedPropertyList _tiedProperties;
 340 };
 341
 342 TerrainSamplerImplementation::TerrainSamplerImplementation( SGPropertyNode_ptr rootNode ) :
 343     _rootNode( rootNode ),
 344     _enabled(true)
 345 {
 346 }
 347
 348 TerrainSamplerImplementation::~TerrainSamplerImplementation()
 349 {
 350 }
 351
 352 SGSubsystem::InitStatus TerrainSamplerImplementation::incrementalInit()
 353 {
 354   init();
 355   return INIT_DONE;
 356 }
 357
 358 void TerrainSamplerImplementation::init()
 359 {
 360     PropertyList areaNodes = _rootNode->getChildren( "area" );
 361
 362     for( PropertyList::size_type i = 0; i < areaNodes.size(); i++ )
 363         set_subsystem( areaSubsystemName(i), new AreaSampler( areaNodes[i] ) );
 364
 365     SGSubsystemGroup::init();
 366 }
 367
 368 void TerrainSamplerImplementation::postinit()
 369 {
 370     SGSubsystemGroup::bind();//
 371 }
 372
 373 void TerrainSamplerImplementation::reinit()
 374 {
 375     for( unsigned i = 0;; i++ ) {
 376         string subsystemName = areaSubsystemName(i);
 377         SGSubsystem * subsys = get_subsystem( subsystemName );
 378         if( subsys == NULL )
 379             break;
 380         remove_subsystem( subsystemName );
 381         subsys->unbind();
 382         delete subsys;
 383     }
 384
 385     init();
 386 }
 387
 388 void TerrainSamplerImplementation::bind()
 389 {
 390     SGSubsystemGroup::bind();
 391     _tiedProperties.Tie( _rootNode->getNode("enabled",true), &_enabled );
 392 }
 393
 394 void TerrainSamplerImplementation::unbind()
 395 {
 396     _tiedProperties.Untie();
 397     SGSubsystemGroup::unbind();
 398 }
 399
 400 void TerrainSamplerImplementation::update( double dt )
 401 {
 402     if( !(_enabled && dt > SGLimitsd::min()) )
 403         return;
 404     SGSubsystemGroup::update(dt);
 405 }
 406
 407 /* ----------------------------------------------------------------------- */
 408
 409 /* implementation of the TerrainSampler factory to hide the implementation
 410    details */
 411 TerrainSampler * TerrainSampler::createInstance( SGPropertyNode_ptr rootNode )
 412 {
 413     return new TerrainSamplerImplementation( rootNode );
 414 }
 415
 416 } // namespace
 417