C# How to convert Quaternions to euler angles (XYZ)

user_14188654 0
Viewed 1903

I have seen many questions to conversions between Euler angles and Quaternion, but I never found any working solution. Maybe you can help me why this is not returning the right values. I need the conversion between Quaternions(XYZ) to Euler angles and this is the code I am currently using:

        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
            };
        }

Thx everyone.

euler-angles quaternions math c#
1 Answers

Your title is from Euler angles to Quaternions but you sample code is 'supposed' to convert from Quaternion to Euler.

Is below what you are looking for?

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;
    }
}

UPDATE: Using built-in classes for Quaternion and Euler 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;
    }
user_5607840
Related