ompl/util/src/RandomNumbers.cpp
00001 /********************************************************************* 00002 * Software License Agreement (BSD License) 00003 * 00004 * Copyright (c) 2008, Willow Garage, Inc. 00005 * All rights reserved. 00006 * 00007 * Redistribution and use in source and binary forms, with or without 00008 * modification, are permitted provided that the following conditions 00009 * are met: 00010 * 00011 * * Redistributions of source code must retain the above copyright 00012 * notice, this list of conditions and the following disclaimer. 00013 * * Redistributions in binary form must reproduce the above 00014 * copyright notice, this list of conditions and the following 00015 * disclaimer in the documentation and/or other materials provided 00016 * with the distribution. 00017 * * Neither the name of the Willow Garage nor the names of its 00018 * contributors may be used to endorse or promote products derived 00019 * from this software without specific prior written permission. 00020 * 00021 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 00022 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 00023 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 00024 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 00025 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 00026 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 00027 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 00028 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 00029 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 00030 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 00031 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 00032 * POSSIBILITY OF SUCH DAMAGE. 00033 *********************************************************************/ 00034 00035 /* Author: Ioan Sucan */ 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/thread/once.hpp> 00044 #include <boost/scoped_ptr.hpp> 00045 #include <boost/date_time/posix_time/posix_time.hpp> 00046 #include <boost/math/constants/constants.hpp> 00047 00049 namespace 00050 { 00054 class RNGSeedGenerator 00055 { 00056 public: 00057 RNGSeedGenerator() : 00058 someSeedsGenerated_(false), 00059 firstSeed_((boost::uint32_t)(boost::posix_time::microsec_clock::universal_time() - 00060 boost::posix_time::ptime(boost::date_time::min_date_time)).total_microseconds()), 00061 sGen_(firstSeed_), 00062 sDist_(1, 1000000000), 00063 s_(sGen_, sDist_) 00064 { 00065 } 00066 00067 boost::uint32_t firstSeed() 00068 { 00069 boost::mutex::scoped_lock slock(rngMutex_); 00070 return firstSeed_; 00071 } 00072 00073 void setSeed(boost::uint32_t seed) 00074 { 00075 boost::mutex::scoped_lock slock(rngMutex_); 00076 if (seed > 0) 00077 { 00078 if (someSeedsGenerated_) 00079 { 00080 OMPL_ERROR("Random number generation already started. Changing seed now will not lead to deterministic sampling."); 00081 } 00082 else 00083 { 00084 // In this case, since no seeds have been generated yet, so we remember this seed as the first one. 00085 firstSeed_ = seed; 00086 } 00087 } 00088 else 00089 { 00090 if (someSeedsGenerated_) 00091 { 00092 OMPL_WARN("Random generator seed cannot be 0. Ignoring seed."); 00093 return; 00094 } 00095 else 00096 { 00097 OMPL_WARN("Random generator seed cannot be 0. Using 1 instead."); 00098 seed = 1; 00099 } 00100 } 00101 sGen_.seed(seed); 00102 } 00103 00104 boost::uint32_t nextSeed() 00105 { 00106 boost::mutex::scoped_lock slock(rngMutex_); 00107 someSeedsGenerated_ = true; 00108 return s_(); 00109 } 00110 00111 private: 00112 bool someSeedsGenerated_; 00113 boost::uint32_t firstSeed_; 00114 boost::mutex rngMutex_; 00115 boost::lagged_fibonacci607 sGen_; 00116 boost::uniform_int<> sDist_; 00117 boost::variate_generator<boost::lagged_fibonacci607&, boost::uniform_int<> > s_; 00118 }; 00119 00120 static boost::once_flag g_once = BOOST_ONCE_INIT; 00121 static boost::scoped_ptr<RNGSeedGenerator> g_RNGSeedGenerator; 00122 00123 void initRNGSeedGenerator() 00124 { 00125 g_RNGSeedGenerator.reset(new RNGSeedGenerator()); 00126 } 00127 00128 RNGSeedGenerator& getRNGSeedGenerator() 00129 { 00130 boost::call_once(&initRNGSeedGenerator, g_once); 00131 return *g_RNGSeedGenerator; 00132 } 00133 } // namespace 00135 00136 boost::uint32_t ompl::RNG::getSeed() 00137 { 00138 return getRNGSeedGenerator().firstSeed(); 00139 } 00140 00141 void ompl::RNG::setSeed(boost::uint32_t seed) 00142 { 00143 getRNGSeedGenerator().setSeed(seed); 00144 } 00145 00146 ompl::RNG::RNG() : 00147 generator_(getRNGSeedGenerator().nextSeed()), 00148 uniDist_(0, 1), 00149 normalDist_(0, 1), 00150 uni_(generator_, uniDist_), 00151 normal_(generator_, normalDist_) 00152 { 00153 } 00154 00155 double ompl::RNG::halfNormalReal(double r_min, double r_max, double focus) 00156 { 00157 assert(r_min <= r_max); 00158 00159 const double mean = r_max - r_min; 00160 double v = gaussian(mean, mean/focus); 00161 00162 if (v > mean) v = 2.0 * mean - v; 00163 double r = v >= 0.0 ? v + r_min : r_min; 00164 return r > r_max ? r_max : r; 00165 } 00166 00167 int ompl::RNG::halfNormalInt(int r_min, int r_max, double focus) 00168 { 00169 int r = (int)floor(halfNormalReal((double)r_min, (double)(r_max) + 1.0, focus)); 00170 return (r > r_max) ? r_max : r; 00171 } 00172 00173 // From: "Uniform Random Rotations", Ken Shoemake, Graphics Gems III, 00174 // pg. 124-132 00175 void ompl::RNG::quaternion(double value[4]) 00176 { 00177 double x0 = uni_(); 00178 double r1 = sqrt(1.0 - x0), r2 = sqrt(x0); 00179 double t1 = 2.0 * boost::math::constants::pi<double>() * uni_(), t2 = 2.0 * boost::math::constants::pi<double>() * uni_(); 00180 double c1 = cos(t1), s1 = sin(t1); 00181 double c2 = cos(t2), s2 = sin(t2); 00182 value[0] = s1 * r1; 00183 value[1] = c1 * r1; 00184 value[2] = s2 * r2; 00185 value[3] = c2 * r2; 00186 } 00187 00188 // From Effective Sampling and Distance Metrics for 3D Rigid Body Path Planning, by James Kuffner, ICRA 2004 00189 void ompl::RNG::eulerRPY(double value[3]) 00190 { 00191 value[0] = boost::math::constants::pi<double>() * (-2.0 * uni_() + 1.0); 00192 value[1] = acos(1.0 - 2.0 * uni_()) - boost::math::constants::pi<double>() / 2.0; 00193 value[2] = boost::math::constants::pi<double>() * (-2.0 * uni_() + 1.0); 00194 }