5 #ifndef CNOID_UTIL_EIGEN_UTIL_H_INCLUDED
6 #define CNOID_UTIL_EIGEN_UTIL_H_INCLUDED
14 const double PI = 3.14159265358979323846;
15 const double PI_2 = 1.57079632679489661923;
17 inline double degree(
double rad) {
return (180.0 * rad /
PI); }
18 inline double radian(
double deg) {
return (
PI * deg / 180.0); }
20 template<
typename Derived>
21 inline Eigen::Matrix<typename Eigen::MatrixBase<Derived>::Scalar, 3, 1>
23 Vector3 ea = R.eulerAngles(2, 1, 0);
24 return Vector3(ea[2], ea[1], ea[0]);
37 M << 0.0, -x(2), x(1),