00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037 #include "ompl/util/RandomNumbers.h"
00038 #include "ompl/util/Exception.h"
00039 #include "ompl/util/Console.h"
00040 #include <boost/random/lagged_fibonacci.hpp>
00041 #include <boost/random/uniform_int.hpp>
00042 #include <boost/thread/mutex.hpp>
00043 #include <boost/date_time/posix_time/posix_time.hpp>
00044 #include <boost/math/constants/constants.hpp>
00045
00047 static boost::uint32_t& getUserSetSeed(void)
00048 {
00049 static boost::uint32_t userSetSeed = 0;
00050 return userSetSeed;
00051 }
00052
00054 static bool& getFirstSeedGenerated(void)
00055 {
00056 static bool firstSeedGenerated = false;
00057 return firstSeedGenerated;
00058 }
00059
00061 static boost::uint32_t firstSeed(void)
00062 {
00064 static boost::uint32_t firstSeedValue = 0;
00065
00066 static boost::mutex fsLock;
00067 boost::mutex::scoped_lock slock(fsLock);
00068
00069 if (getFirstSeedGenerated())
00070 return firstSeedValue;
00071
00072 if (getUserSetSeed() != 0)
00073 firstSeedValue = getUserSetSeed();
00074 else
00075 firstSeedValue =
00076 (boost::uint32_t)(boost::posix_time::microsec_clock::universal_time() -
00077 boost::posix_time::ptime(boost::date_time::min_date_time)).total_microseconds();
00078 getFirstSeedGenerated() = true;
00079
00080 return firstSeedValue;
00081 }
00082
00086 static boost::uint32_t nextSeed(void)
00087 {
00088 static boost::mutex rngMutex;
00089 boost::mutex::scoped_lock slock(rngMutex);
00090 static boost::lagged_fibonacci607 sGen(firstSeed());
00091 static boost::uniform_int<> sDist(1, 1000000000);
00092 static boost::variate_generator<boost::lagged_fibonacci607&, boost::uniform_int<> > s(sGen, sDist);
00093 return s();
00094 }
00095
00096 boost::uint32_t ompl::RNG::getSeed(void)
00097 {
00098 return firstSeed();
00099 }
00100
00101 void ompl::RNG::setSeed(boost::uint32_t seed)
00102 {
00103 if (getFirstSeedGenerated())
00104 {
00105 logError("Random number generation already started. Changing seed now will not lead to deterministic sampling.");
00106 }
00107 if (seed == 0)
00108 {
00109 logWarn("Random generator seed cannot be 0. Using 1 instead.");
00110 getUserSetSeed() = 1;
00111 }
00112 else
00113 getUserSetSeed() = seed;
00114 }
00115
00116 ompl::RNG::RNG(void) : generator_(nextSeed()),
00117 uniDist_(0, 1),
00118 normalDist_(0, 1),
00119 uni_(generator_, uniDist_),
00120 normal_(generator_, normalDist_)
00121 {
00122 }
00123
00124 double ompl::RNG::halfNormalReal(double r_min, double r_max, double focus)
00125 {
00126 assert(r_min <= r_max);
00127
00128 const double mean = r_max - r_min;
00129 double v = gaussian(mean, mean/focus);
00130
00131 if (v > mean) v = 2.0 * mean - v;
00132 double r = v >= 0.0 ? v + r_min : r_min;
00133 return r > r_max ? r_max : r;
00134 }
00135
00136 int ompl::RNG::halfNormalInt(int r_min, int r_max, double focus)
00137 {
00138 int r = (int)floor(halfNormalReal((double)r_min, (double)(r_max) + 1.0, focus));
00139 return (r > r_max) ? r_max : r;
00140 }
00141
00142
00143
00144 void ompl::RNG::quaternion(double value[4])
00145 {
00146 double x0 = uni_();
00147 double r1 = sqrt(1.0 - x0), r2 = sqrt(x0);
00148 double t1 = 2.0 * boost::math::constants::pi<double>() * uni_(), t2 = 2.0 * boost::math::constants::pi<double>() * uni_();
00149 double c1 = cos(t1), s1 = sin(t1);
00150 double c2 = cos(t2), s2 = sin(t2);
00151 value[0] = s1 * r1;
00152 value[1] = c1 * r1;
00153 value[2] = s2 * r2;
00154 value[3] = c2 * r2;
00155 }
00156
00157
00158 void ompl::RNG::eulerRPY(double value[3])
00159 {
00160 value[0] = boost::math::constants::pi<double>() * (2.0 * uni_() - 1.0);
00161 value[1] = acos(1.0 - 2.0 * uni_()) - boost::math::constants::pi<double>() / 2.0;
00162 value[2] = boost::math::constants::pi<double>() * (2.0 * uni_() - 1.0);
00163 }