四元数插值公式
Slerp ( q 0 , q 1 ; t ) = sin [ ( 1 − t ) Ω ] sin Ω q 0 + sin [ t Ω ] sin Ω q 1 , 0 ≤ t ≤ 1 {\displaystyle \operatorname {Slerp} (q_{0},q_{1};t)={\frac {\sin {[(1-t)\Omega }]}{\sin \Omega }}q_{0}+{\frac {\sin[t\Omega ]}{\sin \Omega }}q_{1}}, 0 ≤ t ≤ 1 Slerp(q0,q1;t)=sinΩsin[(1−t)Ω]q0+sinΩsin[tΩ]q1,0≤t≤1
原理可以看这篇博客
根据公式我们可以比较清晰的分析四元数插值Eigen源码，源码如下所示：
/** \returns the spherical linear interpolation between the two quaternions* \c *this and \a other at the parameter \a t in [0;1].* * This represents an interpolation for a constant motion between \c *this and \a other,* see also http://en.wikipedia.org/wiki/Slerp.*//** * 返回两个四元数之间的球面线性插值* onst Scalar& t : 插值的比例系数，插值时刻距离起始四元数的时间* 占两个四元数时间间隔的多少* other：插值的另一个四元数$q_{1}$**/
template <class Derived>
template <class OtherDerived>
EIGEN_DEVICE_FUNC Quaternion<typename internal::traits<Derived>::Scalar>
QuaternionBase<Derived>::slerp(const Scalar& t, const QuaternionBase<OtherDerived>& other) const
{
// 使用std::acos和std::sinEIGEN_USING_STD_MATH(acos)EIGEN_USING_STD_MATH(sin)// Scalar类型的1，NumTraits<Scalar>::epsilon()，参考这个https://eigen.tuxfamily.org/dox/structEigen_1_1NumTraits.htmlconst Scalar one = Scalar(1) - NumTraits<Scalar>::epsilon();// 两个四元数向量的点积，因为四元数的模长都为1所以点积就是夹角的余弦值Scalar d = this->dot(other);/*** 如果四元数点积的结果是负值（夹角大于90°），那么后面的插值就会在4D球面上绕远路。* 为了解决这个问题，将余弦值取绝对值保证这个旋转走的是最短路径。**/Scalar absD = numext::abs(d);Scalar scale0;Scalar scale1;
// 如果两个四元数的夹角的cos值接近1的话，为了避免除法出问题，直接算极限 lim_{t->0}{sint/t} = 1if(absD>=one){scale0 = Scalar(1) - t;scale1 = t;}else{// theta is the angle between the 2 quaternions// 计算四元数向量之间夹角Scalar theta = acos(absD);Scalar sinTheta = sin(theta);scale0 = sin( ( Scalar(1) - t ) * theta) / sinTheta;scale1 = sin( ( t * theta) ) / sinTheta;}if(d<Scalar(0)) scale1 = -scale1;
// coeffs返回的是以(x,y,z,w)顺序的四元数，因为如果通过double直接对四元数初始化赋值的话它的顺序是(w,x,y,z)return Quaternion<Scalar>(scale0 * coeffs() + scale1 * other.coeffs());
}
