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:
|
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 |
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]; } |