1 /*
2 Copyright (C) 1996-1997 Id Software, Inc.
3
4 This program is free software; you can redistribute it and/or
5 modify it under the terms of the GNU General Public License
6 as published by the Free Software Foundation; either version 2
7 of the License, or (at your option) any later version.
8
9 This program is distributed in the hope that it will be useful,
10 but WITHOUT ANY WARRANTY; without even the implied warranty of
11 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
12
13 See the GNU General Public License for more details.
14
15 You should have received a copy of the GNU General Public License
16 along with this program; if not, write to the Free Software
17 Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
18
19 */
20 // mathlib.c -- math primitives
21
22 #include <math.h>
23 #include "quakedef.h"
24
25 void Sys_Error (char *error, ...);
26
27 vec3_t vec3_origin = {0,0,0};
28 int nanmask = 255<<23;
29
30 /*-----------------------------------------------------------------*/
31
ProjectPointOnPlane(vec3_t dst,const vec3_t p,const vec3_t normal)32 void ProjectPointOnPlane( vec3_t dst, const vec3_t p, const vec3_t normal )
33 {
34 float d;
35 vec3_t n;
36 float inv_denom;
37
38 inv_denom = 1.0F / DotProduct( normal, normal );
39
40 d = DotProduct( normal, p ) * inv_denom;
41
42 n[0] = normal[0] * inv_denom;
43 n[1] = normal[1] * inv_denom;
44 n[2] = normal[2] * inv_denom;
45
46 dst[0] = p[0] - d * n[0];
47 dst[1] = p[1] - d * n[1];
48 dst[2] = p[2] - d * n[2];
49 }
50
51 /*
52 ** assumes "src" is normalized
53 */
54 /*void PerpendicularVector( vec3_t dst, const vec3_t src )
55 {
56 int pos;
57 int i;
58 float minelem = 1.0F;
59 vec3_t tempvec;
60
61 for ( pos = 0, i = 0; i < 3; i++ )
62 {
63 if ( fabs( src[i] ) < minelem )
64 {
65 pos = i;
66 minelem = fabs( src[i] );
67 }
68 }
69 tempvec[0] = tempvec[1] = tempvec[2] = 0.0F;
70 tempvec[pos] = 1.0F;
71
72 ProjectPointOnPlane( dst, tempvec, src );
73
74 VectorNormalize( dst );
75 }*/
76
PerpendicularVector(vec3_t dst,const vec3_t src)77 void PerpendicularVector( vec3_t dst, const vec3_t src ) //Optimized a bit :) - Eradicator
78 {
79 int pos;
80 float minelem;
81
82 if (src[0])
83 {
84 dst[0] = 0;
85 if (src[1])
86 {
87 dst[1] = 0;
88 if (src[2])
89 {
90 dst[2] = 0;
91 pos = 0;
92 minelem = fabs(src[0]);
93 if (fabs(src[1]) < minelem)
94 {
95 pos = 1;
96 minelem = fabs(src[1]);
97 }
98 if (fabs(src[2]) < minelem)
99 pos = 2;
100
101 dst[pos] = 1;
102 dst[0] -= src[pos] * src[0];
103 dst[1] -= src[pos] * src[1];
104 dst[2] -= src[pos] * src[2];
105
106 VectorNormalize(dst);
107 }
108 else
109 dst[2] = 1;
110 }
111 else
112 {
113 dst[1] = 1;
114 dst[2] = 0;
115 }
116 }
117 else
118 {
119 dst[0] = 1;
120 dst[1] = 0;
121 dst[2] = 0;
122 }
123 }
124
125 #ifdef _WIN32
126 #pragma optimize( "", off )
127 #endif
128
129
RotatePointAroundVector(vec3_t dst,const vec3_t dir,const vec3_t point,float degrees)130 void RotatePointAroundVector( vec3_t dst, const vec3_t dir, const vec3_t point, float degrees )
131 {
132 float m[3][3];
133 float im[3][3];
134 float zrot[3][3];
135 float tmpmat[3][3];
136 float rot[3][3];
137 int i;
138 vec3_t vr, vup, vf;
139
140 vf[0] = dir[0];
141 vf[1] = dir[1];
142 vf[2] = dir[2];
143
144 PerpendicularVector( vr, dir );
145 CrossProduct( vr, vf, vup );
146
147 m[0][0] = vr[0];
148 m[1][0] = vr[1];
149 m[2][0] = vr[2];
150
151 m[0][1] = vup[0];
152 m[1][1] = vup[1];
153 m[2][1] = vup[2];
154
155 m[0][2] = vf[0];
156 m[1][2] = vf[1];
157 m[2][2] = vf[2];
158
159 memcpy( im, m, sizeof( im ) );
160
161 im[0][1] = m[1][0];
162 im[0][2] = m[2][0];
163 im[1][0] = m[0][1];
164 im[1][2] = m[2][1];
165 im[2][0] = m[0][2];
166 im[2][1] = m[1][2];
167
168 memset( zrot, 0, sizeof( zrot ) );
169 zrot[0][0] = zrot[1][1] = zrot[2][2] = 1.0F;
170
171 zrot[0][0] = cos( DEG2RAD( degrees ) );
172 zrot[0][1] = sin( DEG2RAD( degrees ) );
173 zrot[1][0] = -sin( DEG2RAD( degrees ) );
174 zrot[1][1] = cos( DEG2RAD( degrees ) );
175
176 R_ConcatRotations( m, zrot, tmpmat );
177 R_ConcatRotations( tmpmat, im, rot );
178
179 for ( i = 0; i < 3; i++ )
180 {
181 dst[i] = rot[i][0] * point[0] + rot[i][1] * point[1] + rot[i][2] * point[2];
182 }
183 }
184
185 #ifdef _WIN32
186 #pragma optimize( "", on )
187 #endif
188
189 /*-----------------------------------------------------------------*/
190
191
anglemod(float a)192 float anglemod(float a)
193 {
194 #if 0
195 if (a >= 0)
196 a -= 360*(int)(a/360);
197 else
198 a += 360*( 1 + (int)(-a/360) );
199 #endif
200 a = (360.0/65536) * ((int)(a*(65536/360.0)) & 65535);
201 return a;
202 }
203
204 /*
205 ==================
206 BOPS_Error
207
208 Split out like this for ASM to call.
209 ==================
210 */
BOPS_Error(void)211 void BOPS_Error (void)
212 {
213 Sys_Error ("BoxOnPlaneSide: Bad signbits");
214 }
215
216
217 #if !id386
218
219 /*
220 ==================
221 BoxOnPlaneSide
222
223 Returns 1, 2, or 1 + 2
224 ==================
225 */
BoxOnPlaneSide(vec3_t emins,vec3_t emaxs,mplane_t * p)226 int BoxOnPlaneSide (vec3_t emins, vec3_t emaxs, mplane_t *p)
227 {
228 float dist1, dist2;
229 int sides;
230
231 #if 0 // this is done by the BOX_ON_PLANE_SIDE macro before calling this
232 // function
233 // fast axial cases
234 if (p->type < 3)
235 {
236 if (p->dist <= emins[p->type])
237 return 1;
238 if (p->dist >= emaxs[p->type])
239 return 2;
240 return 3;
241 }
242 #endif
243
244 // general case
245 switch (p->signbits)
246 {
247 case 0:
248 dist1 = p->normal[0]*emaxs[0] + p->normal[1]*emaxs[1] + p->normal[2]*emaxs[2];
249 dist2 = p->normal[0]*emins[0] + p->normal[1]*emins[1] + p->normal[2]*emins[2];
250 break;
251 case 1:
252 dist1 = p->normal[0]*emins[0] + p->normal[1]*emaxs[1] + p->normal[2]*emaxs[2];
253 dist2 = p->normal[0]*emaxs[0] + p->normal[1]*emins[1] + p->normal[2]*emins[2];
254 break;
255 case 2:
256 dist1 = p->normal[0]*emaxs[0] + p->normal[1]*emins[1] + p->normal[2]*emaxs[2];
257 dist2 = p->normal[0]*emins[0] + p->normal[1]*emaxs[1] + p->normal[2]*emins[2];
258 break;
259 case 3:
260 dist1 = p->normal[0]*emins[0] + p->normal[1]*emins[1] + p->normal[2]*emaxs[2];
261 dist2 = p->normal[0]*emaxs[0] + p->normal[1]*emaxs[1] + p->normal[2]*emins[2];
262 break;
263 case 4:
264 dist1 = p->normal[0]*emaxs[0] + p->normal[1]*emaxs[1] + p->normal[2]*emins[2];
265 dist2 = p->normal[0]*emins[0] + p->normal[1]*emins[1] + p->normal[2]*emaxs[2];
266 break;
267 case 5:
268 dist1 = p->normal[0]*emins[0] + p->normal[1]*emaxs[1] + p->normal[2]*emins[2];
269 dist2 = p->normal[0]*emaxs[0] + p->normal[1]*emins[1] + p->normal[2]*emaxs[2];
270 break;
271 case 6:
272 dist1 = p->normal[0]*emaxs[0] + p->normal[1]*emins[1] + p->normal[2]*emins[2];
273 dist2 = p->normal[0]*emins[0] + p->normal[1]*emaxs[1] + p->normal[2]*emaxs[2];
274 break;
275 case 7:
276 dist1 = p->normal[0]*emins[0] + p->normal[1]*emins[1] + p->normal[2]*emins[2];
277 dist2 = p->normal[0]*emaxs[0] + p->normal[1]*emaxs[1] + p->normal[2]*emaxs[2];
278 break;
279 default:
280 dist1 = dist2 = 0; // shut up compiler
281 BOPS_Error ();
282 break;
283 }
284
285 #if 0
286 int i;
287 vec3_t corners[2];
288
289 for (i=0 ; i<3 ; i++)
290 {
291 if (plane->normal[i] < 0)
292 {
293 corners[0][i] = emins[i];
294 corners[1][i] = emaxs[i];
295 }
296 else
297 {
298 corners[1][i] = emins[i];
299 corners[0][i] = emaxs[i];
300 }
301 }
302 dist = DotProduct (plane->normal, corners[0]) - plane->dist;
303 dist2 = DotProduct (plane->normal, corners[1]) - plane->dist;
304 sides = 0;
305 if (dist1 >= 0)
306 sides = 1;
307 if (dist2 < 0)
308 sides |= 2;
309
310 #endif
311
312 sides = 0;
313 if (dist1 >= p->dist)
314 sides = 1;
315 if (dist2 < p->dist)
316 sides |= 2;
317
318 #ifdef PARANOID
319 if (sides == 0)
320 Sys_Error ("BoxOnPlaneSide: sides==0");
321 #endif
322
323 return sides;
324 }
325
326 #endif
327
328
AngleVectors(vec3_t angles,vec3_t forward,vec3_t right,vec3_t up)329 void AngleVectors (vec3_t angles, vec3_t forward, vec3_t right, vec3_t up)
330 {
331 float angle;
332 float sr, sp, sy, cr, cp, cy;
333
334 angle = angles[YAW] * (M_PI*2 / 360);
335 sy = sin(angle);
336 cy = cos(angle);
337 angle = angles[PITCH] * (M_PI*2 / 360);
338 sp = sin(angle);
339 cp = cos(angle);
340 angle = angles[ROLL] * (M_PI*2 / 360);
341 sr = sin(angle);
342 cr = cos(angle);
343
344 forward[0] = cp*cy;
345 forward[1] = cp*sy;
346 forward[2] = -sp;
347 right[0] = (-1*sr*sp*cy+-1*cr*-sy);
348 right[1] = (-1*sr*sp*sy+-1*cr*cy);
349 right[2] = -1*sr*cp;
350 up[0] = (cr*sp*cy+-sr*-sy);
351 up[1] = (cr*sp*sy+-sr*cy);
352 up[2] = cr*cp;
353 }
354
VectorCompare(vec3_t v1,vec3_t v2)355 int VectorCompare (vec3_t v1, vec3_t v2)
356 {
357 int i;
358
359 for (i=0 ; i<3 ; i++)
360 if (v1[i] != v2[i])
361 return 0;
362
363 return 1;
364 }
365
VectorMA(vec3_t veca,float scale,vec3_t vecb,vec3_t vecc)366 void VectorMA (vec3_t veca, float scale, vec3_t vecb, vec3_t vecc)
367 {
368 vecc[0] = veca[0] + scale*vecb[0];
369 vecc[1] = veca[1] + scale*vecb[1];
370 vecc[2] = veca[2] + scale*vecb[2];
371 }
372
373
_DotProduct(vec3_t v1,vec3_t v2)374 vec_t _DotProduct (vec3_t v1, vec3_t v2)
375 {
376 return v1[0]*v2[0] + v1[1]*v2[1] + v1[2]*v2[2];
377 }
378
_VectorSubtract(vec3_t veca,vec3_t vecb,vec3_t out)379 void _VectorSubtract (vec3_t veca, vec3_t vecb, vec3_t out)
380 {
381 out[0] = veca[0]-vecb[0];
382 out[1] = veca[1]-vecb[1];
383 out[2] = veca[2]-vecb[2];
384 }
385
_VectorAdd(vec3_t veca,vec3_t vecb,vec3_t out)386 void _VectorAdd (vec3_t veca, vec3_t vecb, vec3_t out)
387 {
388 out[0] = veca[0]+vecb[0];
389 out[1] = veca[1]+vecb[1];
390 out[2] = veca[2]+vecb[2];
391 }
392
_VectorCopy(vec3_t in,vec3_t out)393 void _VectorCopy (vec3_t in, vec3_t out)
394 {
395 out[0] = in[0];
396 out[1] = in[1];
397 out[2] = in[2];
398 }
399
CrossProduct(vec3_t v1,vec3_t v2,vec3_t cross)400 void CrossProduct (vec3_t v1, vec3_t v2, vec3_t cross)
401 {
402 cross[0] = v1[1]*v2[2] - v1[2]*v2[1];
403 cross[1] = v1[2]*v2[0] - v1[0]*v2[2];
404 cross[2] = v1[0]*v2[1] - v1[1]*v2[0];
405 }
406
407 double sqrt(double x);
408
Length(vec3_t v)409 vec_t Length(vec3_t v)
410 {
411 int i;
412 float length;
413
414 length = 0;
415 for (i=0 ; i< 3 ; i++)
416 length += v[i]*v[i];
417 length = sqrt (length); // FIXME
418
419 return length;
420 }
421
VectorNormalize(vec3_t v)422 float VectorNormalize (vec3_t v)
423 {
424 float length, ilength;
425
426 length = v[0]*v[0] + v[1]*v[1] + v[2]*v[2];
427 length = sqrt (length); // FIXME
428
429 if (length)
430 {
431 ilength = 1/length;
432 v[0] *= ilength;
433 v[1] *= ilength;
434 v[2] *= ilength;
435 }
436
437 return length;
438
439 }
440
VectorInverse(vec3_t v)441 void VectorInverse (vec3_t v)
442 {
443 v[0] = -v[0];
444 v[1] = -v[1];
445 v[2] = -v[2];
446 }
447
VectorScale(vec3_t in,vec_t scale,vec3_t out)448 void VectorScale (vec3_t in, vec_t scale, vec3_t out)
449 {
450 out[0] = in[0]*scale;
451 out[1] = in[1]*scale;
452 out[2] = in[2]*scale;
453 }
454
455
Q_log2(int val)456 int Q_log2(int val)
457 {
458 int answer=0;
459 while (val>>=1)
460 answer++;
461 return answer;
462 }
463
464
465 /*
466 ================
467 R_ConcatRotations
468 ================
469 */
R_ConcatRotations(float in1[3][3],float in2[3][3],float out[3][3])470 void R_ConcatRotations (float in1[3][3], float in2[3][3], float out[3][3])
471 {
472 out[0][0] = in1[0][0] * in2[0][0] + in1[0][1] * in2[1][0] +
473 in1[0][2] * in2[2][0];
474 out[0][1] = in1[0][0] * in2[0][1] + in1[0][1] * in2[1][1] +
475 in1[0][2] * in2[2][1];
476 out[0][2] = in1[0][0] * in2[0][2] + in1[0][1] * in2[1][2] +
477 in1[0][2] * in2[2][2];
478 out[1][0] = in1[1][0] * in2[0][0] + in1[1][1] * in2[1][0] +
479 in1[1][2] * in2[2][0];
480 out[1][1] = in1[1][0] * in2[0][1] + in1[1][1] * in2[1][1] +
481 in1[1][2] * in2[2][1];
482 out[1][2] = in1[1][0] * in2[0][2] + in1[1][1] * in2[1][2] +
483 in1[1][2] * in2[2][2];
484 out[2][0] = in1[2][0] * in2[0][0] + in1[2][1] * in2[1][0] +
485 in1[2][2] * in2[2][0];
486 out[2][1] = in1[2][0] * in2[0][1] + in1[2][1] * in2[1][1] +
487 in1[2][2] * in2[2][1];
488 out[2][2] = in1[2][0] * in2[0][2] + in1[2][1] * in2[1][2] +
489 in1[2][2] * in2[2][2];
490 }
491
492
493 /*
494 ================
495 R_ConcatTransforms
496 ================
497 */
R_ConcatTransforms(float in1[3][4],float in2[3][4],float out[3][4])498 void R_ConcatTransforms (float in1[3][4], float in2[3][4], float out[3][4])
499 {
500 out[0][0] = in1[0][0] * in2[0][0] + in1[0][1] * in2[1][0] +
501 in1[0][2] * in2[2][0];
502 out[0][1] = in1[0][0] * in2[0][1] + in1[0][1] * in2[1][1] +
503 in1[0][2] * in2[2][1];
504 out[0][2] = in1[0][0] * in2[0][2] + in1[0][1] * in2[1][2] +
505 in1[0][2] * in2[2][2];
506 out[0][3] = in1[0][0] * in2[0][3] + in1[0][1] * in2[1][3] +
507 in1[0][2] * in2[2][3] + in1[0][3];
508 out[1][0] = in1[1][0] * in2[0][0] + in1[1][1] * in2[1][0] +
509 in1[1][2] * in2[2][0];
510 out[1][1] = in1[1][0] * in2[0][1] + in1[1][1] * in2[1][1] +
511 in1[1][2] * in2[2][1];
512 out[1][2] = in1[1][0] * in2[0][2] + in1[1][1] * in2[1][2] +
513 in1[1][2] * in2[2][2];
514 out[1][3] = in1[1][0] * in2[0][3] + in1[1][1] * in2[1][3] +
515 in1[1][2] * in2[2][3] + in1[1][3];
516 out[2][0] = in1[2][0] * in2[0][0] + in1[2][1] * in2[1][0] +
517 in1[2][2] * in2[2][0];
518 out[2][1] = in1[2][0] * in2[0][1] + in1[2][1] * in2[1][1] +
519 in1[2][2] * in2[2][1];
520 out[2][2] = in1[2][0] * in2[0][2] + in1[2][1] * in2[1][2] +
521 in1[2][2] * in2[2][2];
522 out[2][3] = in1[2][0] * in2[0][3] + in1[2][1] * in2[1][3] +
523 in1[2][2] * in2[2][3] + in1[2][3];
524 }
525
526
527 /*
528 ===================
529 FloorDivMod
530
531 Returns mathematically correct (floor-based) quotient and remainder for
532 numer and denom, both of which should contain no fractional part. The
533 quotient must fit in 32 bits.
534 ====================
535 */
536
FloorDivMod(double numer,double denom,int * quotient,int * rem)537 void FloorDivMod (double numer, double denom, int *quotient,
538 int *rem)
539 {
540 int q, r;
541 double x;
542
543 #ifndef PARANOID
544 if (denom <= 0.0)
545 Sys_Error ("FloorDivMod: bad denominator %d\n", denom);
546
547 // if ((floor(numer) != numer) || (floor(denom) != denom))
548 // Sys_Error ("FloorDivMod: non-integer numer or denom %f %f\n",
549 // numer, denom);
550 #endif
551
552 if (numer >= 0.0)
553 {
554
555 x = floor(numer / denom);
556 q = (int)x;
557 r = (int)floor(numer - (x * denom));
558 }
559 else
560 {
561 //
562 // perform operations with positive values, and fix mod to make floor-based
563 //
564 x = floor(-numer / denom);
565 q = -(int)x;
566 r = (int)floor(-numer - (x * denom));
567 if (r != 0)
568 {
569 q--;
570 r = (int)denom - r;
571 }
572 }
573
574 *quotient = q;
575 *rem = r;
576 }
577
578
579 /*
580 ===================
581 GreatestCommonDivisor
582 ====================
583 */
GreatestCommonDivisor(int i1,int i2)584 int GreatestCommonDivisor (int i1, int i2)
585 {
586 if (i1 > i2)
587 {
588 if (i2 == 0)
589 return (i1);
590 return GreatestCommonDivisor (i2, i1 % i2);
591 }
592 else
593 {
594 if (i1 == 0)
595 return (i2);
596 return GreatestCommonDivisor (i1, i2 % i1);
597 }
598 }
599
600
601 #if !id386
602
603 // TODO: move to nonintel.c
604
605 /*
606 ===================
607 Invert24To16
608
609 Inverts an 8.24 value to a 16.16 value
610 ====================
611 */
612
Invert24To16(fixed16_t val)613 fixed16_t Invert24To16(fixed16_t val)
614 {
615 if (val < 256)
616 return (0xFFFFFFFF);
617
618 return (fixed16_t)
619 (((double)0x10000 * (double)0x1000000 / (double)val) + 0.5);
620 }
621
622 #endif
623
Mat_Mul_1x4_4x4(matrix_1x4 a,matrix_4x4 b,matrix_1x4 result)624 void Mat_Mul_1x4_4x4(matrix_1x4 a,
625 matrix_4x4 b,
626 matrix_1x4 result)
627 {
628 // this function multiplies a 1x4 by a 4x4 and stores the result in a 1x4
629
630 int index_j, // column index
631 index_k; // row index
632
633 float sum; // temp used to hold sum of products
634
635 // loop thru columns of b
636
637 for (index_j=0; index_j<4; index_j++)
638 {
639
640 // multiply ith row of a by jth column of b and store the sum
641 // of products in the position i,j of result
642
643 sum=0;
644
645 for (index_k=0; index_k<4; index_k++)
646 sum+=a[index_k]*b[index_k][index_j];
647
648 // store result
649
650 result[index_j] = sum;
651
652 } // end for index_j
653
654 } // end Mat_Mul_1x4_4x4
655
656 /*
657 PENTA: Easy & fast matrix inversions with some quirks (just what Carmack likes ;) )
658 Thnx to http://www.cs.unc.edu/~gotz/code/affinverse.html
659 Find the inverse of a matrix that is made up of only scales, rotations,
660 and translations.
661 */
MatrixAffineInverse(matrix_4x4 m,matrix_4x4 result)662 void MatrixAffineInverse( matrix_4x4 m, matrix_4x4 result )
663 {
664 float Tx, Ty, Tz;
665
666 // The rotational part of the matrix is simply the transpose of the
667 // original matrix.
668 result[0][0] = m[0][0];
669 result[1][0] = m[0][1];
670 result[2][0] = m[0][2];
671
672 result[0][1] = m[1][0];
673 result[1][1] = m[1][1];
674 result[2][1] = m[1][2];
675
676 result[0][2] = m[2][0];
677 result[1][2] = m[2][1];
678 result[2][2] = m[2][2];
679
680 // The right column vector of the matrix should always be [ 0 0 0 1 ]
681 // In most cases. . . you don't need this column at all because it'll
682 // never be used in the program, but since this code is used with GL
683 // and it does consider this column, it is here.
684 result[0][3] = result[1][3] = result[2][3] = 0;
685 result[3][3] = 1;
686
687 // The translation components of the original matrix.
688 Tx = m[3][0];
689 Ty = m[3][1];
690 Tz = m[3][2];
691
692 // Rresult = -(Tm * Rm) to get the translation part of the inverse
693 result[3][0] = -( m[0][0] * Tx + m[0][1] * Ty + m[0][2] * Tz );
694 result[3][1] = -( m[1][0] * Tx + m[1][1] * Ty + m[1][2] * Tz );
695 result[3][2] = -( m[2][0] * Tx + m[2][1] * Ty + m[2][2] * Tz );
696 }
697