5 #ifndef CNOID_UTIL_EIGEN_UTIL_H 6 #define CNOID_UTIL_EIGEN_UTIL_H 10 #if defined(WIN32) && !defined(BOOST_NO_CXX11_ALLOCATOR) 11 #include <boost/version.hpp> 12 #if (BOOST_VERSION >= 105900) 13 #define BOOST_NO_CXX11_ALLOCATOR 17 #include <boost/make_shared.hpp> 22 const double PI = 3.14159265358979323846;
23 const double PI_2 = 1.57079632679489661923;
28 inline double degree(
double rad) {
return TO_DEGREE * rad; }
29 inline double radian(
double deg) {
return TO_RADIAN * deg; }
30 inline float degree(
float rad) {
return (
float)TO_DEGREE * rad; }
31 inline float radian(
float deg) {
return (
float)TO_RADIAN * deg; }
32 inline double radian(
int deg) {
return TO_RADIAN * deg; }
61 M << 0.0, -x(2), x(1),
77 return boost::allocate_shared<T>(Eigen::aligned_allocator<T>());
79 template<
class T,
class P1>
81 return boost::allocate_shared<T>(Eigen::aligned_allocator<T>(), p1);
83 template<
class T,
class P1,
class P2>
85 return boost::allocate_shared<T>(Eigen::aligned_allocator<T>(), p1, p2);
87 template<
class T,
class P1,
class P2,
class P3>
89 return boost::allocate_shared<T>(Eigen::aligned_allocator<T>(), p1, p2, p3);
91 template<
class T,
class P1,
class P2,
class P3,
class P4>
93 return boost::allocate_shared<T>(Eigen::aligned_allocator<T>(), p1, p2, p3, p4);
const double PI
Definition: EigenUtil.h:22
double radian(double deg)
Definition: EigenUtil.h:29
void normalizeRotation(Matrix3 &R)
Definition: EigenUtil.cpp:116
Vector3 omegaFromRot(const Matrix3 &R)
Definition: EigenUtil.cpp:67
double degree(double rad)
Definition: EigenUtil.h:28
const double TO_RADIAN
Definition: EigenUtil.h:26
boost::shared_ptr< T > make_shared_aligned()
Definition: EigenUtil.h:76
Eigen::Affine3d Affine3
Definition: EigenTypes.h:64
bool toVector3(const std::string &s, Vector3 &out_v)
Definition: EigenUtil.cpp:95
Matrix3 hat(const Vector3 &x)
Definition: EigenUtil.h:59
const double TO_DEGREE
Definition: EigenUtil.h:25
std::string str(const Vector3 &v)
Definition: EigenUtil.cpp:90
Eigen::Transform< double, 3, Eigen::AffineCompact > Position
Definition: EigenTypes.h:73
Defines the minimum processing for performing pasing file for STL.
Definition: AbstractSceneLoader.h:9
Vector3 rpyFromRot(const Matrix3 &R)
Definition: EigenUtil.cpp:27
Matrix3 rotFromRpy(double r, double p, double y)
Definition: EigenUtil.cpp:9
const double PI_2
Definition: EigenUtil.h:23
Eigen::Vector3d Vector3
Definition: EigenTypes.h:58
#define CNOID_EXPORT
Definition: Util/exportdecl.h:37
Eigen::Matrix3d Matrix3
Definition: EigenTypes.h:57