C/C++ 四元素轉歐拉角 歐拉角轉四元素 四元素矩陣旋轉 附源碼

最近在玩陀螺儀，分享些心得供大家參考，以下是源碼：

// Conversion_Euler_to_Quaternion.cpp : 定義控制台應用程式的入口點。
#include "stdafx.h"
#include <stdio.h>
#include <string.h>
#include <stdint.h>
#include <math.h>

#define PI       3.14159265358979323846   // pi
/*
一個四元數可以表示為q = w + xi + yj + zk
四元數的乘法定義如下，利用簡單的配置設定律就是了：
q1 * q2 =
(w1*w2 - x1*x2 - y1*y2 - z1*z2) +
(w1*x2 + x1*w2 + y1*z2 - z1*y2) i +
(w1*y2 - x1*z2 + y1*w2 + z1*x2) j +
(w1*z2 + x1*y2 - y1*x2 + z1*w2) k
*/
typedef struct
{
	float w, x, y, z;
}Quat_t;//四元素結構體
typedef struct
{
	float Pitch;
	float Roll;
	float Yaw;
}Euler_t;//歐拉角結構體

//歐拉角轉四元素
//euler_angle：輸入歐拉角
//q1：輸出四元素
int Conversion_Euler_to_Quaternion(Quat_t q1, Euler_t euler_angle)
{
	euler_angle.Yaw = euler_angle.Yaw *  PI / 180;
	euler_angle.Pitch = euler_angle.Pitch * PI / 180;
	euler_angle.Roll = euler_angle.Roll * PI / 180;
	double c1 = acos(euler_angle.Yaw / 2);
	double s1 = asin(euler_angle.Yaw / 2);
	double c2 = acos(euler_angle.Pitch / 2);
	double s2 = asin(euler_angle.Pitch / 2);
	double c3 = acos(euler_angle.Roll / 2);
	double s3 = asin(euler_angle.Roll / 2);
	double c1c2 = c1 * c2;
	double s1s2 = s1 * s2;
	q1.w = (c1c2 * c3 + s1s2 * s3);
	q1.x = (c1c2 * s3 + s1s2 * c3);
	q1.y = (s1 * c2 * c3 + c1 * s2 * s3);
	q1.z = (c1 * s2 * c3 - s1 * c2 * s3);
	return 0;
}
//四元素轉歐拉角
//quat:輸入四元素
//euler:輸出歐拉角
int Conversion_Quaternion_to_Euler(Quat_t *quat, Euler_t *euler)
{
	double q0, q1, q2, q3;
	q0 = quat->w;
	q1 = quat->x;
	q2 = quat->y;
	q3 = quat->z;
	euler->Pitch = (float)(asin(-2 * q1 * q3 + 2 * q0* q2)* 57.3); // pitch
	euler->Roll = (float)(atan2(2 * q2 * q3 + 2 * q0 * q1, -2 * q1 * q1 - 2 * q2* q2 + 1)* 57.3); // roll
	euler->Yaw = (float)(atan2(2 * (q1*q2 + q0*q3), q0*q0 + q1*q1 - q2*q2 - q3*q3) * 57.3);
	return 0;
}
//四元素旋轉
//quat1:輸入原始四元素
//quat2:輸入旋轉向量四元素
//傳回：輸出旋轉後的四元素
int quat_pro(Quat_t *quat1, Quat_t *quat2, Quat_t *quat3)
{
	float w1, x1, y1, z1;
	float w2, x2, y2, z2;
	w2 = quat1->w;
	x2 = quat1->x;
	y2 = quat1->y;
	z2 = quat1->z;

	w1 = quat2->w;
	x1 = quat2->x;
	y1 = quat2->y;
	z1 = quat2->z;

	quat3->w = w1*w2 - x1*x2 - y1*y2 - z1*z2;
	quat3->x = w1*x2 + x1*w2 + y1*z2 - z1*y2;
	quat3->y = w1*y2 - x1*z2 + y1*w2 + z1*x2;
	quat3->z = w1*z2 + x1*y2 - y1*x2 + z1*w2;
	return 0;
}
//驗證測試
int test(void)
{
	Euler_t euler;
	Quat_t quat_rool_90 = { -0.7071,0.7071,0,0 };//rool方向上旋轉90度
	Quat_t quat1 = { 1,0,0,0 };
	Quat_t quat_res = { 0,0,0,0 };
	printf("\r\n源四元素（w,x,y,z） is:%f,%f,%f,%f", quat1.w, quat1.x, quat1.y, quat1.z);
	Conversion_Quaternion_to_Euler(&quat1, &euler);//四元素轉歐拉角
	printf("\r\n源歐拉角（yaw,pitch,roll） is:%f,%f,%f", euler.Yaw, euler.Pitch, euler.Roll);

	printf("\r\n\r\n旋轉向量（w,x,y,z） is:%f,%f,%f,%f", quat_rool_90.w, quat_rool_90.x, quat_rool_90.y, quat_rool_90.z);
	Conversion_Quaternion_to_Euler(&quat_rool_90, &euler);//四元素轉歐拉角
	printf("\r\n旋轉向量轉歐拉角（yaw,pitch,roll） is:%f,%f,%f", euler.Yaw, euler.Pitch, euler.Roll);

	quat_pro(&quat1, &quat_rool_90, &quat_res);
	printf("\r\n\r\n旋轉後的四元素（w,x,y,z）   is:%f,%f,%f,%f", quat_res.w, quat_res.x, quat_res.y, quat_res.z);
	Conversion_Quaternion_to_Euler(&quat_res, &euler);//四元素轉歐拉角
	printf("\r\n旋轉後的歐拉角（yaw,pitch,roll） is:%f,%f,%f", euler.Yaw, euler.Pitch, euler.Roll);
	return 0;
}
int main(void)
{
	test();
	getchar();
	return 0;
}

           

運作結果：