39 #ifndef HPP_FCL_TRANSFORM_H 40 #define HPP_FCL_TRANSFORM_H 51 static inline std::ostream& operator << (std::ostream& o,
const Quaternion3f& q)
53 o <<
"(" << q.w() <<
" " << q.x() <<
" " << q.y() <<
" " << q.z() <<
")";
74 template <
typename Matrixx3Like,
typename Vector3Like>
76 const Eigen::MatrixBase<Vector3Like>& T_) :
82 template <
typename Vector3Like>
84 const Eigen::MatrixBase<Vector3Like>& T_) :
85 R(q_.toRotationMatrix()),
136 template <
typename Matrix3Like,
typename Vector3Like>
138 const Eigen::MatrixBase<Vector3Like>& T_)
147 R = q_.toRotationMatrix();
152 template<
typename Derived>
159 template<
typename Derived>
168 R = q_.toRotationMatrix();
172 template <
typename Derived>
181 R.transposeInPlace();
189 return Transform3f (R.transpose(), - R.transpose() * T);
195 return Transform3f(R.transpose() * other.R, R.transpose() * (other.T - T));
215 return R.isIdentity() && T.isZero();
232 return !(*
this == other);
236 template<
typename Derived>
239 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:69
Quaternion3f fromAxisAngle(const Eigen::MatrixBase< Derived > &axis, FCL_REAL angle)
Definition: transform.h:237
double FCL_REAL
Definition: data_types.h:66
Eigen::Quaternion< FCL_REAL > Quaternion3f
Definition: transform.h:49
Eigen::Matrix< FCL_REAL, 3, 1 > Vec3f
Definition: data_types.h:67
#define HPP_FCL_DLLAPI
Definition: config.hh:64