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