1 ///////////////////////////////////////////////////////////////////////
2 //
3 // File : mathSpec.h
4 // Description : Type independent header
5 //
6 ////////////////////////////////////////////////////////////////////////
7
8
9
10
initv(SCALAR_TYPE * r,const SCALAR_TYPE x)11 inline void initv(SCALAR_TYPE *r,const SCALAR_TYPE x) {
12 r[0] = x;
13 r[1] = x;
14 r[2] = x;
15 }
16
initv(SCALAR_TYPE * r,const SCALAR_TYPE x,const SCALAR_TYPE y,const SCALAR_TYPE z)17 inline void initv(SCALAR_TYPE *r,const SCALAR_TYPE x,const SCALAR_TYPE y,const SCALAR_TYPE z) {
18 r[0] = x;
19 r[1] = y;
20 r[2] = z;
21 }
22
addvv(SCALAR_TYPE * result,const SCALAR_TYPE * s1,const SCALAR_TYPE * s2)23 inline void addvv(SCALAR_TYPE *result,const SCALAR_TYPE *s1,const SCALAR_TYPE *s2) {
24 result[0] = s1[0] + s2[0];
25 result[1] = s1[1] + s2[1];
26 result[2] = s1[2] + s2[2];
27 }
28
addvv(SCALAR_TYPE * result,const SCALAR_TYPE * s1)29 inline void addvv(SCALAR_TYPE *result,const SCALAR_TYPE *s1) {
30 result[0] += s1[0];
31 result[1] += s1[1];
32 result[2] += s1[2];
33 }
34
addvf(SCALAR_TYPE * result,const SCALAR_TYPE s1)35 inline void addvf(SCALAR_TYPE *result,const SCALAR_TYPE s1) {
36 result[0] += s1;
37 result[1] += s1;
38 result[2] += s1;
39 }
40
subvf(SCALAR_TYPE * result,const SCALAR_TYPE s1)41 inline void subvf(SCALAR_TYPE *result,const SCALAR_TYPE s1) {
42 result[0] -= s1;
43 result[1] -= s1;
44 result[2] -= s1;
45 }
46
addvf(SCALAR_TYPE * result,const SCALAR_TYPE * s1,const SCALAR_TYPE s2)47 inline void addvf(SCALAR_TYPE *result,const SCALAR_TYPE *s1,const SCALAR_TYPE s2) {
48 result[0] = s1[0] + s2;
49 result[1] = s1[1] + s2;
50 result[2] = s1[2] + s2;
51 }
52
subvf(SCALAR_TYPE * result,const SCALAR_TYPE * s1,const SCALAR_TYPE s2)53 inline void subvf(SCALAR_TYPE *result,const SCALAR_TYPE *s1,const SCALAR_TYPE s2) {
54 result[0] = s1[0] - s2;
55 result[1] = s1[1] - s2;
56 result[2] = s1[2] - s2;
57 }
58
subvv(SCALAR_TYPE * result,const SCALAR_TYPE * s1,const SCALAR_TYPE * s2)59 inline void subvv(SCALAR_TYPE *result,const SCALAR_TYPE *s1,const SCALAR_TYPE *s2) {
60 result[0] = s1[0] - s2[0];
61 result[1] = s1[1] - s2[1];
62 result[2] = s1[2] - s2[2];
63 }
64
subvv(SCALAR_TYPE * result,const SCALAR_TYPE * s1)65 inline void subvv(SCALAR_TYPE *result,const SCALAR_TYPE *s1) {
66 result[0] -= s1[0];
67 result[1] -= s1[1];
68 result[2] -= s1[2];
69 }
70
mulvf(SCALAR_TYPE * result,const SCALAR_TYPE * v,const SCALAR_TYPE m)71 inline void mulvf(SCALAR_TYPE *result,const SCALAR_TYPE *v,const SCALAR_TYPE m) {
72 result[0] = v[0]*m;
73 result[1] = v[1]*m;
74 result[2] = v[2]*m;
75 }
76
mulvf(SCALAR_TYPE * result,const SCALAR_TYPE m)77 inline void mulvf(SCALAR_TYPE *result,const SCALAR_TYPE m) {
78 result[0] *= m;
79 result[1] *= m;
80 result[2] *= m;
81 }
82
mulvv(SCALAR_TYPE * result,const SCALAR_TYPE * s1,const SCALAR_TYPE * s2)83 inline void mulvv(SCALAR_TYPE *result,const SCALAR_TYPE *s1,const SCALAR_TYPE *s2) {
84 result[0] = s1[0]*s2[0];
85 result[1] = s1[1]*s2[1];
86 result[2] = s1[2]*s2[2];
87 }
88
mulvv(SCALAR_TYPE * result,const SCALAR_TYPE * s1)89 inline void mulvv(SCALAR_TYPE *result,const SCALAR_TYPE *s1) {
90 result[0] *= s1[0];
91 result[1] *= s1[1];
92 result[2] *= s1[2];
93 }
94
divvv(SCALAR_TYPE * result,const SCALAR_TYPE * s1,const SCALAR_TYPE * s2)95 inline void divvv(SCALAR_TYPE *result,const SCALAR_TYPE *s1,const SCALAR_TYPE *s2) {
96 result[0] = s1[0] / s2[0];
97 result[1] = s1[1] / s2[1];
98 result[2] = s1[2] / s2[2];
99 }
100
divvv(SCALAR_TYPE * result,const SCALAR_TYPE * s1)101 inline void divvv(SCALAR_TYPE *result,const SCALAR_TYPE *s1) {
102 result[0] /= s1[0];
103 result[1] /= s1[1];
104 result[2] /= s1[2];
105 }
106
crossvv(SCALAR_TYPE * result,const SCALAR_TYPE * s1,const SCALAR_TYPE * s2)107 inline void crossvv(SCALAR_TYPE *result,const SCALAR_TYPE *s1,const SCALAR_TYPE *s2) {
108 result[0] = (s1[1]*s2[2] - s1[2]*s2[1]);
109 result[1] = (s1[2]*s2[0] - s1[0]*s2[2]);
110 result[2] = (s1[0]*s2[1] - s1[1]*s2[0]);
111 }
112
dotvv(const SCALAR_TYPE * s1,const SCALAR_TYPE * s2)113 inline SCALAR_TYPE dotvv(const SCALAR_TYPE *s1,const SCALAR_TYPE *s2) {
114 return (SCALAR_TYPE) (s1[0]*s2[0] + s1[1]*s2[1] + s1[2]*s2[2]);
115 }
116
interpolatev(SCALAR_TYPE * result,const SCALAR_TYPE * s1,const SCALAR_TYPE * s2,const SCALAR_TYPE alpha)117 inline void interpolatev(SCALAR_TYPE *result,const SCALAR_TYPE *s1,const SCALAR_TYPE *s2,const SCALAR_TYPE alpha) {
118 result[0] = (float) (s1[0]*(1.0-(double)alpha) + s2[0]*(double)alpha);
119 result[1] = (float) (s1[1]*(1.0-(double)alpha) + s2[1]*(double)alpha);
120 result[2] = (float) (s1[2]*(1.0-(double)alpha) + s2[2]*(double)alpha);
121 }
122
interpolatea(const SCALAR_TYPE s1,const SCALAR_TYPE s2,const SCALAR_TYPE alpha)123 inline SCALAR_TYPE interpolatea(const SCALAR_TYPE s1,const SCALAR_TYPE s2,const SCALAR_TYPE alpha) {
124 if (s2 >= s1) {
125 SCALAR_TYPE d = fmod((SCALAR_TYPE) (s2 - s1),(SCALAR_TYPE) (2*C_PI));
126 if (d > C_PI) {
127 return s2 + (SCALAR_TYPE) ((2*C_PI - d)*(1-alpha));
128 } else {
129 return s1 + (SCALAR_TYPE) (d*alpha);
130 }
131
132
133
134 } else {
135 return interpolatea(s2,s1,1-alpha);
136 }
137 }
138
movvv(SCALAR_TYPE * dest,const SCALAR_TYPE * src)139 inline void movvv(SCALAR_TYPE *dest,const SCALAR_TYPE *src) {
140 dest[0] = src[0];
141 dest[1] = src[1];
142 dest[2] = src[2];
143 }
144
lengthv(const SCALAR_TYPE * v)145 inline SCALAR_TYPE lengthv(const SCALAR_TYPE *v) {
146 return SQRT(dotvv(v,v));
147 }
148
normalizev(SCALAR_TYPE * r,const SCALAR_TYPE * v)149 inline void normalizev(SCALAR_TYPE *r,const SCALAR_TYPE *v) {
150 const double l = 1 / sqrt(v[0]*v[0] + v[1]*v[1] + v[2]*v[2]);
151
152 r[0] = (SCALAR_TYPE) (v[0]*l);
153 r[1] = (SCALAR_TYPE) (v[1]*l);
154 r[2] = (SCALAR_TYPE) (v[2]*l);
155 }
156
normalizev(SCALAR_TYPE * v)157 inline void normalizev(SCALAR_TYPE *v) {
158 const double l = 1 / sqrt(v[0]*v[0] + v[1]*v[1] + v[2]*v[2]);
159
160 v[0] = (SCALAR_TYPE) (v[0]*l);
161 v[1] = (SCALAR_TYPE) (v[1]*l);
162 v[2] = (SCALAR_TYPE) (v[2]*l);
163 }
164
mulmm(SCALAR_TYPE * result,const SCALAR_TYPE * s1,const SCALAR_TYPE * s2)165 inline void mulmm(SCALAR_TYPE *result,const SCALAR_TYPE *s1,const SCALAR_TYPE *s2) {
166 int i,j,k;
167
168 for (i=0;i<4;i++)
169 for (j=0;j<4;j++) {
170 double dest = 0;
171
172 for (k=0;k<4;k++) dest += s1[element(i,k)]*s2[element(k,j)];
173
174 result[element(i,j)] = (SCALAR_TYPE) dest;
175 }
176 }
177
mulmm(SCALAR_TYPE * result,const SCALAR_TYPE * s1,const SCALAR_TYPE * s2,const SCALAR_TYPE * s3)178 inline void mulmm(SCALAR_TYPE *result,const SCALAR_TYPE *s1,const SCALAR_TYPE *s2,const SCALAR_TYPE *s3) {
179 MATRIX_TYPE mtmp;
180
181 mulmm(mtmp,s1,s2);
182 mulmm(result,mtmp,s3);
183 }
184
mulmp(SCALAR_TYPE * result,const SCALAR_TYPE * s1,const SCALAR_TYPE * s2)185 inline void mulmp(SCALAR_TYPE *result,const SCALAR_TYPE *s1,const SCALAR_TYPE *s2) {
186 const SCALAR_TYPE x = s1[element(0,0)]*s2[0]+s1[element(0,1)]*s2[1]+s1[element(0,2)]*s2[2]+s1[element(0,3)];
187 const SCALAR_TYPE y = s1[element(1,0)]*s2[0]+s1[element(1,1)]*s2[1]+s1[element(1,2)]*s2[2]+s1[element(1,3)];
188 const SCALAR_TYPE z = s1[element(2,0)]*s2[0]+s1[element(2,1)]*s2[1]+s1[element(2,2)]*s2[2]+s1[element(2,3)];
189 const SCALAR_TYPE w = s1[element(3,0)]*s2[0]+s1[element(3,1)]*s2[1]+s1[element(3,2)]*s2[2]+s1[element(3,3)];
190
191
192 if (w != 1) {
193 const double l = 1 / (double) w;
194
195 result[0] = (SCALAR_TYPE) (x*l);
196 result[1] = (SCALAR_TYPE) (y*l);
197 result[2] = (SCALAR_TYPE) (z*l);
198 } else {
199 result[0] = x;
200 result[1] = y;
201 result[2] = z;
202 }
203 }
204
mulmv(SCALAR_TYPE * result,const SCALAR_TYPE * s1,const SCALAR_TYPE * s2)205 inline void mulmv(SCALAR_TYPE *result,const SCALAR_TYPE *s1,const SCALAR_TYPE *s2) {
206 const SCALAR_TYPE x = s1[element(0,0)]*s2[0]+s1[element(0,1)]*s2[1]+s1[element(0,2)]*s2[2];
207 const SCALAR_TYPE y = s1[element(1,0)]*s2[0]+s1[element(1,1)]*s2[1]+s1[element(1,2)]*s2[2];
208 const SCALAR_TYPE z = s1[element(2,0)]*s2[0]+s1[element(2,1)]*s2[1]+s1[element(2,2)]*s2[2];
209
210 result[0] = x;
211 result[1] = y;
212 result[2] = z;
213 }
214
mulmn(SCALAR_TYPE * result,const SCALAR_TYPE * s1,const SCALAR_TYPE * s2)215 inline void mulmn(SCALAR_TYPE *result,const SCALAR_TYPE *s1,const SCALAR_TYPE *s2) {
216 const SCALAR_TYPE x = s1[element(0,0)]*s2[0]+s1[element(1,0)]*s2[1]+s1[element(2,0)]*s2[2];
217 const SCALAR_TYPE y = s1[element(0,1)]*s2[0]+s1[element(1,1)]*s2[1]+s1[element(2,1)]*s2[2];
218 const SCALAR_TYPE z = s1[element(0,2)]*s2[0]+s1[element(1,2)]*s2[1]+s1[element(2,2)]*s2[2];
219
220 result[0] = x;
221 result[1] = y;
222 result[2] = z;
223 }
224
mulmp4(SCALAR_TYPE * result,const SCALAR_TYPE * s1,const SCALAR_TYPE * s2)225 inline void mulmp4(SCALAR_TYPE *result,const SCALAR_TYPE *s1,const SCALAR_TYPE *s2) {
226 const SCALAR_TYPE x = s1[element(0,0)]*s2[0]+s1[element(0,1)]*s2[1]+s1[element(0,2)]*s2[2]+s1[element(0,3)]*s2[3];
227 const SCALAR_TYPE y = s1[element(1,0)]*s2[0]+s1[element(1,1)]*s2[1]+s1[element(1,2)]*s2[2]+s1[element(1,3)]*s2[3];
228 const SCALAR_TYPE z = s1[element(2,0)]*s2[0]+s1[element(2,1)]*s2[1]+s1[element(2,2)]*s2[2]+s1[element(2,3)]*s2[3];
229 const SCALAR_TYPE w = s1[element(3,0)]*s2[0]+s1[element(3,1)]*s2[1]+s1[element(3,2)]*s2[2]+s1[element(3,3)]*s2[3];
230
231
232 result[0] = x;
233 result[1] = y;
234 result[2] = z;
235 result[3] = w;
236 }
237
238
mulmv4(SCALAR_TYPE * result,const SCALAR_TYPE * s1,const SCALAR_TYPE * s2)239 inline void mulmv4(SCALAR_TYPE *result,const SCALAR_TYPE *s1,const SCALAR_TYPE *s2) {
240 const SCALAR_TYPE x = s1[element(0,0)]*s2[0]+s1[element(0,1)]*s2[1]+s1[element(0,2)]*s2[2];
241 const SCALAR_TYPE y = s1[element(1,0)]*s2[0]+s1[element(1,1)]*s2[1]+s1[element(1,2)]*s2[2];
242 const SCALAR_TYPE z = s1[element(2,0)]*s2[0]+s1[element(2,1)]*s2[1]+s1[element(2,2)]*s2[2];
243 const SCALAR_TYPE w = s1[element(3,0)]*s2[0]+s1[element(3,1)]*s2[1]+s1[element(3,2)]*s2[2];
244
245 result[0] = x;
246 result[1] = y;
247 result[2] = z;
248 result[3] = w;
249 }
250
mulmn4(SCALAR_TYPE * result,const SCALAR_TYPE * s1,const SCALAR_TYPE * s2)251 inline void mulmn4(SCALAR_TYPE *result,const SCALAR_TYPE *s1,const SCALAR_TYPE *s2) {
252 const SCALAR_TYPE x = s1[element(0,0)]*s2[0]+s1[element(1,0)]*s2[1]+s1[element(2,0)]*s2[2];
253 const SCALAR_TYPE y = s1[element(0,1)]*s2[0]+s1[element(1,1)]*s2[1]+s1[element(2,1)]*s2[2];
254 const SCALAR_TYPE z = s1[element(0,2)]*s2[0]+s1[element(1,2)]*s2[1]+s1[element(2,2)]*s2[2];
255 const SCALAR_TYPE w = s1[element(0,3)]*s2[0]+s1[element(1,3)]*s2[1]+s1[element(2,3)]*s2[2];
256
257 result[0] = x;
258 result[1] = y;
259 result[2] = z;
260 result[3] = w;
261 }
262
mulpm(SCALAR_TYPE * result,const SCALAR_TYPE * s1,const SCALAR_TYPE * s2)263 inline void mulpm(SCALAR_TYPE *result,const SCALAR_TYPE *s1,const SCALAR_TYPE *s2) {
264 const SCALAR_TYPE x = s1[0]*s2[element(0,0)] + s1[1]*s2[element(1,0)] + s1[2]*s2[element(2,0)] + s2[element(3,0)];
265 const SCALAR_TYPE y = s1[0]*s2[element(0,1)] + s1[1]*s2[element(1,1)] + s1[2]*s2[element(2,1)] + s2[element(3,1)];
266 const SCALAR_TYPE z = s1[0]*s2[element(0,2)] + s1[1]*s2[element(1,2)] + s1[2]*s2[element(2,2)] + s2[element(3,2)];
267 const SCALAR_TYPE w = s1[0]*s2[element(0,3)] + s1[1]*s2[element(1,3)] + s1[2]*s2[element(2,3)] + s2[element(3,3)];
268
269 if (w != 1) {
270 const double l = 1 / (double) w;
271
272 result[0] = (SCALAR_TYPE) (x*l);
273 result[1] = (SCALAR_TYPE) (y*l);
274 result[2] = (SCALAR_TYPE) (z*l);
275 } else {
276 result[0] = x;
277 result[1] = y;
278 result[2] = z;
279 }
280 }
281
mulpm4(SCALAR_TYPE * result,const SCALAR_TYPE * s1,const SCALAR_TYPE * s2)282 inline void mulpm4(SCALAR_TYPE *result,const SCALAR_TYPE *s1,const SCALAR_TYPE *s2) {
283 const SCALAR_TYPE x = s1[0]*s2[element(0,0)] + s1[1]*s2[element(1,0)] + s1[2]*s2[element(2,0)] + s1[3]*s2[element(3,0)];
284 const SCALAR_TYPE y = s1[0]*s2[element(0,1)] + s1[1]*s2[element(1,1)] + s1[2]*s2[element(2,1)] + s1[3]*s2[element(3,1)];
285 const SCALAR_TYPE z = s1[0]*s2[element(0,2)] + s1[1]*s2[element(1,2)] + s1[2]*s2[element(2,2)] + s1[3]*s2[element(3,2)];
286 const SCALAR_TYPE w = s1[0]*s2[element(0,3)] + s1[1]*s2[element(1,3)] + s1[2]*s2[element(2,3)] + s1[3]*s2[element(3,3)];
287
288
289 result[0] = x;
290 result[1] = y;
291 result[2] = z;
292 result[3] = w;
293 }
294
295
mulvm(SCALAR_TYPE * result,const SCALAR_TYPE * s1,const SCALAR_TYPE * s2)296 inline void mulvm(SCALAR_TYPE *result,const SCALAR_TYPE *s1,const SCALAR_TYPE *s2) {
297 const SCALAR_TYPE x = s1[0]*s2[element(0,0)] + s1[1]*s2[element(1,0)] + s1[2]*s2[element(2,0)];
298 const SCALAR_TYPE y = s1[0]*s2[element(0,1)] + s1[1]*s2[element(1,1)] + s1[2]*s2[element(2,1)];
299 const SCALAR_TYPE z = s1[0]*s2[element(0,2)] + s1[1]*s2[element(1,2)] + s1[2]*s2[element(2,2)];
300
301 result[0] = x;
302 result[1] = y;
303 result[2] = z;
304 }
305
mulmf(SCALAR_TYPE * result,const SCALAR_TYPE * s,const SCALAR_TYPE t)306 inline void mulmf(SCALAR_TYPE *result,const SCALAR_TYPE *s,const SCALAR_TYPE t) {
307 int i;
308
309 for (i=0;i<16;i++)
310 result[i] = s[i]*t;
311 }
312
mulmf(SCALAR_TYPE * result,const SCALAR_TYPE t)313 inline void mulmf(SCALAR_TYPE *result,const SCALAR_TYPE t) {
314 int i;
315
316 for (i=0;i<16;i++)
317 result[i] *= t;
318 }
319
addmm(SCALAR_TYPE * result,const SCALAR_TYPE * s1,const SCALAR_TYPE * s2)320 inline void addmm(SCALAR_TYPE *result,const SCALAR_TYPE *s1,const SCALAR_TYPE *s2) {
321 int i;
322
323 for (i=0;i<16;i++)
324 result[i] = s1[i] + s2[i];
325 }
326
submm(SCALAR_TYPE * result,const SCALAR_TYPE * s1,const SCALAR_TYPE * s2)327 inline void submm(SCALAR_TYPE *result,const SCALAR_TYPE *s1,const SCALAR_TYPE *s2) {
328 int i;
329
330 for (i=0;i<16;i++)
331 result[i] = s1[i] - s2[i];
332 }
333
addmm(SCALAR_TYPE * result,const SCALAR_TYPE * s)334 inline void addmm(SCALAR_TYPE *result,const SCALAR_TYPE *s) {
335 int i;
336
337 for (i=0;i<16;i++)
338 result[i] += s[i];
339 }
340
submm(SCALAR_TYPE * result,const SCALAR_TYPE * s)341 inline void submm(SCALAR_TYPE *result,const SCALAR_TYPE *s) {
342 int i;
343
344 for (i=0;i<16;i++)
345 result[i] -= s[i];
346 }
347
348
movmm(SCALAR_TYPE * dest,const SCALAR_TYPE * src)349 inline void movmm(SCALAR_TYPE *dest,const SCALAR_TYPE *src) {
350 int i;
351
352 for (i=0;i<16;i++)
353 dest[i] = src[i];
354 }
355
transposem(SCALAR_TYPE * dest,const SCALAR_TYPE * src)356 inline void transposem(SCALAR_TYPE *dest,const SCALAR_TYPE *src) {
357 int i,j;
358
359 for (i=0;i<4;i++) {
360 for (j=0;j<4;j++) {
361 dest[element(i,j)] = src[element(j,i)];
362 }
363 }
364 }
365
area(SCALAR_TYPE x1,SCALAR_TYPE y1,SCALAR_TYPE x2,SCALAR_TYPE y2,SCALAR_TYPE x3,SCALAR_TYPE y3)366 inline SCALAR_TYPE area(SCALAR_TYPE x1,SCALAR_TYPE y1,SCALAR_TYPE x2,SCALAR_TYPE y2,SCALAR_TYPE x3,SCALAR_TYPE y3) {
367 const double acx = x1 - x3;
368 const double bcx = x2 - x3;
369 const double acy = y1 - y3;
370 const double bcy = y2 - y3;
371 return (SCALAR_TYPE) (acx * bcy - acy * bcx);
372 }
373
mulmvv(SCALAR_TYPE * dest,const SCALAR_TYPE * s1,const SCALAR_TYPE * s2)374 inline void mulmvv(SCALAR_TYPE *dest,const SCALAR_TYPE *s1,const SCALAR_TYPE *s2) {
375 dest[element(0,0)] = s1[0]*s2[0];
376 dest[element(1,0)] = s1[1]*s2[0];
377 dest[element(2,0)] = s1[2]*s2[0];
378 dest[element(3,0)] = 0;
379 dest[element(0,1)] = s1[0]*s2[1];
380 dest[element(1,1)] = s1[1]*s2[1];
381 dest[element(2,1)] = s1[2]*s2[1];
382 dest[element(3,1)] = 0;
383 dest[element(0,2)] = s1[0]*s2[2];
384 dest[element(1,2)] = s1[1]*s2[2];
385 dest[element(2,2)] = s1[2]*s2[2];
386 dest[element(3,2)] = 0;
387 dest[element(0,3)] = 0;
388 dest[element(1,3)] = 0;
389 dest[element(2,3)] = 0;
390 dest[element(3,3)] = 1;
391 }
392
clampf(const SCALAR_TYPE mi,const SCALAR_TYPE l,const SCALAR_TYPE ma)393 inline SCALAR_TYPE clampf(const SCALAR_TYPE mi,const SCALAR_TYPE l,const SCALAR_TYPE ma) {
394 return (l < mi ? mi : (l > ma ? ma : l));
395 }
396
clampv(const SCALAR_TYPE * mi,SCALAR_TYPE * l,const SCALAR_TYPE * ma)397 inline void clampv(const SCALAR_TYPE *mi,SCALAR_TYPE *l,const SCALAR_TYPE *ma) {
398 l[0] = (l[0] < mi[0] ? mi[0] : (l[0] > ma[0] ? ma[0] : l[0]));
399 l[1] = (l[1] < mi[1] ? mi[1] : (l[1] > ma[1] ? ma[1] : l[1]));
400 l[2] = (l[2] < mi[2] ? mi[2] : (l[2] > ma[2] ? ma[2] : l[2]));
401 }
402
403
404
405
406
407 ////////////////////////////////////////////////////////////////////////////
408 //
409 //
410 //
411 // Misc box routines
412 //
413 //
414 //
415 ////////////////////////////////////////////////////////////////////////////
416
417
418
419
420 // True if point v is inside the box (bmin,bmax)
inBox(const SCALAR_TYPE * bmin,const SCALAR_TYPE * bmax,const SCALAR_TYPE * v)421 inline int inBox(const SCALAR_TYPE *bmin,const SCALAR_TYPE *bmax,const SCALAR_TYPE *v) {
422 int i;
423
424 for (i=0;i<3;i++) {
425 if ((v[i] < bmin[i]) || (v[i] > bmax[i])) return FALSE;
426 }
427
428 return TRUE;
429 }
430
431
432
433 // Expand the box (bmin,bmax) so that point v is inside it
addBox(SCALAR_TYPE * bmin,SCALAR_TYPE * bmax,const SCALAR_TYPE * v)434 inline void addBox(SCALAR_TYPE *bmin,SCALAR_TYPE *bmax,const SCALAR_TYPE *v) {
435 int i;
436
437 for (i=0;i<3;i++) {
438 if (v[i] < bmin[i]) bmin[i] = v[i];
439 if (v[i] > bmax[i]) bmax[i] = v[i];
440 }
441 }
442
443 // True if two given boxes intersect
intersectBox(const SCALAR_TYPE * bmin1,const SCALAR_TYPE * bmax1,const SCALAR_TYPE * bmin2,const SCALAR_TYPE * bmax2)444 inline int intersectBox(const SCALAR_TYPE *bmin1,const SCALAR_TYPE *bmax1,const SCALAR_TYPE *bmin2,const SCALAR_TYPE *bmax2) {
445 int i;
446
447 for (i=0;i<3;i++) {
448 if ((bmin1[i] > bmax2[i]) || (bmax1[i] < bmin2[i])) return FALSE;
449 }
450
451 return TRUE;
452 }
453
454 // True if a ray intersects a box
intersectBox(const SCALAR_TYPE * bmin,const SCALAR_TYPE * bmax,const SCALAR_TYPE * F,const SCALAR_TYPE * D,const double * invD,SCALAR_TYPE & tmin,SCALAR_TYPE & tmax)455 inline int intersectBox(const SCALAR_TYPE *bmin,const SCALAR_TYPE *bmax,const SCALAR_TYPE *F,const SCALAR_TYPE *D,const double *invD,SCALAR_TYPE &tmin,SCALAR_TYPE &tmax) {
456 SCALAR_TYPE tnear,tfar;
457 SCALAR_TYPE t1,t2;
458 unsigned int i;
459
460 tnear = tmin;
461 tfar = tmax;
462
463 for (i=0;i<3;i++) {
464 if (D[i] == 0) {
465 if ((F[i] > bmax[i]) || (F[i] < bmin[i])) return FALSE;
466 } else {
467 t1 = (SCALAR_TYPE) ((bmin[i] - F[i]) * invD[i]);
468 t2 = (SCALAR_TYPE) ((bmax[i] - F[i]) * invD[i]);
469
470 if (t1 < t2) {
471 if (t1 > tnear) tnear = t1;
472 if (t2 < tfar) tfar = t2;
473 } else {
474 if (t2 > tnear) tnear = t2;
475 if (t1 < tfar) tfar = t1;
476 }
477
478 if (tnear > tfar) return FALSE;
479 }
480 }
481
482 tmin = tnear;
483 tmax = tfar;
484
485 return TRUE;
486 }
487
488
489
490
491 // If a ray intersects a box, returned t is < tmax (the same as above but takes inverse of the direction)
nearestBox(const SCALAR_TYPE * bmin,const SCALAR_TYPE * bmax,const SCALAR_TYPE * F,const double * invD,SCALAR_TYPE tmin,SCALAR_TYPE tmax)492 inline SCALAR_TYPE nearestBox(const SCALAR_TYPE *bmin,const SCALAR_TYPE *bmax,const SCALAR_TYPE *F,const double *invD,SCALAR_TYPE tmin,SCALAR_TYPE tmax) {
493 SCALAR_TYPE tnear,tfar;
494 SCALAR_TYPE t1,t2;
495 unsigned int i;
496
497 tnear = tmin;
498 tfar = tmax;
499
500 for (i=0;i<3;i++) {
501 t1 = (SCALAR_TYPE) ((bmin[i] - F[i]) * invD[i]);
502 t2 = (SCALAR_TYPE) ((bmax[i] - F[i]) * invD[i]);
503
504 if (t1 < t2) {
505 if (t1 > tnear) tnear = t1;
506 if (t2 < tfar) tfar = t2;
507 } else {
508 if (t2 > tnear) tnear = t2;
509 if (t1 < tfar) tfar = t1;
510 }
511
512 if (tnear > tfar) return C_INFINITY;
513 }
514
515 // return nearest t if we're external
516 return (SCALAR_TYPE) tnear;
517 }
518
519
520
521 // Reflect I around N
reflect(SCALAR_TYPE * r,const SCALAR_TYPE * I,const SCALAR_TYPE * N)522 inline void reflect(SCALAR_TYPE *r,const SCALAR_TYPE *I,const SCALAR_TYPE *N) {
523 mulvf(r,N,-2*dotvv(N,I));
524 addvv(r,I);
525 }
526
527 // Refrect I around N
refract(SCALAR_TYPE * r,const SCALAR_TYPE * I,const SCALAR_TYPE * N,SCALAR_TYPE eta)528 inline void refract(SCALAR_TYPE *r,const SCALAR_TYPE *I,const SCALAR_TYPE *N,SCALAR_TYPE eta) {
529 VECTOR_TYPE vtmp;
530 SCALAR_TYPE IdotN,k;
531 IdotN = dotvv(N,I);
532 k = 1 - eta*eta*(1-IdotN*IdotN);
533 if (k <= 0) {
534 //initv(r,0);
535 movvv(r,I);
536 } else {
537 mulvf(vtmp,N,(eta*IdotN+SQRT(k)));
538 mulvf(r,I,eta);
539 subvv(r,vtmp);
540 }
541 }
542
543 // The fresnel function
fresnel(const SCALAR_TYPE * I,const SCALAR_TYPE * N,SCALAR_TYPE eta,SCALAR_TYPE & Kr,SCALAR_TYPE & Kt,SCALAR_TYPE * R,SCALAR_TYPE * T)544 inline void fresnel(const SCALAR_TYPE *I,const SCALAR_TYPE *N,SCALAR_TYPE eta,SCALAR_TYPE &Kr,SCALAR_TYPE &Kt,SCALAR_TYPE *R,SCALAR_TYPE *T) {
545 const SCALAR_TYPE e = 1 / eta;
546 const SCALAR_TYPE c = -dotvv(I,N);
547 const SCALAR_TYPE t = e*e+c*c-1;
548 const SCALAR_TYPE g = SQRT(max(t,0));
549 const SCALAR_TYPE a = (g - c) / (g + c);
550 const SCALAR_TYPE b = (c*(g+c) - 1) / (c*(g-c) + 1);
551
552 Kr = 0.5f*a*a*(1 + b*b);
553 Kr = min(Kr,1);
554 Kr = max(Kr,0);
555 Kt = 1 - Kr;
556 reflect(R,I,N);
557 refract(T,I,N,eta);
558 }
559
ptlined(SCALAR_TYPE * A,SCALAR_TYPE * B,SCALAR_TYPE * P)560 inline SCALAR_TYPE ptlined(SCALAR_TYPE *A,SCALAR_TYPE *B,SCALAR_TYPE *P) {
561 SCALAR_TYPE vtmp[3],vtmp2[3],vtmp3[3];
562 SCALAR_TYPE l;
563
564 subvv(vtmp,P,B);
565 subvv(vtmp2,A,B);
566 if (dotvv(vtmp,vtmp2) <= 0) {
567 l = SQRT(dotvv(vtmp,vtmp));
568 } else {
569 mulvf(vtmp2,-1);
570 subvv(vtmp,P,A);
571 if (dotvv(vtmp,vtmp2) <= 0) {
572 l = SQRT(dotvv(vtmp,vtmp));
573 } else {
574 subvv(vtmp,B,A);
575 subvv(vtmp2,B,P);
576 crossvv(vtmp3,vtmp,vtmp2);
577 l = SQRT(dotvv(vtmp3,vtmp3))/SQRT(dotvv(vtmp,vtmp));
578 }
579 }
580 return l;
581 }
582
583
584
585
586
587
588
589
590
591
592
593
594
595 ////////////////////////////////////////////////////////////////////////////
596 //
597 //
598 //
599 // Misc matrix operations that are too long to implement as inline
600 // These are defined in algebra.cpp
601 //
602 //
603 //
604 ////////////////////////////////////////////////////////////////////////////
605 void skewsymm(SCALAR_TYPE *,const SCALAR_TYPE *); // Create a skew symmetric matrix from matrix
606 SCALAR_TYPE determinantm(const SCALAR_TYPE *); // Take the determinant of a 4x4 matrix
607 void identitym(SCALAR_TYPE *); // Construct identity matrix
608 void translatem(SCALAR_TYPE *,const SCALAR_TYPE,const SCALAR_TYPE,const SCALAR_TYPE); // Construct translate matrix
609 void scalem(SCALAR_TYPE *,const SCALAR_TYPE,const SCALAR_TYPE,const SCALAR_TYPE); // Construct scale matrix
610 void rotatem(SCALAR_TYPE *,const SCALAR_TYPE *,const SCALAR_TYPE); // Construct rotate matrix (the second argument is the rotation vector)
611 void rotatem(SCALAR_TYPE *,SCALAR_TYPE,SCALAR_TYPE,SCALAR_TYPE,const SCALAR_TYPE); // Construct rotate matrix (the arguments are: result,x,y,z coordinates of the rotation vector and angle to rotate)
612 void rotatem(SCALAR_TYPE *,const SCALAR_TYPE *);
613 void skewm(SCALAR_TYPE *,const SCALAR_TYPE,const SCALAR_TYPE,const SCALAR_TYPE,const SCALAR_TYPE,const SCALAR_TYPE,const SCALAR_TYPE,const SCALAR_TYPE);
614 int invertm(SCALAR_TYPE *,const SCALAR_TYPE *); // Invert a matrix. Returns 0 if the inversion is successful 1 otherwise (i.e. matrix is singular)
615
616
617
618
619
620
621
622 ///////////////////////////////////////////////////////////////////////
623 // Invert a rigid body transformation
invertRigid(SCALAR_TYPE * dest,const SCALAR_TYPE * src)624 inline void invertRigid(SCALAR_TYPE *dest,const SCALAR_TYPE *src) {
625 MATRIX_TYPE R,Rt,T;
626
627 movmm(Rt,src);
628 Rt[element(0,3)] = 0;
629 Rt[element(1,3)] = 0;
630 Rt[element(2,3)] = 0;
631 transposem(R,Rt);
632
633 identitym(T);
634 T[element(0,3)] = -src[element(0,3)];
635 T[element(1,3)] = -src[element(1,3)];
636 T[element(2,3)] = -src[element(2,3)];
637
638 mulmm(dest,R,T);
639 }
640
641 ///////////////////////////////////////////////////////////////////////
642 // Make a matrix a rotation
makeRotation(SCALAR_TYPE * M)643 inline void makeRotation(SCALAR_TYPE *M) {
644 VECTOR_TYPE vx,vy,vz;
645 VECTOR_TYPE ux,uy,uz;
646
647 initv(vx,M[element(0,0)],M[element(1,0)],M[element(2,0)]);
648 initv(vy,M[element(0,1)],M[element(1,1)],M[element(2,1)]);
649 initv(vz,M[element(0,2)],M[element(1,2)],M[element(2,2)]);
650
651 while(TRUE) {
652 SCALAR_TYPE x,y,z;
653
654 crossvv(ux,vy,vz);
655 crossvv(uy,vz,vx);
656 crossvv(uz,vx,vy);
657
658 normalizev(ux);
659 normalizev(uy);
660 normalizev(uz);
661
662 addvv(vx,ux);
663 addvv(vy,uy);
664 addvv(vz,uz);
665
666 mulvf(vx,0.5f);
667 mulvf(vy,0.5f);
668 mulvf(vz,0.5f);
669
670 x = dotvv(vx,vy);
671 y = dotvv(vy,vz);
672 z = dotvv(vz,vx);
673
674 if ((x*x + y*y + z*z) < (SCALAR_TYPE) 0.000001) break;
675 }
676
677 M[element(0,0)] = vx[0];
678 M[element(1,0)] = vx[1];
679 M[element(2,0)] = vx[2];
680
681 M[element(0,1)] = vy[0];
682 M[element(1,1)] = vy[1];
683 M[element(2,1)] = vy[2];
684
685 M[element(0,2)] = vz[0];
686 M[element(1,2)] = vz[1];
687 M[element(2,2)] = vz[2];
688 }
689
690
691
692
693
694 ////////////////////////////////////////////////////////////////////////////
695 //
696 //
697 //
698 // Misc quaternion operations
699 //
700 //
701 //
702 ////////////////////////////////////////////////////////////////////////////
703
704 ////////////////////////////////////////////////////////////////////////////
705 // Initialize a quaternion
normalizeq(SCALAR_TYPE * q)706 inline void normalizeq(SCALAR_TYPE *q) {
707 const double l = 1 / SQRT(q[0]*q[0] + q[1]*q[1] + q[2]*q[2] + q[3]*q[3]);
708 q[0] = (SCALAR_TYPE) (q[0]*l);
709 q[1] = (SCALAR_TYPE) (q[1]*l);
710 q[2] = (SCALAR_TYPE) (q[2]*l);
711 q[3] = (SCALAR_TYPE) (q[3]*l);
712 }
713
714
715 ////////////////////////////////////////////////////////////////////////////
716 // move a quaternion
movqq(SCALAR_TYPE * dest,const SCALAR_TYPE * src)717 inline void movqq(SCALAR_TYPE *dest,const SCALAR_TYPE *src) {
718 dest[0] = src[0];
719 dest[1] = src[1];
720 dest[2] = src[2];
721 dest[3] = src[3];
722 }
723
724 ////////////////////////////////////////////////////////////////////////////
725 // Initialize a quaternion
initq(SCALAR_TYPE * q,const SCALAR_TYPE x)726 inline void initq(SCALAR_TYPE *q,const SCALAR_TYPE x) {
727 q[0] = x;
728 q[1] = x;
729 q[2] = x;
730 q[3] = x;
731
732 normalizeq(q);
733 }
734
735 ////////////////////////////////////////////////////////////////////////////
736 // Initialize a quaternion
initq(SCALAR_TYPE * q,const SCALAR_TYPE x,const SCALAR_TYPE y,const SCALAR_TYPE z,const SCALAR_TYPE s)737 inline void initq(SCALAR_TYPE *q,const SCALAR_TYPE x,const SCALAR_TYPE y,const SCALAR_TYPE z,const SCALAR_TYPE s) {
738 q[0] = x;
739 q[1] = y;
740 q[2] = z;
741 q[3] = s;
742
743 normalizeq(q);
744 }
745
746 ////////////////////////////////////////////////////////////////////////////
747 // Multiply two quaternions
mulqq(SCALAR_TYPE * res,const SCALAR_TYPE * q1,const SCALAR_TYPE * q2)748 inline void mulqq(SCALAR_TYPE *res,const SCALAR_TYPE *q1,const SCALAR_TYPE *q2) {
749 res[0] = q1[3] * q2[0] + q1[0] * q2[3] + q1[1] * q2[2] - q1[2] * q2[1];
750 res[1] = q1[3] * q2[1] - q1[0] * q2[2] + q1[1] * q2[3] + q1[2] * q2[0];
751 res[2] = q1[3] * q2[2] + q1[0] * q2[1] - q1[1] * q2[0] + q1[2] * q2[3];
752 res[3] = q1[3] * q2[3] - q1[0] * q2[0] - q1[1] * q2[1] - q1[2] * q2[2];
753 }
754
755 ////////////////////////////////////////////////////////////////////////////
756 // Compute the rotation matrix corresponding to a quaternion
qtoR(SCALAR_TYPE * R,const SCALAR_TYPE * q)757 inline void qtoR(SCALAR_TYPE *R,const SCALAR_TYPE *q) {
758 #define a q[0]
759 #define b q[1]
760 #define c q[2]
761 #define s q[3]
762
763
764 R[element(0,0)] = (SCALAR_TYPE) (1-2*b*b-2*c*c);
765 R[element(0,1)] = (SCALAR_TYPE) (2*a*b-2*s*c);
766 R[element(0,2)] = (SCALAR_TYPE) (2*a*c+2*s*b);
767 R[element(0,3)] = 0;
768 R[element(1,0)] = (SCALAR_TYPE) (2*a*b+2*s*c);
769 R[element(1,1)] = (SCALAR_TYPE) (1-2*a*a-2*c*c);
770 R[element(1,2)] = (SCALAR_TYPE) (2*b*c-2*s*a);
771 R[element(1,3)] = 0;
772 R[element(2,0)] = (SCALAR_TYPE) (2*a*c-2*s*b);
773 R[element(2,1)] = (SCALAR_TYPE) (2*b*c+2*s*a);
774 R[element(2,2)] = (SCALAR_TYPE) (1-2*a*a-2*b*b);
775 R[element(2,3)] = 0;
776 R[element(3,0)] = 0;
777 R[element(3,1)] = 0;
778 R[element(3,2)] = 0;
779 R[element(3,3)] = 1;
780
781 #undef a
782 #undef b
783 #undef c
784 #undef s
785 }
786
787
788 ////////////////////////////////////////////////////////////////////////////
789 // Compute the quaternion corresponding to a rotation matrix
qfromR(SCALAR_TYPE * q,const SCALAR_TYPE * R)790 inline void qfromR(SCALAR_TYPE *q,const SCALAR_TYPE *R) {
791 double trace = R[element(0,0)] + R[element(1,1)] + R[element(2,2)] + 1.0;
792 double a,b,c,d;
793
794 if( trace > C_EPSILON ) {
795 d = sqrt(trace)*0.5;
796 a = (R[element(2,1)] - R[element(1,2)]) / (4*d);
797 b = (R[element(0,2)] - R[element(2,0)]) / (4*d);
798 c = (R[element(1,0)] - R[element(0,1)]) / (4*d);
799 } else {
800 if ((R[element(0,0)] > R[element(1,1)]) && (R[element(0,0)] > R[element(2,2)])) {
801 a = sqrt( 1.0 + R[element(0,0)] - R[element(1,1)] - R[element(2,2)])*0.5;
802 b = (R[element(0,1)] + R[element(1,0)] ) / (4*a);
803 c = (R[element(0,2)] + R[element(2,0)] ) / (4*a);
804 d = (R[element(2,1)] - R[element(1,2)] ) / (4*a);
805 } else if (R[element(1,1)] > R[element(2,2)]) {
806 b = sqrt( 1.0 + R[element(1,1)] - R[element(0,0)] - R[element(2,2)])*0.5;
807 a = (R[element(0,1)] + R[element(1,0)] ) / (4*b);
808 c = (R[element(1,2)] + R[element(2,1)] ) / (4*b);
809 d = (R[element(0,2)] - R[element(2,0)] ) / (4*b);
810 } else {
811 c = sqrt(1.0 + R[element(2,2)] - R[element(0,0)] - R[element(1,1)])*0.5;
812 a = (R[element(0,2)] + R[element(2,0)] ) / (4*c);
813 b = (R[element(1,2)] + R[element(2,1)] ) / (4*c);
814 d = (R[element(1,0)] - R[element(0,1)] ) / (4*c);
815 }
816 }
817
818 q[0] = (SCALAR_TYPE) a;
819 q[1] = (SCALAR_TYPE) b;
820 q[2] = (SCALAR_TYPE) c;
821 q[3] = (SCALAR_TYPE) d;
822 }
823
824