All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends
RandomNumbers.cpp
1 /*********************************************************************
2 * Software License Agreement (BSD License)
3 *
4 * Copyright (c) 2008, Willow Garage, Inc.
5 * All rights reserved.
6 *
7 * Redistribution and use in source and binary forms, with or without
8 * modification, are permitted provided that the following conditions
9 * are met:
10 *
11 * * Redistributions of source code must retain the above copyright
12 * notice, this list of conditions and the following disclaimer.
13 * * Redistributions in binary form must reproduce the above
14 * copyright notice, this list of conditions and the following
15 * disclaimer in the documentation and/or other materials provided
16 * with the distribution.
17 * * Neither the name of the Willow Garage nor the names of its
18 * contributors may be used to endorse or promote products derived
19 * from this software without specific prior written permission.
20 *
21 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32 * POSSIBILITY OF SUCH DAMAGE.
33 *********************************************************************/
34 
35 /* Author: Ioan Sucan */
36 
37 #include "ompl/util/RandomNumbers.h"
38 #include "ompl/util/Exception.h"
39 #include "ompl/util/Console.h"
40 #include <boost/random/lagged_fibonacci.hpp>
41 #include <boost/random/uniform_int.hpp>
42 #include <boost/thread/mutex.hpp>
43 #include <boost/date_time/posix_time/posix_time.hpp>
44 #include <boost/math/constants/constants.hpp>
45 
47 static boost::uint32_t& getUserSetSeed(void)
48 {
49  static boost::uint32_t userSetSeed = 0;
50  return userSetSeed;
51 }
52 
54 static bool& getFirstSeedGenerated(void)
55 {
56  static bool firstSeedGenerated = false;
57  return firstSeedGenerated;
58 }
59 
61 static boost::uint32_t firstSeed(void)
62 {
64  static boost::uint32_t firstSeedValue = 0;
65 
66  static boost::mutex fsLock;
67  boost::mutex::scoped_lock slock(fsLock);
68 
69  if (getFirstSeedGenerated())
70  return firstSeedValue;
71 
72  if (getUserSetSeed() != 0)
73  firstSeedValue = getUserSetSeed();
74  else
75  firstSeedValue =
76  (boost::uint32_t)(boost::posix_time::microsec_clock::universal_time() -
77  boost::posix_time::ptime(boost::date_time::min_date_time)).total_microseconds();
78  getFirstSeedGenerated() = true;
79 
80  return firstSeedValue;
81 }
82 
86 static boost::uint32_t nextSeed(void)
87 {
88  static boost::mutex rngMutex;
89  boost::mutex::scoped_lock slock(rngMutex);
90  static boost::lagged_fibonacci607 sGen(firstSeed());
91  static boost::uniform_int<> sDist(1, 1000000000);
92  static boost::variate_generator<boost::lagged_fibonacci607&, boost::uniform_int<> > s(sGen, sDist);
93  return s();
94 }
95 
96 boost::uint32_t ompl::RNG::getSeed(void)
97 {
98  return firstSeed();
99 }
100 
101 void ompl::RNG::setSeed(boost::uint32_t seed)
102 {
103  if (getFirstSeedGenerated())
104  {
105  logError("Random number generation already started. Changing seed now will not lead to deterministic sampling.");
106  }
107  if (seed == 0)
108  {
109  logWarn("Random generator seed cannot be 0. Using 1 instead.");
110  getUserSetSeed() = 1;
111  }
112  else
113  getUserSetSeed() = seed;
114 }
115 
116 ompl::RNG::RNG(void) : generator_(nextSeed()),
117  uniDist_(0, 1),
118  normalDist_(0, 1),
119  uni_(generator_, uniDist_),
120  normal_(generator_, normalDist_)
121 {
122 }
123 
124 double ompl::RNG::halfNormalReal(double r_min, double r_max, double focus)
125 {
126  assert(r_min <= r_max);
127 
128  const double mean = r_max - r_min;
129  double v = gaussian(mean, mean/focus);
130 
131  if (v > mean) v = 2.0 * mean - v;
132  double r = v >= 0.0 ? v + r_min : r_min;
133  return r > r_max ? r_max : r;
134 }
135 
136 int ompl::RNG::halfNormalInt(int r_min, int r_max, double focus)
137 {
138  int r = (int)floor(halfNormalReal((double)r_min, (double)(r_max) + 1.0, focus));
139  return (r > r_max) ? r_max : r;
140 }
141 
142 // From: "Uniform Random Rotations", Ken Shoemake, Graphics Gems III,
143 // pg. 124-132
144 void ompl::RNG::quaternion(double value[4])
145 {
146  double x0 = uni_();
147  double r1 = sqrt(1.0 - x0), r2 = sqrt(x0);
148  double t1 = 2.0 * boost::math::constants::pi<double>() * uni_(), t2 = 2.0 * boost::math::constants::pi<double>() * uni_();
149  double c1 = cos(t1), s1 = sin(t1);
150  double c2 = cos(t2), s2 = sin(t2);
151  value[0] = s1 * r1;
152  value[1] = c1 * r1;
153  value[2] = s2 * r2;
154  value[3] = c2 * r2;
155 }
156 
157 // From Effective Sampling and Distance Metrics for 3D Rigid Body Path Planning, by James Kuffner, ICRA 2004
158 void ompl::RNG::eulerRPY(double value[3])
159 {
160  value[0] = boost::math::constants::pi<double>() * (2.0 * uni_() - 1.0);
161  value[1] = acos(1.0 - 2.0 * uni_()) - boost::math::constants::pi<double>() / 2.0;
162  value[2] = boost::math::constants::pi<double>() * (2.0 * uni_() - 1.0);
163 }