src/Environment/terrainsampler.cxx

   1 // terrainsampler.cxx --\r
   2 //\r
   3 // Written by Torsten Dreyer, started July 2010\r
   4 // Based on local weather implementation in nasal from \r
   5 // Thorsten Renk\r
   6 //\r
   7 // Copyright (C) 2010  Curtis Olson\r
   8 //\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
  13 //\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
  18 //\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
  22 //\r
  23 #ifdef HAVE_CONFIG_H\r
  24 #  include <config.h>\r
  25 #endif\r
  26 \r
  27 #include <Main/fg_props.hxx>\r
  28 #include <simgear/math/sg_random.h>\r
  29 #include <Scenery/scenery.hxx>\r
  30 #include <deque>\r
  31 \r
  32 #include "terrainsampler.hxx"\r
  33 using simgear::PropertyList;\r
  34 \r
  35 #include "tiedpropertylist.hxx"\r
  36 \r
  37 namespace Environment {\r
  38 /**\r
  39  * @brief Class for presampling the terrain roughness\r
  40  */\r
  41 class AreaSampler : public SGSubsystem {\r
  42 public:\r
  43         AreaSampler( SGPropertyNode_ptr rootNode );\r
  44         virtual ~AreaSampler();\r
  45         void update( double dt );\r
  46     void bind();\r
  47     void unbind();\r
  48 \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
  51 \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
  56     }\r
  57 \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
  62     }\r
  63 \r
  64     int getElevationHistogramCount() const { return _elevationHistogramCount; }\r
  65 \r
  66 private:\r
  67     void analyse();\r
  68 \r
  69     SGPropertyNode_ptr _rootNode;\r
  70 \r
  71     bool _enabled;\r
  72     bool _useAircraftPosition;\r
  73         double _latitude_deg;\r
  74         double _longitude_deg;\r
  75         double _orientation_rad;\r
  76         int _radius;\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
  83 \r
  84     double _altOffset;\r
  85     double _altMedian;\r
  86     double _altMin;\r
  87     double _altLayered;\r
  88     double _altMean;\r
  89 \r
  90     SGPropertyNode_ptr _positionLatitudeNode;\r
  91     SGPropertyNode_ptr _positionLongitudeNode;\r
  92 \r
  93     deque<double> elevations;\r
  94     TiedPropertyList _tiedProperties;\r
  95 };\r
  96 \r
  97 AreaSampler::AreaSampler( SGPropertyNode_ptr rootNode ) :\r
  98     _rootNode(rootNode),\r
  99     _enabled(true),\r
 100     _useAircraftPosition(false),\r
 101     _latitude_deg(0.0),\r
 102         _longitude_deg(0.0),\r
 103         _orientation_rad(0.0),\r
 104         _radius(40000.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
 111     _altOffset(0),\r
 112     _altMedian(0),\r
 113     _altMin(0),\r
 114     _altLayered(0),\r
 115     _altMean(0)\r
 116 {\r
 117     _positionLatitudeNode = fgGetNode( "/position/latitude-deg", true );\r
 118     _positionLongitudeNode = fgGetNode( "/position/longitude-deg", true );\r
 119 }\r
 120 \r
 121 AreaSampler::~AreaSampler()\r
 122 {\r
 123 }\r
 124 \r
 125 \r
 126 void AreaSampler::bind()\r
 127 {\r
 128     _tiedProperties.setRoot( _rootNode );\r
 129     _tiedProperties.Tie( "enabled", &_enabled );\r
 130 \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
 144 \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
 151 }\r
 152 \r
 153 void AreaSampler::unbind()\r
 154 {\r
 155     _tiedProperties.Untie();\r
 156 }\r
 157 \r
 158 void AreaSampler::update( double dt )\r
 159 {\r
 160         if( !(_enabled && dt > SGLimitsd::min()) )\r
 161         return;\r
 162 \r
 163     if( _useAircraftPosition ) {\r
 164         _longitude_deg = _positionLongitudeNode->getDoubleValue();\r
 165         _latitude_deg = _positionLatitudeNode->getDoubleValue();\r
 166     }\r
 167 \r
 168     SGGeoc center = SGGeoc::fromGeod( SGGeod::fromDegM( _longitude_deg, _latitude_deg, SG_MAX_ELEVATION_M ) );\r
 169 \r
 170     FGScenery * scenery = globals->get_scenery();\r
 171         for( int i = 0; \r
 172                 i < _samples_per_frame; \r
 173                 i++ ) {\r
 174 \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
 181         \r
 182         if( elevations.size() >= (deque<unsigned>::size_type)_max_samples ) {\r
 183             analyse();\r
 184             elevations.resize( _max_samples - _analyze_every );\r
 185         }\r
 186         }\r
 187 }\r
 188 \r
 189 void AreaSampler::analyse()\r
 190 {\r
 191     double sum;\r
 192 \r
 193         vector<int> histogram(_elevationHistogramCount,0);\r
 194     \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
 197         histogram[idx]++;\r
 198     }\r
 199 \r
 200     _altMedian = 0.0;\r
 201     sum = 0.0;\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
 206             break;\r
 207         }\r
 208     }\r
 209 \r
 210     _altOffset = 0.0;\r
 211     sum = 0.0;\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
 216             break;\r
 217         }\r
 218     }\r
 219 \r
 220    _altMean = 0.0;\r
 221     for(  vector<int>::size_type i = 0; i < histogram.size(); i++ ) {\r
 222         _altMean += histogram[i] * i;\r
 223     }\r
 224     _altMean *= _elevationHistogramStep;\r
 225     if( elevations.size() != 0.0 ) _altMean /= elevations.size();\r
 226 \r
 227     _altMin = 0.0;\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
 231             break;\r
 232         }\r
 233     }\r
 234 \r
 235     double alt_low_min = 0.0;\r
 236     double n_max = 0.0;\r
 237     sum = 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
 243             break;\r
 244         }\r
 245     }\r
 246 \r
 247     _altLayered = 0.5 * (_altMin + _altOffset);\r
 248 \r
 249 #if 0\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
 256 #endif\r
 257 #if 0\r
 258 append(alt_50_array, alt_med);\r
 259 #endif\r
 260 }\r
 261 \r
 262 /* --------------------- End of AreaSampler implementation ------------- */\r
 263 \r
 264 /* --------------------- TerrainSamplerImplementation -------------------------- */\r
 265 \r
 266 class TerrainSamplerImplementation : public TerrainSampler\r
 267 {\r
 268 public:\r
 269         TerrainSamplerImplementation ( SGPropertyNode_ptr rootNode );\r
 270         virtual ~TerrainSamplerImplementation ();\r
 271         \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
 277 private:\r
 278     inline string areaSubsystemName( unsigned i ) {\r
 279       ostringstream name;\r
 280       name <<  "area" << i;\r
 281       return name.str();\r
 282     }\r
 283 \r
 284         SGPropertyNode_ptr _rootNode;\r
 285     bool _enabled;\r
 286     TiedPropertyList _tiedProperties;\r
 287 };\r
 288 \r
 289 TerrainSamplerImplementation::TerrainSamplerImplementation( SGPropertyNode_ptr rootNode ) :\r
 290     _rootNode( rootNode ),\r
 291     _enabled(true)\r
 292 {\r
 293 }\r
 294 \r
 295 TerrainSamplerImplementation::~TerrainSamplerImplementation()\r
 296 {\r
 297 }\r
 298 \r
 299 void TerrainSamplerImplementation::init()\r
 300 {\r
 301     PropertyList areaNodes = _rootNode->getChildren( "area" );\r
 302     \r
 303     for( PropertyList::size_type i = 0; i < areaNodes.size(); i++ )\r
 304         set_subsystem( areaSubsystemName(i), new AreaSampler( areaNodes[i] ) );\r
 305 \r
 306     SGSubsystemGroup::bind();// bind the subsystems before the get init()ed\r
 307     SGSubsystemGroup::init();\r
 308 }\r
 309 \r
 310 void TerrainSamplerImplementation::reinit()\r
 311 {\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
 316             break;\r
 317         remove_subsystem( subsystemName );\r
 318     }\r
 319     \r
 320     init();\r
 321 }\r
 322 \r
 323 void TerrainSamplerImplementation::bind()\r
 324 {\r
 325     SGSubsystemGroup::bind();\r
 326     _tiedProperties.Tie( _rootNode->getNode("enabled",true), &_enabled );\r
 327 }\r
 328 \r
 329 void TerrainSamplerImplementation::unbind()\r
 330 {\r
 331     _tiedProperties.Untie();\r
 332     SGSubsystemGroup::unbind();\r
 333 }\r
 334 \r
 335 void TerrainSamplerImplementation::update( double dt )\r
 336 {\r
 337         if( !(_enabled && dt > SGLimitsd::min()) )\r
 338                 return;\r
 339     SGSubsystemGroup::update(dt);\r
 340 }\r
 341 \r
 342 /* ----------------------------------------------------------------------- */\r
 343 \r
 344 /* implementation of the TerrainSampler factory to hide the implementation\r
 345    details */\r
 346 TerrainSampler::~TerrainSampler ()\r
 347 {\r
 348 }\r
 349 \r
 350 TerrainSampler * TerrainSampler::createInstance( SGPropertyNode_ptr rootNode )\r
 351 {\r
 352         return new TerrainSamplerImplementation( rootNode );\r
 353 }\r
 354 \r
 355 } // namespace\r
 356 \r