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