19 #ifndef SURGSIM_MATH_QUATERNION_H
20 #define SURGSIM_MATH_QUATERNION_H
25 #include <Eigen/Geometry>
48 template <
typename T,
int VOpt>
51 return Eigen::Quaternion<T>(Eigen::AngleAxis<T>(angle, axis));
59 template <
typename T,
int QOpt>
60 inline Eigen::Quaternion<T, QOpt>
negate(
const Eigen::Quaternion<T, QOpt>& q)
62 return Eigen::Quaternion<T, QOpt>(q.coeffs() * -1.0);
75 template <
typename T,
int QOpt,
int VOpt>
77 T* angle, Eigen::Matrix<T, 3, 1, VOpt>* axis)
81 if (quaternion.w() >= T(0))
83 Eigen::AngleAxis<T> angleAxis(quaternion);
84 *angle = angleAxis.angle();
85 *axis = angleAxis.axis();
89 Eigen::AngleAxis<T> angleAxis(
negate(quaternion));
90 *angle = angleAxis.angle();
91 *axis = angleAxis.axis();
100 template <
typename T,
int QOpt>
105 if (quaternion.w() >= T(0))
107 Eigen::AngleAxis<T> angleAxis(quaternion);
108 return angleAxis.angle();
112 Eigen::AngleAxis<T> angleAxis(
negate(quaternion));
113 return angleAxis.angle();
124 template <
typename T,
int TOpt,
int VOpt>
126 const Eigen::Transform<T, 3, Eigen::Isometry, TOpt>& start,
127 Eigen::Matrix<T, 3, 1, VOpt>* rotationVector)
129 Eigen::Quaternion<T> q1(end.linear());
130 Eigen::Quaternion<T> q2(start.linear());
132 Eigen::Matrix<T, 3, 1, VOpt> axis;
134 *rotationVector = angle * axis;
148 template <
typename T,
int QOpt>
149 inline Eigen::Quaternion<T, QOpt>
interpolate(
const Eigen::Quaternion<T, QOpt>& q0,
150 const Eigen::Quaternion<T, QOpt>& q1, T t)
152 Eigen::Quaternion<T, QOpt> result;
154 result = q0.slerp(t, q1);
162 #endif // SURGSIM_MATH_QUATERNION_H