SLERP interpolation
1. Description
SLERP means Spherical Linear Interpolation and represents a very popular technique to interpolate between two 3D rotations in a mathematically sounded way while producing visually smooth paths (see article at Wikipedia).
This screenshot represents an animation of a 3D pose between two given poses (see the example: samples/slerp_demo):
2. C++ Implementation
SLERP is implemented in MRPT in the function mrpt::math::slerp, which is overloaded to support quaternions (pure rotations), but also complete 3D translations+rotations (types CPose3D and CPose3DQuat).
This is the current implementation:
template <typename T>
void slerp(
const CQuaternion<T> & q0,
const CQuaternion<T> & q1,
const double t,
CQuaternion<T> & q)
{
ASSERTDEB_(t>=0 && t<=1)
// See: http://www.euclideanspace.com/maths/algebra/realNormedAlgebra/quaternions/slerp/index.htm
// Angle between q0-q1:
double cosHalfTheta = q0[0]*q1[0]+q0[1]*q1[1]+q0[2]*q1[2]+q0[3]*q1[3];
// if qa=qb or qa=-qb then theta = 0 and we can return qa
if (std::abs(cosHalfTheta) >= 1.0)
{
q = q0;
return;
}
bool reverse_q1 = false;
if (cosHalfTheta < 0) // Always follow the shortest path
{
reverse_q1 = true;
cosHalfTheta = -cosHalfTheta;
}
// Calculate temporary values.
const double halfTheta = acos(cosHalfTheta);
const double sinHalfTheta = std::sqrt(1.0 - square(cosHalfTheta));
// if theta = 180 degrees then result is not fully defined
// we could rotate around any axis normal to qa or qb
if (std::abs(sinHalfTheta) < 0.001)
{
if (!reverse_q1)
for (int i=0;i<4;i++) q[i] = (1-t)*q0[i] + t*q1[i];
else for (int i=0;i<4;i++) q[i] = (1-t)*q0[i] - t*q1[i];
return;
}
const double A = sin((1-t) * halfTheta)/sinHalfTheta;
const double B = sin(t*halfTheta)/sinHalfTheta;
if (!reverse_q1)
for (int i=0;i<4;i++) q[i] = A*q0[i] + B*q1[i];
else for (int i=0;i<4;i++) q[i] = A*q0[i] - B*q1[i];
}