Matrix4x4矩阵变换、欧拉角转四元数、角度转弧度

Matrix4x4

 1 // 重置矩阵
 2 void MatrixIdentity(float m[4][4])
 3 {
 4     m[0][0] = 1; m[0][1] = 0; m[0][2] = 0; m[0][3] = 0;
 5     m[1][0] = 0; m[1][1] = 1; m[1][2] = 0; m[1][3] = 0;
 6     m[2][0] = 0; m[2][1] = 0; m[2][2] = 1; m[2][3] = 0;
 7     m[3][0] = 0; m[3][1] = 0; m[3][2] = 0; m[3][3] = 1;
 8 }
 9
10 // 缩放变换
11 void MatrixScale(float m[4][4], const float s[3])
12 {
13     m[0][0] = s[0];
14     m[1][1] = s[1];
15     m[2][2] = s[2];
16 }
17
18 // 平移变换
19 void MatrixTranslation(float m[4][4], const float t[3])
20 {
21     m[3][0] = t[0];
22     m[3][1] = t[1];
23     m[3][2] = t[2];
24 }
25
26 // 旋转变换
27 void MatrixRotation(float m[4][4], float a, int x, int y, int z)
28 {
29     if (x)
30     {
31         m[1][1] =  cos(a);
32         m[1][2] =  sin(a);
33         m[2][1] = -sin(a);
34         m[2][2] =  cos(a);
35     }
36     if (y)
37     {
38         m[0][0] =  cos(a);
39         m[0][2] = -sin(a);
40         m[2][0] =  sin(a);
41         m[2][2] =  cos(a);
42     }
43     if (z)
44     {
45         m[0][0] =  cos(a);
46         m[0][1] =  sin(a);
47         m[1][0] = -sin(a);
48         m[1][1] =  cos(a);
49     }
50 }
51
52 // 矩阵相乘
53 void MatrixConcat(const float in1[4][4], const float in2[4][4], float out[4][4])
54 {
55     out[0][0] = in1[0][0]*in2[0][0] + in1[0][1]*in2[1][0] + in1[0][2]*in2[2][0] + in1[0][3]*in2[3][0];
56     out[0][1] = in1[0][0]*in2[0][1] + in1[0][1]*in2[1][1] + in1[0][2]*in2[2][1] + in1[0][3]*in2[3][1];
57     out[0][2] = in1[0][0]*in2[0][2] + in1[0][1]*in2[1][2] + in1[0][2]*in2[2][2] + in1[0][3]*in2[3][2];
58     out[0][3] = in1[0][0]*in2[0][3] + in1[0][1]*in2[1][3] + in1[0][2]*in2[2][3] + in1[0][3]*in2[3][3];
59
60     out[1][0] = in1[1][0]*in2[0][0] + in1[1][1]*in2[1][0] + in1[1][2]*in2[2][0] + in1[1][3]*in2[3][0];
61     out[1][1] = in1[1][0]*in2[0][1] + in1[1][1]*in2[1][1] + in1[1][2]*in2[2][1] + in1[1][3]*in2[3][1];
62     out[1][2] = in1[1][0]*in2[0][2] + in1[1][1]*in2[1][2] + in1[1][2]*in2[2][2] + in1[1][3]*in2[3][2];
63     out[1][3] = in1[1][0]*in2[0][3] + in1[1][1]*in2[1][3] + in1[1][2]*in2[2][3] + in1[1][3]*in2[3][3];
64
65     out[2][0] = in1[2][0]*in2[0][0] + in1[2][1]*in2[1][0] + in1[2][2]*in2[2][0] + in1[2][3]*in2[3][0];
66     out[2][1] = in1[2][0]*in2[0][1] + in1[2][1]*in2[1][1] + in1[2][2]*in2[2][1] + in1[2][3]*in2[3][1];
67     out[2][2] = in1[2][0]*in2[0][2] + in1[2][1]*in2[1][2] + in1[2][2]*in2[2][2] + in1[2][3]*in2[3][2];
68     out[2][3] = in1[2][0]*in2[0][3] + in1[2][1]*in2[1][3] + in1[2][2]*in2[2][3] + in1[2][3]*in2[3][3];
69
70     out[3][0] = in1[3][0]*in2[0][0] + in1[3][1]*in2[1][0] + in1[3][2]*in2[2][0] + in1[3][3]*in2[3][0];
71     out[3][1] = in1[3][0]*in2[0][1] + in1[3][1]*in2[1][1] + in1[3][2]*in2[2][1] + in1[3][3]*in2[3][1];
72     out[3][2] = in1[3][0]*in2[0][2] + in1[3][1]*in2[1][2] + in1[3][2]*in2[2][2] + in1[3][3]*in2[3][2];
73     out[3][3] = in1[3][0]*in2[0][3] + in1[3][1]*in2[1][2] + in1[3][2]*in2[2][3] + in1[3][3]*in2[3][3];
74 }
75
76 // 应用变换
77 void VectorTransform(const float in[3], const float m[4][4], float out[3])
78 {
79     out[0] = in[0]*m[0][0] + in[1]*m[1][0] + in[2]*m[2][0] + 1*m[3][0];
80     out[1] = in[0]*m[0][1] + in[1]*m[1][1] + in[2]*m[2][1] + 1*m[3][1];
81     out[2] = in[0]*m[0][2] + in[1]*m[1][2] + in[2]*m[2][2] + 1*m[3][2];
82 }

