]>
Commit | Line | Data |
---|---|---|
3a9019d9 FG |
1 | //Copyright (c) 2008-2016 Emil Dotchevski and Reverge Studios, Inc.\r |
2 | \r | |
3 | //Distributed under the Boost Software License, Version 1.0. (See accompanying\r | |
4 | //file LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)\r | |
5 | \r | |
6 | #ifndef UUID_907229FCB3A711DE83C152F855D89593\r | |
7 | #define UUID_907229FCB3A711DE83C152F855D89593\r | |
8 | \r | |
9 | #include <limits>\r | |
10 | #include <math.h>\r | |
11 | #include <assert.h>\r | |
12 | #include <memory.h>\r | |
13 | #include <stdlib.h>\r | |
14 | \r | |
15 | namespace\r | |
16 | test_qvm\r | |
17 | {\r | |
18 | namespace\r | |
19 | detail\r | |
20 | {\r | |
21 | inline\r | |
22 | float\r | |
23 | sin( float a )\r | |
24 | {\r | |
25 | return ::sinf(a);\r | |
26 | }\r | |
27 | \r | |
28 | inline\r | |
29 | double\r | |
30 | sin( double a )\r | |
31 | {\r | |
32 | return ::sin(a);\r | |
33 | }\r | |
34 | \r | |
35 | inline\r | |
36 | float\r | |
37 | cos( float a )\r | |
38 | {\r | |
39 | return ::cosf(a);\r | |
40 | }\r | |
41 | \r | |
42 | inline\r | |
43 | double\r | |
44 | cos( double a )\r | |
45 | {\r | |
46 | return ::cos(a);\r | |
47 | }\r | |
48 | \r | |
49 | inline\r | |
50 | float\r | |
51 | abs( float a )\r | |
52 | {\r | |
53 | return ::fabsf(a);\r | |
54 | }\r | |
55 | \r | |
56 | inline\r | |
57 | double\r | |
58 | abs( double a )\r | |
59 | {\r | |
60 | return ::fabs(a);\r | |
61 | }\r | |
62 | \r | |
63 | inline\r | |
64 | float\r | |
65 | atan2( float a, float b )\r | |
66 | {\r | |
67 | return ::atan2f(a,b);\r | |
68 | }\r | |
69 | \r | |
70 | inline\r | |
71 | double\r | |
72 | atan2( double a, double b )\r | |
73 | {\r | |
74 | return ::atan2(a,b);\r | |
75 | }\r | |
76 | \r | |
77 | template <class T>\r | |
78 | T\r | |
79 | determinant( T * * a, int n )\r | |
80 | {\r | |
81 | int i,j,j1,j2;\r | |
82 | T det = 0;\r | |
83 | T * * m = 0;\r | |
84 | assert(n>=1);\r | |
85 | if( n==1 )\r | |
86 | det = a[0][0];\r | |
87 | else if( n==2 )\r | |
88 | det = a[0][0] * a[1][1] - a[1][0] * a[0][1];\r | |
89 | else\r | |
90 | {\r | |
91 | det = 0;\r | |
92 | for( j1=0; j1<n; j1++ )\r | |
93 | {\r | |
94 | m = static_cast<T * *>(malloc((n-1)*sizeof(T *)));\r | |
95 | for( i=0; i<n-1; i++ )\r | |
96 | m[i] = static_cast<T *>(malloc((n-1)*sizeof(T)));\r | |
97 | for( i=1; i<n; i++ )\r | |
98 | {\r | |
99 | j2 = 0;\r | |
100 | for( j=0; j<n; j++ )\r | |
101 | {\r | |
102 | if( j==j1 )\r | |
103 | continue;\r | |
104 | m[i-1][j2] = a[i][j];\r | |
105 | j2++;\r | |
106 | }\r | |
107 | }\r | |
108 | det += T(pow(-1.0,1.0+j1+1.0)) * a[0][j1] * determinant(m,n-1);\r | |
109 | for( i=0; i<n-1; i++ )\r | |
110 | free(m[i]);\r | |
111 | free(m);\r | |
112 | }\r | |
113 | }\r | |
114 | return(det);\r | |
115 | }\r | |
116 | \r | |
117 | template <class T,int N>\r | |
118 | void\r | |
119 | cofactor( T * * a, T (&b)[N][N] )\r | |
120 | {\r | |
121 | int i,j,ii,jj,i1,j1;\r | |
122 | T det;\r | |
123 | T * * c;\r | |
124 | c = static_cast<T * *>(malloc((N-1)*sizeof(T *)));\r | |
125 | for( i=0; i<N-1; i++ )\r | |
126 | c[i] = static_cast<T *>(malloc((N-1)*sizeof(T)));\r | |
127 | for( j=0; j<N; j++ )\r | |
128 | {\r | |
129 | for( i=0; i<N; i++ )\r | |
130 | {\r | |
131 | i1 = 0;\r | |
132 | for( ii=0; ii<N; ii++ )\r | |
133 | {\r | |
134 | if( ii==i )\r | |
135 | continue;\r | |
136 | j1 = 0;\r | |
137 | for( jj=0; jj<N; jj++ )\r | |
138 | {\r | |
139 | if( jj==j )\r | |
140 | continue;\r | |
141 | c[i1][j1] = a[ii][jj];\r | |
142 | j1++;\r | |
143 | }\r | |
144 | i1++;\r | |
145 | }\r | |
146 | det = determinant(c,N-1);\r | |
147 | b[i][j] = T(pow(-1.0,i+j+2.0)) * det;\r | |
148 | }\r | |
149 | }\r | |
150 | for( i=0; i<N-1; i++ )\r | |
151 | free(c[i]);\r | |
152 | free(c);\r | |
153 | }\r | |
154 | }\r | |
155 | \r | |
156 | template <class T,int D>\r | |
157 | T\r | |
158 | determinant( T (&in)[D][D] )\r | |
159 | {\r | |
160 | T * * m = static_cast<T * *>(malloc(D*sizeof(T *)));\r | |
161 | for( int i=0; i!=D; ++i )\r | |
162 | {\r | |
163 | m[i] = static_cast<T *>(malloc(D*sizeof(T)));\r | |
164 | for( int j=0; j!=D; ++j )\r | |
165 | m[i][j]=in[i][j];\r | |
166 | }\r | |
167 | T det=::test_qvm::detail::determinant(m,D);\r | |
168 | for( int i=0; i<D; ++i )\r | |
169 | free(m[i]);\r | |
170 | free(m);\r | |
171 | return det;\r | |
172 | }\r | |
173 | \r | |
174 | template <class T,int D>\r | |
175 | void\r | |
176 | inverse( T (&out)[D][D], T (&in)[D][D] )\r | |
177 | {\r | |
178 | T * * m = static_cast<T * *>(malloc(D*sizeof(T *)));\r | |
179 | for( int i=0; i!=D; ++i )\r | |
180 | {\r | |
181 | m[i] = static_cast<T *>(malloc(D*sizeof(T)));\r | |
182 | for( int j=0; j!=D; ++j )\r | |
183 | m[i][j]=in[i][j];\r | |
184 | }\r | |
185 | T det=::test_qvm::detail::determinant(m,D);\r | |
186 | assert(det!=T(0));\r | |
187 | T f=T(1)/det;\r | |
188 | T b[D][D];\r | |
189 | ::test_qvm::detail::cofactor(m,b);\r | |
190 | for( int i=0; i<D; ++i )\r | |
191 | free(m[i]);\r | |
192 | free(m);\r | |
193 | for( int i=0; i!=D; ++i )\r | |
194 | for( int j=0; j!=D; ++j )\r | |
195 | out[j][i]=b[i][j]*f;\r | |
196 | }\r | |
197 | \r | |
198 | template <class T,int M,int N>\r | |
199 | void\r | |
200 | init_m( T (&r)[M][N], T start=T(0), T step=T(0) )\r | |
201 | {\r | |
202 | for( int i=0; i<M; ++i )\r | |
203 | for( int j=0; j<N; ++j,start+=step )\r | |
204 | r[i][j] = start;\r | |
205 | }\r | |
206 | \r | |
207 | template <class T,int D>\r | |
208 | void\r | |
209 | init_v( T (&r)[D], T start=T(0), T step=T(0) )\r | |
210 | {\r | |
211 | for( int i=0; i<D; ++i,start+=step )\r | |
212 | r[i] = start;\r | |
213 | }\r | |
214 | \r | |
215 | template <class T,int M,int N>\r | |
216 | void\r | |
217 | zero_mat( T (&r)[M][N] )\r | |
218 | {\r | |
219 | for( int i=0; i<M; ++i )\r | |
220 | for( int j=0; j<N; ++j )\r | |
221 | r[i][j] = T(0);\r | |
222 | }\r | |
223 | \r | |
224 | template <class T,int D>\r | |
225 | void\r | |
226 | zero_vec( T (&r)[D] )\r | |
227 | {\r | |
228 | for( int i=0; i<D; ++i )\r | |
229 | r[i] = T(0);\r | |
230 | }\r | |
231 | \r | |
232 | template <class T,int D>\r | |
233 | void\r | |
234 | identity( T (&r)[D][D] )\r | |
235 | {\r | |
236 | for( int i=0; i<D; ++i )\r | |
237 | for( int j=0; j<D; ++j )\r | |
238 | r[i][j] = (i==j) ? T(1) : T(0);\r | |
239 | }\r | |
240 | \r | |
241 | template <class T,class U,class V,int M,int N>\r | |
242 | void\r | |
243 | add_m( T (&r)[M][N], U (&a)[M][N], V (&b)[M][N] )\r | |
244 | {\r | |
245 | for( int i=0; i<M; ++i )\r | |
246 | for( int j=0; j<N; ++j )\r | |
247 | r[i][j] = a[i][j] + b[i][j];\r | |
248 | }\r | |
249 | \r | |
250 | template <class T,class U,class V,int D>\r | |
251 | void\r | |
252 | add_v( T (&r)[D], U (&a)[D], V (&b)[D] )\r | |
253 | {\r | |
254 | for( int i=0; i<D; ++i )\r | |
255 | r[i] = a[i] + b[i];\r | |
256 | }\r | |
257 | \r | |
258 | template <class T,class U,class V,int M,int N>\r | |
259 | void\r | |
260 | subtract_m( T (&r)[M][N], U (&a)[M][N], V (&b)[M][N] )\r | |
261 | {\r | |
262 | for( int i=0; i<M; ++i )\r | |
263 | for( int j=0; j<N; ++j )\r | |
264 | r[i][j] = a[i][j] - b[i][j];\r | |
265 | }\r | |
266 | \r | |
267 | template <class T,class U,class V,int D>\r | |
268 | void\r | |
269 | subtract_v( T (&r)[D], U (&a)[D], V (&b)[D] )\r | |
270 | {\r | |
271 | for( int i=0; i<D; ++i )\r | |
272 | r[i] = a[i] - b[i];\r | |
273 | }\r | |
274 | \r | |
275 | template <class T,int D,class U>\r | |
276 | void\r | |
277 | rotation_x( T (&r)[D][D], U angle )\r | |
278 | {\r | |
279 | identity(r);\r | |
280 | T c=::test_qvm::detail::cos(angle);\r | |
281 | T s=::test_qvm::detail::sin(angle);\r | |
282 | r[1][1]=c;\r | |
283 | r[1][2]=-s;\r | |
284 | r[2][1]=s;\r | |
285 | r[2][2]=c;\r | |
286 | }\r | |
287 | \r | |
288 | template <class T,int D,class U>\r | |
289 | void\r | |
290 | rotation_y( T (&r)[D][D], U angle )\r | |
291 | {\r | |
292 | identity(r);\r | |
293 | T c=::test_qvm::detail::cos(angle);\r | |
294 | T s=::test_qvm::detail::sin(angle);\r | |
295 | r[0][0]=c;\r | |
296 | r[0][2]=s;\r | |
297 | r[2][0]=-s;\r | |
298 | r[2][2]=c;\r | |
299 | }\r | |
300 | \r | |
301 | template <class T,int D,class U>\r | |
302 | void\r | |
303 | rotation_z( T (&r)[D][D], U angle )\r | |
304 | {\r | |
305 | identity(r);\r | |
306 | T c=::test_qvm::detail::cos(angle);\r | |
307 | T s=::test_qvm::detail::sin(angle);\r | |
308 | r[0][0]=c;\r | |
309 | r[0][1]=-s;\r | |
310 | r[1][0]=s;\r | |
311 | r[1][1]=c;\r | |
312 | }\r | |
313 | \r | |
314 | template <class T,int D>\r | |
315 | void\r | |
316 | translation( T (&r)[D][D], T (&t)[D-1] )\r | |
317 | {\r | |
318 | identity(r);\r | |
319 | for( int i=0; i!=D-1; ++i )\r | |
320 | r[i][D-1]=t[i];\r | |
321 | }\r | |
322 | \r | |
323 | template <class R,class T,class U,int M,int N,int P>\r | |
324 | void\r | |
325 | multiply_m( R (&r)[M][P], T (&a)[M][N], U (&b)[N][P] )\r | |
326 | {\r | |
327 | for( int i=0; i<M; ++i )\r | |
328 | for( int j=0; j<P; ++j )\r | |
329 | {\r | |
330 | R x=0;\r | |
331 | for( int k=0; k<N; ++k )\r | |
332 | x += R(a[i][k])*R(b[k][j]);\r | |
333 | r[i][j] = x;\r | |
334 | }\r | |
335 | }\r | |
336 | \r | |
337 | template <class R,class T,class U,int M,int N>\r | |
338 | void\r | |
339 | multiply_mv( R (&r)[M], T (&a)[M][N], U (&b)[N] )\r | |
340 | {\r | |
341 | for( int i=0; i<M; ++i )\r | |
342 | {\r | |
343 | R x=0;\r | |
344 | for( int k=0; k<N; ++k )\r | |
345 | x += R(a[i][k])*R(b[k]);\r | |
346 | r[i] = x;\r | |
347 | }\r | |
348 | }\r | |
349 | \r | |
350 | template <class R,class T,class U,int N,int P>\r | |
351 | void\r | |
352 | multiply_vm( R (&r)[P], T (&a)[N], U (&b)[N][P] )\r | |
353 | {\r | |
354 | for( int j=0; j<P; ++j )\r | |
355 | {\r | |
356 | R x=0;\r | |
357 | for( int k=0; k<N; ++k )\r | |
358 | x += R(a[k])*R(b[k][j]);\r | |
359 | r[j] = x;\r | |
360 | }\r | |
361 | }\r | |
362 | \r | |
363 | template <class T,class U,int M,int N,class S>\r | |
364 | void\r | |
365 | scalar_multiply_m( T (&r)[M][N], U (&a)[M][N], S scalar )\r | |
366 | {\r | |
367 | for( int i=0; i<M; ++i )\r | |
368 | for( int j=0; j<N; ++j )\r | |
369 | r[i][j] = a[i][j]*scalar;\r | |
370 | }\r | |
371 | \r | |
372 | template <class T,class U,int D,class S>\r | |
373 | void\r | |
374 | scalar_multiply_v( T (&r)[D], U (&a)[D], S scalar )\r | |
375 | {\r | |
376 | for( int i=0; i<D; ++i )\r | |
377 | r[i] = a[i]*scalar;\r | |
378 | }\r | |
379 | \r | |
380 | template <class T,int M,int N>\r | |
381 | void\r | |
382 | transpose( T (&r)[M][N], T (&a)[N][M] )\r | |
383 | {\r | |
384 | for( int i=0; i<M; ++i )\r | |
385 | for( int j=0; j<N; ++j )\r | |
386 | r[i][j] = a[j][i];\r | |
387 | }\r | |
388 | \r | |
389 | template <class R,class T,class U,int D>\r | |
390 | R\r | |
391 | dot( T (&a)[D], U (&b)[D] )\r | |
392 | {\r | |
393 | R r=R(0);\r | |
394 | for( int i=0; i<D; ++i )\r | |
395 | r+=a[i]*b[i];\r | |
396 | return r;\r | |
397 | }\r | |
398 | \r | |
399 | template <class T,int M,int N>\r | |
400 | T\r | |
401 | norm_squared( T (&m)[M][N] )\r | |
402 | {\r | |
403 | T f=T(0);\r | |
404 | for( int i=0; i<M; ++i )\r | |
405 | for( int j=0; j<N; ++j )\r | |
406 | {\r | |
407 | T x=m[i][j];\r | |
408 | f+=x*x;\r | |
409 | }\r | |
410 | return f;\r | |
411 | }\r | |
412 | \r | |
413 | template <class T>\r | |
414 | inline\r | |
415 | void\r | |
416 | matrix_perspective_lh( T (&r)[4][4], T fov_y, T aspect_ratio, T zn, T zf )\r | |
417 | {\r | |
418 | T ys=T(1)/::tanf(fov_y/T(2));\r | |
419 | T xs=ys/aspect_ratio;\r | |
420 | zero_mat(r);\r | |
421 | r[0][0] = xs;\r | |
422 | r[1][1] = ys;\r | |
423 | r[2][2] = zf/(zf-zn);\r | |
424 | r[2][3] = -zn*zf/(zf-zn);\r | |
425 | r[3][2] = 1;\r | |
426 | }\r | |
427 | \r | |
428 | template <class T>\r | |
429 | inline\r | |
430 | void\r | |
431 | matrix_perspective_rh( T (&r)[4][4], T fov_y, T aspect_ratio, T zn, T zf )\r | |
432 | {\r | |
433 | matrix_perspective_lh(r,fov_y,aspect_ratio,zn,zf);\r | |
434 | r[2][2]=-r[2][2];\r | |
435 | r[3][2]=-r[3][2];\r | |
436 | }\r | |
437 | }\r | |
438 | \r | |
439 | #endif\r |