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 boost::mutex::scoped_lock slock(rngMutex);
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 return s();
00086 }
00087
00088 boost::uint32_t ompl::RNG::getSeed(void)
00089 {
00090 return firstSeed();
00091 }
00092
00093 void ompl::RNG::setSeed(boost::uint32_t seed)
00094 {
00095 if (firstSeedGenerated)
00096 {
00097 msg::Interface msg;
00098 msg.error("Random number generation already started. Changing seed now will not lead to deterministic sampling.");
00099 }
00100 if (seed == 0)
00101 {
00102 msg::Interface msg;
00103 msg.warn("Random generator seed cannot be 0. Using 1 instead.");
00104 userSetSeed = 1;
00105 }
00106 else
00107 userSetSeed = seed;
00108 }
00109
00110 ompl::RNG::RNG(void) : generator_(nextSeed()),
00111 uniDist_(0, 1),
00112 normalDist_(0, 1),
00113 uni_(generator_, uniDist_),
00114 normal_(generator_, normalDist_)
00115 {
00116 }
00117
00118 double ompl::RNG::halfNormalReal(double r_min, double r_max, double focus)
00119 {
00120 assert(r_min <= r_max);
00121
00122 const double mean = r_max - r_min;
00123 double v = gaussian(mean, mean/focus);
00124
00125 if (v > mean) v = 2.0 * mean - v;
00126 double r = v >= 0.0 ? v + r_min : r_min;
00127 return r > r_max ? r_max : r;
00128 }
00129
00130 int ompl::RNG::halfNormalInt(int r_min, int r_max, double focus)
00131 {
00132 int r = (int)floor(halfNormalReal((double)r_min, (double)(r_max) + 1.0, focus));
00133 return (r > r_max) ? r_max : r;
00134 }
00135
00136
00137
00138 void ompl::RNG::quaternion(double value[4])
00139 {
00140 double x0 = uni_();
00141 double r1 = sqrt(1.0 - x0), r2 = sqrt(x0);
00142 double t1 = 2.0 * boost::math::constants::pi<double>() * uni_(), t2 = 2.0 * boost::math::constants::pi<double>() * uni_();
00143 double c1 = cos(t1), s1 = sin(t1);
00144 double c2 = cos(t2), s2 = sin(t2);
00145 value[0] = s1 * r1;
00146 value[1] = c1 * r1;
00147 value[2] = s2 * r2;
00148 value[3] = c2 * r2;
00149 }
00150
00151
00152 void ompl::RNG::eulerRPY(double value[3])
00153 {
00154 value[0] = boost::math::constants::pi<double>() * (2.0 * uni_() - 1.0);
00155 value[1] = acos(1.0 - 2.0 * uni_()) - boost::math::constants::pi<double>() / 2.0;
00156 value[2] = boost::math::constants::pi<double>() * (2.0 * uni_() - 1.0);
00157 }