|
539
|
1 #include <stdlib.h>
|
|
|
2 #include <iostream>
|
|
|
3 #include <string.h>
|
|
|
4 #include <math.h>
|
|
|
5 #include "sys.h"
|
|
|
6 using namespace std;
|
|
|
7
|
|
|
8 void noMoreMemory()
|
|
|
9 {
|
|
|
10 cout << "can't allocate memory\n";
|
|
|
11 exit(1);
|
|
|
12 }
|
|
|
13
|
|
|
14 void
|
|
|
15 transMatrix(float *m0, float *m1, float *v)
|
|
|
16 {
|
|
|
17 memcpy(m0, m1, sizeof(float)*16);
|
|
|
18
|
|
|
19 m0[12] = m1[12] + v[0];
|
|
|
20 m0[13] = m1[13] + v[1];
|
|
|
21 m0[14] = m1[14] + v[2];
|
|
|
22 m0[15] = m1[15] + v[3];
|
|
|
23 }
|
|
|
24
|
|
|
25 void
|
|
|
26 unitMatrix(float *m)
|
|
|
27 {
|
|
|
28 bzero(m, sizeof(float)*16);
|
|
|
29
|
|
|
30 m[0] = 1.0f;
|
|
|
31 m[5] = 1.0f;
|
|
|
32 m[10] = 1.0f;
|
|
|
33 m[15] = 1.0f;
|
|
|
34 }
|
|
|
35
|
|
|
36 void
|
|
|
37 inversMatrix(float *m0, float *m1)
|
|
|
38 {
|
|
|
39 float m[16];
|
|
|
40
|
|
|
41 for (int i = 0; i < 4; i++) {
|
|
|
42 for (int j = 0; j < 4; j++) {
|
|
|
43 m[i*4+j] = m1[j*4+i];
|
|
|
44 }
|
|
|
45 }
|
|
|
46
|
|
|
47 m[12] = -(m1[12]*m[0] + m1[13]*m[1] + m1[14]*m[2]);
|
|
|
48 m[13] = -(m1[12]*m[4] + m1[13]*m[5] + m1[14]*m[6]);
|
|
|
49 m[14] = -(m1[12]*m[8] + m1[13]*m[9] + m1[14]*m[10]);
|
|
|
50 m[3] = m[7] = m[11] = 0.0f;
|
|
|
51 m[15] = 1.0f;
|
|
|
52
|
|
|
53 memcpy(m0, m, sizeof(float)*16);
|
|
|
54 }
|
|
|
55
|
|
|
56 /**
|
|
|
57 * マトリックス m にベクトル v1 を右から乗算して、v0に与える
|
|
|
58 * @param[out] v0 output vector (float[4])
|
|
|
59 * @param[in] v1 input vector (float[4])
|
|
|
60 * @param[in] m matrix (float[16])
|
|
|
61 */
|
|
|
62 void
|
|
|
63 applyMatrix(float *v0, float *m, float *v1)
|
|
|
64 {
|
|
|
65 for (int i = 0; i < 4; i++) {
|
|
|
66 v0[i] = v1[0]*m[i] + v1[1]*m[i+4] + v1[2]*m[i+8] + v1[3]*m[i+12];
|
|
|
67 }
|
|
|
68 }
|
|
|
69
|
|
|
70 /**
|
|
|
71 * ベクトルの正規化
|
|
|
72 *
|
|
|
73 * @param[out] v0 output vector
|
|
|
74 * @param[in] v1 input vector
|
|
|
75 */
|
|
|
76 void
|
|
|
77 normalize(float *v0, float *v1)
|
|
|
78 {
|
|
|
79 float norm, dnorm;
|
|
|
80
|
|
|
81 norm = sqrt(v1[0]*v1[0] + v1[1]*v1[1] + v1[2]*v1[2]);
|
|
|
82 if (norm > 0) {
|
|
|
83 dnorm = 1.0/norm;
|
|
|
84 v0[0] = v1[0]*dnorm;
|
|
|
85 v0[1] = v1[1]*dnorm;
|
|
|
86 v0[2] = v1[2]*dnorm;
|
|
|
87 v0[3] = v1[3]*dnorm;
|
|
|
88 }
|
|
|
89 }
|
|
|
90
|
|
|
91 /**
|
|
|
92 * ベクトルの減算 v0 = v1 - v2
|
|
|
93 */
|
|
|
94 void
|
|
|
95 subVector(float *v0, float *v1, float *v2)
|
|
|
96 {
|
|
|
97 v0[0] = v1[0] - v2[0];
|
|
|
98 v0[1] = v1[1] - v2[1];
|
|
|
99 v0[2] = v1[2] - v2[2];
|
|
|
100 v0[3] = v1[3] - v2[3];
|
|
|
101 }
|
|
|
102
|
|
|
103 /**
|
|
|
104 * ベクトルの外積 v0 = v1 x v2
|
|
|
105 */
|
|
|
106 void
|
|
|
107 outerProduct(float *v0, float *v1, float *v2)
|
|
|
108 {
|
|
|
109 v0[0] = v1[1] * v2[2] - v1[2] * v2[1];
|
|
|
110 v0[1] = v1[2] * v2[0] - v1[0] * v2[2];
|
|
|
111 v0[2] = v1[0] * v2[1] - v1[1] * v2[0];
|
|
|
112 v0[3] = 0;
|
|
|
113 }
|
|
|
114
|
|
|
115 void
|
|
|
116 transposeMatrix(float *m0, float *m1)
|
|
|
117 {
|
|
|
118 float t[16];
|
|
|
119
|
|
|
120 for (int i = 0; i < 4; i++) {
|
|
|
121 for (int j = 0; j < 4; j++) {
|
|
|
122 t[i*4+j] = m1[j*4+i];
|
|
|
123 }
|
|
|
124 }
|
|
|
125
|
|
|
126 memcpy(m0, t, sizeof(float)*16);
|
|
|
127 }
|
|
|
128
|
|
|
129 /**
|
|
|
130 * ベクトルの内積 f = v0 * v1
|
|
|
131 */
|
|
|
132 float
|
|
|
133 innerProduct(float *v0, float *v1)
|
|
|
134 {
|
|
|
135 return (v0[0]*v1[0] + v0[1]*v1[1] + v0[2]*v1[2]);
|
|
|
136 }
|
|
|
137
|
|
|
138 void matrix4x4(float *xyz, float *xyz1, float *xyz2) //xyz[16]
|
|
|
139 {
|
|
|
140 for(int t=0; t<16; t+=4)
|
|
|
141 {
|
|
|
142 for(int i=0; i<4; i++)
|
|
|
143 {
|
|
|
144 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];
|
|
|
145 }
|
|
|
146 }
|
|
|
147 }
|
|
|
148
|
|
|
149 void get_matrix( float *matrix, float *rxyz, float *txyz, float *stack)
|
|
|
150 {
|
|
|
151 float radx,rady,radz;
|
|
|
152 radx = rxyz[0]*3.14/180;
|
|
|
153 rady = rxyz[1]*3.14/180;
|
|
|
154 radz = rxyz[2]*3.14/180;
|
|
|
155
|
|
|
156 float sinx = sin(radx);
|
|
|
157 float cosx = cos(radx);
|
|
|
158 float siny = sin(rady);
|
|
|
159 float cosy = cos(rady);
|
|
|
160 float sinz = sin(radz);
|
|
|
161 float cosz = cos(radz);
|
|
|
162
|
|
|
163 /* View Transform */
|
|
|
164 matrix[0] = cosz*cosy+sinz*sinx*siny;
|
|
|
165 matrix[1] = sinz*cosx;
|
|
|
166 matrix[2] = -cosz*siny+sinz*sinx*cosy;
|
|
|
167 matrix[3] = 0;
|
|
|
168 matrix[4] = -sinz*cosy+cosz*sinx*siny;
|
|
|
169 matrix[5] = cosz*cosx;
|
|
|
170 matrix[6] = sinz*siny+cosz*sinx*cosy;
|
|
|
171 matrix[7] = 0;
|
|
|
172 matrix[8] = cosx*siny;
|
|
|
173 matrix[9] = -sinx;
|
|
|
174 matrix[10] = cosx*cosy;
|
|
|
175 matrix[11] = 0;
|
|
|
176 matrix[12] = txyz[0];
|
|
|
177 matrix[13] = txyz[1];
|
|
|
178 matrix[14] = txyz[2];
|
|
|
179 matrix[15] = 1;
|
|
|
180
|
|
|
181 float m[16];
|
|
|
182
|
|
|
183 for(int i=0; i<16; i++)
|
|
|
184 {
|
|
|
185 m[i] = matrix[i];
|
|
|
186 }
|
|
|
187
|
|
|
188 if(stack)
|
|
|
189 {
|
|
|
190 matrix4x4(matrix, m, stack);
|
|
|
191 }
|
|
|
192
|
|
|
193 }
|
|
|
194
|
|
|
195 void rotate_x(float *xyz, float r)
|
|
|
196 {
|
|
|
197 float rad = r*3.14/180;
|
|
|
198
|
|
|
199 xyz[0] = xyz[0];
|
|
|
200 xyz[1] = xyz[1]*cos(rad) - xyz[2]*sin(rad);
|
|
|
201 xyz[2] = xyz[1]*sin(rad) + xyz[2]*cos(rad);
|
|
|
202 }
|
|
|
203
|
|
|
204 void rotate_y(float *xyz, float r)
|
|
|
205 {
|
|
|
206 float rad = r*3.14/180;
|
|
|
207
|
|
|
208 xyz[0] = xyz[0]*cos(rad) + xyz[2]*sin(rad);
|
|
|
209 xyz[1] = xyz[1];
|
|
|
210 xyz[2] = -xyz[0]*sin(rad) + xyz[2]*cos(rad);
|
|
|
211 }
|
|
|
212
|
|
|
213 void rotate_z(float *xyz, float r)
|
|
|
214 {
|
|
|
215 float rad = r*3.14/180;
|
|
|
216
|
|
|
217 xyz[0] = xyz[0]*cos(rad) - xyz[1]*sin(rad);
|
|
|
218 xyz[1] = xyz[0]*sin(rad) + xyz[1]*cos(rad);
|
|
|
219 xyz[2] = xyz[2];
|
|
|
220 }
|
|
|
221
|
|
|
222 void rotate(float *xyz, float *matrix)
|
|
|
223 {
|
|
|
224 float abc[4];
|
|
|
225 abc[0] = xyz[0];
|
|
|
226 abc[1] = xyz[1];
|
|
|
227 abc[2] = xyz[2];
|
|
|
228 abc[3] = xyz[3];
|
|
|
229
|
|
|
230 for(int i=0; i<4; i++)
|
|
|
231 {
|
|
|
232 //xyz[i] = abc[0]*rot[i] + abc[1]*rot[i+4] + abc[2]*rot[i+8] + abc[3]*rot[i+12];
|
|
|
233 xyz[i] = abc[0]*matrix[i] + abc[1]*matrix[i+4] + abc[2]*matrix[i+8] + abc[3]*matrix[i+12];
|
|
|
234 }
|
|
|
235 }
|
|
|
236
|
|
|
237
|
|
|
238 void translate(float *xyz, float x, float y, float z)
|
|
|
239 {
|
|
|
240 xyz[0] += x;
|
|
|
241 xyz[1] += y;
|
|
|
242 xyz[2] += z;
|
|
|
243 }
|
