2 Copyright (C) 1996-1997 Id Software, Inc.
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.
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.
13 See the GNU General Public License for more details.
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.
20 // mathlib.c -- math primitives
25 void Sys_Error (char *error, ...);
27 vec3_t vec3_origin = {0,0,0};
28 int nanmask = 255<<23;
30 /*-----------------------------------------------------------------*/
32 #define DEG2RAD( a ) ( a * M_PI ) / 180.0F
34 void ProjectPointOnPlane( vec3_t dst, const vec3_t p, const vec3_t normal )
40 inv_denom = 1.0F / DotProduct( normal, normal );
42 d = DotProduct( normal, p ) * inv_denom;
44 n[0] = normal[0] * inv_denom;
45 n[1] = normal[1] * inv_denom;
46 n[2] = normal[2] * inv_denom;
48 dst[0] = p[0] - d * n[0];
49 dst[1] = p[1] - d * n[1];
50 dst[2] = p[2] - d * n[2];
54 ** assumes "src" is normalized
56 void PerpendicularVector( vec3_t dst, const vec3_t src )
64 ** find the smallest magnitude axially aligned vector
66 for ( pos = 0, i = 0; i < 3; i++ )
68 if ( fabs( src[i] ) < minelem )
71 minelem = fabs( src[i] );
74 tempvec[0] = tempvec[1] = tempvec[2] = 0.0F;
78 ** project the point onto the plane defined by src
80 ProjectPointOnPlane( dst, tempvec, src );
83 ** normalize the result
85 VectorNormalize( dst );
89 #pragma optimize( "", off )
93 void RotatePointAroundVector( vec3_t dst, const vec3_t dir, const vec3_t point, float degrees )
107 PerpendicularVector( vr, dir );
108 CrossProduct( vr, vf, vup );
122 memcpy( im, m, sizeof( im ) );
131 memset( zrot, 0, sizeof( zrot ) );
132 zrot[0][0] = zrot[1][1] = zrot[2][2] = 1.0F;
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 ) );
139 R_ConcatRotations( m, zrot, tmpmat );
140 R_ConcatRotations( tmpmat, im, rot );
142 for ( i = 0; i < 3; i++ )
144 dst[i] = rot[i][0] * point[0] + rot[i][1] * point[1] + rot[i][2] * point[2];
149 #pragma optimize( "", on )
152 /*-----------------------------------------------------------------*/
155 float anglemod(float a)
159 a -= 360*(int)(a/360);
161 a += 360*( 1 + (int)(-a/360) );
163 a = (360.0/65536) * ((int)(a*(65536/360.0)) & 65535);
171 Split out like this for ASM to call.
175 void BOPS_Error (void)
177 Sys_Error ("BoxOnPlaneSide: Bad signbits");
188 Returns 1, 2, or 1 + 2
192 int BoxOnPlaneSide (vec3_t emins, vec3_t emaxs, mplane_t *p)
197 #if 0 // this is done by the BOX_ON_PLANE_SIDE macro before calling this
202 if (p->dist <= emins[p->type])
204 if (p->dist >= emaxs[p->type])
214 dist1 = p->normal[0]*emaxs[0] + p->normal[1]*emaxs[1] + p->normal[2]*emaxs[2];
215 dist2 = p->normal[0]*emins[0] + p->normal[1]*emins[1] + p->normal[2]*emins[2];
218 dist1 = p->normal[0]*emins[0] + p->normal[1]*emaxs[1] + p->normal[2]*emaxs[2];
219 dist2 = p->normal[0]*emaxs[0] + p->normal[1]*emins[1] + p->normal[2]*emins[2];
222 dist1 = p->normal[0]*emaxs[0] + p->normal[1]*emins[1] + p->normal[2]*emaxs[2];
223 dist2 = p->normal[0]*emins[0] + p->normal[1]*emaxs[1] + p->normal[2]*emins[2];
226 dist1 = p->normal[0]*emins[0] + p->normal[1]*emins[1] + p->normal[2]*emaxs[2];
227 dist2 = p->normal[0]*emaxs[0] + p->normal[1]*emaxs[1] + p->normal[2]*emins[2];
230 dist1 = p->normal[0]*emaxs[0] + p->normal[1]*emaxs[1] + p->normal[2]*emins[2];
231 dist2 = p->normal[0]*emins[0] + p->normal[1]*emins[1] + p->normal[2]*emaxs[2];
234 dist1 = p->normal[0]*emins[0] + p->normal[1]*emaxs[1] + p->normal[2]*emins[2];
235 dist2 = p->normal[0]*emaxs[0] + p->normal[1]*emins[1] + p->normal[2]*emaxs[2];
238 dist1 = p->normal[0]*emaxs[0] + p->normal[1]*emins[1] + p->normal[2]*emins[2];
239 dist2 = p->normal[0]*emins[0] + p->normal[1]*emaxs[1] + p->normal[2]*emaxs[2];
242 dist1 = p->normal[0]*emins[0] + p->normal[1]*emins[1] + p->normal[2]*emins[2];
243 dist2 = p->normal[0]*emaxs[0] + p->normal[1]*emaxs[1] + p->normal[2]*emaxs[2];
246 dist1 = dist2 = 0; // shut up compiler
255 for (i=0 ; i<3 ; i++)
257 if (plane->normal[i] < 0)
259 corners[0][i] = emins[i];
260 corners[1][i] = emaxs[i];
264 corners[1][i] = emins[i];
265 corners[0][i] = emaxs[i];
268 dist = DotProduct (plane->normal, corners[0]) - plane->dist;
269 dist2 = DotProduct (plane->normal, corners[1]) - plane->dist;
279 if (dist1 >= p->dist)
286 Sys_Error ("BoxOnPlaneSide: sides==0");
295 int BoxOnPlaneSide0 (vec3_t emins, vec3_t emaxs, mplane_t *p) {return (((p->normal[0]*emaxs[0] + p->normal[1]*emaxs[1] + p->normal[2]*emaxs[2]) >= p->dist) | (((p->normal[0]*emins[0] + p->normal[1]*emins[1] + p->normal[2]*emins[2]) < p->dist) << 1));}
296 int BoxOnPlaneSide1 (vec3_t emins, vec3_t emaxs, mplane_t *p) {return (((p->normal[0]*emins[0] + p->normal[1]*emaxs[1] + p->normal[2]*emaxs[2]) >= p->dist) | (((p->normal[0]*emaxs[0] + p->normal[1]*emins[1] + p->normal[2]*emins[2]) < p->dist) << 1));}
297 int BoxOnPlaneSide2 (vec3_t emins, vec3_t emaxs, mplane_t *p) {return (((p->normal[0]*emaxs[0] + p->normal[1]*emins[1] + p->normal[2]*emaxs[2]) >= p->dist) | (((p->normal[0]*emins[0] + p->normal[1]*emaxs[1] + p->normal[2]*emins[2]) < p->dist) << 1));}
298 int BoxOnPlaneSide3 (vec3_t emins, vec3_t emaxs, mplane_t *p) {return (((p->normal[0]*emins[0] + p->normal[1]*emins[1] + p->normal[2]*emaxs[2]) >= p->dist) | (((p->normal[0]*emaxs[0] + p->normal[1]*emaxs[1] + p->normal[2]*emins[2]) < p->dist) << 1));}
299 int BoxOnPlaneSide4 (vec3_t emins, vec3_t emaxs, mplane_t *p) {return (((p->normal[0]*emaxs[0] + p->normal[1]*emaxs[1] + p->normal[2]*emins[2]) >= p->dist) | (((p->normal[0]*emins[0] + p->normal[1]*emins[1] + p->normal[2]*emaxs[2]) < p->dist) << 1));}
300 int BoxOnPlaneSide5 (vec3_t emins, vec3_t emaxs, mplane_t *p) {return (((p->normal[0]*emins[0] + p->normal[1]*emaxs[1] + p->normal[2]*emins[2]) >= p->dist) | (((p->normal[0]*emaxs[0] + p->normal[1]*emins[1] + p->normal[2]*emaxs[2]) < p->dist) << 1));}
301 int BoxOnPlaneSide6 (vec3_t emins, vec3_t emaxs, mplane_t *p) {return (((p->normal[0]*emaxs[0] + p->normal[1]*emins[1] + p->normal[2]*emins[2]) >= p->dist) | (((p->normal[0]*emins[0] + p->normal[1]*emaxs[1] + p->normal[2]*emaxs[2]) < p->dist) << 1));}
302 int BoxOnPlaneSide7 (vec3_t emins, vec3_t emaxs, mplane_t *p) {return (((p->normal[0]*emins[0] + p->normal[1]*emins[1] + p->normal[2]*emins[2]) >= p->dist) | (((p->normal[0]*emaxs[0] + p->normal[1]*emaxs[1] + p->normal[2]*emaxs[2]) < p->dist) << 1));}
304 void BoxOnPlaneSideClassify(mplane_t *p)
306 if (p->normal[2] < 0) // 4
308 if (p->normal[1] < 0) // 2
310 if (p->normal[0] < 0) // 1
311 p->BoxOnPlaneSideFunc = BoxOnPlaneSide7;
313 p->BoxOnPlaneSideFunc = BoxOnPlaneSide6;
317 if (p->normal[0] < 0) // 1
318 p->BoxOnPlaneSideFunc = BoxOnPlaneSide5;
320 p->BoxOnPlaneSideFunc = BoxOnPlaneSide4;
325 if (p->normal[1] < 0) // 2
327 if (p->normal[0] < 0) // 1
328 p->BoxOnPlaneSideFunc = BoxOnPlaneSide3;
330 p->BoxOnPlaneSideFunc = BoxOnPlaneSide2;
334 if (p->normal[0] < 0) // 1
335 p->BoxOnPlaneSideFunc = BoxOnPlaneSide1;
337 p->BoxOnPlaneSideFunc = BoxOnPlaneSide0;
342 void AngleVectors (vec3_t angles, vec3_t forward, vec3_t right, vec3_t up)
345 float sr, sp, sy, cr, cp, cy;
347 angle = angles[YAW] * (M_PI*2 / 360);
350 angle = angles[PITCH] * (M_PI*2 / 360);
353 angle = angles[ROLL] * (M_PI*2 / 360);
360 right[0] = (-1*sr*sp*cy+-1*cr*-sy);
361 right[1] = (-1*sr*sp*sy+-1*cr*cy);
363 up[0] = (cr*sp*cy+-sr*-sy);
364 up[1] = (cr*sp*sy+-sr*cy);
368 int VectorCompare (vec3_t v1, vec3_t v2)
372 for (i=0 ; i<3 ; i++)
379 void VectorMA (vec3_t veca, float scale, vec3_t vecb, vec3_t vecc)
381 vecc[0] = veca[0] + scale*vecb[0];
382 vecc[1] = veca[1] + scale*vecb[1];
383 vecc[2] = veca[2] + scale*vecb[2];
387 vec_t _DotProduct (vec3_t v1, vec3_t v2)
389 return v1[0]*v2[0] + v1[1]*v2[1] + v1[2]*v2[2];
392 void _VectorSubtract (vec3_t veca, vec3_t vecb, vec3_t out)
394 out[0] = veca[0]-vecb[0];
395 out[1] = veca[1]-vecb[1];
396 out[2] = veca[2]-vecb[2];
399 void _VectorAdd (vec3_t veca, vec3_t vecb, vec3_t out)
401 out[0] = veca[0]+vecb[0];
402 out[1] = veca[1]+vecb[1];
403 out[2] = veca[2]+vecb[2];
406 void _VectorCopy (vec3_t in, vec3_t out)
413 // LordHavoc: changed CrossProduct to a #define
415 void CrossProduct (vec3_t v1, vec3_t v2, vec3_t cross)
417 cross[0] = v1[1]*v2[2] - v1[2]*v2[1];
418 cross[1] = v1[2]*v2[0] - v1[0]*v2[2];
419 cross[2] = v1[0]*v2[1] - v1[1]*v2[0];
423 double sqrt(double x);
425 vec_t Length(vec3_t v)
431 for (i=0 ; i< 3 ; i++)
433 length = sqrt (length); // FIXME
438 // LordHavoc: renamed these to Length, and made the normal ones #define
439 float VectorNormalizeLength (vec3_t v)
441 float length, ilength;
443 length = v[0]*v[0] + v[1]*v[1] + v[2]*v[2];
444 length = sqrt (length); // FIXME
458 float VectorNormalizeLength2 (vec3_t v, vec3_t dest) // LordHavoc: added to allow copying while doing the calculation...
460 float length, ilength;
462 length = v[0]*v[0] + v[1]*v[1] + v[2]*v[2];
463 length = sqrt (length); // FIXME
468 dest[0] = v[0] * ilength;
469 dest[1] = v[1] * ilength;
470 dest[2] = v[2] * ilength;
473 dest[0] = dest[1] = dest[2] = 0;
479 void VectorInverse (vec3_t v)
486 void VectorScale (vec3_t in, vec_t scale, vec3_t out)
488 out[0] = in[0]*scale;
489 out[1] = in[1]*scale;
490 out[2] = in[2]*scale;
508 void R_ConcatRotations (float in1[3][3], float in2[3][3], float out[3][3])
510 out[0][0] = in1[0][0] * in2[0][0] + in1[0][1] * in2[1][0] + in1[0][2] * in2[2][0];
511 out[0][1] = in1[0][0] * in2[0][1] + in1[0][1] * in2[1][1] + in1[0][2] * in2[2][1];
512 out[0][2] = in1[0][0] * in2[0][2] + in1[0][1] * in2[1][2] + in1[0][2] * in2[2][2];
513 out[1][0] = in1[1][0] * in2[0][0] + in1[1][1] * in2[1][0] + in1[1][2] * in2[2][0];
514 out[1][1] = in1[1][0] * in2[0][1] + in1[1][1] * in2[1][1] + in1[1][2] * in2[2][1];
515 out[1][2] = in1[1][0] * in2[0][2] + in1[1][1] * in2[1][2] + in1[1][2] * in2[2][2];
516 out[2][0] = in1[2][0] * in2[0][0] + in1[2][1] * in2[1][0] + in1[2][2] * in2[2][0];
517 out[2][1] = in1[2][0] * in2[0][1] + in1[2][1] * in2[1][1] + in1[2][2] * in2[2][1];
518 out[2][2] = in1[2][0] * in2[0][2] + in1[2][1] * in2[1][2] + in1[2][2] * in2[2][2];
527 void R_ConcatTransforms (float in1[3][4], float in2[3][4], float out[3][4])
529 out[0][0] = in1[0][0] * in2[0][0] + in1[0][1] * in2[1][0] + in1[0][2] * in2[2][0];
530 out[0][1] = in1[0][0] * in2[0][1] + in1[0][1] * in2[1][1] + in1[0][2] * in2[2][1];
531 out[0][2] = in1[0][0] * in2[0][2] + in1[0][1] * in2[1][2] + in1[0][2] * in2[2][2];
532 out[0][3] = in1[0][0] * in2[0][3] + in1[0][1] * in2[1][3] + in1[0][2] * in2[2][3] + in1[0][3];
533 out[1][0] = in1[1][0] * in2[0][0] + in1[1][1] * in2[1][0] + in1[1][2] * in2[2][0];
534 out[1][1] = in1[1][0] * in2[0][1] + in1[1][1] * in2[1][1] + in1[1][2] * in2[2][1];
535 out[1][2] = in1[1][0] * in2[0][2] + in1[1][1] * in2[1][2] + in1[1][2] * in2[2][2];
536 out[1][3] = in1[1][0] * in2[0][3] + in1[1][1] * in2[1][3] + in1[1][2] * in2[2][3] + in1[1][3];
537 out[2][0] = in1[2][0] * in2[0][0] + in1[2][1] * in2[1][0] + in1[2][2] * in2[2][0];
538 out[2][1] = in1[2][0] * in2[0][1] + in1[2][1] * in2[1][1] + in1[2][2] * in2[2][1];
539 out[2][2] = in1[2][0] * in2[0][2] + in1[2][1] * in2[1][2] + in1[2][2] * in2[2][2];
540 out[2][3] = in1[2][0] * in2[0][3] + in1[2][1] * in2[1][3] + in1[2][2] * in2[2][3] + in1[2][3];
548 Returns mathematically correct (floor-based) quotient and remainder for
549 numer and denom, both of which should contain no fractional part. The
550 quotient must fit in 32 bits.
554 void FloorDivMod (double numer, double denom, int *quotient,
562 Sys_Error ("FloorDivMod: bad denominator %d\n", denom);
564 // if ((floor(numer) != numer) || (floor(denom) != denom))
565 // Sys_Error ("FloorDivMod: non-integer numer or denom %f %f\n",
572 x = floor(numer / denom);
574 r = (int)floor(numer - (x * denom));
579 // perform operations with positive values, and fix mod to make floor-based
581 x = floor(-numer / denom);
583 r = (int)floor(-numer - (x * denom));
598 GreatestCommonDivisor
601 int GreatestCommonDivisor (int i1, int i2)
607 return GreatestCommonDivisor (i2, i1 % i2);
613 return GreatestCommonDivisor (i1, i2 % i1);
620 // TODO: move to nonintel.c
626 Inverts an 8.24 value to a 16.16 value
630 fixed16_t Invert24To16(fixed16_t val)
636 (((double)0x10000 * (double)0x1000000 / (double)val) + 0.5);