当前位置：首页 > news >正文

欧拉角、四元数与旋转矩阵的C语言转换实现

news 2025/8/9 5:24:38

欧拉角、四元数与旋转矩阵的C语言转换实现

在3D图形学、机器人控制和航空航天等领域，欧拉角（Euler Angles）、四元数（Quaternions）和旋转矩阵（Rotation Matrix）是最常用的旋转表示方法。本文将提供它们之间的完整C语言转换代码，并解释关键步骤。

1. 基本数据结构定义

首先定义三种旋转表示的数据结构：

#include <math.h>
#include <stdio.h>// 欧拉角（弧度制，Z-Y-X顺序）
typedef struct {float roll;   // X轴旋转 (ϕ)float pitch;  // Y轴旋转 (θ)float yaw;    // Z轴旋转 (ψ)
} EulerAngles;// 四元数 (w + xi + yj + zk)
typedef struct {float w, x, y, z;
} Quaternion;// 3x3旋转矩阵 (列主序)
typedef struct {float m[3][3];
} RotationMatrix;

2. 欧拉角 → 旋转矩阵

按Z-Y-X顺序（航空航天常用）：

RotationMatrix eulerToMatrix(EulerAngles euler) {float c1 = cosf(euler.yaw);    // cos(ψ)float s1 = sinf(euler.yaw);    // sin(ψ)float c2 = cosf(euler.pitch);  // cos(θ)float s2 = sinf(euler.pitch);  // sin(θ)float c3 = cosf(euler.roll);   // cos(ϕ)float s3 = sinf(euler.roll);   // sin(ϕ)RotationMatrix R;R.m[0][0] = c1 * c2;R.m[0][1] = c1 * s2 * s3 - s1 * c3;R.m[0][2] = c1 * s2 * c3 + s1 * s3;R.m[1][0] = s1 * c2;R.m[1][1] = s1 * s2 * s3 + c1 * c3;R.m[1][2] = s1 * s2 * c3 - c1 * s3;R.m[2][0] = -s2;R.m[2][1] = c2 * s3;R.m[2][2] = c2 * c3;return R;
}

3. 旋转矩阵 → 欧拉角

EulerAngles matrixToEuler(RotationMatrix R) {EulerAngles euler;// 计算俯仰角 (θ)euler.pitch = asinf(-R.m[2][0]);// 避免万向锁情况（cosθ ≈ 0）if (cosf(euler.pitch) > 1e-6) {euler.roll = atan2f(R.m[2][1], R.m[2][2]);euler.yaw = atan2f(R.m[1][0], R.m[0][0]);} else {// 万向锁时，滚转和偏航角退化euler.roll = atan2f(-R.m[0][1], R.m[1][1]);euler.yaw = 0.0f;}return euler;
}

4. 四元数 → 旋转矩阵

RotationMatrix quaternionToMatrix(Quaternion q) {float xx = q.x * q.x;float yy = q.y * q.y;float zz = q.z * q.z;float xy = q.x * q.y;float xz = q.x * q.z;float yz = q.y * q.z;float wx = q.w * q.x;float wy = q.w * q.y;float wz = q.w * q.z;RotationMatrix R;R.m[0][0] = 1.0f - 2.0f * (yy + zz);R.m[0][1] = 2.0f * (xy - wz);R.m[0][2] = 2.0f * (xz + wy);R.m[1][0] = 2.0f * (xy + wz);R.m[1][1] = 1.0f - 2.0f * (xx + zz);R.m[1][2] = 2.0f * (yz - wx);R.m[2][0] = 2.0f * (xz - wy);R.m[2][1] = 2.0f * (yz + wx);R.m[2][2] = 1.0f - 2.0f * (xx + yy);return R;
}

5. 旋转矩阵 → 四元数

