1 /*
2 mathlib.c
3
4 math primitives
5
6 Copyright (C) 1996-1997 Id Software, Inc.
7
8 This program is free software; you can redistribute it and/or
9 modify it under the terms of the GNU General Public License
10 as published by the Free Software Foundation; either version 2
11 of the License, or (at your option) any later version.
12
13 This program is distributed in the hope that it will be useful,
14 but WITHOUT ANY WARRANTY; without even the implied warranty of
15 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
16
17 See the GNU General Public License for more details.
18
19 You should have received a copy of the GNU General Public License
20 along with this program; if not, write to:
21
22 Free Software Foundation, Inc.
23 59 Temple Place - Suite 330
24 Boston, MA 02111-1307, USA
25
26 */
27 #ifdef HAVE_CONFIG_H
28 # include "config.h"
29 #endif
30
31 #ifdef HAVE_STRING_H
32 # include <string.h>
33 #endif
34 #ifdef HAVE_STRINGS_H
35 # include <strings.h>
36 #endif
37
38 #include <math.h>
39
40 #define IMPLEMENT_R_Cull
41 #define IMPLEMENT_VectorNormalize
42
43 #include "QF/mathlib.h"
44 #include "QF/qtypes.h"
45 #include "QF/sys.h"
46
47 VISIBLE int nanmask = 255 << 23;
48 static plane_t _frustum[4];
49 VISIBLE plane_t *const frustum = _frustum;
50 static vec3_t _vec3_origin = { 0, 0, 0 };
51 VISIBLE const vec_t * const vec3_origin = _vec3_origin;
52 static quat_t _quat_origin = { 0, 0, 0, 0 };
53 VISIBLE const vec_t * const quat_origin = _quat_origin;
54
55 #define DEG2RAD(a) (a * (M_PI / 180.0))
56
57 #define FMANTBITS 23
58 #define FMANTMASK ((1 << FMANTBITS) - 1)
59 #define FEXPBITS 8
60 #define FEXPMASK ((1 << FEXPBITS) - 1)
61 #define FBIAS (1 << (FEXPBITS - 1))
62 #define FEXPMAX ((1 << FEXPBITS) - 1)
63
64 #define HMANTBITS 10
65 #define HMANTMASK ((1 << HMANTBITS) - 1)
66 #define HEXPBITS 5
67 #define HEXPMASK ((1 << HEXPBITS) - 1)
68 #define HBIAS (1 << (HEXPBITS - 1))
69 #define HEXPMAX ((1 << HEXPBITS) - 1)
70
71 int16_t
FloatToHalf(float x)72 FloatToHalf (float x)
73 {
74 union {
75 float f;
76 uint32_t u;
77 } uf;
78 unsigned sign;
79 int exp;
80 unsigned mant;
81 int16_t half;
82
83 uf.f = x;
84 sign = (uf.u >> (FEXPBITS + FMANTBITS)) & 1;
85 exp = ((uf.u >> FMANTBITS) & FEXPMASK) - FBIAS + HBIAS;
86 mant = (uf.u & FMANTMASK) >> (FMANTBITS - HMANTBITS);
87 if (exp <= 0) {
88 mant |= 1 << HMANTBITS;
89 mant >>= min (1 - exp, HMANTBITS + 1);
90 exp = 0;
91 } else if (exp >= HEXPMAX) {
92 mant = 0;
93 exp = HEXPMAX;
94 }
95 half = (sign << (HEXPBITS + HMANTBITS)) | (exp << HMANTBITS) | mant;
96 return half;
97 }
98
99 float
HalfToFloat(int16_t x)100 HalfToFloat (int16_t x)
101 {
102 union {
103 float f;
104 uint32_t u;
105 } uf;
106 unsigned sign;
107 int exp;
108 unsigned mant;
109
110 sign = (x >> (HEXPBITS + HMANTBITS)) & 1;
111 exp = ((x >> HMANTBITS) & HEXPMASK);
112 mant = (x & HMANTMASK) << (FMANTBITS - HMANTBITS);
113
114 if (exp == 0) {
115 if (mant) {
116 while (mant < (1 << FMANTBITS)) {
117 mant <<= 1;
118 exp--;
119 }
120 mant &= (1 << FMANTBITS) - 1;
121 exp += FBIAS - HBIAS + 1;
122 }
123 } else if (exp == HEXPMAX) {
124 exp = FEXPMAX;
125 } else {
126 exp += FBIAS - HBIAS;
127 }
128 uf.u = (sign << (FEXPBITS + FMANTBITS)) | (exp << FMANTBITS) | mant;
129 return uf.f;
130 }
131
132 static void
ProjectPointOnPlane(vec3_t dst,const vec3_t p,const vec3_t normal)133 ProjectPointOnPlane (vec3_t dst, const vec3_t p, const vec3_t normal)
134 {
135 float inv_denom, d;
136 vec3_t n;
137
138 inv_denom = 1.0F / DotProduct (normal, normal);
139
140 d = DotProduct (normal, p) * inv_denom;
141
142 VectorScale (normal, inv_denom * d, n);
143
144 VectorSubtract (p, n, dst);
145 }
146
147 // assumes "src" is normalized
148 static void
PerpendicularVector(vec3_t dst,const vec3_t src)149 PerpendicularVector (vec3_t dst, const vec3_t src)
150 {
151 int pos, i;
152 float minelem = 1.0F;
153 vec3_t tempvec;
154
155 /* find the smallest magnitude axially aligned vector */
156 for (pos = 0, i = 0; i < 3; i++) {
157 if (fabs (src[i]) < minelem) {
158 pos = i;
159 minelem = fabs (src[i]);
160 }
161 }
162 VectorZero (tempvec);
163 tempvec[pos] = 1.0F;
164
165 /* project the point onto the plane defined by src */
166 ProjectPointOnPlane (dst, tempvec, src);
167
168 /* normalize the result */
169 VectorNormalize (dst);
170 }
171
172 #if defined(_WIN32) && !defined(__GNUC__)
173 # pragma optimize( "", off )
174 #endif
175
176 VISIBLE void
VectorVectors(const vec3_t forward,vec3_t right,vec3_t up)177 VectorVectors(const vec3_t forward, vec3_t right, vec3_t up)
178 {
179 float d;
180
181 right[0] = forward[2];
182 right[1] = -forward[0];
183 right[2] = forward[1];
184
185 d = DotProduct(forward, right);
186 VectorMultSub (right, d, forward, right);
187 VectorNormalize (right);
188 CrossProduct(right, forward, up);
189 }
190
191 VISIBLE void
RotatePointAroundVector(vec3_t dst,const vec3_t axis,const vec3_t point,float degrees)192 RotatePointAroundVector (vec3_t dst, const vec3_t axis, const vec3_t point,
193 float degrees)
194 {
195 float m[3][3];
196 float im[3][3];
197 float zrot[3][3];
198 float tmpmat[3][3];
199 float rot[3][3];
200 int i;
201 vec3_t vr, vup, vf;
202
203 VectorCopy (axis, vf);
204
205 PerpendicularVector (vr, axis);
206 CrossProduct (vr, vf, vup);
207
208 m[0][0] = vr[0];
209 m[1][0] = vr[1];
210 m[2][0] = vr[2];
211
212 m[0][1] = vup[0];
213 m[1][1] = vup[1];
214 m[2][1] = vup[2];
215
216 m[0][2] = vf[0];
217 m[1][2] = vf[1];
218 m[2][2] = vf[2];
219
220 memcpy (im, m, sizeof (im));
221
222 im[0][1] = m[1][0];
223 im[0][2] = m[2][0];
224 im[1][0] = m[0][1];
225 im[1][2] = m[2][1];
226 im[2][0] = m[0][2];
227 im[2][1] = m[1][2];
228
229 memset (zrot, 0, sizeof (zrot));
230 zrot[0][0] = zrot[1][1] = zrot[2][2] = 1.0F;
231
232 zrot[0][0] = cos (DEG2RAD (degrees));
233 zrot[0][1] = sin (DEG2RAD (degrees));
234 zrot[1][0] = -sin (DEG2RAD (degrees));
235 zrot[1][1] = cos (DEG2RAD (degrees));
236
237 R_ConcatRotations (m, zrot, tmpmat);
238 R_ConcatRotations (tmpmat, im, rot);
239
240 for (i = 0; i < 3; i++) {
241 dst[i] = DotProduct (rot[i], point);
242 }
243 }
244
245 VISIBLE void
QuatMult(const quat_t q1,const quat_t q2,quat_t out)246 QuatMult (const quat_t q1, const quat_t q2, quat_t out)
247 {
248 vec_t s;
249 vec3_t v;
250
251 s = q1[0] * q2[0] - DotProduct (q1 + 1, q2 + 1);
252 CrossProduct (q1 + 1, q2 + 1, v);
253 VectorMultAdd (v, q1[0], q2 + 1, v);
254 VectorMultAdd (v, q2[0], q1 + 1, out + 1);
255 out[0] = s;
256 }
257
258 VISIBLE void
QuatMultVec(const quat_t q,const vec3_t v,vec3_t out)259 QuatMultVec (const quat_t q, const vec3_t v, vec3_t out)
260 {
261 vec_t s;
262 vec3_t tv;
263
264 s = -DotProduct (q + 1, v);
265 CrossProduct (q + 1, v, tv);
266 VectorMultAdd (tv, q[0], v, tv);
267 CrossProduct (q + 1, tv, out);
268 VectorMultSub (out, s, q + 1, out);
269 VectorMultAdd (out, q[0], tv, out);
270 }
271
272 VISIBLE void
QuatInverse(const quat_t in,quat_t out)273 QuatInverse (const quat_t in, quat_t out)
274 {
275 quat_t q;
276 vec_t m;
277
278 m = QDotProduct (in, in); // in * in*
279 QuatConj (in, q);
280 QuatScale (q, 1 / m, out);
281 }
282
283 VISIBLE void
QuatExp(const quat_t a,quat_t b)284 QuatExp (const quat_t a, quat_t b)
285 {
286 vec3_t n;
287 vec_t th;
288 vec_t r;
289 vec_t c, s;
290
291 VectorCopy (a + 1, n);
292 th = VectorNormalize (n);
293 r = expf (a[0]);
294 c = cosf (th);
295 s = sinf (th);
296 VectorScale (n, r * s, b + 1);
297 b[0] = r * c;
298 }
299
300 VISIBLE void
QuatToMatrix(const quat_t q,vec_t * m,int homogenous,int vertical)301 QuatToMatrix (const quat_t q, vec_t *m, int homogenous, int vertical)
302 {
303 vec_t aa, ab, ac, ad, bb, bc, bd, cc, cd, dd;
304 vec_t *_m[4] = {
305 m + (homogenous ? 0 : 0),
306 m + (homogenous ? 4 : 3),
307 m + (homogenous ? 8 : 6),
308 m + (homogenous ? 12 : 9),
309 };
310
311 aa = q[0] * q[0];
312 ab = q[0] * q[1];
313 ac = q[0] * q[2];
314 ad = q[0] * q[3];
315
316 bb = q[1] * q[1];
317 bc = q[1] * q[2];
318 bd = q[1] * q[3];
319
320 cc = q[2] * q[2];
321 cd = q[2] * q[3];
322
323 dd = q[3] * q[3];
324
325 if (vertical) {
326 VectorSet (aa + bb - cc - dd, 2 * (bc + ad), 2 * (bd - ac), _m[0]);
327 VectorSet (2 * (bc - ad), aa - bb + cc - dd, 2 * (cd + ab), _m[1]);
328 VectorSet (2 * (bd + ac), 2 * (cd - ab), aa - bb - cc + dd, _m[2]);
329 } else {
330 VectorSet (aa + bb - cc - dd, 2 * (bc - ad), 2 * (bd + ac), _m[0]);
331 VectorSet (2 * (bc + ad), aa - bb + cc - dd, 2 * (cd - ab), _m[1]);
332 VectorSet (2 * (bd - ac), 2 * (cd + ab), aa - bb - cc + dd, _m[2]);
333 }
334 if (homogenous) {
335 _m[0][3] = 0;
336 _m[1][3] = 0;
337 _m[2][3] = 0;
338 VectorZero (_m[3]);
339 _m[3][3] = 1;
340 }
341 }
342
343 #if defined(_WIN32) && !defined(__GNUC__)
344 # pragma optimize( "", on )
345 #endif
346
347 VISIBLE float
anglemod(float a)348 anglemod (float a)
349 {
350 a = (360.0 / 65536) * ((int) (a * (65536 / 360.0)) & 65535);
351 return a;
352 }
353
354 /*
355 BOPS_Error
356
357 Split out like this for ASM to call.
358 */
359 void __attribute__ ((noreturn)) BOPS_Error (void);
360 VISIBLE void __attribute__ ((noreturn))
BOPS_Error(void)361 BOPS_Error (void)
362 {
363 Sys_Error ("BoxOnPlaneSide: Bad signbits");
364 }
365
366 #ifndef USE_INTEL_ASM
367
368 /*
369 BoxOnPlaneSide
370
371 Returns 1, 2, or 1 + 2
372 */
373 VISIBLE int
BoxOnPlaneSide(const vec3_t emins,const vec3_t emaxs,plane_t * p)374 BoxOnPlaneSide (const vec3_t emins, const vec3_t emaxs, plane_t *p)
375 {
376 float dist1, dist2;
377 int sides;
378
379 #if 0
380 // this is done by the BOX_ON_PLANE_SIDE macro before
381 // calling this function
382
383 // fast axial cases
384 if (p->type < 3) {
385 if (p->dist <= emins[p->type])
386 return 1;
387 if (p->dist >= emaxs[p->type])
388 return 2;
389 return 3;
390 }
391 #endif
392
393 // general case
394 switch (p->signbits) {
395 case 0:
396 dist1 = p->normal[0] * emaxs[0] + p->normal[1] * emaxs[1] +
397 p->normal[2] * emaxs[2];
398 dist2 = p->normal[0] * emins[0] + p->normal[1] * emins[1] +
399 p->normal[2] * emins[2];
400 break;
401 case 1:
402 dist1 = p->normal[0] * emins[0] + p->normal[1] * emaxs[1] +
403 p->normal[2] * emaxs[2];
404 dist2 = p->normal[0] * emaxs[0] + p->normal[1] * emins[1] +
405 p->normal[2] * emins[2];
406 break;
407 case 2:
408 dist1 = p->normal[0] * emaxs[0] + p->normal[1] * emins[1] +
409 p->normal[2] * emaxs[2];
410 dist2 = p->normal[0] * emins[0] + p->normal[1] * emaxs[1] +
411 p->normal[2] * emins[2];
412 break;
413 case 3:
414 dist1 = p->normal[0] * emins[0] + p->normal[1] * emins[1] +
415 p->normal[2] * emaxs[2];
416 dist2 = p->normal[0] * emaxs[0] + p->normal[1] * emaxs[1] +
417 p->normal[2] * emins[2];
418 break;
419 case 4:
420 dist1 = p->normal[0] * emaxs[0] + p->normal[1] * emaxs[1] +
421 p->normal[2] * emins[2];
422 dist2 = p->normal[0] * emins[0] + p->normal[1] * emins[1] +
423 p->normal[2] * emaxs[2];
424 break;
425 case 5:
426 dist1 = p->normal[0] * emins[0] + p->normal[1] * emaxs[1] +
427 p->normal[2] * emins[2];
428 dist2 = p->normal[0] * emaxs[0] + p->normal[1] * emins[1] +
429 p->normal[2] * emaxs[2];
430 break;
431 case 6:
432 dist1 = p->normal[0] * emaxs[0] + p->normal[1] * emins[1] +
433 p->normal[2] * emins[2];
434 dist2 = p->normal[0] * emins[0] + p->normal[1] * emaxs[1] +
435 p->normal[2] * emaxs[2];
436 break;
437 case 7:
438 dist1 = p->normal[0] * emins[0] + p->normal[1] * emins[1] +
439 p->normal[2] * emins[2];
440 dist2 = p->normal[0] * emaxs[0] + p->normal[1] * emaxs[1] +
441 p->normal[2] * emaxs[2];
442 break;
443 default:
444 BOPS_Error ();
445 }
446
447 #if 0
448 int i;
449 vec3_t corners[2];
450
451 for (i = 0; i < 3; i++) {
452 if (plane->normal[i] < 0) {
453 corners[0][i] = emins[i];
454 corners[1][i] = emaxs[i];
455 } else {
456 corners[1][i] = emins[i];
457 corners[0][i] = emaxs[i];
458 }
459 }
460 dist = DotProduct (plane->normal, corners[0]) - plane->dist;
461 dist2 = DotProduct (plane->normal, corners[1]) - plane->dist;
462 sides = 0;
463 if (dist1 >= 0)
464 sides = 1;
465 if (dist2 < 0)
466 sides |= 2;
467 #endif
468
469 sides = 0;
470 if (dist1 >= p->dist)
471 sides = 1;
472 if (dist2 < p->dist)
473 sides |= 2;
474
475 #ifdef PARANOID
476 if (sides == 0)
477 Sys_Error ("BoxOnPlaneSide: sides==0");
478 #endif
479
480 return sides;
481 }
482 #endif
483
484 /*
485 angles is a left(?) handed system: 'pitch yaw roll' with x (pitch) axis to
486 the right, y (yaw) axis up and z (roll) axis forward.
487
488 the math in AngleVectors has the entity frame as left handed with x
489 (forward) axis forward, y (right) axis to the right and z (up) up. However,
490 the world is a right handed system with x to the right, y forward and
491 z up.
492
493 pitch =
494 cp 0 -sp
495 0 1 0
496 sp 0 cp
497
498 yaw =
499 cy sy 0
500 -sy cy 0
501 0 0 1
502
503 roll =
504 1 0 0
505 0 cr sr
506 0 -sr cr
507
508 final = roll * (pitch * yaw)
509 final =
510 [forward]
511 [-right] -ve due to left handed to right handed conversion
512 [up]
513 */
514 VISIBLE void
AngleVectors(const vec3_t angles,vec3_t forward,vec3_t right,vec3_t up)515 AngleVectors (const vec3_t angles, vec3_t forward, vec3_t right, vec3_t up)
516 {
517 float angle, sr, sp, sy, cr, cp, cy;
518
519 angle = angles[YAW] * (M_PI * 2 / 360);
520 sy = sin (angle);
521 cy = cos (angle);
522 angle = angles[PITCH] * (M_PI * 2 / 360);
523 sp = sin (angle);
524 cp = cos (angle);
525 angle = angles[ROLL] * (M_PI * 2 / 360);
526 sr = sin (angle);
527 cr = cos (angle);
528
529 forward[0] = cp * cy;
530 forward[1] = cp * sy;
531 forward[2] = -sp;
532 // need to flip right because it's a left handed system in a right handed
533 // world
534 right[0] = -1 * (sr * sp * cy + cr * -sy);
535 right[1] = -1 * (sr * sp * sy + cr * cy);
536 right[2] = -1 * (sr * cp);
537 up[0] = (cr * sp * cy + -sr * -sy);
538 up[1] = (cr * sp * sy + -sr * cy);
539 up[2] = cr * cp;
540 }
541
542 VISIBLE void
AngleQuat(const vec3_t angles,quat_t q)543 AngleQuat (const vec3_t angles, quat_t q)
544 {
545 float alpha, sr, sp, sy, cr, cp, cy;
546
547 // alpha is half the angle
548 alpha = angles[YAW] * (M_PI / 360);
549 sy = sin (alpha);
550 cy = cos (alpha);
551 alpha = angles[PITCH] * (M_PI / 360);
552 sp = sin (alpha);
553 cp = cos (alpha);
554 alpha = angles[ROLL] * (M_PI / 360);
555 sr = sin (alpha);
556 cr = cos (alpha);
557
558 QuatSet (cy * cp * cr + sy * sp * sr,
559 cy * cp * sr - sy * sp * cr,
560 cy * sp * cr + sy * cp * sr,
561 sy * cp * cr - cy * sp * sr,
562 q);
563 }
564
565 VISIBLE int
_VectorCompare(const vec3_t v1,const vec3_t v2)566 _VectorCompare (const vec3_t v1, const vec3_t v2)
567 {
568 int i;
569
570 for (i = 0; i < 3; i++)
571 if (fabs (v1[i] - v2[i]) > EQUAL_EPSILON)
572 return 0;
573
574 return 1;
575 }
576
577 VISIBLE void
_VectorMA(const vec3_t veca,float scale,const vec3_t vecb,vec3_t vecc)578 _VectorMA (const vec3_t veca, float scale, const vec3_t vecb, vec3_t vecc)
579 {
580 vecc[0] = veca[0] + scale * vecb[0];
581 vecc[1] = veca[1] + scale * vecb[1];
582 vecc[2] = veca[2] + scale * vecb[2];
583 }
584
585 VISIBLE vec_t
_DotProduct(const vec3_t v1,const vec3_t v2)586 _DotProduct (const vec3_t v1, const vec3_t v2)
587 {
588 return v1[0] * v2[0] + v1[1] * v2[1] + v1[2] * v2[2];
589 }
590
591 VISIBLE void
_VectorSubtract(const vec3_t veca,const vec3_t vecb,vec3_t out)592 _VectorSubtract (const vec3_t veca, const vec3_t vecb, vec3_t out)
593 {
594 out[0] = veca[0] - vecb[0];
595 out[1] = veca[1] - vecb[1];
596 out[2] = veca[2] - vecb[2];
597 }
598
599 VISIBLE void
_VectorAdd(const vec3_t veca,const vec3_t vecb,vec3_t out)600 _VectorAdd (const vec3_t veca, const vec3_t vecb, vec3_t out)
601 {
602 out[0] = veca[0] + vecb[0];
603 out[1] = veca[1] + vecb[1];
604 out[2] = veca[2] + vecb[2];
605 }
606
607 VISIBLE void
_VectorCopy(const vec3_t in,vec3_t out)608 _VectorCopy (const vec3_t in, vec3_t out)
609 {
610 out[0] = in[0];
611 out[1] = in[1];
612 out[2] = in[2];
613 }
614
615 VISIBLE void
CrossProduct(const vec3_t v1,const vec3_t v2,vec3_t cross)616 CrossProduct (const vec3_t v1, const vec3_t v2, vec3_t cross)
617 {
618 float v10 = v1[0];
619 float v11 = v1[1];
620 float v12 = v1[2];
621 float v20 = v2[0];
622 float v21 = v2[1];
623 float v22 = v2[2];
624
625 cross[0] = v11 * v22 - v12 * v21;
626 cross[1] = v12 * v20 - v10 * v22;
627 cross[2] = v10 * v21 - v11 * v20;
628 }
629
630 VISIBLE vec_t
_VectorLength(const vec3_t v)631 _VectorLength (const vec3_t v)
632 {
633 float length;
634
635 length = sqrt (DotProduct (v, v));
636 return length;
637 }
638
639 VISIBLE vec_t
_VectorNormalize(vec3_t v)640 _VectorNormalize (vec3_t v)
641 {
642 int i;
643 double length;
644
645 length = 0;
646 for (i = 0; i < 3; i++)
647 length += v[i] * v[i];
648 length = sqrt (length);
649 if (length == 0)
650 return 0;
651
652 for (i = 0; i < 3; i++)
653 v[i] /= length;
654
655 return length;
656 }
657
658 VISIBLE void
_VectorScale(const vec3_t in,vec_t scale,vec3_t out)659 _VectorScale (const vec3_t in, vec_t scale, vec3_t out)
660 {
661 out[0] = in[0] * scale;
662 out[1] = in[1] * scale;
663 out[2] = in[2] * scale;
664 }
665
666 VISIBLE int
Q_log2(int val)667 Q_log2 (int val)
668 {
669 int answer = 0;
670
671 while ((val >>= 1) != 0)
672 answer++;
673 return answer;
674 }
675
676 VISIBLE void
R_ConcatRotations(float in1[3][3],float in2[3][3],float out[3][3])677 R_ConcatRotations (float in1[3][3], float in2[3][3], float out[3][3])
678 {
679 out[0][0] = in1[0][0] * in2[0][0] + in1[0][1] * in2[1][0] +
680 in1[0][2] * in2[2][0];
681 out[0][1] = in1[0][0] * in2[0][1] + in1[0][1] * in2[1][1] +
682 in1[0][2] * in2[2][1];
683 out[0][2] = in1[0][0] * in2[0][2] + in1[0][1] * in2[1][2] +
684 in1[0][2] * in2[2][2];
685 out[1][0] = in1[1][0] * in2[0][0] + in1[1][1] * in2[1][0] +
686 in1[1][2] * in2[2][0];
687 out[1][1] = in1[1][0] * in2[0][1] + in1[1][1] * in2[1][1] +
688 in1[1][2] * in2[2][1];
689 out[1][2] = in1[1][0] * in2[0][2] + in1[1][1] * in2[1][2] +
690 in1[1][2] * in2[2][2];
691 out[2][0] = in1[2][0] * in2[0][0] + in1[2][1] * in2[1][0] +
692 in1[2][2] * in2[2][0];
693 out[2][1] = in1[2][0] * in2[0][1] + in1[2][1] * in2[1][1] +
694 in1[2][2] * in2[2][1];
695 out[2][2] = in1[2][0] * in2[0][2] + in1[2][1] * in2[1][2] +
696 in1[2][2] * in2[2][2];
697 }
698
699 VISIBLE void
R_ConcatTransforms(float in1[3][4],float in2[3][4],float out[3][4])700 R_ConcatTransforms (float in1[3][4], float in2[3][4], float out[3][4])
701 {
702 out[0][0] = in1[0][0] * in2[0][0] + in1[0][1] * in2[1][0] +
703 in1[0][2] * in2[2][0];
704 out[0][1] = in1[0][0] * in2[0][1] + in1[0][1] * in2[1][1] +
705 in1[0][2] * in2[2][1];
706 out[0][2] = in1[0][0] * in2[0][2] + in1[0][1] * in2[1][2] +
707 in1[0][2] * in2[2][2];
708 out[0][3] = in1[0][0] * in2[0][3] + in1[0][1] * in2[1][3] +
709 in1[0][2] * in2[2][3] + in1[0][3];
710 out[1][0] = in1[1][0] * in2[0][0] + in1[1][1] * in2[1][0] +
711 in1[1][2] * in2[2][0];
712 out[1][1] = in1[1][0] * in2[0][1] + in1[1][1] * in2[1][1] +
713 in1[1][2] * in2[2][1];
714 out[1][2] = in1[1][0] * in2[0][2] + in1[1][1] * in2[1][2] +
715 in1[1][2] * in2[2][2];
716 out[1][3] = in1[1][0] * in2[0][3] + in1[1][1] * in2[1][3] +
717 in1[1][2] * in2[2][3] + in1[1][3];
718 out[2][0] = in1[2][0] * in2[0][0] + in1[2][1] * in2[1][0] +
719 in1[2][2] * in2[2][0];
720 out[2][1] = in1[2][0] * in2[0][1] + in1[2][1] * in2[1][1] +
721 in1[2][2] * in2[2][1];
722 out[2][2] = in1[2][0] * in2[0][2] + in1[2][1] * in2[1][2] +
723 in1[2][2] * in2[2][2];
724 out[2][3] = in1[2][0] * in2[0][3] + in1[2][1] * in2[1][3] +
725 in1[2][2] * in2[2][3] + in1[2][3];
726 }
727
728 /*
729 FloorDivMod
730
731 Returns mathematically correct (floor-based) quotient and remainder for
732 numer and denom, both of which should contain no fractional part. The
733 quotient must fit in 32 bits.
734 */
735 VISIBLE void
FloorDivMod(double numer,double denom,int * quotient,int * rem)736 FloorDivMod (double numer, double denom, int *quotient, int *rem)
737 {
738 double x;
739 int q, r;
740
741 #ifndef PARANOID
742 if (denom <= 0.0)
743 Sys_Error ("FloorDivMod: bad denominator %f", denom);
744
745 // if ((floor(numer) != numer) || (floor(denom) != denom))
746 // Sys_Error ("FloorDivMod: non-integer numer or denom %f %f",
747 // numer, denom);
748 #endif
749
750 if (numer >= 0.0) {
751 x = floor (numer / denom);
752 q = (int) x;
753 r = (int) floor (numer - (x * denom));
754 } else {
755 // perform operations with positive values, and fix mod to make
756 // floor-based
757 x = floor (-numer / denom);
758 q = -(int) x;
759 r = (int) floor (-numer - (x * denom));
760 if (r != 0) {
761 q--;
762 r = (int) denom - r;
763 }
764 }
765
766 *quotient = q;
767 *rem = r;
768 }
769
770 VISIBLE int
GreatestCommonDivisor(int i1,int i2)771 GreatestCommonDivisor (int i1, int i2)
772 {
773 if (i1 > i2) {
774 if (i2 == 0)
775 return (i1);
776 return GreatestCommonDivisor (i2, i1 % i2);
777 } else {
778 if (i1 == 0)
779 return (i2);
780 return GreatestCommonDivisor (i1, i2 % i1);
781 }
782 }
783
784 #ifndef USE_INTEL_ASM
785 /*
786 Invert24To16
787
788 Inverts an 8.24 value to a 16.16 value
789 */
790 VISIBLE fixed16_t
Invert24To16(fixed16_t val)791 Invert24To16 (fixed16_t val)
792 {
793 if (val < 256)
794 return (0xFFFFFFFF);
795
796 return (fixed16_t)
797 (((double) 0x10000 * (double) 0x1000000 / (double) val) + 0.5);
798 }
799 #endif
800
801 void
Mat3Init(const quat_t rot,const vec3_t scale,mat3_t mat)802 Mat3Init (const quat_t rot, const vec3_t scale, mat3_t mat)
803 {
804 QuatToMatrix (rot, mat, 0, 1);
805 VectorScale (mat + 0, scale[0], mat + 0);
806 VectorScale (mat + 3, scale[1], mat + 3);
807 VectorScale (mat + 6, scale[2], mat + 6);
808 }
809
810 void
Mat3Transpose(const mat3_t a,mat3_t b)811 Mat3Transpose (const mat3_t a, mat3_t b)
812 {
813 vec_t t;
814 int i, j;
815
816 for (i = 0; i < 2; i++) {
817 b[i * 3 + i] = a[i * 3 + i]; // in case b != a
818 for (j = i + 1; j < 3; j++) {
819 t = a[i * 3 + j]; // in case b == a
820 b[i * 3 + j] = a[j * 3 + i];
821 b[j * 3 + i] = t;
822 }
823 }
824 b[i * 3 + i] = a[i * 3 + i]; // in case b != a
825 }
826
827 vec_t
Mat3Determinant(const mat3_t m)828 Mat3Determinant (const mat3_t m)
829 {
830 vec3_t t;
831 CrossProduct (m + 3, m + 6, t);
832 return DotProduct (m + 0, t);
833 }
834
835 typedef vec_t mat2_t[2 * 2];
836
837 static void
Mat3Sub2(const mat3_t m3,mat2_t m2,int i,int j)838 Mat3Sub2 (const mat3_t m3, mat2_t m2, int i, int j)
839 {
840 int si, sj, di, dj;
841
842 for (di = 0; di < 2; di++) {
843 for (dj = 0; dj < 2; dj++) {
844 si = di + ((di >= i) ? 1 : 0);
845 sj = dj + ((dj >= j) ? 1 : 0);
846 m2[di * 2 + dj] = m3[si * 3 + sj];
847 }
848 }
849 }
850
851 static vec_t
Mat2Det(const mat2_t m)852 Mat2Det (const mat2_t m)
853 {
854 return m[0] * m[3] - m[1] * m[2];
855 }
856
857 int
Mat3Inverse(const mat3_t a,mat3_t b)858 Mat3Inverse (const mat3_t a, mat3_t b)
859 {
860 mat3_t tb;
861 mat2_t m2;
862 vec_t *m = b;
863 int i, j;
864 vec_t det;
865 vec_t sign[2] = { 1, -1};
866
867 det = Mat3Determinant (a);
868 if (det * det < 1e-6) {
869 Mat3Identity (b);
870 return 0;
871 }
872 if (b == a)
873 m = tb;
874 for (i = 0; i < 3; i++) {
875 for (j = 0; j < 3; j++) {
876 Mat3Sub2 (a, m2, i, j);
877 m[j * 3 + i] = sign[(i + j) & 1] * Mat2Det (m2) / det;
878 }
879 }
880 if (m != b)
881 Mat3Copy (m, b);
882 return 1;
883 }
884
Mat3Mult(const mat3_t a,const mat3_t b,mat3_t c)885 void Mat3Mult (const mat3_t a, const mat3_t b, mat3_t c)
886 {
887 mat3_t ta, tb; // in case c == b or c == a
888 int i, j, k;
889
890 Mat3Transpose (a, ta); // transpose so we can use dot
891 Mat3Copy (b, tb);
892
893 k = 0;
894 for (i = 0; i < 3; i++) {
895 for (j = 0; j < 3; j++) {
896 c[k++] = DotProduct (ta + 3 * j, tb + 3 * i);
897 }
898 }
899 }
900
Mat3MultVec(const mat3_t a,const vec3_t b,vec3_t c)901 void Mat3MultVec (const mat3_t a, const vec3_t b, vec3_t c)
902 {
903 int i;
904 vec3_t tb;
905
906 VectorCopy (b, tb);
907 for (i = 0; i < 3; i++)
908 c[i] = a[i + 0] * tb[0] + a[i + 3] * b[1] + a[i + 6] * b[2];
909 }
910
911 #define sqr(x) ((x) * (x))
Mat3SymEigen(const mat3_t m,vec3_t e)912 void Mat3SymEigen (const mat3_t m, vec3_t e)
913 {
914 vec_t p, q, r;
915 vec_t phi;
916 mat3_t B;
917
918 p = sqr (m[1]) + sqr (m[2]) + sqr (m[5]);
919 if (p < 1e-6) {
920 e[0] = m[0];
921 e[1] = m[4];
922 e[2] = m[8];
923 return;
924 }
925 q = Mat3Trace (m) / 3;
926 p = sqr (m[0] - q) + sqr (m[4] - q) + sqr (m[8] - q) + 2 * p;
927 p = sqrt (p);
928 Mat3Zero (B);
929 B[0] = B[4] = B[8] = q;
930 Mat3Subtract (m, B, B);
931 Mat3Scale (B, 1.0 / p, B);
932 r = Mat3Determinant (B) / 2;
933 if (r >= 1)
934 phi = 0;
935 else if (r <= -1)
936 phi = M_PI / 3;
937 else
938 phi = acos (r) / 3;
939
940 e[0] = q + 2 * p * cos (phi);
941 e[2] = q + 2 * p * cos (phi + M_PI * 2 / 3);
942 e[1] = 3 * q - e[0] - e[2];
943 }
944
945 void
Mat4Init(const quat_t rot,const vec3_t scale,const vec3_t trans,mat4_t mat)946 Mat4Init (const quat_t rot, const vec3_t scale, const vec3_t trans, mat4_t mat)
947 {
948 QuatToMatrix (rot, mat, 1, 1);
949 VectorScale (mat + 0, scale[0], mat + 0);
950 VectorScale (mat + 4, scale[1], mat + 4);
951 VectorScale (mat + 8, scale[2], mat + 8);
952 VectorCopy (trans, mat + 12);
953 }
954
955 void
Mat4Transpose(const mat4_t a,mat4_t b)956 Mat4Transpose (const mat4_t a, mat4_t b)
957 {
958 vec_t t;
959 int i, j;
960
961 for (i = 0; i < 3; i++) {
962 b[i * 4 + i] = a[i * 4 + i]; // in case b != a
963 for (j = i + 1; j < 4; j++) {
964 t = a[i * 4 + j]; // in case b == a
965 b[i * 4 + j] = a[j * 4 + i];
966 b[j * 4 + i] = t;
967 }
968 }
969 b[i * 4 + i] = a[i * 4 + i]; // in case b != a
970 }
971
972 static void
Mat4Sub3(const mat4_t m4,mat3_t m3,int i,int j)973 Mat4Sub3 (const mat4_t m4, mat3_t m3, int i, int j)
974 {
975 int si, sj, di, dj;
976
977 for (di = 0; di < 3; di++) {
978 for (dj = 0; dj < 3; dj++) {
979 si = di + ((di >= i) ? 1 : 0);
980 sj = dj + ((dj >= j) ? 1 : 0);
981 m3[di * 3 + dj] = m4[si * 4 + sj];
982 }
983 }
984 }
985
986 static vec_t
Mat4Det(const mat4_t m)987 Mat4Det (const mat4_t m)
988 {
989 mat3_t t;
990 int i;
991 vec_t res = 0, det, s = 1;
992
993 for (i = 0; i < 4; i++, s = -s) {
994 Mat4Sub3 (m, t, 0, i);
995 det = Mat3Determinant (t);
996 res += m[i] * det * s;
997 }
998 return res;
999 }
1000
1001 int
Mat4Inverse(const mat4_t a,mat4_t b)1002 Mat4Inverse (const mat4_t a, mat4_t b)
1003 {
1004 mat4_t tb;
1005 mat3_t m3;
1006 vec_t *m = b;
1007 int i, j;
1008 vec_t det;
1009 vec_t sign[2] = { 1, -1};
1010
1011 det = Mat4Det (a);
1012 if (det * det < 1e-6) {
1013 Mat4Identity (b);
1014 return 0;
1015 }
1016 if (b == a)
1017 m = tb;
1018 for (i = 0; i < 4; i++) {
1019 for (j = 0; j < 4; j++) {
1020 Mat4Sub3 (a, m3, i, j);
1021 m[j * 4 + i] = sign[(i + j) & 1] * Mat3Determinant (m3) / det;
1022 }
1023 }
1024 if (m != b)
1025 Mat4Copy (m, b);
1026 return 1;
1027 }
1028
1029 void
Mat4Mult(const mat4_t a,const mat4_t b,mat4_t c)1030 Mat4Mult (const mat4_t a, const mat4_t b, mat4_t c)
1031 {
1032 mat4_t ta, tb; // in case c == b or c == a
1033 int i, j, k;
1034
1035 Mat4Transpose (a, ta); // transpose so we can use dot
1036 Mat4Copy (b, tb);
1037
1038 k = 0;
1039 for (i = 0; i < 4; i++) {
1040 for (j = 0; j < 4; j++) {
1041 c[k++] = QDotProduct (ta + 4 * j, tb + 4 * i);
1042 }
1043 }
1044 }
1045
1046 void
Mat4MultVec(const mat4_t a,const vec3_t b,vec3_t c)1047 Mat4MultVec (const mat4_t a, const vec3_t b, vec3_t c)
1048 {
1049 int i;
1050 vec3_t tb;
1051
1052 VectorCopy (b, tb);
1053 for (i = 0; i < 3; i++)
1054 c[i] = a[i + 0] * tb[0] + a[i + 4] * b[1] + a[i + 8] * b[2] + a[i +12];
1055 }
1056
1057 void
Mat4as3MultVec(const mat4_t a,const vec3_t b,vec3_t c)1058 Mat4as3MultVec (const mat4_t a, const vec3_t b, vec3_t c)
1059 {
1060 int i;
1061 vec3_t tb;
1062
1063 VectorCopy (b, tb);
1064 for (i = 0; i < 3; i++)
1065 c[i] = a[i + 0] * tb[0] + a[i + 4] * b[1] + a[i + 8] * b[2];
1066 }
1067
1068 int
Mat3Decompose(const mat3_t mat,quat_t rot,vec3_t shear,vec3_t scale)1069 Mat3Decompose (const mat3_t mat, quat_t rot, vec3_t shear, vec3_t scale)
1070 {
1071 vec3_t row[3], shr, scl;
1072 vec_t l, t;
1073 int i, j;
1074
1075 for (i = 0; i < 3; i++)
1076 for (j = 0; j < 3; j++)
1077 row[j][i] = mat[i * 3 + j];
1078
1079 l = DotProduct (row[0], row[0]);
1080 if (l < 1e-5)
1081 return 0;
1082 scl[0] = sqrt (l);
1083 VectorScale (row[0], 1/scl[0], row[0]);
1084 shr[0] = DotProduct (row[0], row[1]);
1085
1086 VectorMultSub (row[1], shr[0], row[0], row[1]);
1087 l = DotProduct (row[1], row[1]);
1088 if (l < 1e-5)
1089 return 0;
1090 scl[1] = sqrt (l);
1091 shr[0] /= scl[1];
1092 VectorScale (row[1], 1/scl[1], row[1]);
1093 shr[1] = DotProduct (row[0], row[2]);
1094
1095 VectorMultSub (row[2], shr[1], row[0], row[2]);
1096 shr[2] = DotProduct (row[1], row[2]);
1097 VectorMultSub (row[2], shr[2], row[1], row[2]);
1098 l = DotProduct (row[2], row[2]);
1099 if (l < 1e-5)
1100 return 0;
1101 scl[2] = sqrt (l);
1102 shr[1] /= scl[2];
1103 shr[2] /= scl[2];
1104 VectorScale (row[2], 1/scl[2], row[2]);
1105 if (scale)
1106 VectorCopy (scl, scale);
1107 if (shear)
1108 VectorCopy (shr, shear);
1109 if (!rot)
1110 return 1;
1111
1112 t = 1 + row[0][0] + row[1][1] + row[2][2];
1113 if (t >= 1e-5) {
1114 vec_t s = sqrt (t) * 2;
1115 rot[0] = s / 4;
1116 rot[1] = (row[2][1] - row[1][2]) / s;
1117 rot[2] = (row[0][2] - row[2][0]) / s;
1118 rot[3] = (row[1][0] - row[0][1]) / s;
1119 } else {
1120 if (row[0][0] > row[1][1] && row[0][0] > row[2][2]) {
1121 vec_t s = sqrt (1 + row[0][0] - row[1][1] - row[2][2]) * 2;
1122 rot[0] = (row[2][1] - row[1][2]) / s;
1123 rot[1] = s / 4;
1124 rot[2] = (row[1][0] + row[0][1]) / s;
1125 rot[3] = (row[0][2] + row[2][0]) / s;
1126 } else if (row[1][1] > row[2][2]) {
1127 vec_t s = sqrt (1 + row[1][1] - row[0][0] - row[2][2]) * 2;
1128 rot[0] = (row[0][2] - row[2][0]) / s;
1129 rot[1] = (row[1][0] + row[0][1]) / s;
1130 rot[2] = s / 4;
1131 rot[3] = (row[2][1] + row[1][2]) / s;
1132 } else {
1133 vec_t s = sqrt (1 + row[2][2] - row[0][0] - row[1][1]) * 2;
1134 rot[0] = (row[1][0] - row[0][1]) / s;
1135 rot[1] = (row[0][2] + row[2][0]) / s;
1136 rot[2] = (row[2][1] + row[1][2]) / s;
1137 rot[3] = s / 4;
1138 }
1139 }
1140 return 1;
1141 }
1142
1143 int
Mat4Decompose(const mat4_t mat,quat_t rot,vec3_t shear,vec3_t scale,vec3_t trans)1144 Mat4Decompose (const mat4_t mat, quat_t rot, vec3_t shear, vec3_t scale,
1145 vec3_t trans)
1146 {
1147 mat3_t m3;
1148
1149 if (trans)
1150 VectorCopy (mat + 12, trans);
1151 Mat4toMat3 (mat, m3);
1152 return Mat3Decompose (m3, rot, shear, scale);
1153 }
1154