1 #include <math.h>
2
3
4 void InitMatrix(float Matrix[3][3]);
5 void MultMatrixes(float Matrix1[3][3], float Matrix2[3][3], int type);
6 void xRotateMatrix(float x, float Matrix[3][3], int type);
7 void yRotateMatrix(float y, float Matrix[3][3], int type);
8 void zRotateMatrix(float z, float Matrix[3][3], int type);
9
10
InitMatrix(float Matrix[3][3])11 void InitMatrix(float Matrix[3][3])
12 {
13 Matrix[0][0]=1.0;
14 Matrix[0][1]=0.0;
15 Matrix[0][2]=0.0;
16 Matrix[1][0]=0.0;
17 Matrix[1][1]=1.0;
18 Matrix[1][2]=0.0;
19 Matrix[2][0]=0.0;
20 Matrix[2][1]=0.0;
21 Matrix[2][2]=1.0;
22 }
23
24
MultMatrixes(float Matrix1[3][3],float Matrix2[3][3],int type)25 void MultMatrixes(float Matrix1[3][3], float Matrix2[3][3], int type)
26 {
27 float NewMatrix[3][3];
28 int i, j;
29
30 NewMatrix[0][0]=Matrix1[0][0]*Matrix2[0][0]+Matrix1[0][1]*Matrix2[1][0]+
31 Matrix1[0][2]*Matrix2[2][0];
32 NewMatrix[0][1]=Matrix1[0][0]*Matrix2[0][1]+Matrix1[0][1]*Matrix2[1][1]+
33 Matrix1[0][2]*Matrix2[2][1];
34 NewMatrix[0][2]=Matrix1[0][0]*Matrix2[0][2]+Matrix1[0][1]*Matrix2[1][2]+
35 Matrix1[0][2]*Matrix2[2][2];
36 NewMatrix[1][0]=Matrix1[1][0]*Matrix2[0][0]+Matrix1[1][1]*Matrix2[1][0]+
37 Matrix1[1][2]*Matrix2[2][0];
38 NewMatrix[1][1]=Matrix1[1][0]*Matrix2[0][1]+Matrix1[1][1]*Matrix2[1][1]+
39 Matrix1[1][2]*Matrix2[2][1];
40 NewMatrix[1][2]=Matrix1[1][0]*Matrix2[0][2]+Matrix1[1][1]*Matrix2[1][2]+
41 Matrix1[1][2]*Matrix2[2][2];
42 NewMatrix[2][0]=Matrix1[2][0]*Matrix2[0][0]+Matrix1[2][1]*Matrix2[1][0]+
43 Matrix1[2][2]*Matrix2[2][0];
44 NewMatrix[2][1]=Matrix1[2][0]*Matrix2[0][1]+Matrix1[2][1]*Matrix2[1][1]+
45 Matrix1[2][2]*Matrix2[2][1];
46 NewMatrix[2][2]=Matrix1[2][0]*Matrix2[0][2]+Matrix1[2][1]*Matrix2[1][2]+
47 Matrix1[2][2]*Matrix2[2][2];
48
49 for (i=0; i<3; i++) {
50 for (j=0; j<3; j++) {
51 if (type==1) {
52 Matrix1[i][j]=NewMatrix[i][j];
53 } else {
54 Matrix2[i][j]=NewMatrix[i][j];
55 }
56 }
57 }
58 }
59
60
xRotateMatrix(float x,float Matrix[3][3],int type)61 void xRotateMatrix(float x, float Matrix[3][3], int type)
62 {
63 float RotationMatrix[3][3];
64 float sine, cosine;
65
66 sine=sin(x*(M_PI/180.0));
67 cosine=cos(x*(M_PI/180.0));
68
69 RotationMatrix[0][0]=1.0;
70 RotationMatrix[0][1]=0.0;
71 RotationMatrix[0][2]=0.0;
72 RotationMatrix[1][0]=0.0;
73 RotationMatrix[1][1]=cosine;
74 RotationMatrix[1][2]=sine;
75 RotationMatrix[2][0]=0.0;
76 RotationMatrix[2][1]=-sine;
77 RotationMatrix[2][2]=cosine;
78
79 if (type==1) {
80 MultMatrixes(Matrix, RotationMatrix, 1);
81 } else if (type==2) {
82 MultMatrixes(RotationMatrix, Matrix, 2);
83 }
84 }
85
86
yRotateMatrix(float y,float Matrix[3][3],int type)87 void yRotateMatrix(float y, float Matrix[3][3], int type)
88 {
89 float RotationMatrix[3][3];
90 float sine, cosine;
91
92 sine=sin(y*(M_PI/180.0));
93 cosine=cos(y*(M_PI/180.0));
94
95 RotationMatrix[0][0]=cosine;
96 RotationMatrix[0][1]=0.0;
97 RotationMatrix[0][2]=sine;
98 RotationMatrix[1][0]=0.0;
99 RotationMatrix[1][1]=1.0;
100 RotationMatrix[1][2]=0.0;
101 RotationMatrix[2][0]=-sine;
102 RotationMatrix[2][1]=0.0;
103 RotationMatrix[2][2]=cosine;
104
105 if (type==1) {
106 MultMatrixes(Matrix, RotationMatrix, 1);
107 } else if (type==2) {
108 MultMatrixes(RotationMatrix, Matrix, 2);
109 }
110 }
111
112
zRotateMatrix(float z,float Matrix[3][3],int type)113 void zRotateMatrix(float z, float Matrix[3][3], int type)
114 {
115 float RotationMatrix[3][3];
116 float sine, cosine;
117
118 sine=sin(z*(M_PI/180.0));
119 cosine=cos(z*(M_PI/180.0));
120
121 RotationMatrix[0][0]=cosine;
122 RotationMatrix[0][1]=sine;
123 RotationMatrix[0][2]=0.0;
124 RotationMatrix[1][0]=-sine;
125 RotationMatrix[1][1]=cosine;
126 RotationMatrix[1][2]=0.0;
127 RotationMatrix[2][0]=0.0;
128 RotationMatrix[2][1]=0.0;
129 RotationMatrix[2][2]=1.0;
130
131 if (type==1) {
132 MultMatrixes(Matrix, RotationMatrix, 1);
133 } else if (type==2) {
134 MultMatrixes(RotationMatrix, Matrix, 2);
135 }
136 }
137