33 #ifndef DART_MATH_HELPERS_HPP_
34 #define DART_MATH_HELPERS_HPP_
46 #include <Eigen/Dense>
70 const Eigen::Matrix2d
CR((Eigen::Matrix2d() << 0.0, -1.0, 1.0, 0.0).finished());
72 inline int delta(
int _i,
int _j) {
78 template <
typename T>
inline constexpr
79 int sign(T x, std::false_type)
81 return static_cast<T
>(0) < x;
84 template <
typename T>
inline constexpr
85 int sign(T x, std::true_type)
87 return (
static_cast<T
>(0) < x) - (x < static_cast<T>(0));
90 template <
typename T>
inline constexpr
93 return sign(x, std::is_signed<T>());
96 inline double sqr(
double _x) {
100 inline double Tsinc(
double _theta) {
101 return 0.5-sqrt(_theta)/48;
105 return (std::abs(_theta) < 1e-6);
109 return log(_X + sqrt(_X * _X + 1));
113 return log(_X + sqrt(_X * _X - 1));
117 return log((1 + _X)/(1 - _X))/ 2;
121 return log((sqrt(-_X * _X + 1) + 1) / _X);
125 return log((
sign(_X) * sqrt(_X * _X + 1) +1) / _X);
129 return log((_X + 1) / (_X - 1)) / 2;
133 return floor(_x + 0.5);
137 int gintx =
static_cast<int>(std::floor(_x));
138 if (_x - gintx < 0.5)
139 return static_cast<double>(gintx);
141 return static_cast<double>(gintx + 1.0);
144 template <
typename T>
145 inline T
clip(
const T& val,
const T& lower,
const T& upper)
147 return std::max(lower, std::min(val, upper));
150 template <
typename DerivedA,
typename DerivedB>
151 inline typename DerivedA::PlainObject
clip(
152 const Eigen::MatrixBase<DerivedA>& val,
153 const Eigen::MatrixBase<DerivedB>& lower,
154 const Eigen::MatrixBase<DerivedB>& upper)
156 return lower.cwiseMax(val.cwiseMin(upper));
160 return (std::abs(_x - _y) < 1e-6);
173 return _isnan(_v) != 0;
175 return std::isnan(_v);
180 inline bool isNan(
const Eigen::MatrixXd& _m) {
181 for (
int i = 0; i < _m.rows(); ++i)
182 for (
int j = 0; j < _m.cols(); ++j)
195 return std::isinf(_v);
201 inline bool isInf(
const Eigen::MatrixXd& _m) {
202 for (
int i = 0; i < _m.rows(); ++i)
203 for (
int j = 0; j < _m.cols(); ++j)
211 inline bool isSymmetric(
const Eigen::MatrixXd& _m,
double _tol = 1e-6) {
212 std::size_t rows = _m.rows();
213 std::size_t cols = _m.cols();
218 for (std::size_t i = 0; i < rows; ++i) {
219 for (std::size_t j = i + 1; j < cols; ++j) {
220 if (std::abs(_m(i, j) - _m(j, i)) > _tol) {
221 std::cout <<
"A: " << std::endl;
222 for (std::size_t k = 0; k < rows; ++k) {
223 for (std::size_t l = 0; l < cols; ++l)
224 std::cout << std::setprecision(4) << _m(k, l) <<
" ";
225 std::cout << std::endl;
228 std::cout <<
"A(" << i <<
", " << j <<
"): " << _m(i, j) << std::endl;
229 std::cout <<
"A(" << j <<
", " << i <<
"): " << _m(i, j) << std::endl;
239 time_t now = time(0);
240 unsigned char* p =
reinterpret_cast<unsigned char*
>(&now);
244 for (i = 0; i <
sizeof(now); i++)
245 seed = seed * (UCHAR_MAX + 2U) + p[i];
251 inline double random(
double _min,
double _max) {
252 return _min + ((
static_cast<double>(rand()) / (RAND_MAX + 1.0))
259 Eigen::Matrix<double, N, 1> v;
260 for(std::size_t i=0; i<N; ++i)
261 v[i] =
random(_min, _max);
269 return randomVector<N>(-std::abs(_limit), std::abs(_limit));
275 Eigen::VectorXd v = Eigen::VectorXd::Zero(size);
277 for (std::size_t i = 0; i < size; ++i)
292 constexpr
double operator"" _pi(
long double x)
298 constexpr
double operator"" _pi(
unsigned long long int x)
300 return operator"" _pi(
static_cast<long double>(x));
304 constexpr
double operator"" _rad(
long double angle)
310 constexpr
double operator"" _rad(
unsigned long long int angle)
312 return operator"" _rad(
static_cast<long double>(angle));
316 constexpr
double operator"" _deg(
long double angle)
322 constexpr
double operator"" _deg(
unsigned long long int angle)
324 return operator"" _deg(
static_cast<long double>(angle));
334 inline Eigen::Vector4d
Red(
double alpha)
336 return Eigen::Vector4d(0.9, 0.1, 0.1, alpha);
339 inline Eigen::Vector3d
Red()
341 return Eigen::Vector3d(0.9, 0.1, 0.1);
346 return Eigen::Vector3d(1.0, 0.0, 0.5);
351 return Eigen::Vector4d(1.0, 0.0, 0.5, alpha);
354 inline Eigen::Vector4d
Orange(
double alpha)
356 return Eigen::Vector4d(1.0, 0.63, 0.0, alpha);
361 return Eigen::Vector3d(1.0, 0.63, 0.0);
364 inline Eigen::Vector4d
Green(
double alpha)
366 return Eigen::Vector4d(0.1, 0.9, 0.1, alpha);
371 return Eigen::Vector3d(0.1, 0.9, 0.1);
374 inline Eigen::Vector4d
Blue(
double alpha)
376 return Eigen::Vector4d(0.1, 0.1, 0.9, alpha);
381 return Eigen::Vector3d(0.1, 0.1, 0.9);
384 inline Eigen::Vector4d
White(
double alpha)
386 return Eigen::Vector4d(1.0, 1.0, 1.0, alpha);
391 return Eigen::Vector3d(1.0, 1.0, 1.0);
394 inline Eigen::Vector4d
Black(
double alpha)
396 return Eigen::Vector4d(0.05, 0.05, 0.05, alpha);
401 return Eigen::Vector3d(0.05, 0.05, 0.05);
404 inline Eigen::Vector4d
Gray(
double alpha)
406 return Eigen::Vector4d(0.6, 0.6, 0.6, alpha);
411 return Eigen::Vector3d(0.6, 0.6, 0.6);
414 inline Eigen::Vector4d
Random(
double alpha)
Eigen::Vector4d Gray(double alpha)
Definition: Helpers.hpp:404
Eigen::Vector4d Green(double alpha)
Definition: Helpers.hpp:364
Eigen::Vector4d Red(double alpha)
Definition: Helpers.hpp:334
Eigen::Vector4d Blue(double alpha)
Definition: Helpers.hpp:374
Eigen::Vector3d Fuchsia()
Definition: Helpers.hpp:344
Eigen::Vector4d White(double alpha)
Definition: Helpers.hpp:384
Eigen::Vector4d Orange(double alpha)
Definition: Helpers.hpp:354
Eigen::Vector4d Random(double alpha)
Definition: Helpers.hpp:414
Eigen::Vector4d Black(double alpha)
Definition: Helpers.hpp:394
bool isZero(double _theta)
Definition: Helpers.hpp:104
unsigned seedRand()
Definition: Helpers.hpp:238
T clip(const T &val, const T &lower, const T &upper)
Definition: Helpers.hpp:145
constexpr T toDegree(const T &radian)
Definition: Helpers.hpp:63
bool isEqual(double _x, double _y)
Definition: Helpers.hpp:159
double acosech(double _X)
Definition: Helpers.hpp:124
Eigen::Matrix< double, N, 1 > randomVector(double _min, double _max)
Definition: Helpers.hpp:257
double acosh(double _X)
Definition: Helpers.hpp:112
double atanh(double _X)
Definition: Helpers.hpp:116
double round2(double _x)
Definition: Helpers.hpp:136
constexpr int sign(T x, std::false_type)
Definition: Helpers.hpp:79
Eigen::VectorXd randomVectorXd(std::size_t size, double min, double max)
Definition: Helpers.hpp:273
double Tsinc(double _theta)
Definition: Helpers.hpp:100
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:56
double round(double _x)
Definition: Helpers.hpp:132
double random(double _min, double _max)
Definition: Helpers.hpp:251
double sqr(double _x)
Definition: Helpers.hpp:96
bool isInf(double _v)
Returns whether _v is an infinity value (either positive infinity or negative infinity).
Definition: Helpers.hpp:191
bool isSymmetric(const Eigen::MatrixXd &_m, double _tol=1e-6)
Returns whether _m is symmetric or not.
Definition: Helpers.hpp:211
double asech(double _X)
Definition: Helpers.hpp:120
double acotanh(double _X)
Definition: Helpers.hpp:128
double asinh(double _X)
Definition: Helpers.hpp:108
int delta(int _i, int _j)
Definition: Helpers.hpp:72
bool isInt(double _x)
Definition: Helpers.hpp:164
bool isNan(double _v)
Returns whether _v is a NaN (Not-A-Number) value.
Definition: Helpers.hpp:171
Definition: BulletCollisionDetector.cpp:63
static constexpr T pi()
Definition: Constants.hpp:44