如何将欧拉角转换为四元数并提取它们？

Question

当尝试从四元数获得额外的欧拉角时，我似乎正在翻转 y 和 z 分量，但我不明白如何或为什么。我在左手坐标系内，看来我正在构造的四元数是正确的（通过here验证），我读到的所有内容都表明我的ToEuler函数是标准的，根据我的输出，我计算错误

输出

In: 90 0 1
Out: 90 1 0
Quaternion (xyzw): 0.70708 0.00617059 0.00617059 0.70708

代码

#include <iostream>

#define PI_ 3.14159265f
float RadiansToDegrees(float Radians) {
    float RadiansToDegrees = (float)(Radians * (180 / PI_));
    return RadiansToDegrees;
}

float DegreesToRadians(float Degrees) {
    float DegreesToRadians = (float)(Degrees * (PI_ / 180));
    return DegreesToRadians;
}

struct Vector3 {
    float x;
    float y;
    float z;

    Vector3() { }
    Vector3(float x_, float y_, float z_) : x(x_), y(y_), z(z_) { }
};

struct Quaternion {
    float x;
    float y;
    float z;
    float w;

    void RotationXYZ(Vector3 rotation) {
        float halfPitch = rotation.x * 0.5f;
        float halfYaw = rotation.y * 0.5f;
        float halfRoll = rotation.z * 0.5f;

        float sinRoll = sin(halfRoll);
        float cosRoll = cos(halfRoll);
        float sinPitch = sin(halfPitch);
        float cosPitch = cos(halfPitch);
        float sinYaw = sin(halfYaw);
        float cosYaw = cos(halfYaw);

        this->x = (cosYaw * sinPitch * cosRoll) - (sinYaw * cosPitch * sinRoll);
        this->y = (sinYaw * cosPitch * cosRoll) + (cosYaw * sinPitch * sinRoll);
        this->z = (cosYaw * cosPitch * sinRoll) + (sinYaw * sinPitch * cosRoll);
        this->w = (cosYaw * cosPitch * cosRoll) - (sinYaw * sinPitch * sinRoll);
    }

    Vector3 ToEuler() const {
        Vector3 returnVal;

        // Compute roll (x-axis rotation)
        float sinr_cosp = 2 * (w * x + y * z);
        float cosr_cosp = 1 - 2 * (x * x + y * y);
        returnVal.x = std::atan2(sinr_cosp, cosr_cosp);

        // Compute pitch (y-axis rotation)
        float sinp = 2 * (w * y + z * x); 
        if(std::fabs(sinp) >= 1)
            returnVal.y = std::copysign(PI_ / 2, sinp); // use 90 degrees if out of range
        else
            returnVal.y = std::asin(sinp);

        // Compute yaw (z-axis rotation)
        float siny_cosp = 2 * (w * z - x * y);
        float cosy_cosp = 1 - 2 * (y * y + z * z);
        returnVal.z = std::atan2(siny_cosp, cosy_cosp);

        return returnVal;
    }
};

void main() {
    Vector3 inRotation(90.0f, 0.0f, 1.0f);

    Quaternion rotationQuat;
    rotationQuat.RotationXYZ(Vector3(DegreesToRadians(inRotation.x), DegreesToRadians(inRotation.y), DegreesToRadians(inRotation.z)));

    Vector3 outRotation = rotationQuat.ToEuler();
    outRotation = Vector3(RadiansToDegrees(outRotation.x), RadiansToDegrees(outRotation.y), RadiansToDegrees(outRotation.z));

    std::cout << "In: " << inRotation.x << " " << inRotation.y << " " << inRotation.z << std::endl;
    std::cout << "Out: " << outRotation.x << " " << outRotation.y << " " << outRotation.z << std::endl;

    std::cout << "Quaternion (xyzw): " << rotationQuat.x << " " << rotationQuat.y << " " << rotationQuat.z << " " << rotationQuat.w << std::endl;
}

Answer 1

这是一个基于this的似乎可行的提案。显然，计算俯仰和偏航时

Quaternion::ToEuler

以及

Quaternion::RotationXYZ

中存在问题

#include <iostream>
#include <cmath>

using value = double;

static constexpr value PI_ = 3.141592653589793238462643f;

value RadiansToDegrees (value Radians) 
{
    return (value)(Radians * (180 / PI_));
}

value DegreesToRadians (value Degrees) 
{
    return (value)(Degrees * (PI_ / 180));
}

struct Vector3 
{
    value x;
    value y;
    value z;
};

struct Quaternion 
{
    value w;
    value x;
    value y;
    value z;

    void RotationXYZ (Vector3 rotation) 
    {
        value halfRoll  = rotation.x * 0.5f;
        value halfPitch = rotation.y * 0.5f;
        value halfYaw   = rotation.z * 0.5f;
    
        value sinRoll  = sin(halfRoll);
        value cosRoll  = cos(halfRoll);
        value sinPitch = sin(halfPitch);
        value cosPitch = cos(halfPitch);
        value sinYaw   = sin(halfYaw);
        value cosYaw   = cos(halfYaw);

        this->w = (cosRoll * cosPitch * cosYaw) + (sinRoll * sinPitch * sinYaw);
        this->x = (sinRoll * cosPitch * cosYaw) - (cosRoll * sinPitch * sinYaw);
        this->y = (cosRoll * sinPitch * cosYaw) + (sinRoll * cosPitch * sinYaw);
        this->z = (cosRoll * cosPitch * sinYaw) - (sinRoll * sinPitch * cosYaw);
    }
    
    Vector3 ToEuler () const 
    {
        Vector3 returnVal;

        // Compute roll (x-axis rotation)
        value sinr_cosp = 2 * (w * x + y * z);
        value cosr_cosp = 1 - 2 * (x * x + y * y);
        returnVal.x = std::atan2(sinr_cosp, cosr_cosp);

        // Compute pitch (y-axis rotation)
        value sinp = std::sqrt(1 + 2 * (w * y - x * z));
        value cosp = std::sqrt(1 - 2 * (w * y - x * z));
        returnVal.y = 2 * std::atan2(sinp, cosp) - M_PI / 2;

        // Compute yaw (z-axis rotation)
        value siny_cosp = 2 * (w * z + x * y);
        value cosy_cosp = 1 - 2 * (y * y + z * z);
        returnVal.z = std::atan2(siny_cosp, cosy_cosp);

        return returnVal;
    }
};

int main() 
{
    Vector3 inRotation {90.0f, 0.0f, 1.0f};

    Quaternion rotationQuat;
    rotationQuat.RotationXYZ (
        Vector3{
            DegreesToRadians(inRotation.x), 
            DegreesToRadians(inRotation.y), 
            DegreesToRadians(inRotation.z)
        }
    );

    Vector3 outRotation = rotationQuat.ToEuler();
    
    outRotation = Vector3 {
        RadiansToDegrees(outRotation.x), 
        RadiansToDegrees(outRotation.y), 
        RadiansToDegrees(outRotation.z)
    };

    std::cout << "In : " << inRotation.x  << " " << inRotation.y  << " " << inRotation.z  << std::endl;
    std::cout << "Out: " << outRotation.x << " " << outRotation.y << " " << outRotation.z << std::endl;

    std::cout 
        << "Quaternion (xyzw): "
        << rotationQuat.x << " " 
        << rotationQuat.y << " " 
        << rotationQuat.z << " " 
        << rotationQuat.w 
        << std::endl;
}

演示