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