从四元数中提取Pitch Roll旋转四元数(去除Yaw)Quaternion Quaternion_get_PRQuat( Quaternion quat ); 用c语言实现
时间: 2023-10-10 09:11:37 浏览: 46
以下是一个用C语言实现提取Pitch和Roll旋转四元数的函数:
```c
#include <math.h>
typedef struct {
float w, x, y, z;
} Quaternion;
Quaternion Quaternion_get_PRQuat(Quaternion quat) {
Quaternion prQuat;
float sqw = quat.w * quat.w;
float sqx = quat.x * quat.x;
float sqy = quat.y * quat.y;
float sqz = quat.z * quat.z;
float unitLength = sqw + sqx + sqy + sqz;
float ysqr = quat.y * quat.y;
float t0 = 2.0f * (quat.w * quat.x + quat.y * quat.z);
float t1 = 1.0f - 2.0f * (sqx + ysqr);
float roll = atan2f(t0, t1);
float t2 = 2.0f * (quat.w * quat.y - quat.z * quat.x);
t2 = t2 > 1.0f ? 1.0f : t2;
t2 = t2 < -1.0f ? -1.0f : t2;
float pitch = asinf(t2);
prQuat.w = sqrtf(unitLength - sqz);
prQuat.x = sqrtf(sqz);
prQuat.y = 0.0f;
prQuat.z = 0.0f;
Quaternion result;
result.w = prQuat.w * cosf(roll / 2.0f) * cosf(pitch / 2.0f);
result.x = prQuat.x * sinf(roll / 2.0f) * cosf(pitch / 2.0f);
result.y = prQuat.x * cosf(roll / 2.0f) * sinf(pitch / 2.0f);
result.z = -prQuat.w * sinf(roll / 2.0f) * sinf(pitch / 2.0f);
return result;
}
```
这个函数接受一个四元数作为输入,然后返回一个新的四元数,其中包含了提取出的Pitch和Roll旋转。函数首先计算输入四元数的平方和,然后使用平方和计算单位长度。接下来,函数计算出Roll和Pitch的值,并将它们存储在变量roll和pitch中。接着,函数将Pitch和Roll转化为旋转四元数,并将它们存储在变量result中。最后,函数返回result作为输出。
请注意,由于Yaw旋转被去除了,因此返回的四元数只包含Pitch和Roll旋转。