#include <stdio.h>
#include <cmath>
//四元数
struct Quaternion
{
double w, x, y, z;
};
//欧拉角
struct EulerAngles
{
double roll, pitch, yaw;
};
EulerAngles ToEulerAngles(Quaternion q)
{
EulerAngles angles;
// 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 = std::atan2(sinr_cosp, cosr_cosp);
// pitch (y-axis rotation)
double sinp = 2 * (q.w * q.y - q.z * q.x);
if (std::abs(sinp) >= 1)
angles.pitch = std::copysign(M_PI / 2, sinp); // 如果超出范围,使用90度
else
angles.pitch = std::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 = std::atan2(siny_cosp, cosy_cosp);
return angles;
}
int main(void)
{
Quaternion wxyz = {
//输入四元数数据
.w = -0.529,
.x = 0.002,
.y = 0.805,
.z = -0.268,
};
EulerAngles out = ToEulerAngles(wxyz); //转换成欧拉角
printf("r=%f p=%f y=%f ",
out.roll / M_PI * 180.0,
out.pitch / M_PI * 180.0,
out.yaw / M_PI * 180.0);
return 0;
}
测试结果