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 userSetSeed = 0;
00048
00050 static bool firstSeedGenerated = false;
00051
00053 static boost::uint32_t firstSeedValue = 0;
00054
00056 static boost::uint32_t firstSeed(void)
00057 {
00058 static boost::mutex fsLock;
00059 boost::mutex::scoped_lock slock(fsLock);
00060
00061 if (firstSeedGenerated)
00062 return firstSeedValue;
00063
00064 if (userSetSeed != 0)
00065 firstSeedValue = userSetSeed;
00066 else
00067 firstSeedValue =
00068 (boost::uint32_t)(boost::posix_time::microsec_clock::universal_time() -
00069 boost::posix_time::ptime(boost::date_time::min_date_time)).total_microseconds();
00070 firstSeedGenerated = true;
00071
00072 return firstSeedValue;
00073 }
00074
00078 static boost::uint32_t nextSeed(void)
00079 {
00080 static boost::mutex rngMutex;
00081 rngMutex.lock();
00082 static boost::lagged_fibonacci607 sGen(firstSeed());
00083 static boost::uniform_int<> sDist(1, 1000000000);
00084 static boost::variate_generator<boost::lagged_fibonacci607&, boost::uniform_int<> > s(sGen, sDist);
00085 boost::uint32_t v = s();
00086 rngMutex.unlock();
00087 return v;
00088 }
00089
00090 boost::uint32_t ompl::RNG::getSeed(void)
00091 {
00092 return firstSeed();
00093 }
00094
00095 void ompl::RNG::setSeed(boost::uint32_t seed)
00096 {
00097 if (firstSeedGenerated)
00098 {
00099 msg::Interface msg;
00100 msg.error("Random number generation already started. Changing seed now will not lead to deterministic sampling.");
00101 }
00102 if (seed == 0)
00103 {
00104 msg::Interface msg;
00105 msg.warn("Random generator seed cannot be 0. Using 1 instead.");
00106 userSetSeed = 1;
00107 }
00108 else
00109 userSetSeed = seed;
00110 }
00111
00112 ompl::RNG::RNG(void) : generator_(nextSeed()),
00113 uniDist_(0, 1),
00114 normalDist_(0, 1),
00115 uni_(generator_, uniDist_),
00116 normal_(generator_, normalDist_)
00117 {
00118 }
00119
00120 double ompl::RNG::halfNormalReal(double r_min, double r_max, double focus)
00121 {
00122 assert(r_min <= r_max);
00123
00124 const double mean = r_max - r_min;
00125 double v = gaussian(mean, mean/focus);
00126
00127 if (v > mean) v = 2.0 * mean - v;
00128 double r = v >= 0.0 ? v + r_min : r_min;
00129 return r > r_max ? r_max : r;
00130 }
00131
00132 int ompl::RNG::halfNormalInt(int r_min, int r_max, double focus)
00133 {
00134 int r = (int)floor(halfNormalReal((double)r_min, (double)(r_max) + 1.0, focus));
00135 return (r > r_max) ? r_max : r;
00136 }
00137
00138
00139
00140 void ompl::RNG::quaternion(double value[4])
00141 {
00142 double x0 = uni_();
00143 double r1 = sqrt(1.0 - x0), r2 = sqrt(x0);
00144 double t1 = 2.0 * boost::math::constants::pi<double>() * uni_(), t2 = 2.0 * boost::math::constants::pi<double>() * uni_();
00145 double c1 = cos(t1), s1 = sin(t1);
00146 double c2 = cos(t2), s2 = sin(t2);
00147 value[0] = s1 * r1;
00148 value[1] = c1 * r1;
00149 value[2] = s2 * r2;
00150 value[3] = c2 * r2;
00151 }