四元数转欧拉角_在线测试工具
在线测试C程序工具
#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度elseangles.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;
}
测试结果