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 }
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Defines