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 */
PerpendicularVector(vec3_t dst,const vec3_t src)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 /*
62 ** find the smallest magnitude axially aligned vector
63 */
64 for ( pos = 0, i = 0; i < 3; i++ )
65 {
66 if ( fabs( src[i] ) < minelem )
67 {
68 pos = i;
69 minelem = fabs( src[i] );
70 }
71 }
72 tempvec[0] = tempvec[1] = tempvec[2] = 0.0F;
73 tempvec[pos] = 1.0F;
74
75 /*
76 ** project the point onto the plane defined by src
77 */
78 ProjectPointOnPlane( dst, tempvec, src );
79
80 /*
81 ** normalize the result
82 */
83 VectorNormalize( dst );
84 }
85
86 #ifdef _WIN32
87 #pragma optimize( "", off )
88 #endif
89
90
RotatePointAroundVector(vec3_t dst,const vec3_t dir,const vec3_t point,float degrees)91 void RotatePointAroundVector( vec3_t dst, const vec3_t dir, const vec3_t point, float degrees )
92 {
93 float m[3][3];
94 float im[3][3];
95 float zrot[3][3];
96 float tmpmat[3][3];
97 float rot[3][3];
98 int i;
99 vec3_t vr, vup, vf;
100
101 vf[0] = dir[0];
102 vf[1] = dir[1];
103 vf[2] = dir[2];
104
105 PerpendicularVector( vr, dir );
106 CrossProduct( vr, vf, vup );
107
108 m[0][0] = vr[0];
109 m[1][0] = vr[1];
110 m[2][0] = vr[2];
111
112 m[0][1] = vup[0];
113 m[1][1] = vup[1];
114 m[2][1] = vup[2];
115
116 m[0][2] = vf[0];
117 m[1][2] = vf[1];
118 m[2][2] = vf[2];
119
120 memcpy( im, m, sizeof( im ) );
121
122 im[0][1] = m[1][0];
123 im[0][2] = m[2][0];
124 im[1][0] = m[0][1];
125 im[1][2] = m[2][1];
126 im[2][0] = m[0][2];
127 im[2][1] = m[1][2];
128
129 memset( zrot, 0, sizeof( zrot ) );
130 zrot[0][0] = zrot[1][1] = zrot[2][2] = 1.0F;
131
132 zrot[0][0] = cos( DEG2RAD( degrees ) );
133 zrot[0][1] = sin( DEG2RAD( degrees ) );
134 zrot[1][0] = -sin( DEG2RAD( degrees ) );
135 zrot[1][1] = cos( DEG2RAD( degrees ) );
136
137 R_ConcatRotations( m, zrot, tmpmat );
138 R_ConcatRotations( tmpmat, im, rot );
139
140 for ( i = 0; i < 3; i++ )
141 {
142 dst[i] = rot[i][0] * point[0] + rot[i][1] * point[1] + rot[i][2] * point[2];
143 }
144 }
145
146 #ifdef _WIN32
147 #pragma optimize( "", on )
148 #endif
149
150 /*-----------------------------------------------------------------*/
151
152
anglemod(float a)153 float anglemod(float a)
154 {
155 #if 0
156 if (a >= 0)
157 a -= 360*(int)(a/360);
158 else
159 a += 360*( 1 + (int)(-a/360) );
160 #endif
161 a = (360.0/65536) * ((int)(a*(65536/360.0)) & 65535);
162 return a;
163 }
164
165 /*
166 ==================
167 BOPS_Error
168
169 Split out like this for ASM to call.
170 ==================
171 */
BOPS_Error(void)172 void BOPS_Error (void)
173 {
174 Sys_Error ("BoxOnPlaneSide: Bad signbits");
175 }
176
177
178 #if !id386
179
180 /*
181 ==================
182 BoxOnPlaneSide
183
184 Returns 1, 2, or 1 + 2
185 ==================
186 */
BoxOnPlaneSide(vec3_t emins,vec3_t emaxs,mplane_t * p)187 int BoxOnPlaneSide (vec3_t emins, vec3_t emaxs, mplane_t *p)
188 {
189 float dist1, dist2;
190 int sides;
191
192 #if 0 // this is done by the BOX_ON_PLANE_SIDE macro before calling this
193 // function
194 // fast axial cases
195 if (p->type < 3)
196 {
197 if (p->dist <= emins[p->type])
198 return 1;
199 if (p->dist >= emaxs[p->type])
200 return 2;
201 return 3;
202 }
203 #endif
204
205 // general case
206 switch (p->signbits)
207 {
208 case 0:
209 dist1 = p->normal[0]*emaxs[0] + p->normal[1]*emaxs[1] + p->normal[2]*emaxs[2];
210 dist2 = p->normal[0]*emins[0] + p->normal[1]*emins[1] + p->normal[2]*emins[2];
211 break;
212 case 1:
213 dist1 = p->normal[0]*emins[0] + p->normal[1]*emaxs[1] + p->normal[2]*emaxs[2];
214 dist2 = p->normal[0]*emaxs[0] + p->normal[1]*emins[1] + p->normal[2]*emins[2];
215 break;
216 case 2:
217 dist1 = p->normal[0]*emaxs[0] + p->normal[1]*emins[1] + p->normal[2]*emaxs[2];
218 dist2 = p->normal[0]*emins[0] + p->normal[1]*emaxs[1] + p->normal[2]*emins[2];
219 break;
220 case 3:
221 dist1 = p->normal[0]*emins[0] + p->normal[1]*emins[1] + p->normal[2]*emaxs[2];
222 dist2 = p->normal[0]*emaxs[0] + p->normal[1]*emaxs[1] + p->normal[2]*emins[2];
223 break;
224 case 4:
225 dist1 = p->normal[0]*emaxs[0] + p->normal[1]*emaxs[1] + p->normal[2]*emins[2];
226 dist2 = p->normal[0]*emins[0] + p->normal[1]*emins[1] + p->normal[2]*emaxs[2];
227 break;
228 case 5:
229 dist1 = p->normal[0]*emins[0] + p->normal[1]*emaxs[1] + p->normal[2]*emins[2];
230 dist2 = p->normal[0]*emaxs[0] + p->normal[1]*emins[1] + p->normal[2]*emaxs[2];
231 break;
232 case 6:
233 dist1 = p->normal[0]*emaxs[0] + p->normal[1]*emins[1] + p->normal[2]*emins[2];
234 dist2 = p->normal[0]*emins[0] + p->normal[1]*emaxs[1] + p->normal[2]*emaxs[2];
235 break;
236 case 7:
237 dist1 = p->normal[0]*emins[0] + p->normal[1]*emins[1] + p->normal[2]*emins[2];
238 dist2 = p->normal[0]*emaxs[0] + p->normal[1]*emaxs[1] + p->normal[2]*emaxs[2];
239 break;
240 default:
241 dist1 = dist2 = 0; // shut up compiler
242 BOPS_Error ();
243 break;
244 }
245
246 #if 0
247 int i;
248 vec3_t corners[2];
249
250 for (i=0 ; i<3 ; i++)
251 {
252 if (plane->normal[i] < 0)
253 {
254 corners[0][i] = emins[i];
255 corners[1][i] = emaxs[i];
256 }
257 else
258 {
259 corners[1][i] = emins[i];
260 corners[0][i] = emaxs[i];
261 }
262 }
263 dist = DotProduct (plane->normal, corners[0]) - plane->dist;
264 dist2 = DotProduct (plane->normal, corners[1]) - plane->dist;
265 sides = 0;
266 if (dist1 >= 0)
267 sides = 1;
268 if (dist2 < 0)
269 sides |= 2;
270
271 #endif
272
273 sides = 0;
274 if (dist1 >= p->dist)
275 sides = 1;
276 if (dist2 < p->dist)
277 sides |= 2;
278
279 #ifdef PARANOID
280 if (sides == 0)
281 Sys_Error ("BoxOnPlaneSide: sides==0");
282 #endif
283
284 return sides;
285 }
286
287 #endif
288
289
AngleVectors(vec3_t angles,vec3_t forward,vec3_t right,vec3_t up)290 void AngleVectors (vec3_t angles, vec3_t forward, vec3_t right, vec3_t up)
291 {
292 float angle;
293 float sr, sp, sy, cr, cp, cy;
294
295 angle = angles[YAW] * (M_PI*2 / 360);
296 sy = sin(angle);
297 cy = cos(angle);
298 angle = angles[PITCH] * (M_PI*2 / 360);
299 sp = sin(angle);
300 cp = cos(angle);
301 angle = angles[ROLL] * (M_PI*2 / 360);
302 sr = sin(angle);
303 cr = cos(angle);
304
305 forward[0] = cp*cy;
306 forward[1] = cp*sy;
307 forward[2] = -sp;
308 right[0] = (-1*sr*sp*cy+-1*cr*-sy);
309 right[1] = (-1*sr*sp*sy+-1*cr*cy);
310 right[2] = -1*sr*cp;
311 up[0] = (cr*sp*cy+-sr*-sy);
312 up[1] = (cr*sp*sy+-sr*cy);
313 up[2] = cr*cp;
314 }
315
316 // tQER<1>: START
317 // Much faster AngleVectors using lookup tables.
318
319 float tQER_sin[360];
320 float tQER_cos[360];
tQER_AngleVectors(vec3_t angles,vec3_t forward,vec3_t right,vec3_t up)321 void tQER_AngleVectors (vec3_t angles, vec3_t forward, vec3_t right, vec3_t up)
322 {
323 static unsigned char init = 0;
324 float angle;
325 float sr, sp, sy, cr, cp, cy;
326
327 // Initialize the look up table: Very coarse, but Quake doesn't care.
328 if (!init)
329 {
330 for (angle = 0; angle < 360; angle++)
331 {
332 tQER_cos[(int)angle] = cos(angle * (M_PI*2 / 360));
333 tQER_sin[(int)angle] = sin(angle * (M_PI*2 / 360));
334 }
335 init = 1;
336 }
337
338 if (angles[YAW] < 0) angle = angles[YAW] + 360;
339 else angle = angles[YAW];
340 sy = tQER_sin[(int)angle];
341 cy = tQER_cos[(int)angle];
342
343 if (angles[PITCH] < 0) angle = angles[PITCH] + 360;
344 else angle = angles[PITCH];
345 sp = tQER_sin[(int)angle];
346 cp = tQER_cos[(int)angle];
347
348 if (angles[ROLL] < 0) angle = angles[ROLL] + 360;
349 else angle = angles[ROLL];
350 sr = tQER_sin[(int)angle];
351 cr = tQER_cos[(int)angle];
352
353 forward[0] = cp * cy;
354 forward[1] = cp * sy;
355 forward[2] = -sp;
356 right[0] = (-sr * sp * cy + cr * sy);
357 right[1] = (-sr * sp * sy - cr * cy);
358 right[2] = -sr * cp;
359 up[0] = (cr * sp * cy + sr * sy);
360 up[1] = (cr * sp * sy - sr * cy);
361 up[2] = cr * cp;
362 }
363 // tQER<1>: END
364
VectorCompare(vec3_t v1,vec3_t v2)365 int VectorCompare (vec3_t v1, vec3_t v2)
366 {
367 int i;
368
369 for (i=0 ; i<3 ; i++)
370 if (v1[i] != v2[i])
371 return 0;
372
373 return 1;
374 }
375
VectorMA(vec3_t veca,float scale,vec3_t vecb,vec3_t vecc)376 void VectorMA (vec3_t veca, float scale, vec3_t vecb, vec3_t vecc)
377 {
378 vecc[0] = veca[0] + scale*vecb[0];
379 vecc[1] = veca[1] + scale*vecb[1];
380 vecc[2] = veca[2] + scale*vecb[2];
381 }
382
383
_DotProduct(vec3_t v1,vec3_t v2)384 vec_t _DotProduct (vec3_t v1, vec3_t v2)
385 {
386 return v1[0]*v2[0] + v1[1]*v2[1] + v1[2]*v2[2];
387 }
388
_VectorSubtract(vec3_t veca,vec3_t vecb,vec3_t out)389 void _VectorSubtract (vec3_t veca, vec3_t vecb, vec3_t out)
390 {
391 out[0] = veca[0]-vecb[0];
392 out[1] = veca[1]-vecb[1];
393 out[2] = veca[2]-vecb[2];
394 }
395
_VectorAdd(vec3_t veca,vec3_t vecb,vec3_t out)396 void _VectorAdd (vec3_t veca, vec3_t vecb, vec3_t out)
397 {
398 out[0] = veca[0]+vecb[0];
399 out[1] = veca[1]+vecb[1];
400 out[2] = veca[2]+vecb[2];
401 }
402
_VectorCopy(vec3_t in,vec3_t out)403 void _VectorCopy (vec3_t in, vec3_t out)
404 {
405 out[0] = in[0];
406 out[1] = in[1];
407 out[2] = in[2];
408 }
409
CrossProduct(vec3_t v1,vec3_t v2,vec3_t cross)410 void CrossProduct (vec3_t v1, vec3_t v2, vec3_t cross)
411 {
412 cross[0] = v1[1]*v2[2] - v1[2]*v2[1];
413 cross[1] = v1[2]*v2[0] - v1[0]*v2[2];
414 cross[2] = v1[0]*v2[1] - v1[1]*v2[0];
415 }
416
417 double sqrt(double x);
418
Length(vec3_t v)419 vec_t Length(vec3_t v)
420 {
421 int i;
422 float length;
423
424 length = 0;
425 for (i=0 ; i< 3 ; i++)
426 length += v[i]*v[i];
427 length = sqrt (length); // FIXME
428
429 return length;
430 }
431
VectorNormalize(vec3_t v)432 float VectorNormalize (vec3_t v)
433 {
434 float length, ilength;
435
436 length = v[0]*v[0] + v[1]*v[1] + v[2]*v[2];
437 length = sqrt (length); // FIXME
438
439 if (length)
440 {
441 ilength = 1/length;
442 v[0] *= ilength;
443 v[1] *= ilength;
444 v[2] *= ilength;
445 }
446
447 return length;
448
449 }
450
VectorInverse(vec3_t v)451 void VectorInverse (vec3_t v)
452 {
453 v[0] = -v[0];
454 v[1] = -v[1];
455 v[2] = -v[2];
456 }
457
VectorScale(vec3_t in,vec_t scale,vec3_t out)458 void VectorScale (vec3_t in, vec_t scale, vec3_t out)
459 {
460 out[0] = in[0]*scale;
461 out[1] = in[1]*scale;
462 out[2] = in[2]*scale;
463 }
464
465
Q_log2(int val)466 int Q_log2(int val)
467 {
468 int answer=0;
469 while (val>>=1)
470 answer++;
471 return answer;
472 }
473
474
475 /*
476 ================
477 R_ConcatRotations
478 ================
479 */
R_ConcatRotations(float in1[3][3],float in2[3][3],float out[3][3])480 void R_ConcatRotations (float in1[3][3], float in2[3][3], float out[3][3])
481 {
482 out[0][0] = in1[0][0] * in2[0][0] + in1[0][1] * in2[1][0] +
483 in1[0][2] * in2[2][0];
484 out[0][1] = in1[0][0] * in2[0][1] + in1[0][1] * in2[1][1] +
485 in1[0][2] * in2[2][1];
486 out[0][2] = in1[0][0] * in2[0][2] + in1[0][1] * in2[1][2] +
487 in1[0][2] * in2[2][2];
488 out[1][0] = in1[1][0] * in2[0][0] + in1[1][1] * in2[1][0] +
489 in1[1][2] * in2[2][0];
490 out[1][1] = in1[1][0] * in2[0][1] + in1[1][1] * in2[1][1] +
491 in1[1][2] * in2[2][1];
492 out[1][2] = in1[1][0] * in2[0][2] + in1[1][1] * in2[1][2] +
493 in1[1][2] * in2[2][2];
494 out[2][0] = in1[2][0] * in2[0][0] + in1[2][1] * in2[1][0] +
495 in1[2][2] * in2[2][0];
496 out[2][1] = in1[2][0] * in2[0][1] + in1[2][1] * in2[1][1] +
497 in1[2][2] * in2[2][1];
498 out[2][2] = in1[2][0] * in2[0][2] + in1[2][1] * in2[1][2] +
499 in1[2][2] * in2[2][2];
500 }
501
502
503 /*
504 ================
505 R_ConcatTransforms
506 ================
507 */
R_ConcatTransforms(float in1[3][4],float in2[3][4],float out[3][4])508 void R_ConcatTransforms (float in1[3][4], float in2[3][4], float out[3][4])
509 {
510 out[0][0] = in1[0][0] * in2[0][0] + in1[0][1] * in2[1][0] +
511 in1[0][2] * in2[2][0];
512 out[0][1] = in1[0][0] * in2[0][1] + in1[0][1] * in2[1][1] +
513 in1[0][2] * in2[2][1];
514 out[0][2] = in1[0][0] * in2[0][2] + in1[0][1] * in2[1][2] +
515 in1[0][2] * in2[2][2];
516 out[0][3] = in1[0][0] * in2[0][3] + in1[0][1] * in2[1][3] +
517 in1[0][2] * in2[2][3] + in1[0][3];
518 out[1][0] = in1[1][0] * in2[0][0] + in1[1][1] * in2[1][0] +
519 in1[1][2] * in2[2][0];
520 out[1][1] = in1[1][0] * in2[0][1] + in1[1][1] * in2[1][1] +
521 in1[1][2] * in2[2][1];
522 out[1][2] = in1[1][0] * in2[0][2] + in1[1][1] * in2[1][2] +
523 in1[1][2] * in2[2][2];
524 out[1][3] = in1[1][0] * in2[0][3] + in1[1][1] * in2[1][3] +
525 in1[1][2] * in2[2][3] + in1[1][3];
526 out[2][0] = in1[2][0] * in2[0][0] + in1[2][1] * in2[1][0] +
527 in1[2][2] * in2[2][0];
528 out[2][1] = in1[2][0] * in2[0][1] + in1[2][1] * in2[1][1] +
529 in1[2][2] * in2[2][1];
530 out[2][2] = in1[2][0] * in2[0][2] + in1[2][1] * in2[1][2] +
531 in1[2][2] * in2[2][2];
532 out[2][3] = in1[2][0] * in2[0][3] + in1[2][1] * in2[1][3] +
533 in1[2][2] * in2[2][3] + in1[2][3];
534 }
535
536
537 /*
538 ===================
539 FloorDivMod
540
541 Returns mathematically correct (floor-based) quotient and remainder for
542 numer and denom, both of which should contain no fractional part. The
543 quotient must fit in 32 bits.
544 ====================
545 */
546
FloorDivMod(double numer,double denom,int * quotient,int * rem)547 void FloorDivMod (double numer, double denom, int *quotient,
548 int *rem)
549 {
550 int q, r;
551 double x;
552
553 #ifndef PARANOID
554 if (denom <= 0.0)
555 Sys_Error ("FloorDivMod: bad denominator %d\n", denom);
556
557 // if ((floor(numer) != numer) || (floor(denom) != denom))
558 // Sys_Error ("FloorDivMod: non-integer numer or denom %f %f\n",
559 // numer, denom);
560 #endif
561
562 if (numer >= 0.0)
563 {
564
565 x = floor(numer / denom);
566 q = (int)x;
567 r = (int)floor(numer - (x * denom));
568 }
569 else
570 {
571 //
572 // perform operations with positive values, and fix mod to make floor-based
573 //
574 x = floor(-numer / denom);
575 q = -(int)x;
576 r = (int)floor(-numer - (x * denom));
577 if (r != 0)
578 {
579 q--;
580 r = (int)denom - r;
581 }
582 }
583
584 *quotient = q;
585 *rem = r;
586 }
587
588
589 /*
590 ===================
591 GreatestCommonDivisor
592 ====================
593 */
GreatestCommonDivisor(int i1,int i2)594 int GreatestCommonDivisor (int i1, int i2)
595 {
596 if (i1 > i2)
597 {
598 if (i2 == 0)
599 return (i1);
600 return GreatestCommonDivisor (i2, i1 % i2);
601 }
602 else
603 {
604 if (i1 == 0)
605 return (i2);
606 return GreatestCommonDivisor (i1, i2 % i1);
607 }
608 }
609
610
611 #if !id386
612
613 // TODO: move to nonintel.c
614
615 /*
616 ===================
617 Invert24To16
618
619 Inverts an 8.24 value to a 16.16 value
620 ====================
621 */
622
Invert24To16(fixed16_t val)623 fixed16_t Invert24To16(fixed16_t val)
624 {
625 if (val < 256)
626 return (0xFFFFFFFF);
627
628 return (fixed16_t)
629 (((double)0x10000 * (double)0x1000000 / (double)val) + 0.5);
630 }
631
632 #endif
633