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

UPDATE：使用四元数和欧拉角(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;
}