C# 如何将四元数转换为欧拉角 (XYZ)

Question

我见过很多关于欧拉角和四元数之间转换的问题，但我从未找到任何可行的解决方案。也许你可以帮助我为什么这没有返回正确的值。我需要四元数 (XYZ) 到欧拉角之间的转换，这是我目前使用的代码:

        public static Vector3 Q2E(Quaternion q) // Returns the XYZ in ZXY
        {
            Vector3 angles;

            angles.X = (float)Math.Atan2(2 * (q.W * q.X + q.Y * q.Z), 1 - 2 * (q.X * q.X + q.Y * q.Y));
            if (Math.Abs(2 * (q.W * q.Y - q.Z * q.X)) >= 1) angles.Y = (float)Math.CopySign(Math.PI / 2, 2 * (q.W * q.Y - q.Z * q.X));
            else angles.Y = (float)Math.Asin(2 * (q.W * q.Y - q.Z * q.X));
            angles.Z = (float)Math.Atan2(2 * (q.W * q.Z + q.X * q.Y), 1 - 2 * (q.Y * q.Y + q.Z * q.Z));
            
            return new Vector3()
            {
                X = (float)(180 / Math.PI) * angles.X,
                Y = (float)(180 / Math.PI) * angles.Y,
                Z = (float)(180 / Math.PI) * angles.Z
            };
        }

谢谢大家。

Answer 1

您的标题是从欧拉角到四元数，但您的示例代码“应该”是从四元数转换为欧拉角。

下面是您要查找的内容吗？

public class Program
{
    public static void Main(string[] args)
    {
        EulerAngles e = new();

        e.roll = 0.14;
        e.pitch = 1.21;
        e.yaw = 2.1;
      
        // convert the Euler angles to Quaternions:
        Quaternion q = ToQuaternion(e.yaw,e.pitch,e.roll);
       
        // convert the same Quaternion back to Euler angles:
        EulerAngles n = ToEulerAngles(q);

        // verify conversion
        Console.WriteLine($"Q: {q.x} {q.y} {q.z} {q.w}");
        Console.WriteLine($"E: {n.roll} {n.pitch} {n.yaw}");
    }

    public class Quaternion
    {
        public double w;
        public double x;
        public double y;
        public double z;
    }

    public class EulerAngles
    {
        public double roll; // x
        public double pitch; // y
        public double yaw; // z
    }

    public static Quaternion ToQuaternion(double yaw, double pitch, double roll)
    {
        double cy = Math.Cos(yaw * 0.5);
        double sy = Math.Sin(yaw * 0.5);
        double cp = Math.Cos(pitch * 0.5);
        double sp = Math.Sin(pitch * 0.5);
        double cr = Math.Cos(roll * 0.5);
        double sr = Math.Sin(roll * 0.5);

        Quaternion q = new Quaternion();
        q.w = cr * cp * cy + sr * sp * sy;
        q.x = sr * cp * cy - cr * sp * sy;
        q.y = cr * sp * cy + sr * cp * sy;
        q.z = cr * cp * sy - sr * sp * cy;

        return q;
    }

    public static EulerAngles ToEulerAngles(Quaternion q)
    {
        EulerAngles angles = new();

        // roll (x-axis rotation)
        double sinr_cosp = 2 * (q.w * q.x + q.y * q.z);
        double cosr_cosp = 1 - 2 * (q.x * q.x + q.y * q.y);
        angles.roll = Math.Atan2(sinr_cosp, cosr_cosp);

        // pitch (y-axis rotation)
        double sinp = 2 * (q.w * q.y - q.z * q.x);
        if (Math.Abs(sinp) >= 1)
        {
            angles.pitch = Math.CopySign(Math.PI / 2, sinp);
        }
        else
        {
            angles.pitch = Math.Asin(sinp);
        }

        // yaw (z-axis rotation)
        double siny_cosp = 2 * (q.w * q.z + q.x * q.y);
        double cosy_cosp = 1 - 2 * (q.y * q.y + q.z * q.z);
        angles.yaw = Math.Atan2(siny_cosp, cosy_cosp);

        return angles;
    }
}

更新:对四元数和欧拉角 (Vector3) 使用内置类:

    using System.Numerics;

    public static void Main()
    {
        Vector3 v = new() { X = 0.14F, Y = 1.21F, Z = 2.1F };

        Quaternion q = ToQuaternion(v);
        Vector3 n = ToEulerAngles(q);

        Console.WriteLine($"Q: {q.X} {q.Y} {q.Z} {q.W}");
        Console.WriteLine($"E: {n.X} {n.Y} {n.Z}");
    }

    public static Quaternion ToQuaternion(Vector3 v)
    {

        float cy = (float)Math.Cos(v.Z * 0.5);
        float sy = (float)Math.Sin(v.Z * 0.5);
        float cp = (float)Math.Cos(v.Y * 0.5);
        float sp = (float)Math.Sin(v.Y * 0.5);
        float cr = (float)Math.Cos(v.X * 0.5);
        float sr = (float)Math.Sin(v.X * 0.5);

        return new Quaternion
        {
            W = (cr * cp * cy + sr * sp * sy),
            X = (sr * cp * cy - cr * sp * sy),
            Y = (cr * sp * cy + sr * cp * sy),
            Z = (cr * cp * sy - sr * sp * cy)
        };

    }

    public static Vector3 ToEulerAngles(Quaternion q)
    {
        Vector3 angles = new();

        // roll / x
        double sinr_cosp = 2 * (q.W * q.X + q.Y * q.Z);
        double cosr_cosp = 1 - 2 * (q.X * q.X + q.Y * q.Y);
        angles.X = (float)Math.Atan2(sinr_cosp, cosr_cosp);

        // pitch / y
        double sinp = 2 * (q.W * q.Y - q.Z * q.X);
        if (Math.Abs(sinp) >= 1)
        {
            angles.Y = (float)Math.CopySign(Math.PI / 2, sinp);
        }
        else
        {
            angles.Y = (float)Math.Asin(sinp);
        }

        // yaw / z
        double siny_cosp = 2 * (q.W * q.Z + q.X * q.Y);
        double cosy_cosp = 1 - 2 * (q.Y * q.Y + q.Z * q.Z);
        angles.Z = (float)Math.Atan2(siny_cosp, cosy_cosp);

        return angles;
    }