33 #ifndef DART_MATH_HELPERS_HPP_
34 #define DART_MATH_HELPERS_HPP_
47 #include <Eigen/Dense>
72 const Eigen::Matrix2d
CR((Eigen::Matrix2d() << 0.0, -1.0, 1.0, 0.0).finished());
74 inline int delta(
int _i,
int _j)
82 inline constexpr
int sign(T x, std::false_type)
84 return static_cast<T
>(0) < x;
88 inline constexpr
int sign(T x, std::true_type)
90 return (
static_cast<T
>(0) < x) - (x < static_cast<T>(0));
94 inline constexpr
int sign(T x)
96 return sign(x, std::is_signed<T>());
99 inline double sqr(
double _x)
106 return 0.5 - sqrt(_theta) / 48;
111 return (std::abs(_theta) < 1e-6);
116 return log(_X + sqrt(_X * _X + 1));
121 return log(_X + sqrt(_X * _X - 1));
126 return log((1 + _X) / (1 - _X)) / 2;
131 return log((sqrt(-_X * _X + 1) + 1) / _X);
136 return log((
sign(_X) * sqrt(_X * _X + 1) + 1) / _X);
141 return log((_X + 1) / (_X - 1)) / 2;
146 return floor(_x + 0.5);
151 int gintx =
static_cast<int>(std::floor(_x));
152 if (_x - gintx < 0.5)
153 return static_cast<double>(gintx);
155 return static_cast<double>(gintx + 1.0);
158 template <
typename T>
159 inline T
clip(
const T& val,
const T& lower,
const T& upper)
161 return std::max(lower, std::min(val, upper));
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)
170 return lower.cwiseMax(val.cwiseMin(upper));
175 return (std::abs(_x - _y) < 1e-6);
190 return _isnan(_v) != 0;
192 return std::isnan(_v);
197 inline bool isNan(
const Eigen::MatrixXd& _m)
199 for (
int i = 0; i < _m.rows(); ++i)
200 for (
int j = 0; j < _m.cols(); ++j)
214 return std::isinf(_v);
220 inline bool isInf(
const Eigen::MatrixXd& _m)
222 for (
int i = 0; i < _m.rows(); ++i)
223 for (
int j = 0; j < _m.cols(); ++j)
231 inline bool isSymmetric(
const Eigen::MatrixXd& _m,
double _tol = 1e-6)
233 std::size_t rows = _m.rows();
234 std::size_t cols = _m.cols();
239 for (std::size_t i = 0; i < rows; ++i)
241 for (std::size_t j = i + 1; j < cols; ++j)
243 if (std::abs(_m(i, j) - _m(j, i)) > _tol)
245 std::cout <<
"A: " << std::endl;
246 for (std::size_t k = 0; k < rows; ++k)
248 for (std::size_t l = 0; l < cols; ++l)
249 std::cout << std::setprecision(4) << _m(k, l) <<
" ";
250 std::cout << std::endl;
253 std::cout <<
"A(" << i <<
", " << j <<
"): " << _m(i, j) << std::endl;
254 std::cout <<
"A(" << j <<
", " << i <<
"): " << _m(i, j) << std::endl;
265 time_t now = time(0);
266 unsigned char* p =
reinterpret_cast<unsigned char*
>(&now);
270 for (i = 0; i <
sizeof(now); i++)
271 seed = seed * (UCHAR_MAX + 2U) + p[i];
279 inline
double random(
double _min,
double _max)
282 + ((
static_cast<double>(rand()) / (RAND_MAX + 1.0)) * (_max - _min));
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
304 DART_SUPPRESS_DEPRECATED_BEGIN
305 return randomVector<N>(-std::abs(_limit), std::abs(_limit));
306 DART_SUPPRESS_DEPRECATED_END
314 Eigen::VectorXd v = Eigen::VectorXd::Zero(size);
316 DART_SUPPRESS_DEPRECATED_BEGIN
317 for (std::size_t i = 0; i < size; ++i)
319 DART_SUPPRESS_DEPRECATED_END
329 DART_SUPPRESS_DEPRECATED_BEGIN
331 DART_SUPPRESS_DEPRECATED_END
337 constexpr
double operator"" _pi(
long double x)
343 constexpr
double operator"" _pi(
unsigned long long int x)
345 return operator"" _pi(
static_cast<long double>(x));
349 constexpr
double operator"" _rad(
long double angle)
355 constexpr
double operator"" _rad(
unsigned long long int angle)
357 return operator"" _rad(
static_cast<long double>(angle));
361 constexpr
double operator"" _deg(
long double angle)
367 constexpr
double operator"" _deg(
unsigned long long int angle)
369 return operator"" _deg(
static_cast<long double>(angle));
378 inline Eigen::Vector4d
Red(
double alpha)
380 return Eigen::Vector4d(0.9, 0.1, 0.1, alpha);
383 inline Eigen::Vector3d
Red()
385 return Eigen::Vector3d(0.9, 0.1, 0.1);
390 return Eigen::Vector3d(1.0, 0.0, 0.5);
395 return Eigen::Vector4d(1.0, 0.0, 0.5, alpha);
398 inline Eigen::Vector4d
Orange(
double alpha)
400 return Eigen::Vector4d(1.0, 0.63, 0.0, alpha);
405 return Eigen::Vector3d(1.0, 0.63, 0.0);
408 inline Eigen::Vector4d
Green(
double alpha)
410 return Eigen::Vector4d(0.1, 0.9, 0.1, alpha);
415 return Eigen::Vector3d(0.1, 0.9, 0.1);
418 inline Eigen::Vector4d
Blue(
double alpha)
420 return Eigen::Vector4d(0.1, 0.1, 0.9, alpha);
425 return Eigen::Vector3d(0.1, 0.1, 0.9);
428 inline Eigen::Vector4d
White(
double alpha)
430 return Eigen::Vector4d(1.0, 1.0, 1.0, alpha);
435 return Eigen::Vector3d(1.0, 1.0, 1.0);
438 inline Eigen::Vector4d
Black(
double alpha)
440 return Eigen::Vector4d(0.05, 0.05, 0.05, alpha);
445 return Eigen::Vector3d(0.05, 0.05, 0.05);
450 return Eigen::Vector4d(0.9, 0.9, 0.9, alpha);
455 return Eigen::Vector3d(0.9, 0.9, 0.9);
458 inline Eigen::Vector4d
Gray(
double alpha)
460 return Eigen::Vector4d(0.6, 0.6, 0.6, alpha);
465 return Eigen::Vector3d(0.6, 0.6, 0.6);
468 inline Eigen::Vector4d
Random(
double alpha)
470 return Eigen::Vector4d(
479 return Eigen::Vector3d(
#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 °ree)
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