19 #ifndef SURGSIM_MATH_RIGIDTRANSFORM_H 20 #define SURGSIM_MATH_RIGIDTRANSFORM_H 23 #include <Eigen/Geometry> 55 template <
typename M,
typename V>
56 inline Eigen::Transform<typename M::Scalar, M::RowsAtCompileTime, Eigen::Isometry>
makeRigidTransform(
57 const Eigen::MatrixBase<M>& rotation,
const Eigen::MatrixBase<V>& translation)
59 Eigen::Transform<typename M::Scalar, M::RowsAtCompileTime, Eigen::Isometry> rigid;
61 rigid.linear() = rotation;
62 rigid.translation() = translation;
72 template <
typename Q,
typename V>
74 const Eigen::QuaternionBase<Q>& rotation,
const Eigen::MatrixBase<V>& translation)
76 Eigen::Transform<typename Q::Scalar, 3, Eigen::Isometry> rigid;
78 rigid.linear() = rotation.matrix();
79 rigid.translation() = translation;
93 template <
typename T,
int VOpt>
95 const Eigen::Matrix<T, 3, 1, VOpt>& position,
96 const Eigen::Matrix<T, 3, 1, VOpt>& center,
97 const Eigen::Matrix<T, 3, 1, VOpt>& up)
100 Eigen::Transform<T, 3, Eigen::Isometry> rigid;
103 Eigen::Matrix<T, 3, 1, VOpt> forward = (center - position).normalized();
104 Eigen::Matrix<T, 3, 1, VOpt> side = (forward.cross(up)).normalized();
105 Eigen::Matrix<T, 3, 1, VOpt> actualUp = side.cross(forward).normalized();
107 typename Eigen::Transform<T, 3, Eigen::Isometry>::LinearMatrixType rotation;
108 rotation << side[0], actualUp[0], -forward[0],
109 side[1], actualUp[1], -forward[1],
110 side[2], actualUp[2], -forward[2];
112 rigid.linear() = rotation;
113 rigid.translation() = position;
122 template <
typename V>
124 const Eigen::MatrixBase<V>& translation)
126 EIGEN_STATIC_ASSERT_VECTOR_ONLY(Eigen::MatrixBase<V>);
127 Eigen::Transform<typename V::Scalar, V::SizeAtCompileTime, Eigen::Isometry> rigid;
129 rigid.linear().setIdentity();
130 rigid.translation() = translation;
143 template <
typename T,
int TOpt>
145 const Eigen::Transform<T, 3, Eigen::Isometry, TOpt>& t0,
146 const Eigen::Transform<T, 3, Eigen::Isometry, TOpt>& t1,
149 Eigen::Transform<T, 3, Eigen::Isometry> transform;
150 transform.makeAffine();
151 transform.translation() = t0.translation() * (
static_cast<T
>(1.0) - t) + t1.translation() * t;
153 Eigen::Quaternion<T> q0(t0.linear());
154 Eigen::Quaternion<T> q1(t1.linear());
157 transform.linear() =
interpolate(q0, q1, t).matrix();
165 #endif // SURGSIM_MATH_RIGIDTRANSFORM_H Definitions of quaternion types.
Definition: DriveElementFromInputBehavior.cpp:27
Eigen::Transform< float, 2, Eigen::Isometry > RigidTransform2f
A 2D rigid (isometric) transform, represented as floats.
Definition: RigidTransform.h:34
Eigen::Transform< float, 3, Eigen::Isometry > RigidTransform3f
A 3D rigid (isometric) transform, represented as floats.
Definition: RigidTransform.h:38
Eigen::Quaternion< T, QOpt > interpolate(const Eigen::Quaternion< T, QOpt > &q0, const Eigen::Quaternion< T, QOpt > &q1, T t)
Interpolate (slerp) between 2 quaternions.
Definition: Quaternion.h:149
Eigen::Transform< typename M::Scalar, M::RowsAtCompileTime, Eigen::Isometry > makeRigidTransform(const Eigen::MatrixBase< M > &rotation, const Eigen::MatrixBase< V > &translation)
Create a rigid transform using the specified rotation matrix and translation.
Definition: RigidTransform.h:56
Eigen::Transform< double, 2, Eigen::Isometry > RigidTransform2d
A 2D rigid (isometric) transform, represented as doubles.
Definition: RigidTransform.h:42
Eigen::Transform< double, 3, Eigen::Isometry > RigidTransform3d
A 3D rigid (isometric) transform, represented as doubles.
Definition: RigidTransform.h:46
Eigen::Transform< typename V::Scalar, V::SizeAtCompileTime, Eigen::Isometry > makeRigidTranslation(const Eigen::MatrixBase< V > &translation)
Create a rigid transform using the identity rotation and the specified translation.
Definition: RigidTransform.h:123