Line data Source code
1 : /*
2 : * GPAC - Multimedia Framework C SDK
3 : *
4 : * Authors: Jean Le Feuvre
5 : * Copyright (c) Telecom ParisTech 2000-2012
6 : * All rights reserved
7 : *
8 : * This file is part of GPAC / common tools sub-project
9 : *
10 : * GPAC is free software; you can redistribute it and/or modify
11 : * it under the terms of the GNU Lesser General Public License as published by
12 : * the Free Software Foundation; either version 2, or (at your option)
13 : * any later version.
14 : *
15 : * GPAC is distributed in the hope that it will be useful,
16 : * but WITHOUT ANY WARRANTY; without even the implied warranty of
17 : * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
18 : * GNU Lesser General Public License for more details.
19 : *
20 : * You should have received a copy of the GNU Lesser General Public
21 : * License along with this library; see the file COPYING. If not, write to
22 : * the Free Software Foundation, 675 Mass Ave, Cambridge, MA 02139, USA.
23 : *
24 : *
25 : * fixed-point trigo routines taken from freetype
26 : * Copyright 1996-2001, 2002, 2004 by
27 : * David Turner, Robert Wilhelm, and Werner Lemberg.
28 : * License: FTL or GPL
29 : *
30 : */
31 :
32 : #include <gpac/maths.h>
33 :
34 : GF_EXPORT
35 39141 : u32 gf_get_bit_size(u32 MaxVal)
36 : {
37 : u32 k=0;
38 39141 : while ((s32) MaxVal > ((1<<k)-1) ) k+=1;
39 39141 : return k;
40 : }
41 :
42 : #ifdef GPAC_FIXED_POINT
43 :
44 : #ifndef __GNUC__
45 : #pragma message("Compiling with fixed-point arithmetic")
46 : #endif
47 :
48 : #if defined(__SYMBIAN32__) && !defined(__SERIES60_3X__)
49 : typedef long long fix_s64;
50 : #else
51 : typedef s64 fix_s64;
52 : #endif
53 :
54 :
55 : /* the following is 0.2715717684432231 * 2^30 */
56 : #define GF_TRIG_COSCALE 0x11616E8EUL
57 :
58 : /* this table was generated for degrees */
59 : #define GF_TRIG_MAX_ITERS 23
60 :
61 : static const Fixed gf_trig_arctan_table[24] =
62 : {
63 : 4157273L, 2949120L, 1740967L, 919879L, 466945L, 234379L, 117304L,
64 : 58666L, 29335L, 14668L, 7334L, 3667L, 1833L, 917L, 458L, 229L, 115L,
65 : 57L, 29L, 14L, 7L, 4L, 2L, 1L
66 : };
67 :
68 : /* the Cordic shrink factor, multiplied by 2^32 */
69 : #define GF_TRIG_SCALE 1166391785 /* 0x4585BA38UL */
70 :
71 : #define GF_ANGLE_PI (180<<16)
72 : #define GF_ANGLE_PI2 (90<<16)
73 : #define GF_ANGLE_2PI (360<<16)
74 :
75 : #define GF_PAD_FLOOR( x, n ) ( (x) & ~((n)-1) )
76 : #define GF_PAD_ROUND( x, n ) GF_PAD_FLOOR( (x) + ((n)/2), n )
77 : #define GF_PAD_CEIL( x, n ) GF_PAD_FLOOR( (x) + ((n)-1), n )
78 :
79 : static GFINLINE s32 gf_trig_prenorm(GF_Point2D *vec)
80 : {
81 : Fixed x, y, z;
82 : s32 shift;
83 : x = vec->x;
84 : y = vec->y;
85 : z = ( ( x >= 0 ) ? x : - x ) | ( (y >= 0) ? y : -y );
86 : shift = 0;
87 : if ( z < ( 1L << 27 ) ) {
88 : do {
89 : shift++;
90 : z <<= 1;
91 : }
92 : while ( z < ( 1L << 27 ) );
93 :
94 : vec->x = x << shift;
95 : vec->y = y << shift;
96 : }
97 : else if ( z > ( 1L << 28 ) ) {
98 : do {
99 : shift++;
100 : z >>= 1;
101 : } while ( z > ( 1L << 28 ) );
102 :
103 : vec->x = x >> shift;
104 : vec->y = y >> shift;
105 : shift = -shift;
106 : }
107 : return shift;
108 : }
109 :
110 : #define ANGLE_RAD_TO_DEG(_th) ((s32) ( (((fix_s64)_th)*5729582)/100000))
111 : #define ANGLE_DEG_TO_RAD(_th) ((s32) ( (((fix_s64)_th)*100000)/5729582))
112 :
113 : static GFINLINE void gf_trig_pseudo_polarize(GF_Point2D *vec)
114 : {
115 : Fixed theta;
116 : Fixed yi, i;
117 : Fixed x, y;
118 : const Fixed *arctanptr;
119 :
120 : x = vec->x;
121 : y = vec->y;
122 :
123 : /* Get the vector into the right half plane */
124 : theta = 0;
125 : if ( x < 0 ) {
126 : x = -x;
127 : y = -y;
128 : theta = 2 * GF_ANGLE_PI2;
129 : }
130 :
131 : if ( y > 0 )
132 : theta = - theta;
133 :
134 : arctanptr = gf_trig_arctan_table;
135 :
136 : if ( y < 0 ) {
137 : /* Rotate positive */
138 : yi = y + ( x << 1 );
139 : x = x - ( y << 1 );
140 : y = yi;
141 : theta -= *arctanptr++; /* Subtract angle */
142 : } else {
143 : /* Rotate negative */
144 : yi = y - ( x << 1 );
145 : x = x + ( y << 1 );
146 : y = yi;
147 : theta += *arctanptr++; /* Add angle */
148 : }
149 :
150 : i = 0;
151 : do {
152 : if ( y < 0 ) {
153 : /* Rotate positive */
154 : yi = y + ( x >> i );
155 : x = x - ( y >> i );
156 : y = yi;
157 : theta -= *arctanptr++;
158 : } else {
159 : /* Rotate negative */
160 : yi = y - ( x >> i );
161 : x = x + ( y >> i );
162 : y = yi;
163 : theta += *arctanptr++;
164 : }
165 : }
166 : while ( ++i < GF_TRIG_MAX_ITERS );
167 :
168 : /* round theta */
169 : if ( theta >= 0 )
170 : theta = GF_PAD_ROUND( theta, 32 );
171 : else
172 : theta = - GF_PAD_ROUND( -theta, 32 );
173 :
174 : vec->x = x;
175 : vec->y = ANGLE_DEG_TO_RAD(theta);
176 : }
177 :
178 : GF_EXPORT
179 : Fixed gf_atan2(Fixed dy, Fixed dx)
180 : {
181 : GF_Point2D v;
182 : if ( dx == 0 && dy == 0 ) return 0;
183 : v.x = dx;
184 : v.y = dy;
185 : gf_trig_prenorm( &v );
186 : gf_trig_pseudo_polarize( &v );
187 : return v.y;
188 : }
189 :
190 : /*define one of these to enable IntelGPP support and link to gpp_WMMX40_d.lib (debug) or gpp_WMMX40_r.lib (release)
191 : this is not configured by default for lack of real perf increase.*/
192 : #if defined(GPAC_USE_IGPP_HP) || defined(GPAC_USE_IGPP)
193 : # pragma message("Using IntelGPP math library")
194 : #ifdef _DEBUG
195 : # pragma comment(lib, "gpp_WMMX40_d")
196 : #else
197 : # pragma comment(lib, "gpp_WMMX40_r")
198 : #endif
199 :
200 : #include <gpp.h>
201 :
202 : GF_EXPORT
203 : Fixed gf_invfix(Fixed a)
204 : {
205 : Fixed res;
206 : if (!a) return (s32)0x7FFFFFFFL;
207 : gppInv_16_32s(a, &res);
208 : return res;
209 : }
210 : GF_EXPORT
211 : Fixed gf_divfix(Fixed a, Fixed b)
212 : {
213 : Fixed res;
214 : if (!a) return 0;
215 : else if (!b) return (a<0) ? -(s32)0x7FFFFFFFL : (s32)0x7FFFFFFFL;
216 : else if (a==FIX_ONE) gppInv_16_32s(b, &res);
217 : else gppDiv_16_32s(a, b, &res);
218 : return res;
219 : }
220 :
221 : GF_EXPORT
222 : Fixed gf_mulfix(Fixed a, Fixed b)
223 : {
224 : Fixed res;
225 : if (!a || !b) return 0;
226 : else if (b == FIX_ONE) return a;
227 : else gppMul_16_32s(a, b, &res);
228 : return res;
229 : }
230 :
231 : GF_EXPORT
232 : Fixed gf_sqrt(Fixed x)
233 : {
234 : Fixed res;
235 : #ifdef GPAC_USE_IGPP_HP
236 : gppSqrtHP_16_32s(x, &res);
237 : #else
238 : gppSqrtLP_16_32s(x, &res);
239 : #endif
240 : return res;
241 : }
242 :
243 : GF_EXPORT
244 : Fixed gf_cos(Fixed angle)
245 : {
246 : Fixed res;
247 : #ifdef GPAC_USE_IGPP_HP
248 : gppCosHP_16_32s(angle, &res);
249 : #else
250 : gppCosLP_16_32s(angle, &res);
251 : #endif
252 : return res;
253 : }
254 :
255 : GF_EXPORT
256 : Fixed gf_sin(Fixed angle)
257 : {
258 : Fixed res;
259 : #ifdef GPAC_USE_IGPP_HP
260 : gppSinHP_16_32s (angle, &res);
261 : #else
262 : gppSinLP_16_32s (angle, &res);
263 : #endif
264 : return res;
265 : }
266 :
267 :
268 : GF_EXPORT
269 : Fixed gf_tan(Fixed angle)
270 : {
271 : Fixed cosa, sina;
272 : #ifdef GPAC_USE_IGPP_HP
273 : gppSinCosHP_16_32s(angle, &sina, &cosa);
274 : #else
275 : gppSinCosLP_16_32s(angle, &sina, &cosa);
276 : #endif
277 : if (!cosa) return (sina<0) ? -GF_PI2 : GF_PI2;
278 : return gf_divfix(sina, cosa);
279 : }
280 :
281 : GF_EXPORT
282 : GF_Point2D gf_v2d_from_polar(Fixed length, Fixed angle)
283 : {
284 : GF_Point2D vec;
285 : Fixed cosa, sina;
286 : #ifdef GPAC_USE_IGPP_HP
287 : gppSinCosHP_16_32s(angle, &sina, &cosa);
288 : #else
289 : gppSinCosLP_16_32s(angle, &sina, &cosa);
290 : #endif
291 : vec.x = gf_mulfix(length, cosa);
292 : vec.y = gf_mulfix(length, sina);
293 : return vec;
294 : }
295 :
296 : GF_EXPORT
297 : Fixed gf_v2d_len(GF_Point2D *vec)
298 : {
299 : Fixed a, b, res;
300 : if (!vec->x) return ABS(vec->y);
301 : if (!vec->y) return ABS(vec->x);
302 : gppSquare_16_32s(vec->x, &a);
303 : gppSquare_16_32s(vec->y, &b);
304 : #ifdef GPAC_USE_IGPP_HP
305 : gppSqrtHP_16_32s(a+b, &res);
306 : #else
307 : gppSqrtLP_16_32s(a+b, &res);
308 : #endif
309 : return res;
310 : }
311 :
312 : #else
313 :
314 : GF_EXPORT
315 : Fixed gf_invfix(Fixed a)
316 : {
317 : if (!a) return (s32)0x7FFFFFFFL;
318 : return gf_divfix(FIX_ONE, a);
319 : }
320 :
321 : GF_EXPORT
322 : Fixed gf_divfix(Fixed a, Fixed b)
323 : {
324 : s32 s;
325 : u32 q;
326 : if (!a) return 0;
327 : s = 1;
328 : if ( a < 0 ) {
329 : a = -a;
330 : s = -1;
331 : }
332 : if ( b < 0 ) {
333 : b = -b;
334 : s = -s;
335 : }
336 :
337 : if ( b == 0 )
338 : /* check for division by 0 */
339 : q = 0x7FFFFFFFL;
340 : else {
341 : /* compute result directly */
342 : q = (u32)( ( ( (fix_s64)a << 16 ) + ( b >> 1 ) ) / b );
343 : }
344 : return ( s < 0 ? -(s32)q : (s32)q );
345 : }
346 :
347 : GF_EXPORT
348 : Fixed gf_mulfix(Fixed a, Fixed b)
349 : {
350 : Fixed s;
351 : u32 ua, ub;
352 : if ( !a || (b == 0x10000L)) return a;
353 : if (!b) return 0;
354 :
355 : s = a;
356 : a = ABS(a);
357 : s ^= b;
358 : b = ABS(b);
359 :
360 : ua = (u32)a;
361 : ub = (u32)b;
362 :
363 : if ((ua <= 2048) && (ub <= 1048576L)) {
364 : ua = ( ua * ub + 0x8000L ) >> 16;
365 : } else {
366 : u32 al = ua & 0xFFFFL;
367 : ua = ( ua >> 16 ) * ub + al * ( ub >> 16 ) + ( ( al * ( ub & 0xFFFFL ) + 0x8000L ) >> 16 );
368 : }
369 : if (ua & 0x80000000) s=-s;
370 : return ( s < 0 ? -(Fixed)(s32)ua : (Fixed)ua );
371 : }
372 :
373 :
374 : GF_EXPORT
375 : Fixed gf_sqrt(Fixed x)
376 : {
377 : u32 root, rem_hi, rem_lo, test_div;
378 : s32 count;
379 : root = 0;
380 : if ( x > 0 ) {
381 : rem_hi = 0;
382 : rem_lo = x;
383 : count = 24;
384 : do {
385 : rem_hi = ( rem_hi << 2 ) | ( rem_lo >> 30 );
386 : rem_lo <<= 2;
387 : root <<= 1;
388 : test_div = ( root << 1 ) + 1;
389 :
390 : if ( rem_hi >= test_div ) {
391 : rem_hi -= test_div;
392 : root += 1;
393 : }
394 : } while ( --count );
395 : }
396 :
397 : return (Fixed) root;
398 : }
399 :
400 :
401 : /* multiply a given value by the CORDIC shrink factor */
402 : static Fixed gf_trig_downscale(Fixed val)
403 : {
404 : Fixed s;
405 : fix_s64 v;
406 : s = val;
407 : val = ( val >= 0 ) ? val : -val;
408 : #ifdef _MSC_VER
409 : v = ( val * (fix_s64)GF_TRIG_SCALE ) + 0x100000000;
410 : #else
411 : v = ( val * (fix_s64)GF_TRIG_SCALE ) + 0x100000000ULL;
412 : #endif
413 : val = (Fixed)( v >> 32 );
414 : return ( s >= 0 ) ? val : -val;
415 : }
416 :
417 : static void gf_trig_pseudo_rotate(GF_Point2D* vec, Fixed theta)
418 : {
419 : s32 i;
420 : Fixed x, y, xtemp;
421 : const Fixed *arctanptr;
422 :
423 : /*trig funcs are in degrees*/
424 : theta = ANGLE_RAD_TO_DEG(theta);
425 :
426 : x = vec->x;
427 : y = vec->y;
428 :
429 : /* Get angle between -90 and 90 degrees */
430 : while ( theta <= -GF_ANGLE_PI2 ) {
431 : x = -x;
432 : y = -y;
433 : theta += GF_ANGLE_PI;
434 : }
435 :
436 : while ( theta > GF_ANGLE_PI2 ) {
437 : x = -x;
438 : y = -y;
439 : theta -= GF_ANGLE_PI;
440 : }
441 :
442 : /* Initial pseudorotation, with left shift */
443 : arctanptr = gf_trig_arctan_table;
444 : if ( theta < 0 ) {
445 : xtemp = x + ( y << 1 );
446 : y = y - ( x << 1 );
447 : x = xtemp;
448 : theta += *arctanptr++;
449 : } else {
450 : xtemp = x - ( y << 1 );
451 : y = y + ( x << 1 );
452 : x = xtemp;
453 : theta -= *arctanptr++;
454 : }
455 : /* Subsequent pseudorotations, with right shifts */
456 : i = 0;
457 : do {
458 : if ( theta < 0 ) {
459 : xtemp = x + ( y >> i );
460 : y = y - ( x >> i );
461 : x = xtemp;
462 : theta += *arctanptr++;
463 : } else {
464 : xtemp = x - ( y >> i );
465 : y = y + ( x >> i );
466 : x = xtemp;
467 : theta -= *arctanptr++;
468 : }
469 : } while ( ++i < GF_TRIG_MAX_ITERS );
470 :
471 : vec->x = x;
472 : vec->y = y;
473 : }
474 :
475 : /* these macros return 0 for positive numbers, and -1 for negative ones */
476 : #define GF_SIGN_LONG( x ) ( (x) >> ( 32 - 1 ) )
477 : #define GF_SIGN_INT( x ) ( (x) >> ( 32 - 1 ) )
478 : #define GF_SIGN_INT32( x ) ( (x) >> 31 )
479 : #define GF_SIGN_INT16( x ) ( (x) >> 15 )
480 :
481 : static void gf_v2d_rotate(GF_Point2D *vec, Fixed angle)
482 : {
483 : s32 shift;
484 : GF_Point2D v;
485 :
486 : v.x = vec->x;
487 : v.y = vec->y;
488 :
489 : if ( angle && ( v.x != 0 || v.y != 0 ) ) {
490 : shift = gf_trig_prenorm( &v );
491 : gf_trig_pseudo_rotate( &v, angle );
492 : v.x = gf_trig_downscale( v.x );
493 : v.y = gf_trig_downscale( v.y );
494 :
495 : if ( shift > 0 ) {
496 : s32 half = 1L << ( shift - 1 );
497 :
498 : vec->x = ( v.x + half + GF_SIGN_LONG( v.x ) ) >> shift;
499 : vec->y = ( v.y + half + GF_SIGN_LONG( v.y ) ) >> shift;
500 : } else {
501 : shift = -shift;
502 : vec->x = v.x << shift;
503 : vec->y = v.y << shift;
504 : }
505 : }
506 : }
507 :
508 : GF_EXPORT
509 : Fixed gf_v2d_len(GF_Point2D *vec)
510 : {
511 : s32 shift;
512 : GF_Point2D v;
513 : v = *vec;
514 :
515 : /* handle trivial cases */
516 : if ( v.x == 0 ) {
517 : return ( v.y >= 0 ) ? v.y : -v.y;
518 : } else if ( v.y == 0 ) {
519 : return ( v.x >= 0 ) ? v.x : -v.x;
520 : }
521 :
522 : /* general case */
523 : shift = gf_trig_prenorm( &v );
524 : gf_trig_pseudo_polarize( &v );
525 : v.x = gf_trig_downscale( v.x );
526 : if ( shift > 0 )
527 : return ( v.x + ( 1 << ( shift - 1 ) ) ) >> shift;
528 :
529 : return v.x << -shift;
530 : }
531 :
532 :
533 : GF_EXPORT
534 : void gf_v2d_polarize(GF_Point2D *vec, Fixed *length, Fixed *angle)
535 : {
536 : s32 shift;
537 : GF_Point2D v;
538 : v = *vec;
539 : if ( v.x == 0 && v.y == 0 )
540 : return;
541 :
542 : shift = gf_trig_prenorm( &v );
543 : gf_trig_pseudo_polarize( &v );
544 : v.x = gf_trig_downscale( v.x );
545 : *length = ( shift >= 0 ) ? ( v.x >> shift ) : ( v.x << -shift );
546 : *angle = v.y;
547 : }
548 :
549 : GF_EXPORT
550 : GF_Point2D gf_v2d_from_polar(Fixed length, Fixed angle)
551 : {
552 : GF_Point2D vec;
553 : vec.x = length;
554 : vec.y = 0;
555 : gf_v2d_rotate(&vec, angle);
556 : return vec;
557 : }
558 :
559 : GF_EXPORT
560 : Fixed gf_cos(Fixed angle)
561 : {
562 : GF_Point2D v;
563 : v.x = GF_TRIG_COSCALE >> 2;
564 : v.y = 0;
565 : gf_trig_pseudo_rotate( &v, angle );
566 : return v.x / ( 1 << 12 );
567 : }
568 : GF_EXPORT
569 : Fixed gf_sin(Fixed angle)
570 : {
571 : return gf_cos( GF_PI2 - angle );
572 : }
573 : GF_EXPORT
574 : Fixed gf_tan(Fixed angle)
575 : {
576 : GF_Point2D v;
577 : v.x = GF_TRIG_COSCALE >> 2;
578 : v.y = 0;
579 : gf_trig_pseudo_rotate( &v, angle );
580 : return gf_divfix( v.y, v.x );
581 : }
582 :
583 : #endif
584 :
585 : /*common parts*/
586 : GF_EXPORT
587 : Fixed gf_muldiv(Fixed a, Fixed b, Fixed c)
588 : {
589 : s32 s, d;
590 : if (!b || !a) return 0;
591 : s = 1;
592 : if ( a < 0 ) {
593 : a = -a;
594 : s = -1;
595 : }
596 : if ( b < 0 ) {
597 : b = -b;
598 : s = -s;
599 : }
600 : if ( c < 0 ) {
601 : c = -c;
602 : s = -s;
603 : }
604 :
605 : d = (s32)( c > 0 ? ( (fix_s64)a * b + ( c >> 1 ) ) / c : 0x7FFFFFFFL);
606 : return (Fixed) (( s > 0 ) ? d : -d);
607 : }
608 :
609 :
610 : GF_EXPORT
611 : Fixed gf_ceil(Fixed a)
612 : {
613 : return (a >= 0) ? (a + 0xFFFFL) & ~0xFFFFL : -((-a + 0xFFFFL) & ~0xFFFFL);
614 : }
615 :
616 : GF_EXPORT
617 : Fixed gf_floor(Fixed a)
618 : {
619 : return (a >= 0) ? (a & ~0xFFFFL) : -((-a) & ~0xFFFFL);
620 : }
621 :
622 :
623 : /*FIXME*/
624 : GF_EXPORT
625 : Fixed gf_acos(Fixed angle)
626 : {
627 : return FLT2FIX( (Float) acos(FIX2FLT(angle)));
628 : }
629 :
630 : /*FIXME*/
631 : GF_EXPORT
632 : Fixed gf_asin(Fixed angle)
633 : {
634 : return FLT2FIX( (Float) asin(FIX2FLT(angle)));
635 : }
636 :
637 :
638 : #else
639 :
640 :
641 : GF_EXPORT
642 2636328 : GF_Point2D gf_v2d_from_polar(Fixed length, Fixed angle)
643 : {
644 : GF_Point2D vec;
645 2636328 : vec.x = length*(Float) cos(angle);
646 2636328 : vec.y = length*(Float) sin(angle);
647 2636328 : return vec;
648 : }
649 :
650 : GF_EXPORT
651 2683427 : Fixed gf_v2d_len(GF_Point2D *vec)
652 : {
653 2683427 : if (!vec->x) return ABS(vec->y);
654 2619922 : if (!vec->y) return ABS(vec->x);
655 2561803 : return (Fixed) sqrt(vec->x*vec->x + vec->y*vec->y);
656 : }
657 :
658 : /*
659 :
660 : Fixed gf_cos(_a) { return (Float) cos(_a); }
661 : Fixed gf_sin(_a) { return (Float) sin(_a); }
662 : Fixed gf_tan(_a) { return (Float) tan(_a); }
663 : Fixed gf_atan2(_y, _x) { return (Float) atan2(_y, _x); }
664 : Fixed gf_sqrt(_x) { return (Float) sqrt(_x); }
665 : Fixed gf_ceil(_a) { return (Float) ceil(_a); }
666 : Fixed gf_floor(_a) { return (Float) floor(_a); }
667 : Fixed gf_acos(_a) { return (Float) acos(_a); }
668 : Fixed gf_asin(_a) { return (Float) asin(_a); }
669 :
670 : */
671 :
672 : #endif
673 :
674 : GF_EXPORT
675 0 : Fixed gf_v2d_distance(GF_Point2D *a, GF_Point2D *b)
676 : {
677 : GF_Point2D d;
678 0 : d.x = a->x - b->x;
679 0 : d.y = a->y - b->y;
680 0 : return gf_v2d_len(&d);
681 : }
682 :
683 : GF_EXPORT
684 1985517 : Fixed gf_angle_diff(Fixed angle1, Fixed angle2)
685 : {
686 1985517 : Fixed delta = angle2 - angle1;
687 : #ifdef GPAC_FIXED_POINT
688 : delta %= GF_2PI;
689 : if (delta < 0) delta += GF_2PI;
690 : if (delta > GF_PI) delta -= GF_2PI;
691 : #else
692 1985517 : while (delta < 0) delta += GF_2PI;
693 1013175 : while (delta > GF_PI) delta -= GF_2PI;
694 : #endif
695 1985517 : return delta;
696 : }
697 :
698 :
699 :
700 : /*
701 : 2D MATRIX TOOLS
702 : */
703 :
704 : GF_EXPORT
705 3408113 : void gf_mx2d_add_matrix(GF_Matrix2D *_this, GF_Matrix2D *from)
706 : {
707 : GF_Matrix2D bck;
708 5028714 : if (!_this || !from) return;
709 :
710 3408113 : if (gf_mx2d_is_identity(*from)) return;
711 3168767 : else if (gf_mx2d_is_identity(*_this)) {
712 1381255 : gf_mx2d_copy(*_this, *from);
713 : return;
714 : }
715 1787512 : gf_mx2d_copy(bck, *_this);
716 1787512 : _this->m[0] = gf_mulfix(from->m[0], bck.m[0]) + gf_mulfix(from->m[1], bck.m[3]);
717 1787512 : _this->m[1] = gf_mulfix(from->m[0], bck.m[1]) + gf_mulfix(from->m[1], bck.m[4]);
718 1787512 : _this->m[2] = gf_mulfix(from->m[0], bck.m[2]) + gf_mulfix(from->m[1], bck.m[5]) + from->m[2];
719 1787512 : _this->m[3] = gf_mulfix(from->m[3], bck.m[0]) + gf_mulfix(from->m[4], bck.m[3]);
720 1787512 : _this->m[4] = gf_mulfix(from->m[3], bck.m[1]) + gf_mulfix(from->m[4], bck.m[4]);
721 1787512 : _this->m[5] = gf_mulfix(from->m[3], bck.m[2]) + gf_mulfix(from->m[4], bck.m[5]) + from->m[5];
722 : }
723 :
724 :
725 : GF_EXPORT
726 198204 : void gf_mx2d_pre_multiply(GF_Matrix2D *_this, GF_Matrix2D *with)
727 : {
728 : GF_Matrix2D bck;
729 206075 : if (!_this || !with) return;
730 :
731 198204 : if (gf_mx2d_is_identity(*with)) return;
732 194085 : else if (gf_mx2d_is_identity(*_this)) {
733 3752 : gf_mx2d_copy(*_this, *with);
734 : return;
735 : }
736 190333 : gf_mx2d_copy(bck, *_this);
737 190333 : _this->m[0] = gf_mulfix(bck.m[0], with->m[0]) + gf_mulfix(bck.m[1], with->m[3]);
738 190333 : _this->m[1] = gf_mulfix(bck.m[0], with->m[1]) + gf_mulfix(bck.m[1], with->m[4]);
739 190333 : _this->m[2] = gf_mulfix(bck.m[0], with->m[2]) + gf_mulfix(bck.m[1], with->m[5]) + bck.m[2];
740 190333 : _this->m[3] = gf_mulfix(bck.m[3], with->m[0]) + gf_mulfix(bck.m[4], with->m[3]);
741 190333 : _this->m[4] = gf_mulfix(bck.m[3], with->m[1]) + gf_mulfix(bck.m[4], with->m[4]);
742 190333 : _this->m[5] = gf_mulfix(bck.m[3], with->m[2]) + gf_mulfix(bck.m[4], with->m[5]) + bck.m[5];
743 : }
744 :
745 :
746 : GF_EXPORT
747 721066 : void gf_mx2d_add_translation(GF_Matrix2D *_this, Fixed cx, Fixed cy)
748 : {
749 : GF_Matrix2D tmp;
750 751466 : if (!_this || (!cx && !cy) ) return;
751 690666 : gf_mx2d_init(tmp);
752 690666 : tmp.m[2] = cx;
753 690666 : tmp.m[5] = cy;
754 690666 : gf_mx2d_add_matrix(_this, &tmp);
755 : }
756 :
757 :
758 : GF_EXPORT
759 9391 : void gf_mx2d_add_rotation(GF_Matrix2D *_this, Fixed cx, Fixed cy, Fixed angle)
760 : {
761 : GF_Matrix2D tmp;
762 9391 : if (!_this) return;
763 9391 : gf_mx2d_init(tmp);
764 :
765 9391 : gf_mx2d_add_translation(_this, -cx, -cy);
766 :
767 9391 : tmp.m[0] = gf_cos(angle);
768 9391 : tmp.m[4] = tmp.m[0];
769 9391 : tmp.m[3] = gf_sin(angle);
770 9391 : tmp.m[1] = -1 * tmp.m[3];
771 9391 : gf_mx2d_add_matrix(_this, &tmp);
772 9391 : gf_mx2d_add_translation(_this, cx, cy);
773 : }
774 :
775 : GF_EXPORT
776 670231 : void gf_mx2d_add_scale(GF_Matrix2D *_this, Fixed scale_x, Fixed scale_y)
777 : {
778 : GF_Matrix2D tmp;
779 674258 : if (!_this || ((scale_x==FIX_ONE) && (scale_y==FIX_ONE)) ) return;
780 : gf_mx2d_init(tmp);
781 666204 : tmp.m[0] = scale_x;
782 666204 : tmp.m[4] = scale_y;
783 666204 : gf_mx2d_add_matrix(_this, &tmp);
784 : }
785 :
786 : GF_EXPORT
787 2540 : void gf_mx2d_add_scale_at(GF_Matrix2D *_this, Fixed scale_x, Fixed scale_y, Fixed cx, Fixed cy, Fixed angle)
788 : {
789 : GF_Matrix2D tmp;
790 2540 : if (!_this) return;
791 2540 : gf_mx2d_init(tmp);
792 2540 : if (angle) {
793 38 : gf_mx2d_add_rotation(_this, cx, cy, -angle);
794 : }
795 2540 : tmp.m[0] = scale_x;
796 2540 : tmp.m[4] = scale_y;
797 2540 : gf_mx2d_add_matrix(_this, &tmp);
798 2540 : if (angle) gf_mx2d_add_rotation(_this, cx, cy, angle);
799 : }
800 :
801 : GF_EXPORT
802 2 : void gf_mx2d_add_skew(GF_Matrix2D *_this, Fixed skew_x, Fixed skew_y)
803 : {
804 : GF_Matrix2D tmp;
805 2 : if (!_this || (!skew_x && !skew_y) ) return;
806 2 : gf_mx2d_init(tmp);
807 2 : tmp.m[1] = skew_x;
808 2 : tmp.m[3] = skew_y;
809 2 : gf_mx2d_add_matrix(_this, &tmp);
810 : }
811 :
812 : GF_EXPORT
813 2 : void gf_mx2d_add_skew_x(GF_Matrix2D *_this, Fixed angle)
814 : {
815 : GF_Matrix2D tmp;
816 2 : if (!_this) return;
817 2 : gf_mx2d_init(tmp);
818 2 : tmp.m[1] = gf_tan(angle);
819 : tmp.m[3] = 0;
820 2 : gf_mx2d_add_matrix(_this, &tmp);
821 : }
822 :
823 : GF_EXPORT
824 2 : void gf_mx2d_add_skew_y(GF_Matrix2D *_this, Fixed angle)
825 : {
826 : GF_Matrix2D tmp;
827 2 : if (!_this) return;
828 2 : gf_mx2d_init(tmp);
829 : tmp.m[1] = 0;
830 2 : tmp.m[3] = gf_tan(angle);
831 2 : gf_mx2d_add_matrix(_this, &tmp);
832 : }
833 :
834 :
835 : GF_EXPORT
836 129181 : void gf_mx2d_inverse(GF_Matrix2D *_this)
837 : {
838 : #ifdef GPAC_FIXED_POINT
839 : Float _det, _max;
840 : s32 sign, scale;
841 : #endif
842 : Fixed det;
843 : GF_Matrix2D tmp;
844 129983 : if(!_this) return;
845 129181 : if (gf_mx2d_is_identity(*_this)) return;
846 :
847 : #ifdef GPAC_FIXED_POINT
848 : /*we must compute the determinent as a float to avoid fixed-point overflow*/
849 : _det = FIX2FLT(_this->m[0])*FIX2FLT(_this->m[4]) - FIX2FLT(_this->m[1])*FIX2FLT(_this->m[3]);
850 : if (!_det) {
851 : gf_mx2d_init(*_this);
852 : return;
853 : }
854 : sign = _det<0 ? -1 : 1;
855 : _det *= sign;
856 : scale = 1;
857 : _max = FIX2FLT(FIX_MAX);
858 : while (_det / scale > _max) {
859 : scale *= 10;
860 : }
861 : det = sign * FLT2FIX(_det/scale);
862 : #else
863 128388 : det = gf_mulfix(_this->m[0], _this->m[4]) - gf_mulfix(_this->m[1], _this->m[3]);
864 128388 : if (!det) {
865 18 : gf_mx2d_init(*_this);
866 9 : return;
867 : }
868 : #endif
869 :
870 128379 : tmp.m[0] = gf_divfix(_this->m[4], det);
871 128379 : tmp.m[1] = -1 * gf_divfix(_this->m[1], det);
872 128379 : tmp.m[2] = gf_mulfix(gf_divfix(_this->m[1], det), _this->m[5]) - gf_mulfix(gf_divfix(_this->m[4], det), _this->m[2]);
873 :
874 128379 : tmp.m[3] = -1 * gf_divfix(_this->m[3], det);
875 128379 : tmp.m[4] = gf_divfix(_this->m[0], det);
876 128379 : tmp.m[5] = gf_mulfix( gf_divfix(_this->m[3], det), _this->m[2]) - gf_mulfix( gf_divfix(_this->m[5], det), _this->m[0]);
877 :
878 : #ifdef GPAC_FIXED_POINT
879 : if (scale>1) {
880 : tmp.m[0] /= scale;
881 : tmp.m[1] /= scale;
882 : tmp.m[2] /= scale;
883 : tmp.m[3] /= scale;
884 : tmp.m[4] /= scale;
885 : tmp.m[5] /= scale;
886 : }
887 : #endif
888 :
889 128379 : gf_mx2d_copy(*_this, tmp);
890 : }
891 :
892 : GF_EXPORT
893 19971 : Bool gf_mx2d_decompose(GF_Matrix2D *mx, GF_Point2D *scale, Fixed *rotate, GF_Point2D *translate)
894 : {
895 : Fixed det, angle;
896 : Fixed tmp[6];
897 19971 : if(!mx) return GF_FALSE;
898 :
899 19971 : memcpy(tmp, mx->m, sizeof(Fixed)*6);
900 19971 : translate->x = tmp[2];
901 19971 : translate->y = tmp[5];
902 :
903 : /*check ac+bd=0*/
904 19971 : det = gf_mulfix(tmp[0], tmp[3]) + gf_mulfix(tmp[1], tmp[4]);
905 19971 : if (ABS(det) > FIX_EPSILON) {
906 2476 : scale->x = scale->y = 0;
907 2476 : *rotate = 0;
908 2476 : return GF_FALSE;
909 : }
910 17495 : angle = gf_atan2(tmp[3], tmp[4]);
911 17495 : if (angle < FIX_EPSILON) {
912 16408 : scale->x = tmp[0];
913 16408 : scale->y = tmp[4];
914 : } else {
915 1087 : det = gf_cos(angle);
916 1087 : scale->x = gf_divfix(tmp[0], det);
917 1087 : scale->y = gf_divfix(tmp[4], det);
918 : }
919 17495 : *rotate = angle;
920 17495 : return GF_TRUE;
921 : }
922 :
923 : GF_EXPORT
924 41006426 : void gf_mx2d_apply_coords(GF_Matrix2D *_this, Fixed *x, Fixed *y)
925 : {
926 : Fixed _x, _y;
927 41006426 : if (!_this || !x || !y) return;
928 41006426 : _x = gf_mulfix(*x, _this->m[0]) + gf_mulfix(*y, _this->m[1]) + _this->m[2];
929 41006426 : _y = gf_mulfix(*x, _this->m[3]) + gf_mulfix(*y, _this->m[4]) + _this->m[5];
930 41006426 : *x = _x;
931 41006426 : *y = _y;
932 : }
933 :
934 : GF_EXPORT
935 1213914 : void gf_mx2d_apply_point(GF_Matrix2D *_this, GF_Point2D *pt)
936 : {
937 1213914 : gf_mx2d_apply_coords(_this, &pt->x, &pt->y);
938 1213914 : }
939 :
940 : GF_EXPORT
941 233413 : void gf_mx2d_apply_rect(GF_Matrix2D *_this, GF_Rect *rc)
942 : {
943 :
944 : GF_Point2D c1, c2, c3, c4;
945 233413 : c1.x = c2.x = rc->x;
946 233413 : c3.x = c4.x = rc->x + rc->width;
947 233413 : c1.y = c3.y = rc->y;
948 233413 : c2.y = c4.y = rc->y - rc->height;
949 233413 : gf_mx2d_apply_point(_this, &c1);
950 233413 : gf_mx2d_apply_point(_this, &c2);
951 233413 : gf_mx2d_apply_point(_this, &c3);
952 233413 : gf_mx2d_apply_point(_this, &c4);
953 233413 : rc->x = MIN(c1.x, MIN(c2.x, MIN(c3.x, c4.x)));
954 233413 : rc->width = MAX(c1.x, MAX(c2.x, MAX(c3.x, c4.x))) - rc->x;
955 233413 : rc->height = MIN(c1.y, MIN(c2.y, MIN(c3.y, c4.y)));
956 233413 : rc->y = MAX(c1.y, MAX(c2.y, MAX(c3.y, c4.y)));
957 233413 : rc->height = rc->y - rc->height;
958 : assert(rc->height>=0);
959 : assert(rc->width>=0);
960 233413 : }
961 :
962 : /*
963 : RECTANGLE TOOLS
964 : */
965 :
966 : /*transform rect to smallest covering integer pixels rect - this is needed to make sure clearing
967 : of screen is correctly handled, otherwise we have troubles with bitmap hardware blitting (always integer)*/
968 : GF_EXPORT
969 203563 : GF_IRect gf_rect_pixelize(GF_Rect *r)
970 : {
971 : GF_IRect rc;
972 203563 : rc.x = FIX2INT(gf_floor(r->x));
973 203563 : rc.y = FIX2INT(gf_ceil(r->y));
974 203563 : rc.width = FIX2INT(gf_ceil(r->x + r->width)) - rc.x;
975 203563 : rc.height = rc.y - FIX2INT(gf_floor(r->y - r->height));
976 203563 : return rc;
977 : }
978 :
979 :
980 : /*adds @rc2 to @rc1 - the new @rc1 contains the old @rc1 and @rc2*/
981 : GF_EXPORT
982 103599 : void gf_rect_union(GF_Rect *rc1, GF_Rect *rc2)
983 : {
984 103599 : if (!rc1->width || !rc1->height) {
985 17955 : *rc1=*rc2;
986 17955 : return;
987 : }
988 85644 : if (!rc2->width || !rc2->height) return;
989 84551 : if (rc2->x < rc1->x) {
990 359 : rc1->width += rc1->x - rc2->x;
991 359 : rc1->x = rc2->x;
992 : }
993 84551 : if (rc2->x + rc2->width > rc1->x+rc1->width) rc1->width = rc2->x + rc2->width - rc1->x;
994 84551 : if (rc2->y > rc1->y) {
995 1093 : rc1->height += rc2->y - rc1->y;
996 1093 : rc1->y = rc2->y;
997 : }
998 84551 : if (rc2->y - rc2->height < rc1->y - rc1->height) rc1->height = rc1->y - rc2->y + rc2->height;
999 : }
1000 :
1001 : GF_EXPORT
1002 63586 : GF_Rect gf_rect_center(Fixed w, Fixed h)
1003 : {
1004 : GF_Rect rc;
1005 63586 : rc.x=-w/2;
1006 63586 : rc.y=h/2;
1007 : rc.width=w;
1008 : rc.height=h;
1009 63586 : return rc;
1010 : }
1011 :
1012 : GF_EXPORT
1013 1332 : Bool gf_rect_overlaps(GF_Rect rc1, GF_Rect rc2)
1014 : {
1015 1332 : if (! rc2.height || !rc2.width || !rc1.height || !rc1.width) return GF_FALSE;
1016 1332 : if (rc2.x+rc2.width<=rc1.x) return GF_FALSE;
1017 1332 : if (rc2.x>=rc1.x+rc1.width) return GF_FALSE;
1018 1332 : if (rc2.y-rc2.height>=rc1.y) return GF_FALSE;
1019 1332 : if (rc2.y<=rc1.y-rc1.height) return GF_FALSE;
1020 1332 : return GF_TRUE;
1021 : }
1022 :
1023 : GF_EXPORT
1024 4 : Bool gf_rect_equal(GF_Rect *rc1, GF_Rect *rc2)
1025 : {
1026 4 : if ( (rc1->x == rc2->x) && (rc1->y == rc2->y) && (rc1->width == rc2->width) && (rc1->height == rc2->height) )
1027 : return GF_TRUE;
1028 3 : return GF_FALSE;
1029 : }
1030 :
1031 : #ifdef GPAC_FIXED_POINT
1032 :
1033 : /* check if dimension is larger than FIX_ONE*/
1034 : #define IS_HIGH_DIM(_v) ((_v > FIX_ONE) || (_v < (s32)0xFFFF0000))
1035 : /* check if any vector dimension is larger than FIX_ONE*/
1036 : #define VEC_HIGH_MAG(_v) (IS_HIGH_DIM(_v->x) || IS_HIGH_DIM(_v->y) || IS_HIGH_DIM(_v->z) )
1037 :
1038 : //#define FLOAT_COMPUTE
1039 :
1040 : GF_EXPORT
1041 : Fixed gf_vec_len_p(GF_Vec *v)
1042 : {
1043 : /*commented out atm - weird results (not enough precision?)...*/
1044 : //#if defined(GPAC_USE_IGPP_HP) || defined (GPAC_USE_IGPP)
1045 : #if 0
1046 : Fixed res;
1047 : gppVec3DLength_16_32s((GPP_VEC3D *) v, &res);
1048 : return res;
1049 : #elif defined(FLOAT_COMPUTE)
1050 : return FLT2FIX( sqrt( FIX2FLT(v->x) * FIX2FLT(v->x) + FIX2FLT(v->y)*FIX2FLT(v->y) + FIX2FLT(v->z)*FIX2FLT(v->z) ) );
1051 : #else
1052 :
1053 : /*high-magnitude vector, use low precision on frac part to avoid overflow*/
1054 : if (VEC_HIGH_MAG(v)) {
1055 : GF_Vec _v = *v;
1056 : _v.x>>=8;
1057 : _v.y>>=8;
1058 : _v.z>>=8;
1059 : return gf_sqrt( gf_mulfix(_v.x, _v.x) + gf_mulfix(_v.y, _v.y) + gf_mulfix(_v.z, _v.z) ) << 8;
1060 : }
1061 : /*low-res vector*/
1062 : return gf_sqrt( gf_mulfix(v->x, v->x) + gf_mulfix(v->y, v->y) + gf_mulfix(v->z, v->z) );
1063 : #endif
1064 : }
1065 :
1066 : GF_EXPORT
1067 : Fixed gf_vec_len(GF_Vec v)
1068 : {
1069 : return gf_vec_len_p(&v);
1070 :
1071 : }
1072 : GF_EXPORT
1073 : Fixed gf_vec_lensq_p(GF_Vec *v)
1074 : {
1075 : /*commented out atm - weird results (not enough precision?)...*/
1076 : //#if defined(GPAC_USE_IGPP_HP) || defined (GPAC_USE_IGPP)
1077 : #if 0
1078 : Fixed res;
1079 : gppVec3DLengthSq_16_32s((GPP_VEC3D *) v, &res);
1080 : return res;
1081 : #elif defined(FLOAT_COMPUTE)
1082 : return FLT2FIX( FIX2FLT(v->x) * FIX2FLT(v->x) + FIX2FLT(v->y)*FIX2FLT(v->y) + FIX2FLT(v->z)*FIX2FLT(v->z) );
1083 : #else
1084 :
1085 : /*high-magnitude vector, use low precision on frac part to avoid overflow*/
1086 : if (VEC_HIGH_MAG(v)) {
1087 : GF_Vec _v = *v;
1088 : _v.x>>=8;
1089 : _v.y>>=8;
1090 : _v.z>>=8;
1091 : return ( gf_mulfix(_v.x, _v.x) + gf_mulfix(_v.y, _v.y) + gf_mulfix(_v.z, _v.z) ) << 16;
1092 : }
1093 : return gf_mulfix(v->x, v->x) + gf_mulfix(v->y, v->y) + gf_mulfix(v->z, v->z);
1094 :
1095 : #endif
1096 : }
1097 :
1098 : GF_EXPORT
1099 : Fixed gf_vec_lensq(GF_Vec v)
1100 : {
1101 : return gf_vec_lensq_p(&v);
1102 :
1103 : }
1104 :
1105 : GF_EXPORT
1106 : Fixed gf_vec_dot_p(GF_Vec *v1, GF_Vec *v2)
1107 : {
1108 : /*commented out atm - weird results (not enough precision?)...*/
1109 : //#if defined(GPAC_USE_IGPP_HP) || defined (GPAC_USE_IGPP)
1110 : #if 0
1111 : Fixed res;
1112 : gppVec3DDot_16_32s((GPP_VEC3D *) v1, (GPP_VEC3D *) v2, &res);
1113 : return res;
1114 : #elif defined(FLOAT_COMPUTE)
1115 : Float fr = FIX2FLT(v1->x)*FIX2FLT(v2->x) + FIX2FLT(v1->y)*FIX2FLT(v2->y) + FIX2FLT(v1->z)*FIX2FLT(v2->z);
1116 : return FLT2FIX(fr);
1117 : #else
1118 : /*both high-magnitude vectors, use low precision on frac part to avoid overflow
1119 : if only one is, the dot product should still be in proper range*/
1120 : if (0&&VEC_HIGH_MAG(v1) && VEC_HIGH_MAG(v2)) {
1121 : GF_Vec _v1 = *v1;
1122 : GF_Vec _v2 = *v2;
1123 : _v1.x>>=4;
1124 : _v1.y>>=4;
1125 : _v1.z>>=4;
1126 : _v2.x>>=4;
1127 : _v2.y>>=4;
1128 : _v2.z>>=4;
1129 : return ( gf_mulfix(_v1.x, _v2.x) + gf_mulfix(_v1.y, _v2.y) + gf_mulfix(_v1.z, _v2.z) ) << 8;
1130 : }
1131 : return gf_mulfix(v1->x, v2->x) + gf_mulfix(v1->y, v2->y) + gf_mulfix(v1->z, v2->z);
1132 :
1133 : #endif
1134 : }
1135 : Fixed gf_vec_dot(GF_Vec v1, GF_Vec v2)
1136 : {
1137 : return gf_vec_dot_p(&v1, &v2);
1138 :
1139 : }
1140 :
1141 : GF_EXPORT
1142 : void gf_vec_norm(GF_Vec *v)
1143 : {
1144 : /*commented out atm - weird results (not enough precision?)...*/
1145 : //#if defined(GPAC_USE_IGPP_HP) || defined (GPAC_USE_IGPP)
1146 : #if 0
1147 : gppVec3DNormalize_16_32s((GPP_VEC3D *) &v);
1148 : #else
1149 : Fixed __res = gf_vec_len(*v);
1150 : v->x = gf_divfix(v->x, __res);
1151 : v->y = gf_divfix(v->y, __res);
1152 : v->z = gf_divfix(v->z, __res);
1153 : #endif
1154 : }
1155 :
1156 : GF_EXPORT
1157 : GF_Vec gf_vec_scale_p(GF_Vec *v, Fixed f)
1158 : {
1159 : GF_Vec res;
1160 : res.x = gf_mulfix(v->x, f);
1161 : res.y = gf_mulfix(v->y, f);
1162 : res.z = gf_mulfix(v->z, f);
1163 : return res;
1164 : }
1165 :
1166 : GF_EXPORT
1167 : GF_Vec gf_vec_scale(GF_Vec v, Fixed f)
1168 : {
1169 : GF_Vec res;
1170 : res.x = gf_mulfix(v.x, f);
1171 : res.y = gf_mulfix(v.y, f);
1172 : res.z = gf_mulfix(v.z, f);
1173 : return res;
1174 : }
1175 :
1176 : GF_EXPORT
1177 : GF_Vec gf_vec_cross_p(GF_Vec *v1, GF_Vec *v2)
1178 : {
1179 : GF_Vec res;
1180 : /*commented out atm - weird results (not enough precision?)...*/
1181 : //#if defined(GPAC_USE_IGPP_HP) || defined (GPAC_USE_IGPP)
1182 : #if 0
1183 : gppVec3DCross_16_32s((GPP_VEC3D *) v1, (GPP_VEC3D *) v2, (GPP_VEC3D *) &res);
1184 : return res;
1185 : #elif defined(FLOAT_COMPUTE)
1186 : res.x = FLT2FIX( FIX2FLT(v1->y)*FIX2FLT(v2->z) - FIX2FLT(v2->y)*FIX2FLT(v1->z));
1187 : res.y = FLT2FIX( FIX2FLT(v2->x)*FIX2FLT(v1->z) - FIX2FLT(v1->x)*FIX2FLT(v2->z));
1188 : res.z = FLT2FIX( FIX2FLT(v1->x)*FIX2FLT(v2->y) - FIX2FLT(v2->x)*FIX2FLT(v1->y));
1189 : return res;
1190 : #else
1191 :
1192 : /*both high-magnitude vectors, use low precision on frac part to avoid overflow
1193 : if only one is, the cross product should still be in proper range*/
1194 : if (VEC_HIGH_MAG(v1) && VEC_HIGH_MAG(v2)) {
1195 : GF_Vec _v1 = *v1;
1196 : GF_Vec _v2 = *v2;
1197 : _v1.x>>=8;
1198 : _v1.y>>=8;
1199 : _v1.z>>=8;
1200 : _v2.x>>=8;
1201 : _v2.y>>=8;
1202 : _v2.z>>=8;
1203 : res.x = gf_mulfix(_v1.y, _v2.z) - gf_mulfix(_v2.y, _v1.z);
1204 : res.y = gf_mulfix(_v2.x, _v1.z) - gf_mulfix(_v1.x, _v2.z);
1205 : res.z = gf_mulfix(_v1.x, _v2.y) - gf_mulfix(_v2.x, _v1.y);
1206 : res.x<<=16;
1207 : res.y<<=16;
1208 : res.z<<=16;
1209 : return res;
1210 : }
1211 : res.x = gf_mulfix(v1.y, v2.z) - gf_mulfix(v2.y, v1.z);
1212 : res.y = gf_mulfix(v2.x, v1.z) - gf_mulfix(v1.x, v2.z);
1213 : res.z = gf_mulfix(v1.x, v2.y) - gf_mulfix(v2.x, v1.y);
1214 :
1215 : #endif
1216 : return res;
1217 : }
1218 :
1219 : GF_EXPORT
1220 : GF_Vec gf_vec_cross(GF_Vec v1, GF_Vec v2)
1221 : {
1222 : return gf_vec_cross_p(&v1, &v2);
1223 :
1224 : }
1225 : #else
1226 :
1227 : GF_EXPORT
1228 12087580 : Fixed gf_vec_len_p(GF_Vec *v) {
1229 12087580 : return gf_sqrt(v->x*v->x + v->y*v->y + v->z*v->z);
1230 : }
1231 : GF_EXPORT
1232 12087580 : Fixed gf_vec_len(GF_Vec v) {
1233 12087580 : return gf_vec_len_p(&v);
1234 : }
1235 : GF_EXPORT
1236 353 : Fixed gf_vec_lensq_p(GF_Vec *v) {
1237 353 : return v->x*v->x + v->y*v->y + v->z*v->z;
1238 : }
1239 : GF_EXPORT
1240 353 : Fixed gf_vec_lensq(GF_Vec v) {
1241 353 : return gf_vec_lensq_p(&v);
1242 : }
1243 : GF_EXPORT
1244 9249621 : Fixed gf_vec_dot_p(GF_Vec *v1, GF_Vec *v2) {
1245 9249621 : return v1->x*v2->x + v1->y*v2->y + v1->z*v2->z;
1246 : }
1247 : GF_EXPORT
1248 3178649 : Fixed gf_vec_dot(GF_Vec v1, GF_Vec v2) {
1249 3178649 : return gf_vec_dot_p(&v1, &v2);
1250 : }
1251 : GF_EXPORT
1252 11944850 : void gf_vec_norm(GF_Vec *v)
1253 : {
1254 11944850 : Fixed __res = gf_vec_len(*v);
1255 11944850 : if (!__res ) return;
1256 11944674 : if (__res == FIX_ONE) return;
1257 9654604 : __res = 1.0f/__res;
1258 9654604 : v->x *= __res;
1259 9654604 : v->y *= __res;
1260 9654604 : v->z *= __res;
1261 : }
1262 :
1263 : GF_EXPORT
1264 1 : GF_Vec gf_vec_scale_p(GF_Vec *v, Fixed f)
1265 : {
1266 : GF_Vec res;
1267 1 : res.x = v->x * f;
1268 1 : res.y = v->y * f;
1269 1 : res.z = v->z * f;
1270 1 : return res;
1271 : }
1272 :
1273 : GF_EXPORT
1274 10499913 : GF_Vec gf_vec_scale(GF_Vec v, Fixed f)
1275 : {
1276 10499913 : GF_Vec res = v;
1277 10499913 : res.x *= f;
1278 10499913 : res.y *= f;
1279 10499913 : res.z *= f;
1280 10499913 : return res;
1281 : }
1282 :
1283 : GF_EXPORT
1284 404829 : GF_Vec gf_vec_cross_p(GF_Vec *v1, GF_Vec *v2)
1285 : {
1286 : GF_Vec res;
1287 404829 : res.x = v1->y*v2->z - v2->y*v1->z;
1288 404829 : res.y = v2->x*v1->z - v1->x*v2->z;
1289 404829 : res.z = v1->x*v2->y - v2->x*v1->y;
1290 404829 : return res;
1291 : }
1292 :
1293 : GF_EXPORT
1294 404829 : GF_Vec gf_vec_cross(GF_Vec v1, GF_Vec v2)
1295 : {
1296 404829 : return gf_vec_cross_p(&v1, &v2);
1297 : }
1298 : #endif
1299 :
1300 :
1301 : GF_EXPORT
1302 4762 : void gf_mx2d_from_mx(GF_Matrix2D *mat2D, GF_Matrix *mat)
1303 : {
1304 9524 : gf_mx2d_init(*mat2D);
1305 4762 : mat2D->m[0] = mat->m[0];
1306 4762 : mat2D->m[1] = mat->m[4];
1307 4762 : mat2D->m[2] = mat->m[12];
1308 4762 : mat2D->m[3] = mat->m[1];
1309 4762 : mat2D->m[4] = mat->m[5];
1310 4762 : mat2D->m[5] = mat->m[13];
1311 4762 : }
1312 :
1313 : GF_EXPORT
1314 4760 : void gf_mx_apply_rect(GF_Matrix *mat, GF_Rect *rc)
1315 : {
1316 : GF_Matrix2D mat2D;
1317 4760 : gf_mx2d_from_mx(&mat2D, mat);
1318 4760 : gf_mx2d_apply_rect(&mat2D, rc);
1319 4760 : }
1320 :
1321 : GF_EXPORT
1322 151602 : void gf_mx_add_matrix(GF_Matrix *mat, GF_Matrix *mul)
1323 : {
1324 : GF_Matrix tmp;
1325 151602 : gf_mx_init(tmp);
1326 :
1327 151602 : tmp.m[0] = gf_mulfix(mat->m[0],mul->m[0]) + gf_mulfix(mat->m[4],mul->m[1]) + gf_mulfix(mat->m[8],mul->m[2]);
1328 151602 : tmp.m[4] = gf_mulfix(mat->m[0],mul->m[4]) + gf_mulfix(mat->m[4],mul->m[5]) + gf_mulfix(mat->m[8],mul->m[6]);
1329 151602 : tmp.m[8] = gf_mulfix(mat->m[0],mul->m[8]) + gf_mulfix(mat->m[4],mul->m[9]) + gf_mulfix(mat->m[8],mul->m[10]);
1330 151602 : tmp.m[12]= gf_mulfix(mat->m[0],mul->m[12]) + gf_mulfix(mat->m[4],mul->m[13]) + gf_mulfix(mat->m[8],mul->m[14]) + mat->m[12];
1331 151602 : tmp.m[1] = gf_mulfix(mat->m[1],mul->m[0]) + gf_mulfix(mat->m[5],mul->m[1]) + gf_mulfix(mat->m[9],mul->m[2]);
1332 151602 : tmp.m[5] = gf_mulfix(mat->m[1],mul->m[4]) + gf_mulfix(mat->m[5],mul->m[5]) + gf_mulfix(mat->m[9],mul->m[6]);
1333 151602 : tmp.m[9] = gf_mulfix(mat->m[1],mul->m[8]) + gf_mulfix(mat->m[5],mul->m[9]) + gf_mulfix(mat->m[9],mul->m[10]);
1334 151602 : tmp.m[13]= gf_mulfix(mat->m[1],mul->m[12]) + gf_mulfix(mat->m[5],mul->m[13]) + gf_mulfix(mat->m[9],mul->m[14]) + mat->m[13];
1335 151602 : tmp.m[2] = gf_mulfix(mat->m[2],mul->m[0]) + gf_mulfix(mat->m[6],mul->m[1]) + gf_mulfix(mat->m[10],mul->m[2]);
1336 151602 : tmp.m[6] = gf_mulfix(mat->m[2],mul->m[4]) + gf_mulfix(mat->m[6],mul->m[5]) + gf_mulfix(mat->m[10],mul->m[6]);
1337 151602 : tmp.m[10]= gf_mulfix(mat->m[2],mul->m[8]) + gf_mulfix(mat->m[6],mul->m[9]) + gf_mulfix(mat->m[10],mul->m[10]);
1338 151602 : tmp.m[14]= gf_mulfix(mat->m[2],mul->m[12]) + gf_mulfix(mat->m[6],mul->m[13]) + gf_mulfix(mat->m[10],mul->m[14]) + mat->m[14];
1339 151602 : memcpy(mat->m, tmp.m, sizeof(Fixed)*16);
1340 151602 : }
1341 :
1342 : GF_EXPORT
1343 5006 : void gf_mx_add_matrix_2d(GF_Matrix *mat, GF_Matrix2D *mat2D)
1344 : {
1345 : GF_Matrix tmp;
1346 5006 : gf_mx_init(tmp);
1347 :
1348 5006 : tmp.m[0] = gf_mulfix(mat->m[0],mat2D->m[0]) + gf_mulfix(mat->m[4],mat2D->m[3]);
1349 5006 : tmp.m[4] = gf_mulfix(mat->m[0],mat2D->m[1]) + gf_mulfix(mat->m[4],mat2D->m[4]);
1350 5006 : tmp.m[8] = mat->m[8];
1351 5006 : tmp.m[12]= gf_mulfix(mat->m[0],mat2D->m[2]) + gf_mulfix(mat->m[4],mat2D->m[5]) + mat->m[12];
1352 5006 : tmp.m[1] = gf_mulfix(mat->m[1],mat2D->m[0]) + gf_mulfix(mat->m[5],mat2D->m[3]);
1353 5006 : tmp.m[5] = gf_mulfix(mat->m[1],mat2D->m[1]) + gf_mulfix(mat->m[5],mat2D->m[4]);
1354 5006 : tmp.m[9] = mat->m[9];
1355 5006 : tmp.m[13]= gf_mulfix(mat->m[1],mat2D->m[2]) + gf_mulfix(mat->m[5],mat2D->m[5]) + mat->m[13];
1356 5006 : tmp.m[2] = gf_mulfix(mat->m[2],mat2D->m[0]) + gf_mulfix(mat->m[6],mat2D->m[3]);
1357 5006 : tmp.m[6] = gf_mulfix(mat->m[2],mat2D->m[1]) + gf_mulfix(mat->m[6],mat2D->m[4]);
1358 5006 : tmp.m[10]= mat->m[10];
1359 5006 : tmp.m[14]= gf_mulfix(mat->m[2],mat2D->m[2]) + gf_mulfix(mat->m[6],mat2D->m[5]) + mat->m[14];
1360 5006 : memcpy(mat->m, tmp.m, sizeof(Fixed)*16);
1361 5006 : }
1362 :
1363 :
1364 :
1365 : GF_EXPORT
1366 56302 : void gf_mx_add_translation(GF_Matrix *mat, Fixed tx, Fixed ty, Fixed tz)
1367 : {
1368 : Fixed tmp[3];
1369 : u32 i;
1370 56302 : tmp[0] = mat->m[12];
1371 56302 : tmp[1] = mat->m[13];
1372 56302 : tmp[2] = mat->m[14];
1373 225208 : for (i=0; i<3; i++)
1374 168906 : tmp[i] += (gf_mulfix(tx,mat->m[i]) + gf_mulfix(ty,mat->m[i+4]) + gf_mulfix(tz, mat->m[i + 8]));
1375 56302 : mat->m[12] = tmp[0];
1376 56302 : mat->m[13] = tmp[1];
1377 56302 : mat->m[14] = tmp[2];
1378 56302 : }
1379 :
1380 : GF_EXPORT
1381 46135 : void gf_mx_add_scale(GF_Matrix *mat, Fixed sx, Fixed sy, Fixed sz)
1382 : {
1383 : Fixed tmp[3];
1384 : u32 i, j;
1385 :
1386 46135 : tmp[0] = sx;
1387 46135 : tmp[1] = sy;
1388 46135 : tmp[2] = sz;
1389 :
1390 184540 : for (i=0; i<3; i++) {
1391 415215 : for (j=0; j<3; j++) {
1392 415215 : mat->m[i*4 + j] = gf_mulfix(mat->m[j+4 * i], tmp[i]);
1393 : }
1394 : }
1395 46135 : }
1396 :
1397 : GF_EXPORT
1398 4054 : void gf_mx_add_rotation(GF_Matrix *mat, Fixed angle, Fixed x, Fixed y, Fixed z)
1399 : {
1400 : GF_Matrix tmp;
1401 : Fixed xx, yy, zz, xy, xz, yz;
1402 4054 : Fixed nor = gf_sqrt(gf_mulfix(x,x) + gf_mulfix(y,y) + gf_mulfix(z,z));
1403 4054 : Fixed cos_a = gf_cos(angle);
1404 4054 : Fixed sin_a = gf_sin(angle);
1405 4054 : Fixed icos_a = FIX_ONE - cos_a;
1406 :
1407 4054 : if (nor && (nor!=FIX_ONE)) {
1408 2260 : x = gf_divfix(x, nor);
1409 2260 : y = gf_divfix(y, nor);
1410 2260 : z = gf_divfix(z, nor);
1411 : }
1412 4054 : xx = gf_mulfix(x,x);
1413 4054 : yy = gf_mulfix(y,y);
1414 4054 : zz = gf_mulfix(z,z);
1415 4054 : xy = gf_mulfix(x,y);
1416 4054 : xz = gf_mulfix(x,z);
1417 4054 : yz = gf_mulfix(y,z);
1418 4054 : gf_mx_init(tmp);
1419 4054 : tmp.m[0] = gf_mulfix(icos_a,xx) + cos_a;
1420 4054 : tmp.m[1] = gf_mulfix(xy,icos_a) + gf_mulfix(z,sin_a);
1421 4054 : tmp.m[2] = gf_mulfix(xz,icos_a) - gf_mulfix(y,sin_a);
1422 :
1423 4054 : tmp.m[4] = gf_mulfix(xy,icos_a) - gf_mulfix(z,sin_a);
1424 4054 : tmp.m[5] = gf_mulfix(icos_a,yy) + cos_a;
1425 4054 : tmp.m[6] = gf_mulfix(yz,icos_a) + gf_mulfix(x,sin_a);
1426 :
1427 4054 : tmp.m[8] = gf_mulfix(xz,icos_a) + gf_mulfix(y,sin_a);
1428 4054 : tmp.m[9] = gf_mulfix(yz,icos_a) - gf_mulfix(x,sin_a);
1429 4054 : tmp.m[10]= gf_mulfix(icos_a,zz) + cos_a;
1430 :
1431 4054 : gf_mx_add_matrix(mat, &tmp);
1432 4054 : }
1433 :
1434 : GF_EXPORT
1435 10162 : void gf_mx_from_mx2d(GF_Matrix *mat, GF_Matrix2D *mat2D)
1436 : {
1437 20324 : gf_mx_init(*mat);
1438 10162 : mat->m[0] = mat2D->m[0];
1439 10162 : mat->m[4] = mat2D->m[1];
1440 10162 : mat->m[12] = mat2D->m[2];
1441 10162 : mat->m[1] = mat2D->m[3];
1442 10162 : mat->m[5] = mat2D->m[4];
1443 10162 : mat->m[13] = mat2D->m[5];
1444 10162 : }
1445 :
1446 : GF_EXPORT
1447 7751 : Bool gf_mx_equal(GF_Matrix *mx1, GF_Matrix *mx2)
1448 : {
1449 7751 : if (mx1->m[0] != mx2->m[0]) return GF_FALSE;
1450 7500 : if (mx1->m[1] != mx2->m[1]) return GF_FALSE;
1451 7500 : if (mx1->m[2] != mx2->m[2]) return GF_FALSE;
1452 7497 : if (mx1->m[4] != mx2->m[4]) return GF_FALSE;
1453 7497 : if (mx1->m[5] != mx2->m[5]) return GF_FALSE;
1454 7496 : if (mx1->m[6] != mx2->m[6]) return GF_FALSE;
1455 7496 : if (mx1->m[8] != mx2->m[8]) return GF_FALSE;
1456 7496 : if (mx1->m[9] != mx2->m[9]) return GF_FALSE;
1457 7496 : if (mx1->m[10] != mx2->m[10]) return GF_FALSE;
1458 7496 : if (mx1->m[12] != mx2->m[12]) return GF_FALSE;
1459 7496 : if (mx1->m[13] != mx2->m[13]) return GF_FALSE;
1460 7496 : if (mx1->m[14] != mx2->m[14]) return GF_FALSE;
1461 7496 : return GF_TRUE;
1462 : }
1463 :
1464 :
1465 : GF_EXPORT
1466 23651 : void gf_mx_inverse(GF_Matrix *mx)
1467 : {
1468 : #ifdef GPAC_FIXED_POINT
1469 : Float _det, _max;
1470 : s32 sign, scale;
1471 : #endif
1472 : Fixed det;
1473 : GF_Matrix rev;
1474 23651 : gf_mx_init(rev);
1475 :
1476 : assert(! ((mx->m[3] != 0) || (mx->m[7] != 0) || (mx->m[11] != 0) || (mx->m[15] != FIX_ONE)) );
1477 :
1478 :
1479 : #ifdef GPAC_FIXED_POINT
1480 : /*we must compute the determinent as a float to avoid fixed-point overflow*/
1481 : _det = FIX2FLT(mx->m[0])*FIX2FLT(mx->m[5])*FIX2FLT(mx->m[10]);
1482 : _det += FIX2FLT(mx->m[1])*FIX2FLT(mx->m[6])*FIX2FLT(mx->m[8]);
1483 : _det += FIX2FLT(mx->m[2])*FIX2FLT(mx->m[4])*FIX2FLT(mx->m[9]);
1484 : _det -= FIX2FLT(mx->m[2])*FIX2FLT(mx->m[5])*FIX2FLT(mx->m[8]);
1485 : _det -= FIX2FLT(mx->m[1])*FIX2FLT(mx->m[4])*FIX2FLT(mx->m[10]);
1486 : _det -= FIX2FLT(mx->m[0])*FIX2FLT(mx->m[6])*FIX2FLT(mx->m[9]);
1487 :
1488 : if (!_det) {
1489 : gf_mx2d_init(*mx);
1490 : return;
1491 : }
1492 : sign = _det<0 ? -1 : 1;
1493 : _det *= sign;
1494 : scale = 1;
1495 : _max = FIX2FLT(FIX_MAX);
1496 : while (_det / scale > _max) {
1497 : scale *= 10;
1498 : }
1499 : det = sign * FLT2FIX(_det/scale);
1500 : #else
1501 23651 : det = gf_mulfix(gf_mulfix(mx->m[0], mx->m[5]) , mx->m[10]);
1502 23651 : det += gf_mulfix(gf_mulfix(mx->m[1], mx->m[6]) , mx->m[8]);
1503 23651 : det += gf_mulfix(gf_mulfix(mx->m[2], mx->m[4]) , mx->m[9]);
1504 23651 : det -= gf_mulfix(gf_mulfix(mx->m[2], mx->m[5]) , mx->m[8]);
1505 23651 : det -= gf_mulfix(gf_mulfix(mx->m[1], mx->m[4]) , mx->m[10]);
1506 23651 : det -= gf_mulfix(gf_mulfix(mx->m[0], mx->m[6]) , mx->m[9]);
1507 23651 : if (!det) {
1508 0 : gf_mx_init(*mx);
1509 0 : return;
1510 : }
1511 : #endif
1512 :
1513 :
1514 : /* Calculate inverse(A) = adj(A) / det(A) */
1515 23651 : rev.m[0] = gf_muldiv(mx->m[5], mx->m[10], det) - gf_muldiv(mx->m[6], mx->m[9], det);
1516 23651 : rev.m[4] = -gf_muldiv(mx->m[4], mx->m[10], det) + gf_muldiv(mx->m[6], mx->m[8], det);
1517 23651 : rev.m[8] = gf_muldiv(mx->m[4], mx->m[9], det) - gf_muldiv(mx->m[5], mx->m[8], det);
1518 23651 : rev.m[1] = -gf_muldiv(mx->m[1], mx->m[10], det) + gf_muldiv(mx->m[2], mx->m[9], det);
1519 23651 : rev.m[5] = gf_muldiv(mx->m[0], mx->m[10], det) - gf_muldiv(mx->m[2], mx->m[8], det);
1520 23651 : rev.m[9] = -gf_muldiv(mx->m[0], mx->m[9], det) + gf_muldiv(mx->m[1], mx->m[8], det);
1521 23651 : rev.m[2] = gf_muldiv(mx->m[1], mx->m[6], det) - gf_muldiv(mx->m[2], mx->m[5], det);
1522 23651 : rev.m[6] = -gf_muldiv(mx->m[0], mx->m[6], det) + gf_muldiv(mx->m[2], mx->m[4], det);
1523 23651 : rev.m[10] = gf_muldiv(mx->m[0], mx->m[5], det) - gf_muldiv(mx->m[1], mx->m[4], det);
1524 :
1525 : #ifdef GPAC_FIXED_POINT
1526 : if (scale>1) {
1527 : rev.m[0] /= scale;
1528 : rev.m[4] /= scale;
1529 : rev.m[8] /= scale;
1530 : rev.m[1] /= scale;
1531 : rev.m[5] /= scale;
1532 : rev.m[9] /= scale;
1533 : rev.m[2] /= scale;
1534 : rev.m[6] /= scale;
1535 : rev.m[10] /= scale;
1536 : }
1537 : #endif
1538 :
1539 : /* do translation part*/
1540 23651 : rev.m[12] = -( gf_mulfix(mx->m[12], rev.m[0]) + gf_mulfix(mx->m[13], rev.m[4]) + gf_mulfix(mx->m[14], rev.m[8]) );
1541 23651 : rev.m[13] = -( gf_mulfix(mx->m[12], rev.m[1]) + gf_mulfix(mx->m[13], rev.m[5]) + gf_mulfix(mx->m[14], rev.m[9]) );
1542 23651 : rev.m[14] = -( gf_mulfix(mx->m[12], rev.m[2]) + gf_mulfix(mx->m[13], rev.m[6]) + gf_mulfix(mx->m[14], rev.m[10]) );
1543 : gf_mx_copy(*mx, rev);
1544 : }
1545 :
1546 : GF_EXPORT
1547 13387 : void gf_mx_transpose(GF_Matrix *mx)
1548 : {
1549 : GF_Matrix rev;
1550 : int i,j;
1551 :
1552 13387 : gf_mx_init(rev);
1553 66935 : for (i=0; i<4 ; i++) {
1554 214192 : for(j=0; j<4; j++) {
1555 214192 : rev.m[i*4+j] = mx->m[j*4+i];
1556 : }
1557 : }
1558 :
1559 :
1560 : gf_mx_copy(*mx, rev);
1561 13387 : }
1562 :
1563 :
1564 :
1565 :
1566 : GF_EXPORT
1567 1379832 : void gf_mx_apply_vec(GF_Matrix *mx, GF_Vec *pt)
1568 : {
1569 : GF_Vec res;
1570 1379832 : res.x = gf_mulfix(pt->x, mx->m[0]) + gf_mulfix(pt->y, mx->m[4]) + gf_mulfix(pt->z, mx->m[8]) + mx->m[12];
1571 1379832 : res.y = gf_mulfix(pt->x, mx->m[1]) + gf_mulfix(pt->y, mx->m[5]) + gf_mulfix(pt->z, mx->m[9]) + mx->m[13];
1572 1379832 : res.z = gf_mulfix(pt->x, mx->m[2]) + gf_mulfix(pt->y, mx->m[6]) + gf_mulfix(pt->z, mx->m[10]) + mx->m[14];
1573 1379832 : *pt = res;
1574 1379832 : }
1575 :
1576 : GF_EXPORT
1577 1520 : void gf_mx_ortho(GF_Matrix *mx, Fixed left, Fixed right, Fixed bottom, Fixed top, Fixed z_near, Fixed z_far)
1578 : {
1579 3040 : gf_mx_init(*mx);
1580 1520 : mx->m[0] = gf_divfix(2*FIX_ONE, right-left);
1581 1520 : mx->m[5] = gf_divfix(2*FIX_ONE, top-bottom);
1582 1520 : mx->m[10] = gf_divfix(-2*FIX_ONE, z_far-z_near);
1583 1520 : mx->m[12] = gf_divfix(right+left, right-left);
1584 1520 : mx->m[13] = gf_divfix(top+bottom, top-bottom);
1585 1520 : mx->m[14] = gf_divfix(z_far+z_near, z_far-z_near);
1586 : mx->m[15] = FIX_ONE;
1587 1520 : }
1588 :
1589 : GF_EXPORT
1590 1 : void gf_mx_ortho_reverse_z(GF_Matrix *mx, Fixed left, Fixed right, Fixed bottom, Fixed top, Fixed z_near, Fixed z_far)
1591 : {
1592 2 : gf_mx_init(*mx);
1593 1 : mx->m[0] = gf_divfix(2*FIX_ONE, right-left);
1594 1 : mx->m[5] = gf_divfix(2*FIX_ONE, top-bottom);
1595 1 : mx->m[10] = gf_divfix(-FIX_ONE, z_far-z_near);
1596 1 : mx->m[12] = gf_divfix(right+left, right-left);
1597 1 : mx->m[13] = gf_divfix(top+bottom, top-bottom);
1598 1 : mx->m[14] = -gf_divfix(z_near, z_far-z_near);
1599 : mx->m[15] = FIX_ONE;
1600 1 : }
1601 : GF_EXPORT
1602 8076 : void gf_mx_perspective(GF_Matrix *mx, Fixed fieldOfView, Fixed aspectRatio, Fixed z_near, Fixed z_far)
1603 : {
1604 8076 : Fixed f = gf_divfix(gf_cos(fieldOfView/2), gf_sin(fieldOfView/2));
1605 8076 : gf_mx_init(*mx);
1606 8076 : mx->m[0] = gf_divfix(f, aspectRatio);
1607 8076 : mx->m[5] = f;
1608 8076 : mx->m[10] = gf_divfix(z_far+z_near, z_near-z_far);
1609 :
1610 8076 : mx->m[11] = -FIX_ONE;
1611 8076 : mx->m[14] = 2*gf_muldiv(z_near, z_far, z_near-z_far);
1612 :
1613 8076 : mx->m[15] = 0;
1614 8076 : }
1615 :
1616 : GF_EXPORT
1617 1 : void gf_mx_perspective_reverse_z(GF_Matrix *mx, Fixed fieldOfView, Fixed aspectRatio, Fixed z_near, Fixed z_far)
1618 : {
1619 1 : Fixed f = gf_divfix(gf_cos(fieldOfView/2), gf_sin(fieldOfView/2));
1620 1 : gf_mx_init(*mx);
1621 1 : mx->m[0] = gf_divfix(f, aspectRatio);
1622 1 : mx->m[5] = f;
1623 : //see http://dev.theomader.com/depth-precision/
1624 : #if 1
1625 1 : mx->m[10] = -gf_divfix(z_far, z_near-z_far) - FIX_ONE;
1626 1 : mx->m[11] = -FIX_ONE;
1627 1 : mx->m[14] = - gf_muldiv(z_near, z_far, z_near-z_far);
1628 : #else
1629 : mx->m[10] = 0;
1630 : mx->m[11] = - FIX_ONE;
1631 : mx->m[14] = z_near;
1632 : #endif
1633 :
1634 1 : mx->m[15] = 0;
1635 1 : }
1636 : GF_EXPORT
1637 9276 : void gf_mx_lookat(GF_Matrix *mx, GF_Vec eye, GF_Vec center, GF_Vec upVector)
1638 : {
1639 : GF_Vec f, s, u;
1640 :
1641 9276 : gf_vec_diff(f, center, eye);
1642 9276 : gf_vec_norm(&f);
1643 9276 : gf_vec_norm(&upVector);
1644 :
1645 9276 : s = gf_vec_cross(f, upVector);
1646 9276 : u = gf_vec_cross(s, f);
1647 18552 : gf_mx_init(*mx);
1648 :
1649 9276 : mx->m[0] = s.x;
1650 9276 : mx->m[1] = u.x;
1651 9276 : mx->m[2] = -f.x;
1652 9276 : mx->m[4] = s.y;
1653 9276 : mx->m[5] = u.y;
1654 9276 : mx->m[6] = -f.y;
1655 9276 : mx->m[8] = s.z;
1656 9276 : mx->m[9] = u.z;
1657 9276 : mx->m[10] = -f.z;
1658 :
1659 9276 : gf_mx_add_translation(mx, -eye.x, -eye.y, -eye.z);
1660 9276 : }
1661 :
1662 : GF_Vec4 gf_quat_from_matrix(GF_Matrix *mx);
1663 :
1664 : GF_EXPORT
1665 57 : void gf_mx_get_yaw_pitch_roll(GF_Matrix *mx, Fixed *yaw, Fixed *pitch, Fixed *roll)
1666 : {
1667 : Fixed locmat[16];
1668 : assert(mx->m[15]);
1669 57 : memcpy(locmat, mx->m, sizeof(Fixed)*16);
1670 57 : *pitch = (Float) atan(locmat[4]/locmat[0]);
1671 57 : *yaw = (Float) atan(-locmat[8]/gf_sqrt(pow(locmat[9],2) + pow(locmat[10],2)));
1672 57 : *roll = (Float) atan(locmat[9]/locmat[10]);
1673 57 : }
1674 :
1675 : GF_EXPORT
1676 202 : void gf_mx_decompose(GF_Matrix *mx, GF_Vec *translate, GF_Vec *scale, GF_Vec4 *rotate, GF_Vec *shear)
1677 : {
1678 : u32 i, j;
1679 : GF_Vec4 quat;
1680 : Fixed locmat[16];
1681 : GF_Matrix tmp;
1682 : GF_Vec row0, row1, row2;
1683 : Fixed shear_xy, shear_xz, shear_yz;
1684 : assert(mx->m[15]);
1685 :
1686 202 : memcpy(locmat, mx->m, sizeof(Fixed)*16);
1687 : /*no perspective*/
1688 202 : locmat[3] = locmat[7] = locmat[11] = 0;
1689 : /*normalize*/
1690 1010 : for (i=0; i<4; i++) {
1691 3232 : for (j=0; j<4; j++) {
1692 3232 : locmat[4*i+j] = gf_divfix(locmat[4*i+j], locmat[15]);
1693 : }
1694 : }
1695 202 : translate->x = locmat[12];
1696 202 : translate->y = locmat[13];
1697 202 : translate->z = locmat[14];
1698 202 : locmat[12] = locmat[13] = locmat[14] = 0;
1699 202 : row0.x = locmat[0];
1700 202 : row0.y = locmat[1];
1701 202 : row0.z = locmat[2];
1702 202 : row1.x = locmat[4];
1703 202 : row1.y = locmat[5];
1704 202 : row1.z = locmat[6];
1705 202 : row2.x = locmat[8];
1706 202 : row2.y = locmat[9];
1707 202 : row2.z = locmat[10];
1708 :
1709 202 : scale->x = gf_vec_len(row0);
1710 202 : gf_vec_norm(&row0);
1711 202 : shear_xy = gf_vec_dot(row0, row1);
1712 202 : row1.x -= gf_mulfix(row0.x, shear_xy);
1713 202 : row1.y -= gf_mulfix(row0.y, shear_xy);
1714 202 : row1.z -= gf_mulfix(row0.z, shear_xy);
1715 :
1716 202 : scale->y = gf_vec_len(row1);
1717 202 : gf_vec_norm(&row1);
1718 202 : shear->x = gf_divfix(shear_xy, scale->y);
1719 :
1720 202 : shear_xz = gf_vec_dot(row0, row2);
1721 202 : row2.x -= gf_mulfix(row0.x, shear_xz);
1722 202 : row2.y -= gf_mulfix(row0.y, shear_xz);
1723 202 : row2.z -= gf_mulfix(row0.z, shear_xz);
1724 202 : shear_yz = gf_vec_dot(row1, row2);
1725 202 : row2.x -= gf_mulfix(row1.x, shear_yz);
1726 202 : row2.y -= gf_mulfix(row1.y, shear_yz);
1727 202 : row2.z -= gf_mulfix(row1.z, shear_yz);
1728 :
1729 202 : scale->z = gf_vec_len(row2);
1730 202 : gf_vec_norm(&row2);
1731 202 : shear->y = gf_divfix(shear_xz, scale->z);
1732 202 : shear->z = gf_divfix(shear_yz, scale->z);
1733 :
1734 202 : locmat[0] = row0.x;
1735 202 : locmat[4] = row1.x;
1736 202 : locmat[8] = row2.x;
1737 202 : locmat[1] = row0.y;
1738 202 : locmat[5] = row1.y;
1739 202 : locmat[9] = row2.y;
1740 202 : locmat[2] = row0.z;
1741 202 : locmat[6] = row1.z;
1742 202 : locmat[10] = row2.z;
1743 :
1744 : memcpy(tmp.m, locmat, sizeof(Fixed)*16);
1745 202 : quat = gf_quat_from_matrix(&tmp);
1746 202 : *rotate = gf_quat_to_rotation(&quat);
1747 202 : }
1748 :
1749 : GF_EXPORT
1750 20636 : void gf_mx_apply_bbox_sphere(GF_Matrix *mx, GF_BBox *box)
1751 : {
1752 : Fixed var;
1753 20636 : gf_mx_apply_vec(mx, &box->min_edge);
1754 20636 : gf_mx_apply_vec(mx, &box->max_edge);
1755 :
1756 20636 : if (box->min_edge.x > box->max_edge.x)
1757 : {
1758 : var = box->min_edge.x;
1759 292 : box->min_edge.x = box->max_edge.x;
1760 292 : box->max_edge.x = var;
1761 : }
1762 20636 : if (box->min_edge.y > box->max_edge.y)
1763 : {
1764 : var = box->min_edge.y;
1765 326 : box->min_edge.y = box->max_edge.y;
1766 326 : box->max_edge.y = var;
1767 : }
1768 20636 : if (box->min_edge.z > box->max_edge.z)
1769 : {
1770 : var = box->min_edge.z;
1771 456 : box->min_edge.z = box->max_edge.z;
1772 456 : box->max_edge.z = var;
1773 : }
1774 20636 : gf_bbox_refresh(box);
1775 20636 : }
1776 :
1777 : GF_EXPORT
1778 27789 : void gf_mx_apply_bbox(GF_Matrix *mx, GF_BBox *box)
1779 : {
1780 : u32 i;
1781 : GF_Vec v[4];
1782 27789 : v[0] = box->min_edge;
1783 27789 : v[1] = box->min_edge;
1784 27789 : v[1].x = box->max_edge.x;
1785 27789 : v[2] = box->min_edge;
1786 27789 : v[2].y = box->max_edge.y;
1787 27789 : v[3] = box->min_edge;
1788 27789 : v[3].z = box->max_edge.z;
1789 27789 : box->max_edge.x = box->max_edge.y = box->max_edge.z = -FIX_MAX;
1790 27789 : box->min_edge.x = box->min_edge.y = box->min_edge.z = FIX_MAX;
1791 138945 : for (i=0; i<4; i++) {
1792 111156 : gf_mx_apply_vec(mx, &v[i]);
1793 111156 : if (box->min_edge.x > v[i].x) box->min_edge.x = v[i].x;
1794 111156 : if (box->min_edge.y > v[i].y) box->min_edge.y = v[i].y;
1795 111156 : if (box->min_edge.z > v[i].z) box->min_edge.z = v[i].z;
1796 111156 : if (box->max_edge.x < v[i].x) box->max_edge.x = v[i].x;
1797 111156 : if (box->max_edge.y < v[i].y) box->max_edge.y = v[i].y;
1798 111156 : if (box->max_edge.z < v[i].z) box->max_edge.z = v[i].z;
1799 : }
1800 27789 : gf_bbox_refresh(box);
1801 27789 : }
1802 :
1803 : // Apply the rotation portion of a matrix to a vector.
1804 : GF_EXPORT
1805 1127830 : void gf_mx_rotate_vector(GF_Matrix *mx, GF_Vec *pt)
1806 : {
1807 : GF_Vec res;
1808 : Fixed den;
1809 1127830 : res.x = gf_mulfix(pt->x, mx->m[0]) + gf_mulfix(pt->y, mx->m[4]) + gf_mulfix(pt->z, mx->m[8]);
1810 1127830 : res.y = gf_mulfix(pt->x, mx->m[1]) + gf_mulfix(pt->y, mx->m[5]) + gf_mulfix(pt->z, mx->m[9]);
1811 1127830 : res.z = gf_mulfix(pt->x, mx->m[2]) + gf_mulfix(pt->y, mx->m[6]) + gf_mulfix(pt->z, mx->m[10]);
1812 1127830 : den = gf_mulfix(pt->x, mx->m[3]) + gf_mulfix(pt->y, mx->m[7]) + gf_mulfix(pt->z, mx->m[11]) + mx->m[15];
1813 1127830 : if (den == 0) return;
1814 1127830 : res.x = gf_divfix(res.x, den);
1815 1127830 : res.y = gf_divfix(res.y, den);
1816 1127830 : res.z = gf_divfix(res.z, den);
1817 1127830 : *pt = res;
1818 : }
1819 :
1820 : GF_EXPORT
1821 515 : void gf_mx_rotation_matrix_from_vectors(GF_Matrix *mx, GF_Vec x, GF_Vec y, GF_Vec z)
1822 : {
1823 515 : mx->m[0] = x.x;
1824 515 : mx->m[1] = y.x;
1825 515 : mx->m[2] = z.x;
1826 515 : mx->m[3] = 0;
1827 515 : mx->m[4] = x.y;
1828 515 : mx->m[5] = y.y;
1829 515 : mx->m[6] = z.y;
1830 515 : mx->m[7] = 0;
1831 515 : mx->m[8] = x.z;
1832 515 : mx->m[9] = y.z;
1833 515 : mx->m[10] = z.z;
1834 515 : mx->m[11] = 0;
1835 515 : mx->m[12] = 0;
1836 515 : mx->m[13] = 0;
1837 515 : mx->m[14] = 0;
1838 515 : mx->m[15] = FIX_ONE;
1839 515 : }
1840 :
1841 :
1842 : /*we should only need a full matrix product for frustrum setup*/
1843 : GF_EXPORT
1844 2928 : void gf_mx_add_matrix_4x4(GF_Matrix *mat, GF_Matrix *mul)
1845 : {
1846 : GF_Matrix tmp;
1847 : gf_mx_init(tmp);
1848 2928 : tmp.m[0] = gf_mulfix(mat->m[0],mul->m[0]) + gf_mulfix(mat->m[4],mul->m[1]) + gf_mulfix(mat->m[8],mul->m[2]) + gf_mulfix(mat->m[12],mul->m[3]);
1849 2928 : tmp.m[1] = gf_mulfix(mat->m[1],mul->m[0]) + gf_mulfix(mat->m[5],mul->m[1]) + gf_mulfix(mat->m[9],mul->m[2]) + gf_mulfix(mat->m[13],mul->m[3]);
1850 2928 : tmp.m[2] = gf_mulfix(mat->m[2],mul->m[0]) + gf_mulfix(mat->m[6],mul->m[1]) + gf_mulfix(mat->m[10],mul->m[2]) + gf_mulfix(mat->m[14],mul->m[3]);
1851 2928 : tmp.m[3] = gf_mulfix(mat->m[3],mul->m[0]) + gf_mulfix(mat->m[7],mul->m[1]) + gf_mulfix(mat->m[11],mul->m[2]) + gf_mulfix(mat->m[15],mul->m[3]);
1852 2928 : tmp.m[4] = gf_mulfix(mat->m[0],mul->m[4]) + gf_mulfix(mat->m[4],mul->m[5]) + gf_mulfix(mat->m[8],mul->m[6]) + gf_mulfix(mat->m[12],mul->m[7]);
1853 2928 : tmp.m[5] = gf_mulfix(mat->m[1],mul->m[4]) + gf_mulfix(mat->m[5],mul->m[5]) + gf_mulfix(mat->m[9],mul->m[6]) + gf_mulfix(mat->m[13],mul->m[7]);
1854 2928 : tmp.m[6] = gf_mulfix(mat->m[2],mul->m[4]) + gf_mulfix(mat->m[6],mul->m[5]) + gf_mulfix(mat->m[10],mul->m[6]) + gf_mulfix(mat->m[14],mul->m[7]);
1855 2928 : tmp.m[7] = gf_mulfix(mat->m[3],mul->m[4]) + gf_mulfix(mat->m[7],mul->m[5]) + gf_mulfix(mat->m[11],mul->m[6]) + gf_mulfix(mat->m[15],mul->m[7]);
1856 2928 : tmp.m[8] = gf_mulfix(mat->m[0],mul->m[8]) + gf_mulfix(mat->m[4],mul->m[9]) + gf_mulfix(mat->m[8],mul->m[10]) + gf_mulfix(mat->m[12],mul->m[11]);
1857 2928 : tmp.m[9] = gf_mulfix(mat->m[1],mul->m[8]) + gf_mulfix(mat->m[5],mul->m[9]) + gf_mulfix(mat->m[9],mul->m[10]) + gf_mulfix(mat->m[13],mul->m[11]);
1858 2928 : tmp.m[10] = gf_mulfix(mat->m[2],mul->m[8]) + gf_mulfix(mat->m[6],mul->m[9]) + gf_mulfix(mat->m[10],mul->m[10]) + gf_mulfix(mat->m[14],mul->m[11]);
1859 2928 : tmp.m[11] = gf_mulfix(mat->m[3],mul->m[8]) + gf_mulfix(mat->m[7],mul->m[9]) + gf_mulfix(mat->m[11],mul->m[10]) + gf_mulfix(mat->m[15],mul->m[11]);
1860 2928 : tmp.m[12] = gf_mulfix(mat->m[0],mul->m[12]) + gf_mulfix(mat->m[4],mul->m[13]) + gf_mulfix(mat->m[8],mul->m[14]) + gf_mulfix(mat->m[12],mul->m[15]);
1861 2928 : tmp.m[13] = gf_mulfix(mat->m[1],mul->m[12]) + gf_mulfix(mat->m[5],mul->m[13]) + gf_mulfix(mat->m[9],mul->m[14]) + gf_mulfix(mat->m[13],mul->m[15]);
1862 2928 : tmp.m[14] = gf_mulfix(mat->m[2],mul->m[12]) + gf_mulfix(mat->m[6],mul->m[13]) + gf_mulfix(mat->m[10],mul->m[14]) + gf_mulfix(mat->m[14],mul->m[15]);
1863 2928 : tmp.m[15] = gf_mulfix(mat->m[3],mul->m[12]) + gf_mulfix(mat->m[7],mul->m[13]) + gf_mulfix(mat->m[11],mul->m[14]) + gf_mulfix(mat->m[15],mul->m[15]);
1864 2928 : memcpy(mat->m, tmp.m, sizeof(Fixed)*16);
1865 2928 : }
1866 :
1867 :
1868 : GF_EXPORT
1869 54553 : void gf_mx_apply_vec_4x4(GF_Matrix *mx, GF_Vec4 *vec)
1870 : {
1871 : GF_Vec4 res;
1872 54553 : res.x = gf_mulfix(mx->m[0], vec->x) + gf_mulfix(mx->m[4], vec->y) + gf_mulfix(mx->m[8], vec->z) + gf_mulfix(mx->m[12], vec->q);
1873 54553 : res.y = gf_mulfix(mx->m[1], vec->x) + gf_mulfix(mx->m[5], vec->y) + gf_mulfix(mx->m[9], vec->z) + gf_mulfix(mx->m[13], vec->q);
1874 54553 : res.z = gf_mulfix(mx->m[2], vec->x) + gf_mulfix(mx->m[6], vec->y) + gf_mulfix(mx->m[10], vec->z) + gf_mulfix(mx->m[14], vec->q);
1875 54553 : res.q = gf_mulfix(mx->m[3], vec->x) + gf_mulfix(mx->m[7], vec->y) + gf_mulfix(mx->m[11], vec->z) + gf_mulfix(mx->m[15], vec->q);
1876 54553 : *vec = res;
1877 54553 : }
1878 :
1879 : /*
1880 : * Taken from MESA/GLU (LGPL)
1881 : *
1882 : * Compute inverse of 4x4 transformation matrix.
1883 : * Code contributed by Jacques Leroy jle@star.be
1884 : * Return 1 for success, 0 for failure (singular matrix)
1885 : */
1886 :
1887 : GF_EXPORT
1888 2477 : Bool gf_mx_inverse_4x4(GF_Matrix *mx)
1889 : {
1890 :
1891 : #define SWAP_ROWS(a, b) { Fixed *_tmp = a; (a)=(b); (b)=_tmp; }
1892 : Fixed wtmp[4][8];
1893 : Fixed m0, m1, m2, m3, s;
1894 : Fixed *r0, *r1, *r2, *r3;
1895 : GF_Matrix res;
1896 : r0 = wtmp[0]; r1 = wtmp[1]; r2 = wtmp[2]; r3 = wtmp[3];
1897 2477 : r0[0] = mx->m[0];
1898 2477 : r0[1] = mx->m[4];
1899 2477 : r0[2] = mx->m[8];
1900 2477 : r0[3] = mx->m[12];
1901 2477 : r0[4] = FIX_ONE;
1902 2477 : r0[5] = r0[6] = r0[7] = 0;
1903 2477 : r1[0] = mx->m[1];
1904 2477 : r1[1] = mx->m[5];
1905 2477 : r1[2] = mx->m[9];
1906 2477 : r1[3] = mx->m[13];
1907 2477 : r1[5] = FIX_ONE;
1908 2477 : r1[4] = r1[6] = r1[7] = 0;
1909 2477 : r2[0] = mx->m[2];
1910 2477 : r2[1] = mx->m[6];
1911 2477 : r2[2] = mx->m[10];
1912 2477 : r2[3] = mx->m[14];
1913 2477 : r2[6] = FIX_ONE;
1914 2477 : r2[4] = r2[5] = r2[7] = 0;
1915 2477 : r3[0] = mx->m[3];
1916 2477 : r3[1] = mx->m[7];
1917 2477 : r3[2] = mx->m[11];
1918 2477 : r3[3] = mx->m[15];
1919 2477 : r3[7] = FIX_ONE;
1920 2477 : r3[4] = r3[5] = r3[6] = 0;
1921 :
1922 : /* choose pivot - or die */
1923 2477 : if (ABS(r3[0]) > ABS(r2[0])) SWAP_ROWS(r3, r2);
1924 2477 : if (ABS(r2[0]) > ABS(r1[0])) SWAP_ROWS(r2, r1);
1925 2477 : if (ABS(r1[0]) > ABS(r0[0])) SWAP_ROWS(r1, r0);
1926 2477 : if (r0[0]==0) return GF_FALSE;
1927 :
1928 : /*eliminate first variable*/
1929 2477 : m1 = gf_divfix(r1[0], r0[0]);
1930 2477 : m2 = gf_divfix(r2[0], r0[0]);
1931 2477 : m3 = gf_divfix(r3[0], r0[0]);
1932 2477 : s = r0[1];
1933 2477 : r1[1] -= gf_mulfix(m1, s);
1934 2477 : r2[1] -= gf_mulfix(m2, s);
1935 2477 : r3[1] -= gf_mulfix(m3, s);
1936 2477 : s = r0[2];
1937 2477 : r1[2] -= gf_mulfix(m1, s);
1938 2477 : r2[2] -= gf_mulfix(m2, s);
1939 2477 : r3[2] -= gf_mulfix(m3, s);
1940 2477 : s = r0[3];
1941 2477 : r1[3] -= gf_mulfix(m1, s);
1942 2477 : r2[3] -= gf_mulfix(m2, s);
1943 2477 : r3[3] -= gf_mulfix(m3, s);
1944 2477 : s = r0[4];
1945 2477 : if (s != 0) {
1946 2472 : r1[4] -= gf_mulfix(m1, s);
1947 2472 : r2[4] -= gf_mulfix(m2, s);
1948 2472 : r3[4] -= gf_mulfix(m3, s);
1949 : }
1950 2477 : s = r0[5];
1951 2477 : if (s != 0) {
1952 2 : r1[5] -= gf_mulfix(m1, s);
1953 2 : r2[5] -= gf_mulfix(m2, s);
1954 2 : r3[5] -= gf_mulfix(m3, s);
1955 : }
1956 2477 : s = r0[6];
1957 2477 : if (s != 0) {
1958 3 : r1[6] -= gf_mulfix(m1, s);
1959 3 : r2[6] -= gf_mulfix(m2, s);
1960 3 : r3[6] -= gf_mulfix(m3, s);
1961 : }
1962 2477 : s = r0[7];
1963 2477 : if (s != 0) {
1964 0 : r1[7] -= gf_mulfix(m1, s);
1965 0 : r2[7] -= gf_mulfix(m2, s);
1966 0 : r3[7] -= gf_mulfix(m3, s);
1967 : }
1968 :
1969 : /* choose pivot - or die */
1970 2477 : if (fabs(r3[1]) > fabs(r2[1])) SWAP_ROWS(r3, r2);
1971 2477 : if (fabs(r2[1]) > fabs(r1[1])) SWAP_ROWS(r2, r1);
1972 2477 : if (r1[1]==0) return GF_FALSE;
1973 :
1974 : /* eliminate second variable */
1975 2477 : m2 = gf_divfix(r2[1], r1[1]);
1976 2477 : m3 = gf_divfix(r3[1], r1[1]);
1977 2477 : r2[2] -= gf_mulfix(m2, r1[2]);
1978 2477 : r3[2] -= gf_mulfix(m3, r1[2]);
1979 2477 : r2[3] -= gf_mulfix(m2, r1[3]);
1980 2477 : r3[3] -= gf_mulfix(m3, r1[3]);
1981 2477 : s = r1[4];
1982 2477 : if (s!=0) {
1983 811 : r2[4] -= gf_mulfix(m2, s);
1984 811 : r3[4] -= gf_mulfix(m3, s);
1985 : }
1986 2477 : s = r1[5];
1987 2477 : if (s!=0) {
1988 2471 : r2[5] -= gf_mulfix(m2, s);
1989 2471 : r3[5] -= gf_mulfix(m3, s);
1990 : }
1991 2477 : s = r1[6];
1992 2477 : if (s!=0) {
1993 6 : r2[6] -= gf_mulfix(m2, s);
1994 6 : r3[6] -= gf_mulfix(m3, s);
1995 : }
1996 2477 : s = r1[7];
1997 2477 : if (s!=0) {
1998 0 : r2[7] -= gf_mulfix(m2, s);
1999 0 : r3[7] -= gf_mulfix(m3, s);
2000 : }
2001 :
2002 : /* choose pivot - or die */
2003 2477 : if (fabs(r3[2]) > fabs(r2[2])) SWAP_ROWS(r3, r2);
2004 2477 : if (r2[2]==0) return GF_FALSE;
2005 :
2006 : /* eliminate third variable */
2007 2477 : m3 = gf_divfix(r3[2], r2[2]);
2008 2477 : r3[3] -= gf_mulfix(m3, r2[3]);
2009 2477 : r3[4] -= gf_mulfix(m3, r2[4]);
2010 2477 : r3[5] -= gf_mulfix(m3, r2[5]);
2011 2477 : r3[6] -= gf_mulfix(m3, r2[6]);
2012 2477 : r3[7] -= gf_mulfix(m3, r2[7]);
2013 : /* last check */
2014 2477 : if (r3[3]==0) return GF_FALSE;
2015 :
2016 2477 : s = gf_invfix(r3[3]); /* now back substitute row 3 */
2017 2477 : r3[4] = gf_mulfix(r3[4], s);
2018 2477 : r3[5] = gf_mulfix(r3[5], s);
2019 2477 : r3[6] = gf_mulfix(r3[6], s);
2020 2477 : r3[7] = gf_mulfix(r3[7], s);
2021 :
2022 2477 : m2 = r2[3]; /* now back substitute row 2 */
2023 2477 : s = gf_invfix(r2[2]);
2024 2477 : r2[4] = gf_mulfix(s, r2[4] - gf_mulfix(r3[4], m2));
2025 2477 : r2[5] = gf_mulfix(s, r2[5] - gf_mulfix(r3[5], m2));
2026 2477 : r2[6] = gf_mulfix(s, r2[6] - gf_mulfix(r3[6], m2));
2027 2477 : r2[7] = gf_mulfix(s, r2[7] - gf_mulfix(r3[7], m2));
2028 2477 : m1 = r1[3];
2029 2477 : r1[4] -= gf_mulfix(r3[4], m1);
2030 2477 : r1[5] -= gf_mulfix(r3[5], m1);
2031 2477 : r1[6] -= gf_mulfix(r3[6], m1);
2032 2477 : r1[7] -= gf_mulfix(r3[7], m1);
2033 2477 : m0 = r0[3];
2034 2477 : r0[4] -= gf_mulfix(r3[4], m0);
2035 2477 : r0[5] -= gf_mulfix(r3[5], m0);
2036 2477 : r0[6] -= gf_mulfix(r3[6], m0);
2037 2477 : r0[7] -= gf_mulfix(r3[7], m0);
2038 :
2039 2477 : m1 = r1[2]; /* now back substitute row 1 */
2040 2477 : s = gf_invfix(r1[1]);
2041 2477 : r1[4] = gf_mulfix(s, r1[4] - gf_mulfix(r2[4], m1));
2042 2477 : r1[5] = gf_mulfix(s, r1[5] - gf_mulfix(r2[5], m1));
2043 2477 : r1[6] = gf_mulfix(s, r1[6] - gf_mulfix(r2[6], m1));
2044 2477 : r1[7] = gf_mulfix(s, r1[7] - gf_mulfix(r2[7], m1));
2045 2477 : m0 = r0[2];
2046 2477 : r0[4] -= gf_mulfix(r2[4], m0);
2047 2477 : r0[5] -= gf_mulfix(r2[5], m0);
2048 2477 : r0[6] -= gf_mulfix(r2[6], m0);
2049 2477 : r0[7] -= gf_mulfix(r2[7], m0);
2050 :
2051 2477 : m0 = r0[1]; /* now back substitute row 0 */
2052 2477 : s = gf_invfix(r0[0]);
2053 2477 : r0[4] = gf_mulfix(s, r0[4] - gf_mulfix(r1[4], m0));
2054 2477 : r0[5] = gf_mulfix(s, r0[5] - gf_mulfix(r1[5], m0));
2055 2477 : r0[6] = gf_mulfix(s, r0[6] - gf_mulfix(r1[6], m0));
2056 2477 : r0[7] = gf_mulfix(s, r0[7] - gf_mulfix(r1[7], m0));
2057 :
2058 : gf_mx_init(res)
2059 2477 : res.m[0] = r0[4];
2060 2477 : res.m[4] = r0[5];
2061 2477 : res.m[8] = r0[6];
2062 2477 : res.m[12] = r0[7];
2063 2477 : res.m[1] = r1[4];
2064 2477 : res.m[5] = r1[5]; res.m[9] = r1[6];
2065 2477 : res.m[13] = r1[7];
2066 2477 : res.m[2] = r2[4];
2067 2477 : res.m[6] = r2[5];
2068 2477 : res.m[10] = r2[6];
2069 2477 : res.m[14] = r2[7];
2070 2477 : res.m[3] = r3[4];
2071 2477 : res.m[7] = r3[5];
2072 2477 : res.m[11] = r3[6];
2073 2477 : res.m[15] = r3[7];
2074 : gf_mx_copy(*mx, res);
2075 2477 : return GF_TRUE;
2076 : #undef SWAP_ROWS
2077 :
2078 : }
2079 :
2080 :
2081 :
2082 : GF_EXPORT
2083 91 : Bool gf_plane_intersect_line(GF_Plane *plane, GF_Vec *linepoint, GF_Vec *linevec, GF_Vec *outPoint)
2084 : {
2085 : Fixed t, t2;
2086 91 : t2 = gf_vec_dot(plane->normal, *linevec);
2087 91 : if (t2 == 0) return GF_FALSE;
2088 90 : t = - gf_divfix((gf_vec_dot(plane->normal, *linepoint) + plane->d) , t2);
2089 90 : if (t<0) return GF_FALSE;
2090 66 : *outPoint = gf_vec_scale(*linevec, t);
2091 66 : gf_vec_add(*outPoint, *linepoint, *outPoint);
2092 66 : return GF_TRUE;
2093 : }
2094 :
2095 : #if 0 //unused
2096 : Bool gf_plane_exists_intersection(GF_Plane *plane, GF_Plane *with)
2097 : {
2098 : GF_Vec cross;
2099 : cross = gf_vec_cross(with->normal, plane->normal);
2100 : return gf_vec_lensq(cross) > FIX_EPSILON;
2101 : }
2102 :
2103 : Bool gf_plane_intersect_plane(GF_Plane *plane, GF_Plane *with, GF_Vec *linepoint, GF_Vec *linevec)
2104 : {
2105 : Fixed fn00 = gf_vec_len(plane->normal);
2106 : Fixed fn01 = gf_vec_dot(plane->normal, with->normal);
2107 : Fixed fn11 = gf_vec_len(with->normal);
2108 : Fixed det = gf_mulfix(fn00,fn11) - gf_mulfix(fn01,fn01);
2109 : if (fabs(det) > FIX_EPSILON) {
2110 : Fixed fc0, fc1;
2111 : GF_Vec v1, v2;
2112 : fc0 = gf_divfix( gf_mulfix(fn11, -plane->d) + gf_mulfix(fn01, with->d) , det);
2113 : fc1 = gf_divfix( gf_mulfix(fn00, -with->d) + gf_mulfix(fn01, plane->d) , det);
2114 : *linevec = gf_vec_cross(plane->normal, with->normal);
2115 : v1 = gf_vec_scale(plane->normal, fc0);
2116 : v2 = gf_vec_scale(with->normal, fc1);
2117 : gf_vec_add(*linepoint, v1, v2);
2118 : return GF_TRUE;
2119 : }
2120 : return GF_FALSE;
2121 : }
2122 :
2123 : Bool gf_plane_intersect_planes(GF_Plane *plane, GF_Plane *p1, GF_Plane *p2, GF_Vec *outPoint)
2124 : {
2125 : GF_Vec lp, lv;
2126 : if (gf_plane_intersect_plane(plane, p1, &lp, &lv))
2127 : return gf_plane_intersect_line(p2, &lp, &lv, outPoint);
2128 : return GF_FALSE;
2129 : }
2130 :
2131 : #endif
2132 :
2133 :
2134 : GF_EXPORT
2135 562 : GF_Ray gf_ray(GF_Vec start, GF_Vec end)
2136 : {
2137 : GF_Ray r;
2138 562 : r.orig = start;
2139 562 : gf_vec_diff(r.dir, end, start);
2140 562 : gf_vec_norm(&r.dir);
2141 562 : return r;
2142 : }
2143 :
2144 : GF_EXPORT
2145 2088 : void gf_mx_apply_ray(GF_Matrix *mx, GF_Ray *r)
2146 : {
2147 2088 : gf_vec_add(r->dir, r->orig, r->dir);
2148 2088 : gf_mx_apply_vec(mx, &r->orig);
2149 2088 : gf_mx_apply_vec(mx, &r->dir);
2150 2088 : gf_vec_diff(r->dir, r->dir, r->orig);
2151 2088 : gf_vec_norm(&r->dir);
2152 2088 : }
2153 :
2154 : #define XPLANE 0
2155 : #define YPLANE 1
2156 : #define ZPLANE 2
2157 :
2158 :
2159 : GF_EXPORT
2160 125431 : Bool gf_ray_hit_box(GF_Ray *ray, GF_Vec box_min, GF_Vec box_max, GF_Vec *outPoint)
2161 : {
2162 : Fixed t1, t2, tNEAR=FIX_MIN, tFAR=FIX_MAX;
2163 : Fixed temp;
2164 : //s8 xyorz, sign;
2165 :
2166 125431 : if (ray->dir.x == 0) {
2167 372 : if ((ray->orig.x < box_min.x) || (ray->orig.x > box_max.x))
2168 : return GF_FALSE;
2169 : } else {
2170 125059 : t1 = gf_divfix(box_min.x - ray->orig.x, ray->dir.x);
2171 125059 : t2 = gf_divfix(box_max.x - ray->orig.x, ray->dir.x);
2172 125059 : if (t1 > t2) {
2173 : temp = t1;
2174 : t1 = t2;
2175 : t2 = temp;
2176 : }
2177 125059 : if (t1 > tNEAR) {
2178 : tNEAR = t1;
2179 : //xyorz = XPLANE;
2180 : //sign = (ray->dir.x < 0) ? 1 : -1;
2181 : }
2182 125059 : if (t2 < tFAR) tFAR = t2;
2183 125059 : if (tNEAR > tFAR) return GF_FALSE; // box missed
2184 125059 : if (tFAR < 0) return GF_FALSE; // box behind the ray
2185 : }
2186 :
2187 124518 : if (ray->dir.y == 0) {
2188 204 : if ((ray->orig.y < box_min.y) || (ray->orig.y > box_max.y))
2189 : return GF_FALSE;
2190 : } else {
2191 : tNEAR=FIX_MIN;
2192 : tFAR=FIX_MAX;
2193 124314 : t1 = gf_divfix(box_min.y - ray->orig.y, ray->dir.y);
2194 124314 : t2 = gf_divfix(box_max.y - ray->orig.y, ray->dir.y);
2195 124314 : if (t1 > t2) {
2196 : temp = t1;
2197 : t1 = t2;
2198 : t2 = temp;
2199 : }
2200 124314 : if (t1 > tNEAR) {
2201 : tNEAR = t1;
2202 : //xyorz = YPLANE;
2203 : //sign = (ray->dir.y < 0) ? 1 : -1;
2204 : }
2205 124314 : if (t2 < tFAR) tFAR = t2;
2206 124314 : if (tNEAR > tFAR) return GF_FALSE; // box missed
2207 124314 : if (tFAR < 0) return GF_FALSE; // box behind the ray
2208 : }
2209 :
2210 : // Check the Z plane
2211 121231 : if (ray->dir.z == 0) {
2212 46 : if ((ray->orig.z < box_min.z) || (ray->orig.z > box_max.z))
2213 : return GF_FALSE;
2214 : } else {
2215 : tNEAR=FIX_MIN;
2216 : tFAR=FIX_MAX;
2217 121185 : t1 = gf_divfix(box_min.z - ray->orig.z, ray->dir.z);
2218 121185 : t2 = gf_divfix(box_max.z - ray->orig.z, ray->dir.z);
2219 121185 : if (t1 > t2) {
2220 : temp = t1;
2221 : t1 = t2;
2222 : t2 = temp;
2223 : }
2224 121185 : if (t1 > tNEAR) {
2225 : tNEAR = t1;
2226 : //xyorz = ZPLANE;
2227 : //sign = (ray->dir.z < 0) ? 1 : -1;
2228 : }
2229 121185 : if (t2 < tFAR) tFAR = t2;
2230 121185 : if (tNEAR>tFAR) return GF_FALSE; // box missed
2231 121185 : if (tFAR < 0) return GF_FALSE; // box behind the ray
2232 : }
2233 121158 : if (outPoint) {
2234 0 : *outPoint = gf_vec_scale(ray->dir, tNEAR);
2235 0 : gf_vec_add(*outPoint, *outPoint, ray->orig);
2236 : }
2237 : return GF_TRUE;
2238 : }
2239 :
2240 :
2241 : GF_EXPORT
2242 4 : Bool gf_ray_hit_sphere(GF_Ray *ray, GF_Vec *center, Fixed radius, GF_Vec *outPoint)
2243 : {
2244 : GF_Vec radv;
2245 : Fixed dist, center_proj, center_proj_sq, hcord;
2246 4 : if (center) {
2247 1 : gf_vec_diff(radv, *center, ray->orig);
2248 : } else {
2249 3 : radv = ray->orig;
2250 3 : gf_vec_rev(radv);
2251 : }
2252 4 : dist = gf_vec_len(radv);
2253 4 : center_proj = gf_vec_dot(radv, ray->dir);
2254 4 : if (radius + ABS(center_proj) < dist ) return GF_FALSE;
2255 :
2256 3 : center_proj_sq = gf_mulfix(center_proj, center_proj);
2257 3 : hcord = center_proj_sq - gf_mulfix(dist, dist) + gf_mulfix(radius , radius);
2258 3 : if (hcord < 0) return GF_FALSE;
2259 1 : if (center_proj_sq < hcord) return GF_FALSE;
2260 0 : if (outPoint) {
2261 0 : center_proj -= gf_sqrt(hcord);
2262 0 : radv = gf_vec_scale(ray->dir, center_proj);
2263 0 : gf_vec_add(*outPoint, ray->orig, radv);
2264 : }
2265 : return GF_TRUE;
2266 : }
2267 :
2268 : /*
2269 : * Tomas M�ller and Ben Trumbore.
2270 : * Fast, minimum storage ray-triangle intersection.
2271 : * Journal of graphics tools, 2(1):21-28, 1997
2272 : *
2273 : */
2274 : GF_EXPORT
2275 350889 : Bool gf_ray_hit_triangle(GF_Ray *ray, GF_Vec *v0, GF_Vec *v1, GF_Vec *v2, Fixed *dist)
2276 : {
2277 : Fixed u, v, det;
2278 : GF_Vec edge1, edge2, tvec, pvec, qvec;
2279 : /* find vectors for two edges sharing vert0 */
2280 350889 : gf_vec_diff(edge1, *v1, *v0);
2281 350889 : gf_vec_diff(edge2, *v2, *v0);
2282 : /* begin calculating determinant - also used to calculate U parameter */
2283 350889 : pvec = gf_vec_cross(ray->dir, edge2);
2284 : /* if determinant is near zero, ray lies in plane of triangle */
2285 350889 : det = gf_vec_dot(edge1, pvec);
2286 350889 : if (ABS(det) < FIX_EPSILON) return GF_FALSE;
2287 : /* calculate distance from vert0 to ray origin */
2288 345432 : gf_vec_diff(tvec, ray->orig, *v0);
2289 : /* calculate U parameter and test bounds */
2290 345432 : u = gf_divfix(gf_vec_dot(tvec, pvec), det);
2291 345432 : if ((u < 0) || (u > FIX_ONE)) return GF_FALSE;
2292 : /* prepare to test V parameter */
2293 6511 : qvec = gf_vec_cross(tvec, edge1);
2294 : /* calculate V parameter and test bounds */
2295 6511 : v = gf_divfix(gf_vec_dot(ray->dir, qvec), det);
2296 6511 : if ((v < 0) || (u + v > FIX_ONE)) return GF_FALSE;
2297 : #ifdef GPAC_FIXED_POINT
2298 : #define VSCALE 4096
2299 : det /= VSCALE;
2300 : qvec.x /= VSCALE;
2301 : qvec.y /= VSCALE;
2302 : qvec.z /= VSCALE;
2303 : #undef VSCALE
2304 : #endif
2305 : /* calculate t, ray intersects triangle */
2306 467 : *dist = gf_divfix(gf_vec_dot(edge2, qvec), det);
2307 467 : return GF_TRUE;
2308 : }
2309 :
2310 : #if 0 //unused
2311 : Bool gf_ray_hit_triangle_backcull(GF_Ray *ray, GF_Vec *v0, GF_Vec *v1, GF_Vec *v2, Fixed *dist)
2312 : {
2313 : Fixed u, v, det;
2314 : GF_Vec edge1, edge2, tvec, pvec, qvec;
2315 : /* find vectors for two edges sharing vert0 */
2316 : gf_vec_diff(edge1, *v1, *v0);
2317 : gf_vec_diff(edge2, *v2, *v0);
2318 : /* begin calculating determinant - also used to calculate U parameter */
2319 : pvec = gf_vec_cross(ray->dir, edge2);
2320 : /* if determinant is near zero, ray lies in plane of triangle */
2321 : det = gf_vec_dot(edge1, pvec);
2322 : if (det < FIX_EPSILON) return GF_FALSE;
2323 : /* calculate distance from vert0 to ray origin */
2324 : gf_vec_diff(tvec, ray->orig, *v0);
2325 : /* calculate U parameter and test bounds */
2326 : u = gf_vec_dot(tvec, pvec);
2327 : if ((u < 0) || (u > det)) return GF_FALSE;
2328 : /* prepare to test V parameter */
2329 : qvec = gf_vec_cross(tvec, edge1);
2330 : /* calculate V parameter and test bounds */
2331 : v = gf_vec_dot(ray->dir, qvec);
2332 : if ((v < 0) || (u + v > det)) return GF_FALSE;
2333 : /* calculate t, scale parameters, ray intersects triangle */
2334 : *dist = gf_divfix(gf_vec_dot(edge2, qvec), det);
2335 : return GF_TRUE;
2336 : }
2337 : #endif
2338 :
2339 : GF_EXPORT
2340 4 : GF_Vec gf_closest_point_to_line(GF_Vec line_pt, GF_Vec line_vec, GF_Vec pt)
2341 : {
2342 : GF_Vec c;
2343 : Fixed t;
2344 4 : gf_vec_diff(c, pt, line_pt);
2345 4 : t = gf_vec_dot(line_vec, c);
2346 4 : c = gf_vec_scale(line_vec, t);
2347 4 : gf_vec_add(c, c, line_pt);
2348 4 : return c;
2349 : }
2350 :
2351 :
2352 : GF_EXPORT
2353 202 : GF_Vec4 gf_quat_from_matrix(GF_Matrix *mx)
2354 : {
2355 : GF_Vec4 res;
2356 : Fixed diagonal, s;
2357 202 : diagonal = mx->m[0] + mx->m[5] + mx->m[10];
2358 :
2359 202 : if (diagonal > 0) {
2360 202 : s = gf_sqrt(diagonal + FIX_ONE);
2361 202 : res.q = s / 2;
2362 202 : s = gf_invfix(2*s);
2363 202 : res.x = gf_mulfix(mx->m[6] - mx->m[9], s);
2364 202 : res.y = gf_mulfix(mx->m[8] - mx->m[2], s);
2365 202 : res.z = gf_mulfix(mx->m[1] - mx->m[4], s);
2366 : } else {
2367 : Fixed q[4];
2368 : u32 i, j, k;
2369 : static const u32 next[3] = { 1, 2, 0 };
2370 : i = 0;
2371 0 : if (mx->m[5] > mx->m[0]) {
2372 : i = 1;
2373 : }
2374 0 : if (mx->m[10] > mx->m[4*i+i]) {
2375 : i = 2;
2376 : }
2377 0 : j = next[i];
2378 0 : k = next[j];
2379 0 : s = gf_sqrt(FIX_ONE + mx->m[4*i + i] - (mx->m[4*j+j] + mx->m[4*k+k]) );
2380 0 : q[i] = s / 2;
2381 0 : if (s != 0) {
2382 0 : s = gf_invfix(2*s);
2383 : }
2384 0 : q[3] = gf_mulfix(mx->m[4*j+k] - mx->m[4*k+j], s);
2385 0 : q[j] = gf_mulfix(mx->m[4*i+j] + mx->m[4*j+i], s);
2386 0 : q[k] = gf_mulfix(mx->m[4*i+k] + mx->m[4*k+i], s);
2387 0 : res.x = q[0];
2388 0 : res.y = q[1];
2389 0 : res.z = q[2];
2390 0 : res.q = q[3];
2391 : }
2392 202 : return res;
2393 : }
2394 :
2395 : GF_EXPORT
2396 421 : GF_Vec4 gf_quat_to_rotation(GF_Vec4 *quat)
2397 : {
2398 : GF_Vec4 r;
2399 421 : Fixed val = gf_acos(quat->q);
2400 421 : if (val == 0) {
2401 : r.x = r.y = 0;
2402 : r.z = FIX_ONE;
2403 : r.q = 0;
2404 : } else {
2405 : GF_Vec axis;
2406 264 : Fixed sin_val = gf_sin(val);
2407 264 : axis.x = gf_divfix(quat->x, sin_val);
2408 264 : axis.y = gf_divfix(quat->y, sin_val);
2409 264 : axis.z = gf_divfix(quat->z, sin_val);
2410 264 : gf_vec_norm(&axis);
2411 264 : r.x = axis.x;
2412 264 : r.y = axis.y;
2413 264 : r.z = axis.z;
2414 264 : r.q= 2 * val;
2415 : }
2416 421 : return r;
2417 : }
2418 :
2419 : GF_EXPORT
2420 360 : GF_Vec4 gf_quat_from_rotation(GF_Vec4 rot)
2421 : {
2422 : GF_Vec4 res;
2423 : Fixed s;
2424 360 : Fixed scale = gf_sqrt( gf_mulfix(rot.x, rot.x) + gf_mulfix(rot.y, rot.y) + gf_mulfix(rot.z, rot.z));
2425 :
2426 : /* no rotation - use (multiplication ???) identity quaternion */
2427 360 : if (scale == 0) {
2428 : res.q = FIX_ONE;
2429 : res.x = 0;
2430 : res.y = 0;
2431 : res.z = 0;
2432 : } else {
2433 360 : s = gf_sin(rot.q/2);
2434 360 : res.q = gf_cos(rot.q / 2);
2435 360 : res.x = gf_muldiv(s, rot.x, scale);
2436 360 : res.y = gf_muldiv(s, rot.y, scale);
2437 360 : res.z = gf_muldiv(s, rot.z, scale);
2438 360 : gf_quat_norm(res);
2439 : }
2440 360 : return res;
2441 : }
2442 :
2443 : GF_EXPORT
2444 209 : GF_Vec4 gf_quat_from_axis_cos(GF_Vec axis, Fixed cos_a)
2445 : {
2446 : GF_Vec4 r;
2447 209 : if (cos_a < -FIX_ONE) cos_a = -FIX_ONE;
2448 206 : else if (cos_a > FIX_ONE) cos_a = FIX_ONE;
2449 209 : r.x = axis.x;
2450 209 : r.y = axis.y;
2451 209 : r.z = axis.z;
2452 209 : r.q = gf_acos(cos_a);
2453 209 : return gf_quat_from_rotation(r);
2454 : }
2455 :
2456 : static void gf_quat_conjugate(GF_Vec4 *quat)
2457 : {
2458 2 : quat->x *= -1;
2459 2 : quat->y *= -1;
2460 2 : quat->z *= -1;
2461 : }
2462 :
2463 : GF_EXPORT
2464 2 : GF_Vec4 gf_quat_get_inv(GF_Vec4 *quat)
2465 : {
2466 2 : GF_Vec4 ret = *quat;
2467 : gf_quat_conjugate(&ret);
2468 2 : gf_quat_norm(ret);
2469 2 : return ret;
2470 : }
2471 :
2472 :
2473 : GF_EXPORT
2474 537 : GF_Vec4 gf_quat_multiply(GF_Vec4 *q1, GF_Vec4 *q2)
2475 : {
2476 : GF_Vec4 ret;
2477 537 : ret.q = gf_mulfix(q1->q, q2->q) - gf_mulfix(q1->x, q2->x) - gf_mulfix(q1->y, q2->y) - gf_mulfix(q1->z, q2->z);
2478 537 : ret.x = gf_mulfix(q1->q, q2->x) + gf_mulfix(q1->x, q2->q) + gf_mulfix(q1->y, q2->z) - gf_mulfix(q1->z, q2->y);
2479 537 : ret.y = gf_mulfix(q1->q, q2->y) + gf_mulfix(q1->y, q2->q) - gf_mulfix(q1->x, q2->z) + gf_mulfix(q1->z, q2->x);
2480 537 : ret.z = gf_mulfix(q1->q, q2->z) + gf_mulfix(q1->z, q2->q) + gf_mulfix(q1->x, q2->y) - gf_mulfix(q1->y, q2->x);
2481 537 : return ret;
2482 : }
2483 :
2484 : GF_EXPORT
2485 1 : GF_Vec gf_quat_rotate(GF_Vec4 *quat, GF_Vec *vec)
2486 : {
2487 : GF_Vec ret;
2488 : GF_Vec4 q_v, q_i, q_r1, q_r2;
2489 1 : q_v.q = 0;
2490 1 : q_v.x = vec->x;
2491 1 : q_v.y = vec->y;
2492 1 : q_v.z = vec->z;
2493 1 : q_i = gf_quat_get_inv(quat);
2494 1 : q_r1 = gf_quat_multiply(&q_v, &q_i);
2495 1 : q_r2 = gf_quat_multiply(quat, &q_r1);
2496 1 : ret.x = q_r2.x;
2497 1 : ret.y = q_r2.y;
2498 1 : ret.z = q_r2.z;
2499 1 : return ret;
2500 : }
2501 :
2502 : /*
2503 : * Code from www.gamasutra.com/features/19980703/quaternions_01.htm,
2504 : * Listing 5.
2505 : *
2506 : * SLERP(p, q, t) = [p sin((1 - t)a) + q sin(ta)] / sin(a)
2507 : *
2508 : * where a is the arc angle, quaternions pq = cos(q) and 0 <= t <= 1
2509 : */
2510 : GF_EXPORT
2511 1 : GF_Vec4 gf_quat_slerp(GF_Vec4 q1, GF_Vec4 q2, Fixed frac)
2512 : {
2513 : GF_Vec4 res;
2514 : Fixed omega, cosom, sinom, scale0, scale1, q2_array[4];
2515 :
2516 1 : cosom = gf_mulfix(q1.x, q2.x) + gf_mulfix(q1.y, q2.y) + gf_mulfix(q1.z, q2.z) + gf_mulfix(q1.q, q2.q);
2517 1 : if (cosom < 0) {
2518 0 : cosom = -cosom;
2519 0 : q2_array[0] = -q2.x;
2520 0 : q2_array[1] = -q2.y;
2521 0 : q2_array[2] = -q2.z;
2522 0 : q2_array[3] = -q2.q;
2523 : } else {
2524 : q2_array[0] = q2.x;
2525 : q2_array[1] = q2.y;
2526 : q2_array[2] = q2.z;
2527 : q2_array[3] = q2.q;
2528 : }
2529 :
2530 : /* calculate coefficients */
2531 1 : if ((FIX_ONE - cosom) > FIX_EPSILON) {
2532 0 : omega = gf_acos(cosom);
2533 0 : sinom = gf_sin(omega);
2534 0 : scale0 = gf_divfix(gf_sin( gf_mulfix(FIX_ONE - frac, omega)), sinom);
2535 0 : scale1 = gf_divfix(gf_sin( gf_mulfix(frac, omega)), sinom);
2536 : } else {
2537 : /* q1 & q2 are very close, so do linear interpolation */
2538 1 : scale0 = FIX_ONE - frac;
2539 : scale1 = frac;
2540 : }
2541 1 : res.x = gf_mulfix(scale0, q1.x) + gf_mulfix(scale1, q2_array[0]);
2542 1 : res.y = gf_mulfix(scale0, q1.y) + gf_mulfix(scale1, q2_array[1]);
2543 1 : res.z = gf_mulfix(scale0, q1.z) + gf_mulfix(scale1, q2_array[2]);
2544 1 : res.q = gf_mulfix(scale0, q1.q) + gf_mulfix(scale1, q2_array[3]);
2545 1 : return res;
2546 : }
2547 :
2548 :
2549 : /*update center & radius & is_set flag*/
2550 : GF_EXPORT
2551 61821 : void gf_bbox_refresh(GF_BBox *b)
2552 : {
2553 : GF_Vec v;
2554 61821 : gf_vec_add(v, b->min_edge, b->max_edge);
2555 61821 : b->center = gf_vec_scale(v, FIX_ONE / 2);
2556 61821 : gf_vec_diff(v, b->max_edge, b->min_edge);
2557 61821 : b->radius = gf_vec_len(v) / 2;
2558 61821 : b->is_set = GF_TRUE;
2559 61821 : }
2560 :
2561 : GF_EXPORT
2562 1106 : void gf_bbox_from_rect(GF_BBox *box, GF_Rect *rc)
2563 : {
2564 1106 : box->min_edge.x = rc->x;
2565 1106 : box->min_edge.y = rc->y - rc->height;
2566 1106 : box->min_edge.z = 0;
2567 1106 : box->max_edge.x = rc->x + rc->width;
2568 1106 : box->max_edge.y = rc->y;
2569 1106 : box->max_edge.z = 0;
2570 1106 : gf_bbox_refresh(box);
2571 1106 : }
2572 :
2573 : GF_EXPORT
2574 99 : void gf_rect_from_bbox(GF_Rect *rc, GF_BBox *box)
2575 : {
2576 99 : rc->x = box->min_edge.x;
2577 99 : rc->y = box->max_edge.y;
2578 99 : rc->width = box->max_edge.x - box->min_edge.x;
2579 99 : rc->height = box->max_edge.y - box->min_edge.y;
2580 99 : }
2581 :
2582 : GF_EXPORT
2583 12144 : void gf_bbox_grow_point(GF_BBox *box, GF_Vec pt)
2584 : {
2585 12144 : if (pt.x > box->max_edge.x) box->max_edge.x = pt.x;
2586 12144 : if (pt.y > box->max_edge.y) box->max_edge.y = pt.y;
2587 12144 : if (pt.z > box->max_edge.z) box->max_edge.z = pt.z;
2588 12144 : if (pt.x < box->min_edge.x) box->min_edge.x = pt.x;
2589 12144 : if (pt.y < box->min_edge.y) box->min_edge.y = pt.y;
2590 12144 : if (pt.z < box->min_edge.z) box->min_edge.z = pt.z;
2591 12144 : }
2592 :
2593 : GF_EXPORT
2594 16716 : void gf_bbox_union(GF_BBox *b1, GF_BBox *b2)
2595 : {
2596 16716 : if (b2->is_set) {
2597 16716 : if (!b1->is_set) {
2598 10644 : *b1 = *b2;
2599 : } else {
2600 6072 : gf_bbox_grow_point(b1, b2->min_edge);
2601 6072 : gf_bbox_grow_point(b1, b2->max_edge);
2602 6072 : gf_bbox_refresh(b1);
2603 : }
2604 : }
2605 16716 : }
2606 :
2607 : GF_EXPORT
2608 1 : Bool gf_bbox_equal(GF_BBox *b1, GF_BBox *b2)
2609 : {
2610 1 : return (gf_vec_equal(b1->min_edge, b2->min_edge) && gf_vec_equal(b1->max_edge, b2->max_edge));
2611 : }
2612 :
2613 : GF_EXPORT
2614 20636 : Bool gf_bbox_point_inside(GF_BBox *box, GF_Vec *p)
2615 : {
2616 52077 : return (p->x >= box->min_edge.x && p->x <= box->max_edge.x &&
2617 37118 : p->y >= box->min_edge.y && p->y <= box->max_edge.y &&
2618 42054 : p->z >= box->min_edge.z && p->z <= box->max_edge.z);
2619 : }
2620 :
2621 : /*vertices are ordered to respect p vertex indexes (vertex from bbox closer to plane)
2622 : and so that n-vertex (vertex from bbox farther from plane) is 7-p_vx_idx*/
2623 : GF_EXPORT
2624 240856 : void gf_bbox_get_vertices(GF_Vec bmin, GF_Vec bmax, GF_Vec *vecs)
2625 : {
2626 240856 : vecs[0].x = vecs[1].x = vecs[2].x = vecs[3].x = bmax.x;
2627 240856 : vecs[4].x = vecs[5].x = vecs[6].x = vecs[7].x = bmin.x;
2628 240856 : vecs[0].y = vecs[1].y = vecs[4].y = vecs[5].y = bmax.y;
2629 240856 : vecs[2].y = vecs[3].y = vecs[6].y = vecs[7].y = bmin.y;
2630 240856 : vecs[0].z = vecs[2].z = vecs[4].z = vecs[6].z = bmax.z;
2631 240856 : vecs[1].z = vecs[3].z = vecs[5].z = vecs[7].z = bmin.z;
2632 240856 : }
2633 :
2634 :
2635 : GF_EXPORT
2636 21742 : void gf_mx_apply_plane(GF_Matrix *mx, GF_Plane *plane)
2637 : {
2638 : GF_Vec pt, end;
2639 : /*get pt*/
2640 21742 : pt = gf_vec_scale(plane->normal, -plane->d);
2641 21742 : gf_vec_add(end, pt, plane->normal);
2642 21742 : gf_mx_apply_vec(mx, &pt);
2643 21742 : gf_mx_apply_vec(mx, &end);
2644 21742 : gf_vec_diff(plane->normal, end, pt);
2645 21742 : gf_vec_norm(&plane->normal);
2646 21742 : plane->d = - gf_vec_dot(pt, plane->normal);
2647 21742 : }
2648 :
2649 : GF_EXPORT
2650 2166279 : Fixed gf_plane_get_distance(GF_Plane *plane, GF_Vec *p)
2651 : {
2652 2166279 : return gf_vec_dot(*p, plane->normal) + plane->d;
2653 : }
2654 :
2655 :
2656 : /*return p-vertex index (vertex from bbox closer to plane) - index range from 0 to 8*/
2657 : GF_EXPORT
2658 35322 : u32 gf_plane_get_p_vertex_idx(GF_Plane *p)
2659 : {
2660 35322 : if (p->normal.x>=0) {
2661 22449 : if (p->normal.y>=0) return (p->normal.z>=0) ? 0 : 1;
2662 6247 : return (p->normal.z>=0) ? 2 : 3;
2663 : } else {
2664 12873 : if (p->normal.y>=0) return (p->normal.z>=0) ? 4 : 5;
2665 6789 : return (p->normal.z>=0) ? 6 : 7;
2666 : }
2667 : }
2668 :
2669 :
2670 : GF_EXPORT
2671 179 : u32 gf_bbox_plane_relation(GF_BBox *box, GF_Plane *p)
2672 : {
2673 : GF_Vec nearv, farv;
2674 179 : nearv = box->max_edge;
2675 179 : farv = box->min_edge;
2676 179 : if (p->normal.x > 0) {
2677 129 : nearv.x = box->min_edge.x;
2678 129 : farv.x = box->max_edge.x;
2679 : }
2680 179 : if (p->normal.y > 0) {
2681 59 : nearv.y = box->min_edge.y;
2682 59 : farv.y = box->max_edge.y;
2683 : }
2684 179 : if (p->normal.z > 0) {
2685 2 : nearv.z = box->min_edge.z;
2686 2 : farv.z = box->max_edge.z;
2687 : }
2688 179 : if (gf_vec_dot(p->normal, nearv) + p->d > 0) return GF_BBOX_FRONT;
2689 47 : if (gf_vec_dot(p->normal, farv) + p->d > 0) return GF_BBOX_INTER;
2690 6 : return GF_BBOX_BACK;
2691 : }
2692 :
2693 :
2694 : GF_EXPORT
2695 760 : u32 gf_get_next_pow2(u32 s)
2696 : {
2697 : u32 res = 1;
2698 6976 : while (s > res) {
2699 5456 : res <<= 1;
2700 : }
2701 760 : return res;
2702 : }
2703 :
|
