eigen 空间任意轴旋转

eigen 空间任意轴旋转

从一个以弧度为单位的角度和一个必须归一化的轴构造并初始化角度轴旋转。

int main()
{
    Eigen::AngleAxisd v_r(M_PI/4.0, Eigen::Vector3d(1.0,0,0));//任意轴旋转

    Eigen::Quaterniond q(v_r);
    q.normalize();
    Eigen::Vector3d t(0.1, 2.0, 3.0);

    //Eigen::AngleAxisd delta_r(M_PI / 4.0, Eigen::Vector3d(-0.7548, 0, 0).normalized());//任意轴旋转  转轴必须归一化
    //Eigen::Quaterniond deltaQ(delta_r);
    //deltaQ.normalize();
    //Eigen::Quaterniond q_final = (deltaQ*q).normalized();

    Eigen::Vector3d v0(0, 0, 1.0);
    Eigen::Vector3d v0_t = q * v0 + t;

    Eigen::Vector3d v1 = v0_t - t;
    Eigen::Vector3d vv_x = v1.cross(v0);

    double radian_angle = atan2(v0.cross(v1).norm(), v1.transpose() * v0);
    if (v1.cross(v0).z() < 0) {
        radian_angle = 2 * M_PI - radian_angle;
    }

    Eigen::AngleAxisd delta_r(radian_angle, vv_x.normalized());//任意轴旋转
    Eigen::Quaterniond deltaQ(delta_r);
    deltaQ.normalize();

    Eigen::Quaterniond q_final = (deltaQ*q).normalized();
    Eigen::Vector3d eulerAngle = q_final.matrix().eulerAngles(2, 1, 0);
    Eigen::Vector3d v_final = deltaQ * v1;

    return 0;
}