C++ Eigen - 使用四元数实现三维点的坐标变换

关于三维点的旋转的原理，请参考上一篇文章《Eigen C++ 代码实现三维点的旋转原理》

1. 四元数的声明

#include <Eigen/Dense>
#include <iostream>
#include <cmath>
using namespace Eigen;
using namespace std;

#define _MATH_DEFINES_DEFINED

int main()
{
    Quaternionf q1{1,2,3,4}, q2{5,6,7,8}, res;

     cout<<q1 <<endl;
     cout<<q2 <<endl;
}

2. 四元数的相加

#include <Eigen/Dense>
#include <iostream>
#include <cmath>
using namespace Eigen;
using namespace std;

#define _MATH_DEFINES_DEFINED

int main()
{
    Quaternionf q1{1,2,3,4}, q2{5,6,7,8}, res;

     cout<<q1 <<endl;
     cout<<q2 <<endl;
     
    Quaternionf res;
    res.vec() = q1.vec() + q2.vec();
    res.w() = q1.w() + q2.w();

    cout<< res <<endl;
    

}

3. 四元数的数乘（即缩放）

#include <Eigen/Dense>
#include <iostream>
#include <cmath>
using namespace Eigen;
using namespace std;

#define _MATH_DEFINES_DEFINED

int main()
{
    Quaternionf q1{1,2,3,4}, q2{5,6,7,8}, res;

     cout<<q1 <<endl;
     cout<<q2 <<endl;
     
    Quaternionf res;
    res.vec() = 5 * q.vec() ;
    res.w() = 5 * q.w() ;
    cout<< res <<endl;

}

4. 四元数的乘法：方法1

#include <Eigen/Dense>
#include <iostream>
#include <cmath>
using namespace Eigen;
using namespace std;

#define _MATH_DEFINES_DEFINED

int main()
{
    Quaternionf q1{1,2,3,4}, q2{5,6,7,8}, res;

     cout<<q1 <<endl;
     cout<<q2 <<endl;
     
     //乘法方法1： Eigen自带乘法
     cout<< q1*q2<<endl;  //Eigen自带乘法
     

}

5. 四元数的乘法：方法2

#include <Eigen/Dense>
#include <iostream>
#include <cmath>
using namespace Eigen;
using namespace std;

#define _MATH_DEFINES_DEFINED

int main()
{
    Quaternionf q1{1,2,3,4}, q2{5,6,7,8}, res;

     cout<<q1 <<endl;
     cout<<q2 <<endl;
     


    //乘法方法2： 用向量公式1的乘法
    Quaternionf res;
    res.vec() = q1.w() * q2.vec() + q2.w() * q1.vec() + q1.vec().cross(q2.vec());
    res.w() = q1.w() * q2.w() - q1.vec().dot(q2.vec());    
    cout<< res <<endl;
    
}

6. 四元数的乘法：方法3

#include <Eigen/Dense>
#include <iostream>
#include <cmath>
using namespace Eigen;
using namespace std;

#define _MATH_DEFINES_DEFINED

int main()
{
    Quaternionf q1{1,2,3,4}, q2{5,6,7,8}, res;

     cout<<q1 <<endl;
     cout<<q2 <<endl;
  
     //乘法方法3： 用向量公式2的乘法
     Quaternionf res;
    res.w() = q1.w() * q2.w() - q1.x() * q2.x() - q1.y() * q2.y() - q1.z() * q2.z();
    res.x() = q1.w() * q2.x() + q1.x() * q2.w() + q1.y() * q2.z() - q1.z() * q2.y();
    res.y() = q1.w() * q2.y() - q1.x() * q2.z() + q1.y() * q2.w() + q1.z() * q2.x();
    res.z() = q1.w() * q2.z() + q1.x() * q2.y() - q1.y() * q2.x() + q1.z() * q2.w();
  cout<< res <<endl;
}

7. 四元数的旋转点综合示例

#include <Eigen/Dense>
#include <iostream>
#include <cmath>
using namespace Eigen;
using namespace std;
#define _MATH_DEFINES_DEFINED

//四元数相加
Quaternionf qadd(Quaternionf q1, Quaternionf q2)
{
    Quaternionf res;
    res.vec() = q1.vec() + q2.vec();
    res.w() = q1.w() + q2.w();
    return res;
}
//四元数数乘（即缩放）
Quaternionf qscale(Quaternionf q, float scale)
{
    Quaternionf res;
    res.vec() = scale * q.vec() ;
    res.w() = scale * q.w() ;
    return res;
}

//用向量公式的乘法 
Quaternionf qmul(Quaternionf q1, Quaternionf q2)
{
    Quaternionf res;
    res.vec() = q1.w() * q2.vec() + q2.w() * q1.vec() + q1.vec().cross(q2.vec());
    res.w() = q1.w() * q2.w() - q1.vec().dot(q2.vec());
    return res;
}

//用向量公式的乘法 （结果是一样的） 
Quaternionf qmul2(Quaternionf q1, Quaternionf q2) 
{
    Quaternionf res;
    res.w() = q1.w() * q2.w() - q1.x() * q2.x() - q1.y() * q2.y() - q1.z() * q2.z();
    res.x() = q1.w() * q2.x() + q1.x() * q2.w() + q1.y() * q2.z() - q1.z() * q2.y();
    res.y() = q1.w() * q2.y() - q1.x() * q2.z() + q1.y() * q2.w() + q1.z() * q2.x();
    res.z() = q1.w() * q2.z() + q1.x() * q2.y() - q1.y() * q2.x() + q1.z() * q2.w();
    return res;
}

//求逆
Quaternionf qinv(Quaternionf q) 
{
    Quaternionf res;
    res.vec() = - q.vec() ;
    res.w() = q.w();

    float n=q.norm();
    res.vec() /= n*n; 
    res.w() /= n*n; 
    return res;
}

int main()
{
    Quaternionf q1{1,2,3,4}, q2{5,6,7,8}, res;

    // cout<<q1 <<endl;
    // cout<<q2 <<endl;


    // res = qadd(q1, q2); //加法
    // cout<< res <<endl;


    // res =  qscale(q1, 5.0); //数乘
    // cout<< res <<endl;


    // cout<< q1*q2<<endl;  //Eigen自带乘法
    // res = qmul(q1,q2);   //乘法1
    // cout<< res <<endl;
    // res = qmul2(q1,q2);  //乘法2
    // cout<< res <<endl;


    // q1 = q1.normalized();
    // q2 = q2.normalized();


    // cout<< q1.inverse()<<endl;
    // res = qinv(q1);  //求逆
    // cout<< res <<endl;

    //用四元数旋转
    Vector3f point{1.0, 1.0, 0};
    //point = point.normalized();  必须删除- 视频讲解中会删除
    Quaternionf p;
    p.vec() = point;
    p.w() = 0;

    float theta = 45.0 / 180 * 3.14159265358979323846;
    Vector3f axis{0,0,1};

    Quaternionf q{cos(theta/2), sin(theta/2) * axis[0], sin(theta/2) * axis[1], sin(theta/2) * axis[2]};
    cout<<q<<endl;

    Quaternionf p_new = q * p * q.inverse();
    cout<<p_new<<endl;

    Vector3f point_new=p_new.vec();
    cout<<point_new<<endl;
}