欧拉角转四元数、四元数转欧拉角

 1 // 欧拉角转四元数
 2 void AngleQuat(const float in[3], float out[4])
 3 {
 4     out[3] = cos(in[0]/2.0)*cos(in[1]/2.0)*cos(in[2]/2.0) + sin(in[0]/2.0)*sin(in[1]/2.0)*sin(in[2]/2.0);
 5     out[0] = sin(in[0]/2.0)*cos(in[1]/2.0)*cos(in[2]/2.0) - cos(in[0]/2.0)*sin(in[1]/2.0)*sin(in[2]/2.0);
 6     out[1] = cos(in[0]/2.0)*sin(in[1]/2.0)*cos(in[2]/2.0) + sin(in[0]/2.0)*cos(in[1]/2.0)*sin(in[2]/2.0);
 7     out[2] = cos(in[0]/2.0)*cos(in[1]/2.0)*sin(in[2]/2.0) - sin(in[0]/2.0)*sin(in[1]/2.0)*cos(in[2]/2.0);
 8 }
 9
10 // 四元数转欧拉角
11 void QuatAngle(const float in[4], float out[3])
12 {
13     out[0] = atan2(2*(in[3]*in[0]+in[1]*in[2]), 1-2*(in[0]*in[0]+in[1]*in[1]));
14     out[1] = atan(2*(in[3]*in[1]-in[2]*in[0]));
15     out[2] = atan2(2*(in[3]*in[2]+in[0]*in[1]), 1-2*(in[1]*in[1]+in[2]*in[2]));
16 }

角度转弧度、弧度转角度

 1 // 角度转弧度
 2 void DegreeRadian(const float in[3], float out[3])
 3 {
 4     out[0] = in[0] * (M_PI / 180.0);
 5     out[1] = in[1] * (M_PI / 180.0);
 6     out[2] = in[2] * (M_PI / 180.0);
 7 }
 8
 9 // 弧度转角度
10 void RadianDegree(const float in[3], float out[3])
11 {
12     out[0] = in[0] * (180.0 / M_PI);
13     out[1] = in[1] * (180.0 / M_PI);
14     out[2] = in[2] * (180.0 / M_PI);
15 }

四元数转矩阵、矩阵转四元数

 1 // 四元数转矩阵
 2 void QuatMatrix(const float in[4], float out[4][4])
 3 {
 4     float xx, xy, xz, xw;
 5     float yy, yz, yw;
 6     float zz, zw;
 7
 8     xx = in[0]*in[0];
 9     xy = in[0]*in[1];
10     xz = in[0]*in[2];
11     xw = in[0]*in[3];
12     yy = in[1]*in[1];
13     yz = in[1]*in[2];
14     yw = in[1]*in[3];
15     zz = in[2]*in[2];
16     zw = in[2]*in[3];
17
18     out[0][0] = 1-2*(yy+zz);
19     out[0][1] =   2*(xy-zw);
20     out[0][2] =   2*(xz+yw);
21     out[0][3] =   0;
22     out[1][0] =   2*(xy+zw);
23     out[1][1] = 1-2*(xx+zz);
24     out[1][2] =   2*(yz-xw);
25     out[1][3] =   0;
26     out[2][0] =   2*(xz-yw);
27     out[2][1] =   2*(yz+xw);
28     out[2][2] = 1-2*(xx+yy);
29     out[2][3] =   0;
30     out[3][0] =   0;
31     out[3][1] =   0;
32     out[3][2] =   0;
33     out[3][3] =   1;
34 }
35
36 // 矩阵转四元数
37 void MatrixQuat(const float in[4][4], float out[4])
38 {
39     float s;
40
41     if (in[0][0]>in[1][1] && in[0][0]>in[2][2])
42     {
43         s = 2*sqrt(1.0+in[0][0]-in[1][1]-in[2][2]);
44         out[0] = 0.25*s;
45         out[1] = (in[1][0]+in[0][1])/s;
46         out[2] = (in[0][2]+in[2][0])/s;
47         out[3] = (in[2][1]-in[1][2])/s;
48     }
49     else if (in[1][1]>in[2][2])
50     {
51         s = 2*sqrt(1.0+in[1][1]-in[0][0]-in[2][2]);
52         out[0] = (in[1][0]+in[0][1])/s;
53         out[1] = 0.25*s;
54         out[2] = (in[2][1]+in[1][2])/s;
55         out[3] = (in[0][2]-in[2][0])/s;
56     }
57     else
58     {
59         s = 2*sqrt(1.0+in[2][2]-in[0][0]-in[1][1]);
60         out[0] = (in[0][2]+in[2][0])/s;
61         out[1] = (in[2][1]+in[1][2])/s;
62         out[2] = 0.25*s;
63         out[3] = (in[1][0]-in[0][1])/s;
64     }
65 }

