The Mobile Robot Programming Toolkit

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):

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:

{syntaxhighlighter brush:cpp}
template
void slerp(
const CQuaternion & q0,
const CQuaternion & q1,
const double t,
CQuaternion & q)
{
ASSERTDEB_(t>=0 && t<=1)
// Angle between q0-q1:
const 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;
}
// 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)
{
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;
for (int i=0;i<4;i++)
q[i] = A*q0[i] + B*q1[i];
}
{/syntaxhighlighter}

References

• http://www.euclideanspace.com/maths/algebra/realNormedAlgebra/quaternion...
• http://portal.acm.org/citation.cfm?doid=325334.325242