ARGoS
3
A parallel, multi-engine simulator for swarm robotics
|
00001 00009 #include "rng.h" 00010 #include <argos3/core/utility/configuration/argos_exception.h> 00011 #include <argos3/core/utility/logging/argos_log.h> 00012 #include <cstring> 00013 #include <limits> 00014 #include <cmath> 00015 00016 #ifdef ARGOS_WITH_GSL 00017 # include <gsl/gsl_randist.h> 00018 #endif 00019 00020 namespace argos { 00021 00022 /****************************************/ 00023 /****************************************/ 00024 00025 std::map<std::string, CRandom::CCategory*> CRandom::m_mapCategories; 00026 00027 #ifdef ARGOS_WITH_GSL 00028 /* Creates an array of all the available generator types, terminated by a null pointer */ 00029 const gsl_rng_type** CRandom::m_pptRNGTypes = gsl_rng_types_setup(); 00030 #endif 00031 00032 /* Checks that a category exists. It internally creates an iterator that points to the category, if found. */ 00033 #define CHECK_CATEGORY(category) \ 00034 std::map<std::string, CCategory*>::iterator itCategory = m_mapCategories.find(category); \ 00035 if(itCategory == m_mapCategories.end()) { \ 00036 THROW_ARGOSEXCEPTION("CRandom:: can't find category \"" << category << "\"."); \ 00037 } 00038 00039 /****************************************/ 00040 /****************************************/ 00041 00042 CRandom::CRNG::CRNG(UInt32 un_seed, 00043 const std::string& str_type) : 00044 m_unSeed(un_seed), 00045 m_strType(str_type), 00046 m_ptRNG(NULL), 00047 #ifndef ARGOS_WITH_GSL 00048 m_pchRNGState(NULL), 00049 #endif 00050 m_pcIntegerRNGRange(NULL) { 00051 CreateRNG(); 00052 } 00053 00054 /****************************************/ 00055 /****************************************/ 00056 00057 CRandom::CRNG::CRNG(const CRNG& c_rng) : 00058 m_unSeed(c_rng.m_unSeed), 00059 m_strType(c_rng.m_strType), 00060 m_ptRNG(NULL), 00061 #ifndef ARGOS_WITH_GSL 00062 m_pchRNGState(NULL), 00063 #endif 00064 m_pcIntegerRNGRange(new CRange<UInt32>(*c_rng.m_pcIntegerRNGRange)) { 00065 /* Clone RNG of original */ 00066 #ifdef ARGOS_WITH_GSL 00067 m_ptRNG = gsl_rng_clone(c_rng.m_ptRNG); 00068 #else 00069 m_ptRNG = new random_data; 00070 ::memset(m_ptRNG, 0, sizeof(random_data)); 00071 m_pchRNGState = new char[256]; 00072 SInt32 nError = initstate_r(m_unSeed, m_pchRNGState, 256, m_ptRNG); 00073 if(nError != 0) { 00074 THROW_ARGOSEXCEPTION("Unable to create random number generator (initstate_r returned " << nError << ")."); 00075 } 00076 ::memcpy(m_pchRNGState, c_rng.m_pchRNGState, 256); 00077 setstate_r(m_pchRNGState, m_ptRNG); 00078 #endif 00079 } 00080 00081 /****************************************/ 00082 /****************************************/ 00083 00084 CRandom::CRNG::~CRNG() { 00085 DisposeRNG(); 00086 } 00087 00088 /****************************************/ 00089 /****************************************/ 00090 00091 void CRandom::CRNG::CreateRNG() { 00092 #ifdef ARGOS_WITH_GSL 00093 /* Look for RNG type in the RNG type list */ 00094 bool bFound = false; 00095 const gsl_rng_type** pptRNGType = GetRNGTypes(); 00096 while((!bFound) && (pptRNGType != NULL)) { 00097 if(m_strType == (*pptRNGType)->name) { 00098 bFound = true; 00099 } 00100 else { 00101 ++pptRNGType; 00102 } 00103 } 00104 if(!bFound) { 00105 /* RNG type not found, error! */ 00106 THROW_ARGOSEXCEPTION("Unknown random number generator type '" << m_strType << "'."); 00107 } 00108 /* We found the wanted RNG type, create the actual RNG */ 00109 m_ptRNG = gsl_rng_alloc(*pptRNGType); 00110 gsl_rng_set(m_ptRNG, m_unSeed); 00111 /* Initialize RNG range */ 00112 m_pcIntegerRNGRange = new CRange<UInt32>(gsl_rng_min(m_ptRNG), 00113 gsl_rng_max(m_ptRNG)); 00114 #else 00115 /* Initialize RNG */ 00116 m_ptRNG = new random_data; 00117 ::memset(m_ptRNG, 0, sizeof(random_data)); 00118 m_pchRNGState = new char[256]; 00119 SInt32 nError = initstate_r(m_unSeed, m_pchRNGState, 256, m_ptRNG); 00120 if(nError != 0) { 00121 THROW_ARGOSEXCEPTION("Unable to create random number generator (initstate_r returned " << nError << ")."); 00122 } 00123 /* Initialize RNG range */ 00124 m_pcIntegerRNGRange = new CRange<UInt32>(0, RAND_MAX); 00125 #endif 00126 } 00127 00128 /****************************************/ 00129 /****************************************/ 00130 00131 void CRandom::CRNG::DisposeRNG() { 00132 #ifdef ARGOS_WITH_GSL 00133 gsl_rng_free(m_ptRNG); 00134 #else 00135 delete m_ptRNG; 00136 delete[] m_pchRNGState; 00137 #endif 00138 delete m_pcIntegerRNGRange; 00139 } 00140 00141 /****************************************/ 00142 /****************************************/ 00143 00144 void CRandom::CRNG::Reset() { 00145 #ifdef ARGOS_WITH_GSL 00146 gsl_rng_set(m_ptRNG, m_unSeed); 00147 #else 00148 initstate_r(m_unSeed, m_pchRNGState, 256, m_ptRNG); 00149 #endif 00150 } 00151 00152 /****************************************/ 00153 /****************************************/ 00154 00155 bool CRandom::CRNG::Bernoulli(Real f_true) { 00156 #ifdef ARGOS_WITH_GSL 00157 return gsl_rng_uniform(m_ptRNG) < f_true; 00158 #else 00159 UInt32 unNumber; 00160 random_r(m_ptRNG, reinterpret_cast<int32_t*>(&unNumber)); 00161 return m_pcIntegerRNGRange->NormalizeValue(unNumber) < f_true; 00162 #endif 00163 } 00164 00165 /****************************************/ 00166 /****************************************/ 00167 00168 CRadians CRandom::CRNG::Uniform(const CRange<CRadians>& c_range) { 00169 #ifdef ARGOS_WITH_GSL 00170 return c_range.GetMin() + gsl_rng_uniform(m_ptRNG) * c_range.GetSpan(); 00171 #else 00172 UInt32 unNumber; 00173 random_r(m_ptRNG, reinterpret_cast<int32_t*>(&unNumber)); 00174 CRadians cRetVal; 00175 m_pcIntegerRNGRange->MapValueIntoRange(cRetVal, unNumber, c_range); 00176 return cRetVal; 00177 #endif 00178 } 00179 00180 /****************************************/ 00181 /****************************************/ 00182 00183 Real CRandom::CRNG::Uniform(const CRange<Real>& c_range) { 00184 #ifdef ARGOS_WITH_GSL 00185 return c_range.GetMin() + gsl_rng_uniform(m_ptRNG) * c_range.GetSpan(); 00186 #else 00187 UInt32 unNumber; 00188 random_r(m_ptRNG, reinterpret_cast<int32_t*>(&unNumber)); 00189 Real fRetVal; 00190 m_pcIntegerRNGRange->MapValueIntoRange(fRetVal, unNumber, c_range); 00191 return fRetVal; 00192 #endif 00193 } 00194 00195 /****************************************/ 00196 /****************************************/ 00197 00198 SInt32 CRandom::CRNG::Uniform(const CRange<SInt32>& c_range) { 00199 SInt32 nRetVal; 00200 #ifdef ARGOS_WITH_GSL 00201 do { 00202 m_pcIntegerRNGRange->MapValueIntoRange(nRetVal, gsl_rng_get(m_ptRNG), c_range); 00203 } while(nRetVal == c_range.GetMax()); 00204 return nRetVal; 00205 #else 00206 UInt32 unNumber; 00207 random_r(m_ptRNG, reinterpret_cast<int32_t*>(&unNumber)); 00208 do { 00209 m_pcIntegerRNGRange->MapValueIntoRange(nRetVal, unNumber, c_range); 00210 } while(nRetVal == c_range.GetMax()); 00211 return nRetVal; 00212 #endif 00213 } 00214 00215 /****************************************/ 00216 /****************************************/ 00217 00218 UInt32 CRandom::CRNG::Uniform(const CRange<UInt32>& c_range) { 00219 UInt32 unRetVal; 00220 #ifdef ARGOS_WITH_GSL 00221 do { 00222 m_pcIntegerRNGRange->MapValueIntoRange(unRetVal, gsl_rng_get(m_ptRNG), c_range); 00223 } while(unRetVal == c_range.GetMax()); 00224 return unRetVal; 00225 #else 00226 UInt32 unNumber; 00227 random_r(m_ptRNG, reinterpret_cast<int32_t*>(&unNumber)); 00228 do { 00229 m_pcIntegerRNGRange->MapValueIntoRange(unRetVal, unNumber, c_range); 00230 } while(unRetVal == c_range.GetMax()); 00231 return unRetVal; 00232 #endif 00233 } 00234 00235 /****************************************/ 00236 /****************************************/ 00237 00238 Real CRandom::CRNG::Exponential(Real f_mean) { 00239 #ifdef ARGOS_WITH_GSL 00240 return gsl_ran_exponential(m_ptRNG, f_mean); 00241 #else 00242 CRange<Real> fRange(0.0f, 1.0f); 00243 return -log(Uniform(fRange)) / f_mean; 00244 #endif 00245 } 00246 00247 /****************************************/ 00248 /****************************************/ 00249 00250 Real CRandom::CRNG::Gaussian(Real f_std_dev, 00251 Real f_mean) { 00252 #ifdef ARGOS_WITH_GSL 00253 return f_mean + gsl_ran_gaussian(m_ptRNG, f_std_dev); 00254 #else 00255 /* This is the Box-Muller method in its cartesian variant 00256 see http://www.dspguru.com/dsp/howtos/how-to-generate-white-gaussian-noise 00257 */ 00258 CRange<Real> fRange(-1.0f, 1.0f); 00259 Real fNum1, fNum2; 00260 Real fSquare; 00261 do { 00262 fNum1 = Uniform(fRange); 00263 fNum2 = Uniform(fRange); 00264 fSquare = fNum1 * fNum1 + fNum2 * fNum2; 00265 } while(fSquare >= 1); 00266 return f_mean + f_std_dev * fNum1; 00267 #endif 00268 } 00269 00270 /****************************************/ 00271 /****************************************/ 00272 00273 Real CRandom::CRNG::Rayleigh(Real f_sigma) { 00274 #ifdef ARGOS_WITH_GSL 00275 return gsl_ran_rayleigh(m_ptRNG, f_sigma); 00276 #else 00277 /* Draw a number uniformly from (0,1) --- bounds excluded */ 00278 Real fValue; 00279 CRange<Real> cUnitRange(0.0f, 1.0f); 00280 do { 00281 fValue = Uniform(cUnitRange); 00282 } 00283 while(! cUnitRange.WithinMinBoundExcludedMaxBoundExcluded(fValue)); 00284 /* Calculate the value to return from the definition of Rayleigh distribution 00285 * http://en.wikipedia.org/wiki/Rayleigh_distribution#Generating_Rayleigh-distributed_random_variates 00286 */ 00287 return f_sigma * Sqrt(-2.0f * Log(fValue)); 00288 #endif 00289 } 00290 00291 /****************************************/ 00292 /****************************************/ 00293 00294 Real CRandom::CRNG::Lognormal(Real f_sigma, Real f_mu) { 00295 #ifdef ARGOS_WITH_GSL 00296 return gsl_ran_lognormal(m_ptRNG, f_mu, f_sigma); 00297 #else 00298 /* Draw a number uniformly from (0,1) */ 00299 Real fValue; 00300 fValue = Gaussian(1,0); 00301 /* Calculate the value to return from the definition of Lognormal distribution 00302 * http://en.wikipedia.org/wiki/Log-normal_distribution#Generating_log-normally_distributed_random_variates 00303 */ 00304 return std::exp(f_mu + f_sigma * fValue); 00305 #endif 00306 } 00307 00308 /****************************************/ 00309 /****************************************/ 00310 00311 CRandom::CCategory::CCategory(const std::string& str_id, 00312 UInt32 un_seed) : 00313 m_strId(str_id), 00314 m_unSeed(un_seed), 00315 m_cSeeder(un_seed), 00316 m_cSeedRange(1, std::numeric_limits<UInt32>::max()) {} 00317 00318 /****************************************/ 00319 /****************************************/ 00320 00321 CRandom::CCategory::~CCategory() { 00322 while(! m_vecRNGList.empty()) { 00323 delete m_vecRNGList.back(); 00324 m_vecRNGList.pop_back(); 00325 } 00326 } 00327 00328 /****************************************/ 00329 /****************************************/ 00330 00331 void CRandom::CCategory::SetSeed(UInt32 un_seed) { 00332 m_unSeed = un_seed; 00333 m_cSeeder.SetSeed(m_unSeed); 00334 } 00335 00336 /****************************************/ 00337 /****************************************/ 00338 00339 CRandom::CRNG* CRandom::CCategory::CreateRNG(const std::string& str_type) { 00340 /* Get seed from internal RNG */ 00341 UInt32 unSeed = m_cSeeder.Uniform(m_cSeedRange); 00342 /* Create new RNG */ 00343 m_vecRNGList.push_back(new CRNG(unSeed, str_type)); 00344 return m_vecRNGList.back(); 00345 } 00346 00347 /****************************************/ 00348 /****************************************/ 00349 00350 void CRandom::CCategory::ResetRNGs() { 00351 /* Reset internal RNG */ 00352 m_cSeeder.Reset(); 00353 ReseedRNGs(); 00354 /* Reset the RNGs */ 00355 for(size_t i = 0; i < m_vecRNGList.size(); ++i) { 00356 m_vecRNGList[i]->Reset(); 00357 } 00358 } 00359 00360 /****************************************/ 00361 /****************************************/ 00362 00363 void CRandom::CCategory::ReseedRNGs() { 00364 for(size_t i = 0; i < m_vecRNGList.size(); ++i) { 00365 /* Get seed from internal RNG */ 00366 m_vecRNGList[i]->SetSeed(m_cSeeder.Uniform(m_cSeedRange)); 00367 } 00368 } 00369 00370 /****************************************/ 00371 /****************************************/ 00372 00373 bool CRandom::CreateCategory(const std::string& str_category, 00374 UInt32 un_seed) { 00375 /* Is there a category already? */ 00376 std::map<std::string, CCategory*>::iterator itCategory = m_mapCategories.find(str_category); 00377 if(itCategory == m_mapCategories.end()) { 00378 /* No, create it */ 00379 m_mapCategories.insert( 00380 std::pair<std::string, 00381 CRandom::CCategory*>(str_category, 00382 new CRandom::CCategory(str_category, 00383 un_seed))); 00384 return true; 00385 } 00386 return false; 00387 } 00388 00389 /****************************************/ 00390 /****************************************/ 00391 00392 CRandom::CCategory& CRandom::GetCategory(const std::string& str_category) { 00393 CHECK_CATEGORY(str_category); 00394 return *(itCategory->second); 00395 } 00396 00397 /****************************************/ 00398 /****************************************/ 00399 00400 bool CRandom::ExistsCategory(const std::string& str_category) { 00401 try { 00402 CHECK_CATEGORY(str_category); 00403 return true; 00404 } 00405 catch(CARGoSException& ex) { 00406 return false; 00407 } 00408 } 00409 00410 /****************************************/ 00411 /****************************************/ 00412 00413 void CRandom::RemoveCategory(const std::string& str_category) { 00414 CHECK_CATEGORY(str_category); 00415 delete itCategory->second; 00416 m_mapCategories.erase(itCategory); 00417 } 00418 00419 /****************************************/ 00420 /****************************************/ 00421 00422 CRandom::CRNG* CRandom::CreateRNG(const std::string& str_category, 00423 const std::string& str_type) { 00424 CHECK_CATEGORY(str_category); 00425 return itCategory->second->CreateRNG(str_type); 00426 } 00427 00428 /****************************************/ 00429 /****************************************/ 00430 00431 UInt32 CRandom::GetSeedOf(const std::string& str_category) { 00432 CHECK_CATEGORY(str_category); 00433 return itCategory->second->GetSeed(); 00434 } 00435 00436 /****************************************/ 00437 /****************************************/ 00438 00439 void CRandom::SetSeedOf(const std::string& str_category, 00440 UInt32 un_seed) { 00441 CHECK_CATEGORY(str_category); 00442 itCategory->second->SetSeed(un_seed); 00443 } 00444 00445 /****************************************/ 00446 /****************************************/ 00447 00448 void CRandom::Reset() { 00449 for(std::map<std::string, CCategory*>::iterator itCategory = m_mapCategories.begin(); 00450 itCategory != m_mapCategories.end(); 00451 ++itCategory) { 00452 itCategory->second->ResetRNGs(); 00453 } 00454 } 00455 00456 /****************************************/ 00457 /****************************************/ 00458 00459 }