欧拉角转矩阵、矩阵转欧拉角

 1 // 欧拉角转矩阵
 2 void AngleMatrix(const float in[3], float out[4][4])
 3 {
 4     float cx, sx, cy, sy, cz, sz;
 5     float yx, yy;
 6
 7     cx = cos(in[0]);
 8     sx = sin(in[0]);
 9     cy = cos(in[1]);
10     sy = sin(in[1]);
11     cz = cos(in[2]);
12     sz = sin(in[2]);
13
14     yx = sy*cx;
15     yy = sy*sx;
16
17     out[0][0] =  cy*cz;
18     out[0][1] = -cy*sz;
19     out[0][2] =     sy;
20     out[0][3] =  0;
21     out[1][0] =  yy*cz+cx*sz;
22     out[1][1] = -yy*sz+cx*cz;
23     out[1][2] = -sx;
24     out[1][3] =  0;
25     out[2][0] = -yx*cz+sx*sz;
26     out[2][1] =  yx*sz+sx*cz;
27     out[2][2] =  cx*cy;
28     out[2][3] =  0;
29     out[3][0] =  0;
30     out[3][1] =  0;
31     out[3][2] =  0;
32     out[3][3] =  1;
33 }
34
35 // 矩阵转欧拉角
36 void MatrixAngle(const float in[4][4], float out[3])
37 {
38     float c;
39     float tx, ty;
40
41     out[1] = asin(in[0][2]);
42     c = cos(out[1]);
43     if (fabs(c)>0.005)
44     {
45         tx =  in[2][2]/c;
46         ty = -in[1][2]/c;
47         out[0] = atan2(ty, tx);
48         tx =  in[0][0]/c;
49         ty = -in[0][1]/c;
50         out[2] = atan2(ty, tx);
51     }
52     else
53     {
54         out[0] = 0;
55         tx = in[1][1];
56         ty = in[1][0];
57         out[2] = atan2(ty, tx);
58     }
59 }

时间： 2025-01-16 04:15:48

Matrix4x4矩阵变换、欧拉角转四元数、角度转弧度的相关文章

飞控姿态解算中，欧拉角与四元数之间的转换

//欧拉角转四元数 void cjx_from_euler(float roll, float pitch, float yaw) { float cr2 = cosf(roll*0.5f); float cp2 = cosf(pitch*0.5f); float cy2 = cosf(yaw*0.5f); float sr2 = sinf(roll*0.5f); float sp2 = sinf(pitch*0.5f); float sy2 = sinf(yaw*0.5f); q1 = cr2

【3D计算机图形学】变换矩阵、欧拉角、四元数

[3D计算机图形学]变换矩阵.欧拉角.四元数旋转矩阵.欧拉角.四元数主要用于:向量的旋转.坐标系之间的转换.角位移计算.方位的平滑插值计算. 一.变换矩阵: 首先要区分旋转矩阵和变换矩阵: 旋转矩阵:向量绕某一个轴旋转,用3x3的矩阵表示. 变换矩阵:向量的移动.旋转.缩放,用4x4的矩阵表示. 这里额外补充一个知识,就是三维坐标变换是用4x4矩阵(采用齐次坐标)而不是3x3矩阵的原因是:统一平移和缩放(本来是向量加法来描述)为矩阵乘法的形式来计算.所以旋转矩阵也扩展为4x4矩阵,这样一来

