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
32 #define DEG2RAD( a ) ( a * M_PI ) / 180.0F
33
ProjectPointOnPlane(vec3_t dst,const vec3_t p,const vec3_t normal)34 void ProjectPointOnPlane( vec3_t dst, const vec3_t p, const vec3_t normal )
35 {
36 float d;
37 vec3_t n;
38 float inv_denom;
39
40 inv_denom = 1.0F / DotProduct( normal, normal );
41
42 d = DotProduct( normal, p ) * inv_denom;
43
44 n[0] = normal[0] * inv_denom;
45 n[1] = normal[1] * inv_denom;
46 n[2] = normal[2] * inv_denom;
47
48 dst[0] = p[0] - d * n[0];
49 dst[1] = p[1] - d * n[1];
50 dst[2] = p[2] - d * n[2];
51 }
52
53 /*
54 ** assumes "src" is normalized
55 */
PerpendicularVector(vec3_t dst,const vec3_t src)56 void PerpendicularVector( vec3_t dst, const vec3_t src )
57 {
58 int pos;
59 int i;
60 float minelem = 1.0F;
61 vec3_t tempvec;
62
63 /*
64 ** find the smallest magnitude axially aligned vector
65 */
66 for ( pos = 0, i = 0; i < 3; i++ )
67 {
68 if ( fabs( src[i] ) < minelem )
69 {
70 pos = i;
71 minelem = fabs( src[i] );
72 }
73 }
74 tempvec[0] = tempvec[1] = tempvec[2] = 0.0F;
75 tempvec[pos] = 1.0F;
76
77 /*
78 ** project the point onto the plane defined by src
79 */
80 ProjectPointOnPlane( dst, tempvec, src );
81
82 /*
83 ** normalize the result
84 */
85 VectorNormalize( dst );
86 }
87
88 #ifdef _WIN32
89 #pragma optimize( "", off )
90 #endif
91
92
RotatePointAroundVector(vec3_t dst,const vec3_t dir,const vec3_t point,float degrees)93 void RotatePointAroundVector( vec3_t dst, const vec3_t dir, const vec3_t point, float degrees )
94 {
95 float m[3][3];
96 float im[3][3];
97 float zrot[3][3];
98 float tmpmat[3][3];
99 float rot[3][3];
100 int i;
101 vec3_t vr, vup, vf;
102
103 vf[0] = dir[0];
104 vf[1] = dir[1];
105 vf[2] = dir[2];
106
107 PerpendicularVector( vr, dir );
108 CrossProduct( vr, vf, vup );
109
110 m[0][0] = vr[0];
111 m[1][0] = vr[1];
112 m[2][0] = vr[2];
113
114 m[0][1] = vup[0];
115 m[1][1] = vup[1];
116 m[2][1] = vup[2];
117
118 m[0][2] = vf[0];
119 m[1][2] = vf[1];
120 m[2][2] = vf[2];
121
122 memcpy( im, m, sizeof( im ) );
123
124 im[0][1] = m[1][0];
125 im[0][2] = m[2][0];
126 im[1][0] = m[0][1];
127 im[1][2] = m[2][1];
128 im[2][0] = m[0][2];
129 im[2][1] = m[1][2];
130
131 memset( zrot, 0, sizeof( zrot ) );
132 zrot[0][0] = zrot[1][1] = zrot[2][2] = 1.0F;
133
134 zrot[0][0] = cos( DEG2RAD( degrees ) );
135 zrot[0][1] = sin( DEG2RAD( degrees ) );
136 zrot[1][0] = -sin( DEG2RAD( degrees ) );
137 zrot[1][1] = cos( DEG2RAD( degrees ) );
138
139 R_ConcatRotations( m, zrot, tmpmat );
140 R_ConcatRotations( tmpmat, im, rot );
141
142 for ( i = 0; i < 3; i++ )
143 {
144 dst[i] = rot[i][0] * point[0] + rot[i][1] * point[1] + rot[i][2] * point[2];
145 }
146 }
147
148 #ifdef _WIN32
149 #pragma optimize( "", on )
150 #endif
151
152 /*-----------------------------------------------------------------*/
153
anglemod(float a)154 float anglemod(float a)
155 {
156 #if 0
157 if (a >= 0)
158 a -= 360*(int)(a/360);
159 else
160 a += 360*( 1 + (int)(-a/360) );
161 #endif
162 a = (360.0/65536) * ((int)(a*(65536/360.0)) & 65535);
163 return a;
164 }
165
166 /*
167 ==================
168 BOPS_Error
169
170 Split out like this for ASM to call.
171 ==================
172 */
BOPS_Error(void)173 void BOPS_Error (void)
174 {
175 Sys_Error ("BoxOnPlaneSide: Bad signbits");
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
VectorCompare(vec3_t v1,vec3_t v2)316 int VectorCompare (vec3_t v1, vec3_t v2)
317 {
318 int i;
319
320 for (i=0 ; i<3 ; i++)
321 if (v1[i] != v2[i])
322 return 0;
323
324 return 1;
325 }
326
VectorMA(vec3_t veca,float scale,vec3_t vecb,vec3_t vecc)327 void VectorMA (vec3_t veca, float scale, vec3_t vecb, vec3_t vecc)
328 {
329 vecc[0] = veca[0] + scale*vecb[0];
330 vecc[1] = veca[1] + scale*vecb[1];
331 vecc[2] = veca[2] + scale*vecb[2];
332 }
333
334
_DotProduct(vec3_t v1,vec3_t v2)335 vec_t _DotProduct (vec3_t v1, vec3_t v2)
336 {
337 return v1[0]*v2[0] + v1[1]*v2[1] + v1[2]*v2[2];
338 }
339
_VectorSubtract(vec3_t veca,vec3_t vecb,vec3_t out)340 void _VectorSubtract (vec3_t veca, vec3_t vecb, vec3_t out)
341 {
342 out[0] = veca[0]-vecb[0];
343 out[1] = veca[1]-vecb[1];
344 out[2] = veca[2]-vecb[2];
345 }
346
_VectorAdd(vec3_t veca,vec3_t vecb,vec3_t out)347 void _VectorAdd (vec3_t veca, vec3_t vecb, vec3_t out)
348 {
349 out[0] = veca[0]+vecb[0];
350 out[1] = veca[1]+vecb[1];
351 out[2] = veca[2]+vecb[2];
352 }
353
_VectorCopy(vec3_t in,vec3_t out)354 void _VectorCopy (vec3_t in, vec3_t out)
355 {
356 out[0] = in[0];
357 out[1] = in[1];
358 out[2] = in[2];
359 }
360
CrossProduct(vec3_t v1,vec3_t v2,vec3_t cross)361 void CrossProduct (vec3_t v1, vec3_t v2, vec3_t cross)
362 {
363 cross[0] = v1[1]*v2[2] - v1[2]*v2[1];
364 cross[1] = v1[2]*v2[0] - v1[0]*v2[2];
365 cross[2] = v1[0]*v2[1] - v1[1]*v2[0];
366 }
367
368 double sqrt(double x);
369
Length(vec3_t v)370 vec_t Length(vec3_t v)
371 {
372 int i;
373 float length;
374
375 length = 0;
376 for (i=0 ; i< 3 ; i++)
377 length += v[i]*v[i];
378 length = sqrt (length); // FIXME
379
380 return length;
381 }
382
VectorNormalize(vec3_t v)383 float VectorNormalize (vec3_t v)
384 {
385 float length, ilength;
386
387 length = v[0]*v[0] + v[1]*v[1] + v[2]*v[2];
388 length = sqrt (length); // FIXME
389
390 if (length)
391 {
392 ilength = 1/length;
393 v[0] *= ilength;
394 v[1] *= ilength;
395 v[2] *= ilength;
396 }
397
398 return length;
399
400 }
401
VectorInverse(vec3_t v)402 void VectorInverse (vec3_t v)
403 {
404 v[0] = -v[0];
405 v[1] = -v[1];
406 v[2] = -v[2];
407 }
408
VectorScale(vec3_t in,vec_t scale,vec3_t out)409 void VectorScale (vec3_t in, vec_t scale, vec3_t out)
410 {
411 out[0] = in[0]*scale;
412 out[1] = in[1]*scale;
413 out[2] = in[2]*scale;
414 }
415
416
Q_log2(int val)417 int Q_log2(int val)
418 {
419 int answer=0;
420 while ((val>>=1) != 0)
421 answer++;
422 return answer;
423 }
424
425
426 /*
427 ================
428 R_ConcatRotations
429 ================
430 */
R_ConcatRotations(float in1[3][3],float in2[3][3],float out[3][3])431 void R_ConcatRotations (float in1[3][3], float in2[3][3], float out[3][3])
432 {
433 out[0][0] = in1[0][0] * in2[0][0] + in1[0][1] * in2[1][0] +
434 in1[0][2] * in2[2][0];
435 out[0][1] = in1[0][0] * in2[0][1] + in1[0][1] * in2[1][1] +
436 in1[0][2] * in2[2][1];
437 out[0][2] = in1[0][0] * in2[0][2] + in1[0][1] * in2[1][2] +
438 in1[0][2] * in2[2][2];
439 out[1][0] = in1[1][0] * in2[0][0] + in1[1][1] * in2[1][0] +
440 in1[1][2] * in2[2][0];
441 out[1][1] = in1[1][0] * in2[0][1] + in1[1][1] * in2[1][1] +
442 in1[1][2] * in2[2][1];
443 out[1][2] = in1[1][0] * in2[0][2] + in1[1][1] * in2[1][2] +
444 in1[1][2] * in2[2][2];
445 out[2][0] = in1[2][0] * in2[0][0] + in1[2][1] * in2[1][0] +
446 in1[2][2] * in2[2][0];
447 out[2][1] = in1[2][0] * in2[0][1] + in1[2][1] * in2[1][1] +
448 in1[2][2] * in2[2][1];
449 out[2][2] = in1[2][0] * in2[0][2] + in1[2][1] * in2[1][2] +
450 in1[2][2] * in2[2][2];
451 }
452
453
454 /*
455 ================
456 R_ConcatTransforms
457 ================
458 */
R_ConcatTransforms(float in1[3][4],float in2[3][4],float out[3][4])459 void R_ConcatTransforms (float in1[3][4], float in2[3][4], float out[3][4])
460 {
461 out[0][0] = in1[0][0] * in2[0][0] + in1[0][1] * in2[1][0] +
462 in1[0][2] * in2[2][0];
463 out[0][1] = in1[0][0] * in2[0][1] + in1[0][1] * in2[1][1] +
464 in1[0][2] * in2[2][1];
465 out[0][2] = in1[0][0] * in2[0][2] + in1[0][1] * in2[1][2] +
466 in1[0][2] * in2[2][2];
467 out[0][3] = in1[0][0] * in2[0][3] + in1[0][1] * in2[1][3] +
468 in1[0][2] * in2[2][3] + in1[0][3];
469 out[1][0] = in1[1][0] * in2[0][0] + in1[1][1] * in2[1][0] +
470 in1[1][2] * in2[2][0];
471 out[1][1] = in1[1][0] * in2[0][1] + in1[1][1] * in2[1][1] +
472 in1[1][2] * in2[2][1];
473 out[1][2] = in1[1][0] * in2[0][2] + in1[1][1] * in2[1][2] +
474 in1[1][2] * in2[2][2];
475 out[1][3] = in1[1][0] * in2[0][3] + in1[1][1] * in2[1][3] +
476 in1[1][2] * in2[2][3] + in1[1][3];
477 out[2][0] = in1[2][0] * in2[0][0] + in1[2][1] * in2[1][0] +
478 in1[2][2] * in2[2][0];
479 out[2][1] = in1[2][0] * in2[0][1] + in1[2][1] * in2[1][1] +
480 in1[2][2] * in2[2][1];
481 out[2][2] = in1[2][0] * in2[0][2] + in1[2][1] * in2[1][2] +
482 in1[2][2] * in2[2][2];
483 out[2][3] = in1[2][0] * in2[0][3] + in1[2][1] * in2[1][3] +
484 in1[2][2] * in2[2][3] + in1[2][3];
485 }
486
487
488 /*
489 ===================
490 FloorDivMod
491
492 Returns mathematically correct (floor-based) quotient and remainder for
493 numer and denom, both of which should contain no fractional part. The
494 quotient must fit in 32 bits.
495 ====================
496 */
497
FloorDivMod(double numer,double denom,int * quotient,int * rem)498 void FloorDivMod (double numer, double denom, int *quotient,
499 int *rem)
500 {
501 int q, r;
502 double x;
503
504 #ifndef PARANOID
505 if (denom <= 0.0)
506 Sys_Error ("FloorDivMod: bad denominator %d\n", denom);
507
508 // if ((floor(numer) != numer) || (floor(denom) != denom))
509 // Sys_Error ("FloorDivMod: non-integer numer or denom %f %f\n",
510 // numer, denom);
511 #endif
512
513 if (numer >= 0.0)
514 {
515
516 x = floor(numer / denom);
517 q = (int)x;
518 r = (int)floor(numer - (x * denom));
519 }
520 else
521 {
522 //
523 // perform operations with positive values, and fix mod to make floor-based
524 //
525 x = floor(-numer / denom);
526 q = -(int)x;
527 r = (int)floor(-numer - (x * denom));
528 if (r != 0)
529 {
530 q--;
531 r = (int)denom - r;
532 }
533 }
534
535 *quotient = q;
536 *rem = r;
537 }
538
539
540 /*
541 ===================
542 GreatestCommonDivisor
543 ====================
544 */
GreatestCommonDivisor(int i1,int i2)545 int GreatestCommonDivisor (int i1, int i2)
546 {
547 if (i1 > i2)
548 {
549 if (i2 == 0)
550 return (i1);
551 return GreatestCommonDivisor (i2, i1 % i2);
552 }
553 else
554 {
555 if (i1 == 0)
556 return (i2);
557 return GreatestCommonDivisor (i1, i2 % i1);
558 }
559 }
560
561
562 #if !id386
563
564 // TODO: move to nonintel.c
565
566 /*
567 ===================
568 Invert24To16
569
570 Inverts an 8.24 value to a 16.16 value
571 ====================
572 */
573
Invert24To16(fixed16_t val)574 fixed16_t Invert24To16(fixed16_t val)
575 {
576 if (val < 256)
577 return (0xFFFFFFFF);
578
579 return (fixed16_t)
580 (((double)0x10000 * (double)0x1000000 / (double)val) + 0.5);
581 }
582
583 #endif
584