DART 6.7.3
Loading...
Searching...
No Matches
Helpers.hpp
Go to the documentation of this file.
1/*
2 * Copyright (c) 2011-2019, 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
51#include "dart/math/Random.hpp"
52
53namespace dart {
54namespace math {
55
56//==============================================================================
57template <typename T>
58constexpr T toRadian(const T& degree)
59{
60 return degree * constants<T>::pi() / 180.0;
61}
62
63//==============================================================================
64template <typename T>
65constexpr T toDegree(const T& radian)
66{
67 return radian * 180.0 / constants<T>::pi();
68}
69
72const Eigen::Matrix2d CR((Eigen::Matrix2d() << 0.0, -1.0, 1.0, 0.0).finished());
73
74inline int delta(int _i, int _j) {
75 if (_i == _j)
76 return 1;
77 return 0;
78}
79
80template <typename T> inline constexpr
81int sign(T x, std::false_type)
82{
83 return static_cast<T>(0) < x;
84}
85
86template <typename T> inline constexpr
87int sign(T x, std::true_type)
88{
89 return (static_cast<T>(0) < x) - (x < static_cast<T>(0));
90}
91
92template <typename T> inline constexpr
93int sign(T x)
94{
95 return sign(x, std::is_signed<T>());
96}
97
98inline double sqr(double _x) {
99 return _x*_x;
100}
101
102inline double Tsinc(double _theta) {
103 return 0.5-sqrt(_theta)/48;
104}
105
106inline bool isZero(double _theta) {
107 return (std::abs(_theta) < 1e-6);
108}
109
110inline double asinh(double _X) {
111 return log(_X + sqrt(_X * _X + 1));
112}
113
114inline double acosh(double _X) {
115 return log(_X + sqrt(_X * _X - 1));
116}
117
118inline double atanh(double _X) {
119 return log((1 + _X)/(1 - _X))/ 2;
120}
121
122inline double asech(double _X) {
123 return log((sqrt(-_X * _X + 1) + 1) / _X);
124}
125
126inline double acosech(double _X) {
127 return log((sign(_X) * sqrt(_X * _X + 1) +1) / _X);
128}
129
130inline double acotanh(double _X) {
131 return log((_X + 1) / (_X - 1)) / 2;
132}
133
134inline double round(double _x) {
135 return floor(_x + 0.5);
136}
137
138inline double round2(double _x) {
139 int gintx = static_cast<int>(std::floor(_x));
140 if (_x - gintx < 0.5)
141 return static_cast<double>(gintx);
142 else
143 return static_cast<double>(gintx + 1.0);
144}
145
146template <typename T>
147inline T clip(const T& val, const T& lower, const T& upper)
148{
149 return std::max(lower, std::min(val, upper));
150}
151
152template <typename DerivedA, typename DerivedB>
153inline typename DerivedA::PlainObject clip(
154 const Eigen::MatrixBase<DerivedA>& val,
155 const Eigen::MatrixBase<DerivedB>& lower,
156 const Eigen::MatrixBase<DerivedB>& upper)
157{
158 return lower.cwiseMax(val.cwiseMin(upper));
159}
160
161inline bool isEqual(double _x, double _y) {
162 return (std::abs(_x - _y) < 1e-6);
163}
164
165// check if it is an integer
166inline bool isInt(double _x) {
167 if (isEqual(round(_x), _x))
168 return true;
169 return false;
170}
171
173inline bool isNan(double _v) {
174#ifdef _WIN32
175 return _isnan(_v) != 0;
176#else
177 return std::isnan(_v);
178#endif
179}
180
182inline bool isNan(const Eigen::MatrixXd& _m) {
183 for (int i = 0; i < _m.rows(); ++i)
184 for (int j = 0; j < _m.cols(); ++j)
185 if (isNan(_m(i, j)))
186 return true;
187
188 return false;
189}
190
193inline bool isInf(double _v) {
194#ifdef _WIN32
195 return !_finite(_v);
196#else
197 return std::isinf(_v);
198#endif
199}
200
203inline bool isInf(const Eigen::MatrixXd& _m) {
204 for (int i = 0; i < _m.rows(); ++i)
205 for (int j = 0; j < _m.cols(); ++j)
206 if (isInf(_m(i, j)))
207 return true;
208
209 return false;
210}
211
213inline bool isSymmetric(const Eigen::MatrixXd& _m, double _tol = 1e-6) {
214 std::size_t rows = _m.rows();
215 std::size_t cols = _m.cols();
216
217 if (rows != cols)
218 return false;
219
220 for (std::size_t i = 0; i < rows; ++i) {
221 for (std::size_t j = i + 1; j < cols; ++j) {
222 if (std::abs(_m(i, j) - _m(j, i)) > _tol) {
223 std::cout << "A: " << std::endl;
224 for (std::size_t k = 0; k < rows; ++k) {
225 for (std::size_t l = 0; l < cols; ++l)
226 std::cout << std::setprecision(4) << _m(k, l) << " ";
227 std::cout << std::endl;
228 }
229
230 std::cout << "A(" << i << ", " << j << "): " << _m(i, j) << std::endl;
231 std::cout << "A(" << j << ", " << i << "): " << _m(i, j) << std::endl;
232 return false;
233 }
234 }
235 }
236
237 return true;
238}
239
240inline unsigned seedRand() {
241 time_t now = time(0);
242 unsigned char* p = reinterpret_cast<unsigned char*>(&now);
243 unsigned seed = 0;
244 std::size_t i;
245
246 for (i = 0; i < sizeof(now); i++)
247 seed = seed * (UCHAR_MAX + 2U) + p[i];
248
249 srand(seed);
250 return seed;
251}
252
255inline double random(double _min, double _max) {
256 return _min + ((static_cast<double>(rand()) / (RAND_MAX + 1.0))
257 * (_max - _min));
258}
259
261template<int N>
263Eigen::Matrix<double, N, 1> randomVector(double _min, double _max)
264{
265 Eigen::Matrix<double, N, 1> v;
266DART_SUPPRESS_DEPRECATED_BEGIN
267 for(std::size_t i=0; i<N; ++i)
268 v[i] = random(_min, _max);
269DART_SUPPRESS_DEPRECATED_END
270
271 return v;
272}
273
275template<int N>
277Eigen::Matrix<double, N, 1> randomVector(double _limit)
278{
279DART_SUPPRESS_DEPRECATED_BEGIN
280 return randomVector<N>(-std::abs(_limit), std::abs(_limit));
281DART_SUPPRESS_DEPRECATED_END
282}
283
284//==============================================================================
287inline Eigen::VectorXd randomVectorXd(std::size_t size, double min, double max)
288{
289 Eigen::VectorXd v = Eigen::VectorXd::Zero(size);
290
291DART_SUPPRESS_DEPRECATED_BEGIN
292 for (std::size_t i = 0; i < size; ++i)
293 v[i] = random(min, max);
294DART_SUPPRESS_DEPRECATED_END
295
296 return v;
297}
298
299//==============================================================================
302inline Eigen::VectorXd randomVectorXd(std::size_t size, double limit)
303{
304DART_SUPPRESS_DEPRECATED_BEGIN
305 return randomVectorXd(size, -std::abs(limit), std::abs(limit));
306DART_SUPPRESS_DEPRECATED_END
307}
308
309namespace suffixes {
310
311//==============================================================================
312constexpr double operator"" _pi(long double x)
313{
314 return x * constants<double>::pi();
315}
316
317//==============================================================================
318constexpr double operator"" _pi(unsigned long long int x)
319{
320 return operator"" _pi(static_cast<long double>(x));
321}
322
323//==============================================================================
324constexpr double operator"" _rad(long double angle)
325{
326 return angle;
327}
328
329//==============================================================================
330constexpr double operator"" _rad(unsigned long long int angle)
331{
332 return operator"" _rad(static_cast<long double>(angle));
333}
334
335//==============================================================================
336constexpr double operator"" _deg(long double angle)
337{
338 return toRadian(angle);
339}
340
341//==============================================================================
342constexpr double operator"" _deg(unsigned long long int angle)
343{
344 return operator"" _deg(static_cast<long double>(angle));
345}
346
347} // namespace suffixes
348
349} // namespace math
350
351namespace Color
352{
353
354inline Eigen::Vector4d Red(double alpha)
355{
356 return Eigen::Vector4d(0.9, 0.1, 0.1, alpha);
357}
358
359inline Eigen::Vector3d Red()
360{
361 return Eigen::Vector3d(0.9, 0.1, 0.1);
362}
363
364inline Eigen::Vector3d Fuchsia()
365{
366 return Eigen::Vector3d(1.0, 0.0, 0.5);
367}
368
369inline Eigen::Vector4d Fuchsia(double alpha)
370{
371 return Eigen::Vector4d(1.0, 0.0, 0.5, alpha);
372}
373
374inline Eigen::Vector4d Orange(double alpha)
375{
376 return Eigen::Vector4d(1.0, 0.63, 0.0, alpha);
377}
378
379inline Eigen::Vector3d Orange()
380{
381 return Eigen::Vector3d(1.0, 0.63, 0.0);
382}
383
384inline Eigen::Vector4d Green(double alpha)
385{
386 return Eigen::Vector4d(0.1, 0.9, 0.1, alpha);
387}
388
389inline Eigen::Vector3d Green()
390{
391 return Eigen::Vector3d(0.1, 0.9, 0.1);
392}
393
394inline Eigen::Vector4d Blue(double alpha)
395{
396 return Eigen::Vector4d(0.1, 0.1, 0.9, alpha);
397}
398
399inline Eigen::Vector3d Blue()
400{
401 return Eigen::Vector3d(0.1, 0.1, 0.9);
402}
403
404inline Eigen::Vector4d White(double alpha)
405{
406 return Eigen::Vector4d(1.0, 1.0, 1.0, alpha);
407}
408
409inline Eigen::Vector3d White()
410{
411 return Eigen::Vector3d(1.0, 1.0, 1.0);
412}
413
414inline Eigen::Vector4d Black(double alpha)
415{
416 return Eigen::Vector4d(0.05, 0.05, 0.05, alpha);
417}
418
419inline Eigen::Vector3d Black()
420{
421 return Eigen::Vector3d(0.05, 0.05, 0.05);
422}
423
424inline Eigen::Vector4d LightGray(double alpha)
425{
426 return Eigen::Vector4d(0.9, 0.9, 0.9, alpha);
427}
428
429inline Eigen::Vector3d LightGray()
430{
431 return Eigen::Vector3d(0.9, 0.9, 0.9);
432}
433
434inline Eigen::Vector4d Gray(double alpha)
435{
436 return Eigen::Vector4d(0.6, 0.6, 0.6, alpha);
437}
438
439inline Eigen::Vector3d Gray()
440{
441 return Eigen::Vector3d(0.6, 0.6, 0.6);
442}
443
444inline Eigen::Vector4d Random(double alpha)
445{
446 return Eigen::Vector4d(math::Random::uniform(0.0, 1.0),
447 math::Random::uniform(0.0, 1.0),
448 math::Random::uniform(0.0, 1.0),
449 alpha);
450}
451
452inline Eigen::Vector3d Random()
453{
454 return Eigen::Vector3d(math::Random::uniform(0.0, 1.0),
455 math::Random::uniform(0.0, 1.0),
456 math::Random::uniform(0.0, 1.0));
457}
458
459} // namespace Color
460
461} // namespace dart
462
463#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:412
Definition Random-impl.hpp:92
Eigen::Vector3d LightGray()
Definition Helpers.hpp:429
Eigen::Vector3d Red()
Definition Helpers.hpp:359
Eigen::Vector3d Blue()
Definition Helpers.hpp:399
Eigen::Vector3d White()
Definition Helpers.hpp:409
Eigen::Vector3d Fuchsia()
Definition Helpers.hpp:364
Eigen::Vector3d Gray()
Definition Helpers.hpp:439
Eigen::Vector3d Orange()
Definition Helpers.hpp:379
Eigen::Vector3d Black()
Definition Helpers.hpp:419
Eigen::Vector3d Green()
Definition Helpers.hpp:389
Eigen::Vector3d Random()
Definition Helpers.hpp:452
bool isZero(double _theta)
Definition Helpers.hpp:106
unsigned seedRand()
Definition Helpers.hpp:240
Eigen::Matrix< double, N, 1 > randomVector(double _min, double _max)
Definition Helpers.hpp:263
T clip(const T &val, const T &lower, const T &upper)
Definition Helpers.hpp:147
constexpr T toDegree(const T &radian)
Definition Helpers.hpp:65
bool isEqual(double _x, double _y)
Definition Helpers.hpp:161
double acosech(double _X)
Definition Helpers.hpp:126
double acosh(double _X)
Definition Helpers.hpp:114
double atanh(double _X)
Definition Helpers.hpp:118
double round2(double _x)
Definition Helpers.hpp:138
constexpr int sign(T x, std::false_type)
Definition Helpers.hpp:81
Eigen::VectorXd randomVectorXd(std::size_t size, double min, double max)
Definition Helpers.hpp:287
double Tsinc(double _theta)
Definition Helpers.hpp:102
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:134
double random(double _min, double _max)
Definition Helpers.hpp:255
double sqr(double _x)
Definition Helpers.hpp:98
bool isInf(double _v)
Returns whether _v is an infinity value (either positive infinity or negative infinity).
Definition Helpers.hpp:193
bool isSymmetric(const Eigen::MatrixXd &_m, double _tol=1e-6)
Returns whether _m is symmetric or not.
Definition Helpers.hpp:213
double asech(double _X)
Definition Helpers.hpp:122
double acotanh(double _X)
Definition Helpers.hpp:130
double asinh(double _X)
Definition Helpers.hpp:110
int delta(int _i, int _j)
Definition Helpers.hpp:74
bool isInt(double _x)
Definition Helpers.hpp:166
bool isNan(double _v)
Returns whether _v is a NaN (Not-A-Number) value.
Definition Helpers.hpp:173
Definition BulletCollisionDetector.cpp:63
Definition SharedLibraryManager.hpp:43
static constexpr T pi()
Definition Constants.hpp:44