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
154
anglemod(float a)155 float anglemod(float a)
156 {
157 #if 0
158 if (a >= 0)
159 a -= 360*(int)(a/360);
160 else
161 a += 360*( 1 + (int)(-a/360) );
162 #endif
163 a = (360.0/65536) * ((int)(a*(65536/360.0)) & 65535);
164 return a;
165 }
166
167 /*
168 ==================
169 BOPS_Error
170
171 Split out like this for ASM to call.
172 ==================
173 */
BOPS_Error(void)174 void BOPS_Error (void)
175 {
176 Sys_Error ("BoxOnPlaneSide: Bad signbits");
177 }
178
179
180 #if !id386
181
182 /*
183 ==================
184 BoxOnPlaneSide
185
186 Returns 1, 2, or 1 + 2
187 ==================
188 */
BoxOnPlaneSide(vec3_t emins,vec3_t emaxs,mplane_t * p)189 int BoxOnPlaneSide (vec3_t emins, vec3_t emaxs, mplane_t *p)
190 {
191 float dist1, dist2;
192 int sides;
193
194 #if 0 // this is done by the BOX_ON_PLANE_SIDE macro before calling this
195 // function
196 // fast axial cases
197 if (p->type < 3)
198 {
199 if (p->dist <= emins[p->type])
200 return 1;
201 if (p->dist >= emaxs[p->type])
202 return 2;
203 return 3;
204 }
205 #endif
206
207 // general case
208 switch (p->signbits)
209 {
210 case 0:
211 dist1 = p->normal[0]*emaxs[0] + p->normal[1]*emaxs[1] + p->normal[2]*emaxs[2];
212 dist2 = p->normal[0]*emins[0] + p->normal[1]*emins[1] + p->normal[2]*emins[2];
213 break;
214 case 1:
215 dist1 = p->normal[0]*emins[0] + p->normal[1]*emaxs[1] + p->normal[2]*emaxs[2];
216 dist2 = p->normal[0]*emaxs[0] + p->normal[1]*emins[1] + p->normal[2]*emins[2];
217 break;
218 case 2:
219 dist1 = p->normal[0]*emaxs[0] + p->normal[1]*emins[1] + p->normal[2]*emaxs[2];
220 dist2 = p->normal[0]*emins[0] + p->normal[1]*emaxs[1] + p->normal[2]*emins[2];
221 break;
222 case 3:
223 dist1 = p->normal[0]*emins[0] + p->normal[1]*emins[1] + p->normal[2]*emaxs[2];
224 dist2 = p->normal[0]*emaxs[0] + p->normal[1]*emaxs[1] + p->normal[2]*emins[2];
225 break;
226 case 4:
227 dist1 = p->normal[0]*emaxs[0] + p->normal[1]*emaxs[1] + p->normal[2]*emins[2];
228 dist2 = p->normal[0]*emins[0] + p->normal[1]*emins[1] + p->normal[2]*emaxs[2];
229 break;
230 case 5:
231 dist1 = p->normal[0]*emins[0] + p->normal[1]*emaxs[1] + p->normal[2]*emins[2];
232 dist2 = p->normal[0]*emaxs[0] + p->normal[1]*emins[1] + p->normal[2]*emaxs[2];
233 break;
234 case 6:
235 dist1 = p->normal[0]*emaxs[0] + p->normal[1]*emins[1] + p->normal[2]*emins[2];
236 dist2 = p->normal[0]*emins[0] + p->normal[1]*emaxs[1] + p->normal[2]*emaxs[2];
237 break;
238 case 7:
239 dist1 = p->normal[0]*emins[0] + p->normal[1]*emins[1] + p->normal[2]*emins[2];
240 dist2 = p->normal[0]*emaxs[0] + p->normal[1]*emaxs[1] + p->normal[2]*emaxs[2];
241 break;
242 default:
243 dist1 = dist2 = 0; // shut up compiler
244 BOPS_Error ();
245 break;
246 }
247
248 #if 0
249 int i;
250 vec3_t corners[2];
251
252 for (i=0 ; i<3 ; i++)
253 {
254 if (plane->normal[i] < 0)
255 {
256 corners[0][i] = emins[i];
257 corners[1][i] = emaxs[i];
258 }
259 else
260 {
261 corners[1][i] = emins[i];
262 corners[0][i] = emaxs[i];
263 }
264 }
265 dist = DotProduct (plane->normal, corners[0]) - plane->dist;
266 dist2 = DotProduct (plane->normal, corners[1]) - plane->dist;
267 sides = 0;
268 if (dist1 >= 0)
269 sides = 1;
270 if (dist2 < 0)
271 sides |= 2;
272
273 #endif
274
275 sides = 0;
276 if (dist1 >= p->dist)
277 sides = 1;
278 if (dist2 < p->dist)
279 sides |= 2;
280
281 #ifdef PARANOID
282 if (sides == 0)
283 Sys_Error ("BoxOnPlaneSide: sides==0");
284 #endif
285
286 return sides;
287 }
288
289 #endif
290
291
AngleVectors(vec3_t angles,vec3_t forward,vec3_t right,vec3_t up)292 void AngleVectors (vec3_t angles, vec3_t forward, vec3_t right, vec3_t up)
293 {
294 float angle;
295 float sr, sp, sy, cr, cp, cy;
296
297 angle = angles[YAW] * (M_PI*2 / 360);
298 sy = sin(angle);
299 cy = cos(angle);
300 angle = angles[PITCH] * (M_PI*2 / 360);
301 sp = sin(angle);
302 cp = cos(angle);
303 angle = angles[ROLL] * (M_PI*2 / 360);
304 sr = sin(angle);
305 cr = cos(angle);
306
307 forward[0] = cp*cy;
308 forward[1] = cp*sy;
309 forward[2] = -sp;
310 right[0] = (-1*sr*sp*cy+-1*cr*-sy);
311 right[1] = (-1*sr*sp*sy+-1*cr*cy);
312 right[2] = -1*sr*cp;
313 up[0] = (cr*sp*cy+-sr*-sy);
314 up[1] = (cr*sp*sy+-sr*cy);
315 up[2] = cr*cp;
316 }
317
VectorCompare(vec3_t v1,vec3_t v2)318 int VectorCompare (vec3_t v1, vec3_t v2)
319 {
320 int i;
321
322 for (i=0 ; i<3 ; i++)
323 if (v1[i] != v2[i])
324 return 0;
325
326 return 1;
327 }
328
VectorMA(vec3_t veca,float scale,vec3_t vecb,vec3_t vecc)329 void VectorMA (vec3_t veca, float scale, vec3_t vecb, vec3_t vecc)
330 {
331 vecc[0] = veca[0] + scale*vecb[0];
332 vecc[1] = veca[1] + scale*vecb[1];
333 vecc[2] = veca[2] + scale*vecb[2];
334 }
335
336
_DotProduct(vec3_t v1,vec3_t v2)337 vec_t _DotProduct (vec3_t v1, vec3_t v2)
338 {
339 return v1[0]*v2[0] + v1[1]*v2[1] + v1[2]*v2[2];
340 }
341
_VectorSubtract(vec3_t veca,vec3_t vecb,vec3_t out)342 void _VectorSubtract (vec3_t veca, vec3_t vecb, vec3_t out)
343 {
344 out[0] = veca[0]-vecb[0];
345 out[1] = veca[1]-vecb[1];
346 out[2] = veca[2]-vecb[2];
347 }
348
_VectorAdd(vec3_t veca,vec3_t vecb,vec3_t out)349 void _VectorAdd (vec3_t veca, vec3_t vecb, vec3_t out)
350 {
351 out[0] = veca[0]+vecb[0];
352 out[1] = veca[1]+vecb[1];
353 out[2] = veca[2]+vecb[2];
354 }
355
_VectorCopy(vec3_t in,vec3_t out)356 void _VectorCopy (vec3_t in, vec3_t out)
357 {
358 out[0] = in[0];
359 out[1] = in[1];
360 out[2] = in[2];
361 }
362
CrossProduct(vec3_t v1,vec3_t v2,vec3_t cross)363 void CrossProduct (vec3_t v1, vec3_t v2, vec3_t cross)
364 {
365 cross[0] = v1[1]*v2[2] - v1[2]*v2[1];
366 cross[1] = v1[2]*v2[0] - v1[0]*v2[2];
367 cross[2] = v1[0]*v2[1] - v1[1]*v2[0];
368 }
369
370 double sqrt(double x);
371
Length(vec3_t v)372 vec_t Length(vec3_t v)
373 {
374 int i;
375 float length;
376
377 length = 0;
378 for (i=0 ; i< 3 ; i++)
379 length += v[i]*v[i];
380 length = sqrt (length); // FIXME
381
382 return length;
383 }
384
VectorNormalize(vec3_t v)385 float VectorNormalize (vec3_t v)
386 {
387 float length, ilength;
388
389 length = v[0]*v[0] + v[1]*v[1] + v[2]*v[2];
390 length = sqrt (length); // FIXME
391
392 if (length)
393 {
394 ilength = 1/length;
395 v[0] *= ilength;
396 v[1] *= ilength;
397 v[2] *= ilength;
398 }
399
400 return length;
401
402 }
403
VectorInverse(vec3_t v)404 void VectorInverse (vec3_t v)
405 {
406 v[0] = -v[0];
407 v[1] = -v[1];
408 v[2] = -v[2];
409 }
410
VectorScale(vec3_t in,vec_t scale,vec3_t out)411 void VectorScale (vec3_t in, vec_t scale, vec3_t out)
412 {
413 out[0] = in[0]*scale;
414 out[1] = in[1]*scale;
415 out[2] = in[2]*scale;
416 }
417
418
Q_log2(int val)419 int Q_log2(int val)
420 {
421 int answer=0;
422 while (val>>=1)
423 answer++;
424 return answer;
425 }
426
427
428 /*
429 ================
430 R_ConcatRotations
431 ================
432 */
R_ConcatRotations(float in1[3][3],float in2[3][3],float out[3][3])433 void R_ConcatRotations (float in1[3][3], float in2[3][3], float out[3][3])
434 {
435 out[0][0] = in1[0][0] * in2[0][0] + in1[0][1] * in2[1][0] +
436 in1[0][2] * in2[2][0];
437 out[0][1] = in1[0][0] * in2[0][1] + in1[0][1] * in2[1][1] +
438 in1[0][2] * in2[2][1];
439 out[0][2] = in1[0][0] * in2[0][2] + in1[0][1] * in2[1][2] +
440 in1[0][2] * in2[2][2];
441 out[1][0] = in1[1][0] * in2[0][0] + in1[1][1] * in2[1][0] +
442 in1[1][2] * in2[2][0];
443 out[1][1] = in1[1][0] * in2[0][1] + in1[1][1] * in2[1][1] +
444 in1[1][2] * in2[2][1];
445 out[1][2] = in1[1][0] * in2[0][2] + in1[1][1] * in2[1][2] +
446 in1[1][2] * in2[2][2];
447 out[2][0] = in1[2][0] * in2[0][0] + in1[2][1] * in2[1][0] +
448 in1[2][2] * in2[2][0];
449 out[2][1] = in1[2][0] * in2[0][1] + in1[2][1] * in2[1][1] +
450 in1[2][2] * in2[2][1];
451 out[2][2] = in1[2][0] * in2[0][2] + in1[2][1] * in2[1][2] +
452 in1[2][2] * in2[2][2];
453 }
454
455
456 /*
457 ================
458 R_ConcatTransforms
459 ================
460 */
R_ConcatTransforms(float in1[3][4],float in2[3][4],float out[3][4])461 void R_ConcatTransforms (float in1[3][4], float in2[3][4], float out[3][4])
462 {
463 out[0][0] = in1[0][0] * in2[0][0] + in1[0][1] * in2[1][0] +
464 in1[0][2] * in2[2][0];
465 out[0][1] = in1[0][0] * in2[0][1] + in1[0][1] * in2[1][1] +
466 in1[0][2] * in2[2][1];
467 out[0][2] = in1[0][0] * in2[0][2] + in1[0][1] * in2[1][2] +
468 in1[0][2] * in2[2][2];
469 out[0][3] = in1[0][0] * in2[0][3] + in1[0][1] * in2[1][3] +
470 in1[0][2] * in2[2][3] + in1[0][3];
471 out[1][0] = in1[1][0] * in2[0][0] + in1[1][1] * in2[1][0] +
472 in1[1][2] * in2[2][0];
473 out[1][1] = in1[1][0] * in2[0][1] + in1[1][1] * in2[1][1] +
474 in1[1][2] * in2[2][1];
475 out[1][2] = in1[1][0] * in2[0][2] + in1[1][1] * in2[1][2] +
476 in1[1][2] * in2[2][2];
477 out[1][3] = in1[1][0] * in2[0][3] + in1[1][1] * in2[1][3] +
478 in1[1][2] * in2[2][3] + in1[1][3];
479 out[2][0] = in1[2][0] * in2[0][0] + in1[2][1] * in2[1][0] +
480 in1[2][2] * in2[2][0];
481 out[2][1] = in1[2][0] * in2[0][1] + in1[2][1] * in2[1][1] +
482 in1[2][2] * in2[2][1];
483 out[2][2] = in1[2][0] * in2[0][2] + in1[2][1] * in2[1][2] +
484 in1[2][2] * in2[2][2];
485 out[2][3] = in1[2][0] * in2[0][3] + in1[2][1] * in2[1][3] +
486 in1[2][2] * in2[2][3] + in1[2][3];
487 }
488
489
490 /*
491 ===================
492 FloorDivMod
493
494 Returns mathematically correct (floor-based) quotient and remainder for
495 numer and denom, both of which should contain no fractional part. The
496 quotient must fit in 32 bits.
497 ====================
498 */
499
FloorDivMod(double numer,double denom,int * quotient,int * rem)500 void FloorDivMod (double numer, double denom, int *quotient,
501 int *rem)
502 {
503 int q, r;
504 double x;
505
506 #ifndef PARANOID
507 if (denom <= 0.0)
508 Sys_Error ("FloorDivMod: bad denominator %d\n", denom);
509
510 // if ((floor(numer) != numer) || (floor(denom) != denom))
511 // Sys_Error ("FloorDivMod: non-integer numer or denom %f %f\n",
512 // numer, denom);
513 #endif
514
515 if (numer >= 0.0)
516 {
517
518 x = floor(numer / denom);
519 q = (int)x;
520 r = (int)floor(numer - (x * denom));
521 }
522 else
523 {
524 //
525 // perform operations with positive values, and fix mod to make floor-based
526 //
527 x = floor(-numer / denom);
528 q = -(int)x;
529 r = (int)floor(-numer - (x * denom));
530 if (r != 0)
531 {
532 q--;
533 r = (int)denom - r;
534 }
535 }
536
537 *quotient = q;
538 *rem = r;
539 }
540
541
542 /*
543 ===================
544 GreatestCommonDivisor
545 ====================
546 */
GreatestCommonDivisor(int i1,int i2)547 int GreatestCommonDivisor (int i1, int i2)
548 {
549 if (i1 > i2)
550 {
551 if (i2 == 0)
552 return (i1);
553 return GreatestCommonDivisor (i2, i1 % i2);
554 }
555 else
556 {
557 if (i1 == 0)
558 return (i2);
559 return GreatestCommonDivisor (i1, i2 % i1);
560 }
561 }
562
563
564 #if !id386
565
566 // TODO: move to nonintel.c
567
568 /*
569 ===================
570 Invert24To16
571
572 Inverts an 8.24 value to a 16.16 value
573 ====================
574 */
575
Invert24To16(fixed16_t val)576 fixed16_t Invert24To16(fixed16_t val)
577 {
578 if (val < 256)
579 return (0xFFFFFFFF);
580
581 return (fixed16_t)
582 (((double)0x10000 * (double)0x1000000 / (double)val) + 0.5);
583 }
584
585 #endif
586