|
26
|
1 #include <iostream>
|
|
|
2 #include <math.h>
|
|
|
3 #include "sys.h"
|
|
|
4 using namespace std;
|
|
|
5
|
|
|
6 void noMoreMemory()
|
|
|
7 {
|
|
|
8 cout << "can't allocate memory\n";
|
|
|
9 exit(1);
|
|
|
10 }
|
|
|
11
|
|
|
12 void matrix4x4(float *xyz, float *xyz1, float *xyz2) //xyz[16]
|
|
|
13 {
|
|
|
14 for(int t=0; t<16; t+=4)
|
|
|
15 {
|
|
|
16 for(int i=0; i<4; i++)
|
|
|
17 {
|
|
|
18 xyz[t+i] = xyz1[t]*xyz2[i] + xyz1[t+1]*xyz2[4+i] + xyz1[t+2]*xyz2[8+i] + xyz1[t+3]*xyz2[12+i];
|
|
|
19 }
|
|
|
20 }
|
|
|
21 }
|
|
|
22
|
|
|
23 void get_matrix( float *matrix, float *rxyz, float *txyz, float *stack)
|
|
|
24 {
|
|
|
25 float radx,rady,radz;
|
|
|
26 radx = rxyz[0]*3.14/180;
|
|
|
27 rady = rxyz[1]*3.14/180;
|
|
|
28 radz = rxyz[2]*3.14/180;
|
|
|
29
|
|
|
30 float sinx = sin(radx);
|
|
|
31 float cosx = cos(radx);
|
|
|
32 float siny = sin(rady);
|
|
|
33 float cosy = cos(rady);
|
|
|
34 float sinz = sin(radz);
|
|
|
35 float cosz = cos(radz);
|
|
|
36
|
|
|
37 matrix[0] = cosz*cosy+sinz*sinx*siny;
|
|
|
38 matrix[1] =sinz*cosx;
|
|
|
39 matrix[2] = -cosz*siny+sinz*sinx*cosy;
|
|
|
40 matrix[3] = 0;
|
|
|
41 matrix[4] = -sinz*cosy+cosz*sinx*siny;
|
|
|
42 matrix[5] = cosz*cosx;
|
|
|
43 matrix[6] = sinz*siny+cosz*sinx*cosy;
|
|
|
44 matrix[7] = 0;
|
|
|
45 matrix[8] = cosx*siny;
|
|
|
46 matrix[9] = -sinx;
|
|
|
47 matrix[10] = cosx*cosy;
|
|
|
48 matrix[11] = 0;
|
|
|
49 matrix[12] = txyz[0];
|
|
|
50 matrix[13] = txyz[1];
|
|
|
51 matrix[14] = txyz[2];
|
|
|
52 matrix[15] = 1;
|
|
|
53
|
|
|
54 float m[16];
|
|
|
55
|
|
|
56 for(int i=0; i<16; i++)
|
|
|
57 {
|
|
|
58 m[i] = matrix[i];
|
|
|
59 }
|
|
|
60
|
|
|
61 if(stack)
|
|
|
62 {
|
|
|
63 matrix4x4(matrix, m, stack);
|
|
|
64 }
|
|
|
65
|
|
|
66 }
|
|
|
67
|
|
|
68 void rotate_x(float *xyz, float r)
|
|
|
69 {
|
|
|
70 float rad = r*3.14/180;
|
|
|
71
|
|
|
72 xyz[0] = xyz[0];
|
|
|
73 xyz[1] = xyz[1]*cos(rad) - xyz[2]*sin(rad);
|
|
|
74 xyz[2] = xyz[1]*sin(rad) + xyz[2]*cos(rad);
|
|
|
75 }
|
|
|
76
|
|
|
77 void rotate_y(float *xyz, float r)
|
|
|
78 {
|
|
|
79 float rad = r*3.14/180;
|
|
|
80
|
|
|
81 xyz[0] = xyz[0]*cos(rad) + xyz[2]*sin(rad);
|
|
|
82 xyz[1] = xyz[1];
|
|
|
83 xyz[2] = -xyz[0]*sin(rad) + xyz[2]*cos(rad);
|
|
|
84 }
|
|
|
85
|
|
|
86 void rotate_z(float *xyz, float r)
|
|
|
87 {
|
|
|
88 float rad = r*3.14/180;
|
|
|
89
|
|
|
90 xyz[0] = xyz[0]*cos(rad) - xyz[1]*sin(rad);
|
|
|
91 xyz[1] = xyz[0]*sin(rad) + xyz[1]*cos(rad);
|
|
|
92 xyz[2] = xyz[2];
|
|
|
93 }
|
|
|
94
|
|
|
95 void rotate(float *xyz, float *matrix)
|
|
|
96 {
|
|
|
97 float abc[4];
|
|
|
98 abc[0] = xyz[0];
|
|
|
99 abc[1] = xyz[1];
|
|
|
100 abc[2] = xyz[2];
|
|
|
101 abc[3] = xyz[3];
|
|
|
102
|
|
|
103 for(int i=0; i<4; i++)
|
|
|
104 {
|
|
|
105 //xyz[i] = abc[0]*rot[i] + abc[1]*rot[i+4] + abc[2]*rot[i+8] + abc[3]*rot[i+12];
|
|
|
106 xyz[i] = abc[0]*matrix[i] + abc[1]*matrix[i+4] + abc[2]*matrix[i+8] + abc[3]*matrix[i+12];
|
|
|
107 }
|
|
|
108 }
|
|
|
109
|
|
|
110
|
|
|
111 void translate(float *xyz, float x, float y, float z)
|
|
|
112 {
|
|
|
113 xyz[0] += x;
|
|
|
114 xyz[1] += y;
|
|
|
115 xyz[2] += z;
|
|
|
116 }
|
