FCL  0.6.0
Flexible Collision Library
rng-inl.h
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2013-2014, Willow Garage, Inc.
5  * Copyright (c) 2014-2016, Open Source Robotics Foundation
6  * All rights reserved.
7  *
8  * Redistribution and use in source and binary forms, with or without
9  * modification, are permitted provided that the following conditions
10  * are met:
11  *
12  * * Redistributions of source code must retain the above copyright
13  * notice, this list of conditions and the following disclaimer.
14  * * Redistributions in binary form must reproduce the above
15  * copyright notice, this list of conditions and the following
16  * disclaimer in the documentation and/or other materials provided
17  * with the distribution.
18  * * Neither the name of Open Source Robotics Foundation nor the names of its
19  * contributors may be used to endorse or promote products derived
20  * from this software without specific prior written permission.
21  *
22  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
23  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
24  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
25  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
26  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
27  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
28  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
29  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
30  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
31  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
32  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
33  * POSSIBILITY OF SUCH DAMAGE.
34  */
35 
38 #ifndef FCL_MATH_RNG_INL_H
39 #define FCL_MATH_RNG_INL_H
40 
41 #include "fcl/math/rng.h"
42 
43 namespace fcl
44 {
45 
46 //==============================================================================
47 extern template
48 class RNG<double>;
49 
50 //==============================================================================
51 template <typename S>
53  : generator_(detail::Seed::getNextSeed()), uniDist_(0, 1), normalDist_(0, 1)
54 {
55 }
56 
57 //==============================================================================
58 template <typename S>
60 {
61  return uniDist_(generator_);
62 }
63 
64 //==============================================================================
65 template <typename S>
66 S RNG<S>::uniformReal(S lower_bound, S upper_bound)
67 {
68  assert(lower_bound <= upper_bound);
69 
70  return (upper_bound - lower_bound) * uniDist_(generator_) + lower_bound;
71 }
72 
73 //==============================================================================
74 template <typename S>
75 int RNG<S>::uniformInt(int lower_bound, int upper_bound)
76 {
77  int r = (int)floor(uniformReal((S)lower_bound, (S)(upper_bound) + 1.0));
78 
79  return (r > upper_bound) ? upper_bound : r;
80 }
81 
82 //==============================================================================
83 template <typename S>
85 {
86  return uniDist_(generator_) <= 0.5;
87 }
88 
89 //==============================================================================
90 template <typename S>
92 {
93  return normalDist_(generator_);
94 }
95 
96 //==============================================================================
97 template <typename S>
98 S RNG<S>::gaussian(S mean, S stddev)
99 {
100  return normalDist_(generator_) * stddev + mean;
101 }
102 
103 //==============================================================================
104 template <typename S>
105 S RNG<S>::halfNormalReal(S r_min, S r_max, S focus)
106 {
107  assert(r_min <= r_max);
108 
109  const auto mean = r_max - r_min;
110  auto v = gaussian(mean, mean / focus);
111 
112  if (v > mean)
113  v = 2.0 * mean - v;
114 
115  auto r = v >= 0.0 ? v + r_min : r_min;
116 
117  return r > r_max ? r_max : r;
118 }
119 
120 //==============================================================================
121 template <typename S>
122 int RNG<S>::halfNormalInt(int r_min, int r_max, S focus)
123 {
124  int r = (int)std::floor(halfNormalReal(
125  (S)r_min, (S)(r_max) + 1.0, focus));
126 
127  return (r > r_max) ? r_max : r;
128 }
129 
130 //==============================================================================
131 template <typename S>
132 void RNG<S>::quaternion(S value[])
133 {
134  auto x0 = uniDist_(generator_);
135  auto r1 = std::sqrt(1.0 - x0), r2 = std::sqrt(x0);
136  auto t1 = 2.0 * constants<S>::pi() * uniDist_(generator_);
137  auto t2 = 2.0 * constants<S>::pi() * uniDist_(generator_);
138  auto c1 = std::cos(t1);
139  auto s1 = std::sin(t1);
140  auto c2 = std::cos(t2);
141  auto s2 = std::sin(t2);
142  value[0] = s1 * r1;
143  value[1] = c1 * r1;
144  value[2] = s2 * r2;
145  value[3] = c2 * r2;
146 }
147 
148 //==============================================================================
149 template <typename S>
150 void RNG<S>::eulerRPY(S value[])
151 {
152  value[0] = constants<S>::pi() * (2.0 * uniDist_(generator_) - 1.0);
153  value[1] = std::acos(1.0 - 2.0 * uniDist_(generator_)) - constants<S>::pi() / 2.0;
154  value[2] = constants<S>::pi() * (2.0 * uniDist_(generator_) - 1.0);
155 }
156 
157 //==============================================================================
158 template <typename S>
159 void RNG<S>::disk(S r_min, S r_max, S& x, S& y)
160 {
161  auto a = uniform01();
162  auto b = uniform01();
163  auto r = std::sqrt(a * r_max * r_max + (1 - a) * r_min * r_min);
164  auto theta = 2 * constants<S>::pi() * b;
165  x = r * std::cos(theta);
166  y = r * std::sin(theta);
167 }
168 
169 //==============================================================================
170 template <typename S>
172  S r_min, S r_max, S& x, S& y, S& z)
173 {
174  auto a = uniform01();
175  auto b = uniform01();
176  auto c = uniform01();
177  auto r = std::pow(a*std::pow(r_max, 3) + (1 - a)*std::pow(r_min, 3), 1/3.0);
178  auto theta = std::acos(1 - 2 * b);
179  auto phi = 2 * constants<S>::pi() * c;
180 
181  auto costheta = std::cos(theta);
182  auto sintheta = std::sin(theta);
183  auto cosphi = std::cos(phi);
184  auto sinphi = std::sin(phi);
185  x = r * costheta;
186  y = r * sintheta * cosphi;
187  z = r * sintheta * sinphi;
188 }
189 
190 //==============================================================================
191 template <typename S>
192 void RNG<S>::setSeed(uint_fast32_t seed)
193 {
194  if (detail::Seed::isFirstSeedGenerated())
195  {
196  std::cerr << "Random number generation already started. Changing seed now "
197  << "will not lead to deterministic sampling." << std::endl;
198  }
199 
200  if (seed == 0)
201  {
202  std::cerr << "Random generator seed cannot be 0. Using 1 instead."
203  << std::endl;
204  detail::Seed::setUserSetSeed(1);
205  }
206  else
207  {
208  detail::Seed::setUserSetSeed(seed);
209  }
210 }
211 
212 //==============================================================================
213 template <typename S>
214 uint_fast32_t RNG<S>::getSeed()
215 {
216  return detail::Seed::getFirstSeed();
217 }
218 
219 } // namespace fcl
220 
221 #endif
void quaternion(S value[4])
Uniform random unit quaternion sampling. The computed value has the order (x,y,z,w) ...
Definition: rng-inl.h:132
Main namespace.
Definition: broadphase_bruteforce-inl.h:45
void ball(S r_min, S r_max, S &x, S &y, S &z)
Uniform random sample in a ball with radius from r_min to r_max.
Definition: rng-inl.h:171
S uniformReal(S lower_bound, S upper_bound)
Generate a random real within given bounds: [lower_bound, upper_bound)
Definition: rng-inl.h:66
RNG()
Constructor. Always sets a different random seed.
Definition: rng-inl.h:52
static std::uint_fast32_t getSeed()
Get the seed used for random number generation. Passing the returned value to setSeed() at a subseque...
Definition: rng-inl.h:214
void disk(S r_min, S r_max, S &x, S &y)
Uniform random sample on a disk with radius from r_min to r_max.
Definition: rng-inl.h:159
*void eulerRPY(S value[3])
Uniform random sampling of Euler roll-pitch-yaw angles, each in the range [-pi, pi). The computed value has the order (roll, pitch, yaw)
Definition: rng-inl.h:150
S halfNormalReal(S r_min, S r_max, S focus=3.0)
Generate a random real using a half-normal distribution. The value is within specified bounds [r_min...
Definition: rng-inl.h:105
S gaussian(S mean, S stddev)
Generate a random real using a normal distribution with given mean and variance.
Definition: rng-inl.h:98
static constexpr S pi()
The mathematical constant pi.
Definition: constants.h:49
int uniformInt(int lower_bound, int upper_bound)
Generate a random integer within given bounds: [lower_bound, upper_bound].
Definition: rng-inl.h:75
int halfNormalInt(int r_min, int r_max, S focus=3.0)
Generate a random integer using a half-normal distribution. The value is within specified bounds ([r_...
Definition: rng-inl.h:122
S uniform01()
Generate a random real between 0 and 1.
Definition: rng-inl.h:59
bool uniformBool()
Generate a random boolean.
Definition: rng-inl.h:84
static void setSeed(std::uint_fast32_t seed)
Set the seed for random number generation. Use this function to ensure the same sequence of random nu...
Definition: rng-inl.h:192
S gaussian01()
Generate a random real using a normal distribution with mean 0 and variance 1.
Definition: rng-inl.h:91