Quaternion matrixToQuaternion(RotationMatrix R) {Quaternion q;float trace = R.m[0][0] + R.m[1][1] + R.m[2][2];if (trace > 0.0f) {float s = 0.5f / sqrtf(trace + 1.0f);q.w = 0.25f / s;q.x = (R.m[2][1] - R.m[1][2]) * s;q.y = (R.m[0][2] - R.m[2][0]) * s;q.z = (R.m[1][0] - R.m[0][1]) * s;} else {// 选择最大对角元素if (R.m[0][0] > R.m[1][1] && R.m[0][0] > R.m[2][2]) {float s = 2.0f * sqrtf(1.0f + R.m[0][0] - R.m[1][1] - R.m[2][2]);q.w = (R.m[2][1] - R.m[1][2]) / s;q.x = 0.25f * s;q.y = (R.m[0][1] + R.m[1][0]) / s;q.z = (R.m[0][2] + R.m[2][0]) / s;} else if (R.m[1][1] > R.m[2][2]) {float s = 2.0f * sqrtf(1.0f + R.m[1][1] - R.m[0][0] - R.m[2][2]);q.w = (R.m[0][2] - R.m[2][0]) / s;q.x = (R.m[0][1] + R.m[1][0]) / s;q.y = 0.25f * s;q.z = (R.m[1][2] + R.m[2][1]) / s;} else {float s = 2.0f * sqrtf(1.0f + R.m[2][2] - R.m[0][0] - R.m[1][1]);q.w = (R.m[1][0] - R.m[0][1]) / s;q.x = (R.m[0][2] + R.m[2][0]) / s;q.y = (R.m[1][2] + R.m[2][1]) / s;q.z = 0.25f * s;}}// 归一化（可选，但推荐）float norm = sqrtf(q.w*q.w + q.x*q.x + q.y*q.y + q.z*q.z);q.w /= norm;q.x /= norm;q.y /= norm;q.z /= norm;return q;
}

6. 欧拉角 → 四元数

Quaternion eulerToQuaternion(EulerAngles euler) {float cy = cosf(euler.yaw * 0.5f);float sy = sinf(euler.yaw * 0.5f);float cp = cosf(euler.pitch * 0.5f);float sp = sinf(euler.pitch * 0.5f);float cr = cosf(euler.roll * 0.5f);float sr = sinf(euler.roll * 0.5f);Quaternion q;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;
}

7. 四元数 → 欧拉角

EulerAngles quaternionToEuler(Quaternion q) {EulerAngles euler;// Roll (X-axis)float sinr_cosp = 2.0f * (q.w * q.x + q.y * q.z);float cosr_cosp = 1.0f - 2.0f * (q.x * q.x + q.y * q.y);euler.roll = atan2f(sinr_cosp, cosr_cosp);// Pitch (Y-axis)float sinp = 2.0f * (q.w * q.y - q.z * q.x);if (fabs(sinp) >= 1.0f) {euler.pitch = copysignf(M_PI / 2.0f, sinp); // 处理±90°奇异点} else {euler.pitch = asinf(sinp);}// Yaw (Z-axis)float siny_cosp = 2.0f * (q.w * q.z + q.x * q.y);float cosy_cosp = 1.0f - 2.0f * (q.y * q.y + q.z * q.z);euler.yaw = atan2f(siny_cosp, cosy_cosp);return euler;
}

8. 测试示例

int main() {// 初始化一个欧拉角（单位：弧度）EulerAngles euler = {0.1f, 0.2f, 0.3f};// 欧拉角 → 旋转矩阵RotationMatrix R = eulerToMatrix(euler);// 旋转矩阵 → 四元数Quaternion q = matrixToQuaternion(R);// 四元数 → 欧拉角EulerAngles euler_new = quaternionToEuler(q);printf("原始欧拉角: roll=%.3f, pitch=%.3f, yaw=%.3f\n", euler.roll, euler.pitch, euler.yaw);printf("转换后欧拉角: roll=%.3f, pitch=%.3f, yaw=%.3f\n", euler_new.roll, euler_new.pitch, euler_new.yaw);return 0;
}