欧拉角转四元数

二.欧拉角欧拉角指的是:以世界坐标系为参考坐标系(一定记住是世界坐标系),使用x,y,z三个值来分别表示绕(世界的)x轴.y轴.z轴旋转的角度量值.其取值是在[0, 360]间.一般用roll, pitch, yaw来表示这些分量的旋转值.因为是以世界坐标系为参考坐标系,因此每一次的旋转都不会影响到后续的旋转转轴.即:它无法表示任意轴的旋转. 一直以为 x轴是pitch y是yaw ,z 是roll的,今天受到教训了,搞了很久原来,x轴对应的roll,y轴对应的是pitch ,z

三维空间旋转（欧拉角、四元数、旋转矩阵）

姿态角(欧拉角) 姿态角即RPY(roll, pitch,yaw)又叫欧拉角,是由三个角组成的. 俯仰角(pitch) 翻滚角(roll) 偏航角(yaw) 其中最直观的就是其绕刚体自身的X.Y.Z三个轴分别旋转的角度,这就是欧拉角(Euler Angle)表示方法. 需要注意的是,欧拉角的表示方式里,yaw.pitch.roll的顺序对旋转的结果是有影响的. 给定一组欧拉角角度值,比如yaw=45度,pitch=30度,roll=60度,按照yaw-pitch-roll的顺序旋转和按照yaw-

Unity复杂的旋转-欧拉角和四元数

一.欧拉角欧拉角最容易表示,用三个变量X,Y,Z可以直观的表示绕着某个轴的旋转角度. 在Unity里就是Transform组件的Rotation里的X Y Z三个变量代表了欧拉角二.四元数四元数相比于欧拉角就比较复杂了,由四个变量组成(在Unity中称为X,Y,Z,W),但是这些变量的值不代表旋转角度,所以可能给你一个向量(0.7,0,0,0.7)你并不知道实际旋转的角度,当然四元数的详细解释需要数学方面的深入研究,有兴趣的可以自行查找有关资料因为在Unity没有可视化界面可以调整四元数(因

旋转矩阵、欧拉角、四元数理论及其转换关系

博客转载自:http://blog.csdn.net/lql0716/article/details/72597719 1. 概述旋转矩阵.欧拉角.四元数主要用于表示坐标系中的旋转关系,它们之间的转换关系可以减小一些算法的复杂度. 本文主要介绍了旋转矩阵.欧拉角.四元数的基本理论及其之间的转换关系. 2.原理 2.1 旋转矩阵对于两个三维点 p1(x1,y1,z1),p2(x2,y2,z2),由点 p1 经过旋转矩阵 R 旋转到 p2,则有注:旋转矩阵为正交矩阵RRT=E 任意旋转矩阵:

数学基础：角度，弧度，三角函数

角度与弧度角度概念: 公共端点的两条射线组成的图形叫做角,这个公共点叫做角的顶点,这两条射线叫做角的边. 在平面内,一条射线绕它的端点旋转有两个相反的方向,逆时针旋转的角叫做正角,顺时针旋转的角叫做负角.没有旋转叫做零角. 弧度概念: 角是由射线绕它的端点旋转而形成的,在旋转的过程中,射线上的任一点必然形成一条圆弧.不同点形成的圆弧的长度是不同的,但同一圆心角所对的弧与它所在圆的半径的比值是固定的,所以可以通过圆的半径作为单位去度量弧. 角度制: 把圆周360等分,一分是1度,60分等于1度,

iOS角度与弧度转换

在iOS中图片的旋转单位为弧度而不是角度,所以经常会在两者之间进行转换弧度转角度 #define RADIANS_TO_DEGREES(radians) ((radians) * (180.0 / M_PI)) 角度转弧度 #define DEGREES_TO_RADIANS(angle) ((angle) / 180.0 * M_PI) 摘自:角度与弧度的转换  角度弧度定义 " 弧度"和"度"是度量角大小的两种不同的单位.

vs中使用M_PI的问题及解决角度转弧度&根据弧度计算圆周上点的坐标的方法

M_PI 是一个宏定义,圆周率的定义 C/C++ code #define M_PI 3.14159265358979323846 此宏定义和编译器有关,TC中M_PI宏就定义在<math.h>里面. 但vc的<math.h>中没有了M_PI的宏定义.因此编译时会出错,提示:“M_PI”: 未声明的标识符. 解决方法:自己定义. 角度转弧度: #define AngleToRadian(angle) (M_PI/180.0f)*angle 以正东面为0度起点