Go to the documentation of this file.
9 #ifndef mrpt_math_slerp_H
10 #define mrpt_math_slerp_H
44 double cosHalfTheta = q0[0]*q1[0]+q0[1]*q1[1]+q0[2]*q1[2]+q0[3]*q1[3];
51 bool reverse_q1 =
false;
55 cosHalfTheta = -cosHalfTheta;
58 const double halfTheta = acos(cosHalfTheta);
65 for (
int i=0;i<4;i++) q[i] = (1-
t)*q0[i] +
t*q1[i];
66 else for (
int i=0;i<4;i++) q[i] = (1-
t)*q0[i] -
t*q1[i];
69 const double A = sin((1-
t) * halfTheta)/sinHalfTheta;
70 const double B = sin(
t*halfTheta)/sinHalfTheta;
72 for (
int i=0;i<4;i++) q[i] = A*q0[i] + B*q1[i];
73 else for (
int i=0;i<4;i++) q[i] = A*q0[i] - B*q1[i];
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
A class used to store a 3D pose as a translation (x,y,z) and a quaternion (qr,qx,qy,...
void slerp(const CQuaternion< T > &q0, const CQuaternion< T > &q1, const double t, CQuaternion< T > &q)
SLERP interpolation between two quaternions.
const EIGEN_STRONG_INLINE AdjointReturnType t() const
Transpose.
#define ASSERTDEB_(f)
Defines an assertion mechanism - only when compiled in debug.
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
A quaternion, which can represent a 3D rotation as pair , with a real part "r" and a 3D vector ,...
T square(const T x)
Inline function for the square of a number.
Page generated by Doxygen 1.8.17 for MRPT 1.4.0 SVN: at Tue Mar 3 09:15:16 UTC 2020 | | |