DART  6.10.1
Helpers.hpp
Go to the documentation of this file.
1 /*
2  * Copyright (c) 2011-2021, The DART development contributors
3  * All rights reserved.
4  *
5  * The list of contributors can be found at:
6  * https://github.com/dartsim/dart/blob/master/LICENSE
7  *
8  * This file is provided under the following "BSD-style" License:
9  * Redistribution and use in source and binary forms, with or
10  * without modification, are permitted provided that the following
11  * conditions are met:
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  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND
19  * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES,
20  * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
21  * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
22  * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR
23  * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
24  * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
25  * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF
26  * USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
27  * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
28  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
29  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
30  * POSSIBILITY OF SUCH DAMAGE.
31  */
32 
33 #ifndef DART_MATH_HELPERS_HPP_
34 #define DART_MATH_HELPERS_HPP_
35 
36 // Standard Libraries
37 #include <cfloat>
38 #include <climits>
39 #include <cmath>
40 #include <cstdlib>
41 #include <ctime>
42 #include <iomanip>
43 #include <iostream>
44 #include <random>
45 
46 // External Libraries
47 #include <Eigen/Dense>
48 // Local Headers
49 #include "dart/math/Constants.hpp"
50 #include "dart/math/MathTypes.hpp"
51 #include "dart/math/Random.hpp"
52 
53 namespace dart {
54 namespace math {
55 
56 //==============================================================================
57 template <typename T>
58 constexpr T toRadian(const T& degree)
59 {
60  return degree * constants<T>::pi() / 180.0;
61 }
62 
63 //==============================================================================
64 template <typename T>
65 constexpr T toDegree(const T& radian)
66 {
67  return radian * 180.0 / constants<T>::pi();
68 }
69 
72 const Eigen::Matrix2d CR((Eigen::Matrix2d() << 0.0, -1.0, 1.0, 0.0).finished());
73 
74 inline int delta(int _i, int _j)
75 {
76  if (_i == _j)
77  return 1;
78  return 0;
79 }
80 
81 template <typename T>
82 inline constexpr int sign(T x, std::false_type)
83 {
84  return static_cast<T>(0) < x;
85 }
86 
87 template <typename T>
88 inline constexpr int sign(T x, std::true_type)
89 {
90  return (static_cast<T>(0) < x) - (x < static_cast<T>(0));
91 }
92 
93 template <typename T>
94 inline constexpr int sign(T x)
95 {
96  return sign(x, std::is_signed<T>());
97 }
98 
99 inline double sqr(double _x)
100 {
101  return _x * _x;
102 }
103 
104 inline double Tsinc(double _theta)
105 {
106  return 0.5 - sqrt(_theta) / 48;
107 }
108 
109 inline bool isZero(double _theta)
110 {
111  return (std::abs(_theta) < 1e-6);
112 }
113 
114 inline double asinh(double _X)
115 {
116  return log(_X + sqrt(_X * _X + 1));
117 }
118 
119 inline double acosh(double _X)
120 {
121  return log(_X + sqrt(_X * _X - 1));
122 }
123 
124 inline double atanh(double _X)
125 {
126  return log((1 + _X) / (1 - _X)) / 2;
127 }
128 
129 inline double asech(double _X)
130 {
131  return log((sqrt(-_X * _X + 1) + 1) / _X);
132 }
133 
134 inline double acosech(double _X)
135 {
136  return log((sign(_X) * sqrt(_X * _X + 1) + 1) / _X);
137 }
138 
139 inline double acotanh(double _X)
140 {
141  return log((_X + 1) / (_X - 1)) / 2;
142 }
143 
144 inline double round(double _x)
145 {
146  return floor(_x + 0.5);
147 }
148 
149 inline double round2(double _x)
150 {
151  int gintx = static_cast<int>(std::floor(_x));
152  if (_x - gintx < 0.5)
153  return static_cast<double>(gintx);
154  else
155  return static_cast<double>(gintx + 1.0);
156 }
157 
158 template <typename T>
159 inline T clip(const T& val, const T& lower, const T& upper)
160 {
161  return std::max(lower, std::min(val, upper));
162 }
163 
164 template <typename DerivedA, typename DerivedB>
165 inline typename DerivedA::PlainObject clip(
166  const Eigen::MatrixBase<DerivedA>& val,
167  const Eigen::MatrixBase<DerivedB>& lower,
168  const Eigen::MatrixBase<DerivedB>& upper)
169 {
170  return lower.cwiseMax(val.cwiseMin(upper));
171 }
172 
173 inline bool isEqual(double _x, double _y)
174 {
175  return (std::abs(_x - _y) < 1e-6);
176 }
177 
178 // check if it is an integer
179 inline bool isInt(double _x)
180 {
181  if (isEqual(round(_x), _x))
182  return true;
183  return false;
184 }
185 
187 inline bool isNan(double _v)
188 {
189 #ifdef _WIN32
190  return _isnan(_v) != 0;
191 #else
192  return std::isnan(_v);
193 #endif
194 }
195 
197 inline bool isNan(const Eigen::MatrixXd& _m)
198 {
199  for (int i = 0; i < _m.rows(); ++i)
200  for (int j = 0; j < _m.cols(); ++j)
201  if (isNan(_m(i, j)))
202  return true;
203 
204  return false;
205 }
206 
209 inline bool isInf(double _v)
210 {
211 #ifdef _WIN32
212  return !_finite(_v);
213 #else
214  return std::isinf(_v);
215 #endif
216 }
217 
220 inline bool isInf(const Eigen::MatrixXd& _m)
221 {
222  for (int i = 0; i < _m.rows(); ++i)
223  for (int j = 0; j < _m.cols(); ++j)
224  if (isInf(_m(i, j)))
225  return true;
226 
227  return false;
228 }
229 
231 inline bool isSymmetric(const Eigen::MatrixXd& _m, double _tol = 1e-6)
232 {
233  std::size_t rows = _m.rows();
234  std::size_t cols = _m.cols();
235 
236  if (rows != cols)
237  return false;
238 
239  for (std::size_t i = 0; i < rows; ++i)
240  {
241  for (std::size_t j = i + 1; j < cols; ++j)
242  {
243  if (std::abs(_m(i, j) - _m(j, i)) > _tol)
244  {
245  std::cout << "A: " << std::endl;
246  for (std::size_t k = 0; k < rows; ++k)
247  {
248  for (std::size_t l = 0; l < cols; ++l)
249  std::cout << std::setprecision(4) << _m(k, l) << " ";
250  std::cout << std::endl;
251  }
252 
253  std::cout << "A(" << i << ", " << j << "): " << _m(i, j) << std::endl;
254  std::cout << "A(" << j << ", " << i << "): " << _m(i, j) << std::endl;
255  return false;
256  }
257  }
258  }
259 
260  return true;
261 }
262 
263 inline unsigned seedRand()
264 {
265  time_t now = time(0);
266  unsigned char* p = reinterpret_cast<unsigned char*>(&now);
267  unsigned seed = 0;
268  std::size_t i;
269 
270  for (i = 0; i < sizeof(now); i++)
271  seed = seed * (UCHAR_MAX + 2U) + p[i];
272 
273  srand(seed);
274  return seed;
275 }
276 
278 DART_DEPRECATED(6.7)
279 inline double random(double _min, double _max)
280 {
281  return _min
282  + ((static_cast<double>(rand()) / (RAND_MAX + 1.0)) * (_max - _min));
283 }
284 
286 template <int N>
287 DART_DEPRECATED(6.7)
288 Eigen::Matrix<double, N, 1> randomVector(double _min, double _max)
289 {
290  Eigen::Matrix<double, N, 1> v;
291  DART_SUPPRESS_DEPRECATED_BEGIN
292  for (std::size_t i = 0; i < N; ++i)
293  v[i] = random(_min, _max);
294  DART_SUPPRESS_DEPRECATED_END
295 
296  return v;
297 }
298 
300 template <int N>
301 DART_DEPRECATED(6.7)
302 Eigen::Matrix<double, N, 1> randomVector(double _limit)
303 {
304  DART_SUPPRESS_DEPRECATED_BEGIN
305  return randomVector<N>(-std::abs(_limit), std::abs(_limit));
306  DART_SUPPRESS_DEPRECATED_END
307 }
308 
309 //==============================================================================
311 DART_DEPRECATED(6.7)
312 inline Eigen::VectorXd randomVectorXd(std::size_t size, double min, double max)
313 {
314  Eigen::VectorXd v = Eigen::VectorXd::Zero(size);
315 
316  DART_SUPPRESS_DEPRECATED_BEGIN
317  for (std::size_t i = 0; i < size; ++i)
318  v[i] = random(min, max);
319  DART_SUPPRESS_DEPRECATED_END
320 
321  return v;
322 }
323 
324 //==============================================================================
326 DART_DEPRECATED(6.7)
327 inline Eigen::VectorXd randomVectorXd(std::size_t size, double limit)
328 {
329  DART_SUPPRESS_DEPRECATED_BEGIN
330  return randomVectorXd(size, -std::abs(limit), std::abs(limit));
331  DART_SUPPRESS_DEPRECATED_END
332 }
333 
334 namespace suffixes {
335 
336 //==============================================================================
337 constexpr double operator"" _pi(long double x)
338 {
339  return x * constants<double>::pi();
340 }
341 
342 //==============================================================================
343 constexpr double operator"" _pi(unsigned long long int x)
344 {
345  return operator"" _pi(static_cast<long double>(x));
346 }
347 
348 //==============================================================================
349 constexpr double operator"" _rad(long double angle)
350 {
351  return angle;
352 }
353 
354 //==============================================================================
355 constexpr double operator"" _rad(unsigned long long int angle)
356 {
357  return operator"" _rad(static_cast<long double>(angle));
358 }
359 
360 //==============================================================================
361 constexpr double operator"" _deg(long double angle)
362 {
363  return toRadian(angle);
364 }
365 
366 //==============================================================================
367 constexpr double operator"" _deg(unsigned long long int angle)
368 {
369  return operator"" _deg(static_cast<long double>(angle));
370 }
371 
372 } // namespace suffixes
373 
374 } // namespace math
375 
376 namespace Color {
377 
378 inline Eigen::Vector4d Red(double alpha)
379 {
380  return Eigen::Vector4d(0.9, 0.1, 0.1, alpha);
381 }
382 
383 inline Eigen::Vector3d Red()
384 {
385  return Eigen::Vector3d(0.9, 0.1, 0.1);
386 }
387 
388 inline Eigen::Vector3d Fuchsia()
389 {
390  return Eigen::Vector3d(1.0, 0.0, 0.5);
391 }
392 
393 inline Eigen::Vector4d Fuchsia(double alpha)
394 {
395  return Eigen::Vector4d(1.0, 0.0, 0.5, alpha);
396 }
397 
398 inline Eigen::Vector4d Orange(double alpha)
399 {
400  return Eigen::Vector4d(1.0, 0.63, 0.0, alpha);
401 }
402 
403 inline Eigen::Vector3d Orange()
404 {
405  return Eigen::Vector3d(1.0, 0.63, 0.0);
406 }
407 
408 inline Eigen::Vector4d Green(double alpha)
409 {
410  return Eigen::Vector4d(0.1, 0.9, 0.1, alpha);
411 }
412 
413 inline Eigen::Vector3d Green()
414 {
415  return Eigen::Vector3d(0.1, 0.9, 0.1);
416 }
417 
418 inline Eigen::Vector4d Blue(double alpha)
419 {
420  return Eigen::Vector4d(0.1, 0.1, 0.9, alpha);
421 }
422 
423 inline Eigen::Vector3d Blue()
424 {
425  return Eigen::Vector3d(0.1, 0.1, 0.9);
426 }
427 
428 inline Eigen::Vector4d White(double alpha)
429 {
430  return Eigen::Vector4d(1.0, 1.0, 1.0, alpha);
431 }
432 
433 inline Eigen::Vector3d White()
434 {
435  return Eigen::Vector3d(1.0, 1.0, 1.0);
436 }
437 
438 inline Eigen::Vector4d Black(double alpha)
439 {
440  return Eigen::Vector4d(0.05, 0.05, 0.05, alpha);
441 }
442 
443 inline Eigen::Vector3d Black()
444 {
445  return Eigen::Vector3d(0.05, 0.05, 0.05);
446 }
447 
448 inline Eigen::Vector4d LightGray(double alpha)
449 {
450  return Eigen::Vector4d(0.9, 0.9, 0.9, alpha);
451 }
452 
453 inline Eigen::Vector3d LightGray()
454 {
455  return Eigen::Vector3d(0.9, 0.9, 0.9);
456 }
457 
458 inline Eigen::Vector4d Gray(double alpha)
459 {
460  return Eigen::Vector4d(0.6, 0.6, 0.6, alpha);
461 }
462 
463 inline Eigen::Vector3d Gray()
464 {
465  return Eigen::Vector3d(0.6, 0.6, 0.6);
466 }
467 
468 inline Eigen::Vector4d Random(double alpha)
469 {
470  return Eigen::Vector4d(
471  math::Random::uniform(0.0, 1.0),
472  math::Random::uniform(0.0, 1.0),
473  math::Random::uniform(0.0, 1.0),
474  alpha);
475 }
476 
477 inline Eigen::Vector3d Random()
478 {
479  return Eigen::Vector3d(
480  math::Random::uniform(0.0, 1.0),
481  math::Random::uniform(0.0, 1.0),
482  math::Random::uniform(0.0, 1.0));
483 }
484 
485 } // namespace Color
486 
487 } // namespace dart
488 
489 #endif // DART_MATH_HELPERS_HPP_
#define DART_DEPRECATED(version)
Definition: Deprecated.hpp:51
static S uniform(S min, S max)
Returns a random number from an uniform distribution.
Definition: Random-impl.hpp:413
Definition: Random-impl.hpp:92
Eigen::Vector4d Gray(double alpha)
Definition: Helpers.hpp:458
Eigen::Vector4d LightGray(double alpha)
Definition: Helpers.hpp:448
Eigen::Vector4d Green(double alpha)
Definition: Helpers.hpp:408
Eigen::Vector4d Red(double alpha)
Definition: Helpers.hpp:378
Eigen::Vector4d Blue(double alpha)
Definition: Helpers.hpp:418
Eigen::Vector3d Fuchsia()
Definition: Helpers.hpp:388
Eigen::Vector4d White(double alpha)
Definition: Helpers.hpp:428
Eigen::Vector4d Orange(double alpha)
Definition: Helpers.hpp:398
Eigen::Vector4d Random(double alpha)
Definition: Helpers.hpp:468
Eigen::Vector4d Black(double alpha)
Definition: Helpers.hpp:438
bool isZero(double _theta)
Definition: Helpers.hpp:109
unsigned seedRand()
Definition: Helpers.hpp:263
T clip(const T &val, const T &lower, const T &upper)
Definition: Helpers.hpp:159
constexpr T toDegree(const T &radian)
Definition: Helpers.hpp:65
bool isEqual(double _x, double _y)
Definition: Helpers.hpp:173
double acosech(double _X)
Definition: Helpers.hpp:134
Eigen::Matrix< double, N, 1 > randomVector(double _min, double _max)
Definition: Helpers.hpp:288
double acosh(double _X)
Definition: Helpers.hpp:119
double atanh(double _X)
Definition: Helpers.hpp:124
double round2(double _x)
Definition: Helpers.hpp:149
constexpr int sign(T x, std::false_type)
Definition: Helpers.hpp:82
Eigen::VectorXd randomVectorXd(std::size_t size, double min, double max)
Definition: Helpers.hpp:312
double Tsinc(double _theta)
Definition: Helpers.hpp:104
const Eigen::Matrix2d CR((Eigen::Matrix2d()<< 0.0, -1.0, 1.0, 0.0).finished())
a cross b = (CR*a) dot b const Matd CR(2,2,0.0,-1.0,1.0,0.0);
constexpr T toRadian(const T &degree)
Definition: Helpers.hpp:58
double round(double _x)
Definition: Helpers.hpp:144
double random(double _min, double _max)
Definition: Helpers.hpp:279
double sqr(double _x)
Definition: Helpers.hpp:99
bool isInf(double _v)
Returns whether _v is an infinity value (either positive infinity or negative infinity).
Definition: Helpers.hpp:209
bool isSymmetric(const Eigen::MatrixXd &_m, double _tol=1e-6)
Returns whether _m is symmetric or not.
Definition: Helpers.hpp:231
double asech(double _X)
Definition: Helpers.hpp:129
double acotanh(double _X)
Definition: Helpers.hpp:139
double asinh(double _X)
Definition: Helpers.hpp:114
int delta(int _i, int _j)
Definition: Helpers.hpp:74
bool isInt(double _x)
Definition: Helpers.hpp:179
bool isNan(double _v)
Returns whether _v is a NaN (Not-A-Number) value.
Definition: Helpers.hpp:187
Definition: BulletCollisionDetector.cpp:65
Definition: SharedLibraryManager.hpp:46
static constexpr T pi()
Definition: Constants.hpp:45