四元数姿态插补

四元数基础：

链接：Understanding Quaternions 中文翻译《理解四元数》 – Wyman的原创技术博客 – 恭喜你发现我的小站，撩我请加QQ：234707482、Wechat：_Wyman (qiujiawei.com)

英文原文：Understanding Quaternions | 3D Game Engine Programming (3dgep.com)

四元数SLERP姿态插补：

链接：

四元数插值与均值（姿态平滑） - XXX已失联 - 博客园 (cnblogs.com)

 

#include <iostream>
#include <math.h>
 
void slerp(float starting[4], float ending[4], float result[4], float t )
{
    float cosa = starting[0]*ending[0] + starting[1]*ending[1] + starting[2]*ending[2] + starting[3]*ending[3];
    
    // If the dot product is negative, the quaternions have opposite handed-ness and slerp won't take
    // the shorter path. Fix by reversing one quaternion.
    if ( cosa < 0.0f ) 
    {
        ending[0] = -ending[0];
        ending[1] = -ending[1];
        ending[2] = -ending[2];
        ending[3] = -ending[3];
        cosa = -cosa;
    }
    
    float k0, k1;
    
    // If the inputs are too close for comfort, linearly interpolate
    if ( cosa > 0.9995f ) 
    {
        k0 = 1.0f - t;
        k1 = t;
    }
    else 
    {
        float sina = sqrt( 1.0f - cosa*cosa );
        float a = atan2( sina, cosa );
        k0 = sin((1.0f - t)*a)  / sina;
        k1 = sin(t*a) / sina;
    }
    result[0] = starting[0]*k0 + ending[0]*k1;
    result[1] = starting[1]*k0 + ending[1]*k1;
    result[2] = starting[2]*k0 + ending[2]*k1;
    result[3] = starting[3]*k0 + ending[3]*k1;
}
 
 
 
int main()
{
     
    float p[4] = {1,0,0,0};
    float q[4] = {0.707,0,0,0.707};
    float r[4] = {0};
 
    slerp(p,q,r,0.333);
    std::cout<<r[0]<<","<<r[1]<<","<<r[2]<<","<<r[3]<<std::endl;
 
    slerp(p,q,r,0.667);
    std::cout<<r[0]<<","<<r[1]<<","<<r[2]<<","<<r[3]<<std::endl;
 
    return 0;
}