39 #ifndef HPP_FCL_TRANSFORM_H 40 #define HPP_FCL_TRANSFORM_H 43 #include <boost/thread/mutex.hpp> 52 static inline std::ostream& operator << (std::ostream& o,
const Quaternion3f& q)
54 o <<
"(" << q.w() <<
" " << q.x() <<
" " << q.y() <<
" " << q.z() <<
")";
75 template <
typename Matrixx3Like,
typename Vector3Like>
77 const Eigen::MatrixBase<Vector3Like>& T_) :
83 template <
typename Vector3Like>
85 const Eigen::MatrixBase<Vector3Like>& T_) :
86 R(q_.toRotationMatrix()),
132 template <
typename Matrix3Like,
typename Vector3Like>
134 const Eigen::MatrixBase<Vector3Like>& T_)
143 R = q_.toRotationMatrix();
148 template<
typename Derived>
155 template<
typename Derived>
164 R = q_.toRotationMatrix();
168 template <
typename Derived>
177 R.transposeInPlace();
185 return Transform3f (R.transpose(), - R.transpose() * T);
191 return Transform3f(R.transpose() * other.R, R.transpose() * (other.T - T));
211 return R.isIdentity() && T.isZero();
228 return !(*
this == other);
232 template<
typename Derived>
235 return Quaternion3f (Eigen::AngleAxis<FCL_REAL>(angle, axis));
Main namespace.
Definition: AABB.h:43
Eigen::Matrix< FCL_REAL, 3, 3 > Matrix3f
Definition: data_types.h:74
Quaternion3f fromAxisAngle(const Eigen::MatrixBase< Derived > &axis, FCL_REAL angle)
Definition: transform.h:233
double FCL_REAL
Definition: data_types.h:68
Eigen::Quaternion< FCL_REAL > Quaternion3f
Definition: transform.h:50
Eigen::Matrix< FCL_REAL, 3, 1 > Vec3f
Definition: data_types.h:73