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);
167 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));}
168 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));}
169 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));}
170 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));}
171 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));}
172 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));}
173 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));}
174 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));}
176 void BoxOnPlaneSideClassify(mplane_t *p)
178 if (p->normal[2] < 0) // 4
180 if (p->normal[1] < 0) // 2
182 if (p->normal[0] < 0) // 1
183 p->BoxOnPlaneSideFunc = BoxOnPlaneSide7;
185 p->BoxOnPlaneSideFunc = BoxOnPlaneSide6;
189 if (p->normal[0] < 0) // 1
190 p->BoxOnPlaneSideFunc = BoxOnPlaneSide5;
192 p->BoxOnPlaneSideFunc = BoxOnPlaneSide4;
197 if (p->normal[1] < 0) // 2
199 if (p->normal[0] < 0) // 1
200 p->BoxOnPlaneSideFunc = BoxOnPlaneSide3;
202 p->BoxOnPlaneSideFunc = BoxOnPlaneSide2;
206 if (p->normal[0] < 0) // 1
207 p->BoxOnPlaneSideFunc = BoxOnPlaneSide1;
209 p->BoxOnPlaneSideFunc = BoxOnPlaneSide0;
214 void AngleVectors (vec3_t angles, vec3_t forward, vec3_t right, vec3_t up)
217 float sr, sp, sy, cr, cp, cy;
219 angle = angles[YAW] * (M_PI*2 / 360);
222 angle = angles[PITCH] * (M_PI*2 / 360);
225 angle = angles[ROLL] * (M_PI*2 / 360);
232 right[0] = (-1*sr*sp*cy+-1*cr*-sy);
233 right[1] = (-1*sr*sp*sy+-1*cr*cy);
235 up[0] = (cr*sp*cy+-sr*-sy);
236 up[1] = (cr*sp*sy+-sr*cy);
240 int VectorCompare (vec3_t v1, vec3_t v2)
244 for (i=0 ; i<3 ; i++)
251 void VectorMA (vec3_t veca, float scale, vec3_t vecb, vec3_t vecc)
253 vecc[0] = veca[0] + scale*vecb[0];
254 vecc[1] = veca[1] + scale*vecb[1];
255 vecc[2] = veca[2] + scale*vecb[2];
259 vec_t _DotProduct (vec3_t v1, vec3_t v2)
261 return v1[0]*v2[0] + v1[1]*v2[1] + v1[2]*v2[2];
264 void _VectorSubtract (vec3_t veca, vec3_t vecb, vec3_t out)
266 out[0] = veca[0]-vecb[0];
267 out[1] = veca[1]-vecb[1];
268 out[2] = veca[2]-vecb[2];
271 void _VectorAdd (vec3_t veca, vec3_t vecb, vec3_t out)
273 out[0] = veca[0]+vecb[0];
274 out[1] = veca[1]+vecb[1];
275 out[2] = veca[2]+vecb[2];
278 void _VectorCopy (vec3_t in, vec3_t out)
285 // LordHavoc: changed CrossProduct to a #define
287 void CrossProduct (vec3_t v1, vec3_t v2, vec3_t cross)
289 cross[0] = v1[1]*v2[2] - v1[2]*v2[1];
290 cross[1] = v1[2]*v2[0] - v1[0]*v2[2];
291 cross[2] = v1[0]*v2[1] - v1[1]*v2[0];
295 double sqrt(double x);
297 vec_t Length(vec3_t v)
303 for (i=0 ; i< 3 ; i++)
305 length = sqrt (length); // FIXME
310 // LordHavoc: renamed these to Length, and made the normal ones #define
311 float VectorNormalizeLength (vec3_t v)
313 float length, ilength;
315 length = v[0]*v[0] + v[1]*v[1] + v[2]*v[2];
316 length = sqrt (length); // FIXME
330 float VectorNormalizeLength2 (vec3_t v, vec3_t dest) // LordHavoc: added to allow copying while doing the calculation...
332 float length, ilength;
334 length = v[0]*v[0] + v[1]*v[1] + v[2]*v[2];
335 length = sqrt (length); // FIXME
340 dest[0] = v[0] * ilength;
341 dest[1] = v[1] * ilength;
342 dest[2] = v[2] * ilength;
345 dest[0] = dest[1] = dest[2] = 0;
351 void VectorInverse (vec3_t v)
358 void VectorScale (vec3_t in, vec_t scale, vec3_t out)
360 out[0] = in[0]*scale;
361 out[1] = in[1]*scale;
362 out[2] = in[2]*scale;
380 void R_ConcatRotations (float in1[3][3], float in2[3][3], float out[3][3])
382 out[0][0] = in1[0][0] * in2[0][0] + in1[0][1] * in2[1][0] + in1[0][2] * in2[2][0];
383 out[0][1] = in1[0][0] * in2[0][1] + in1[0][1] * in2[1][1] + in1[0][2] * in2[2][1];
384 out[0][2] = in1[0][0] * in2[0][2] + in1[0][1] * in2[1][2] + in1[0][2] * in2[2][2];
385 out[1][0] = in1[1][0] * in2[0][0] + in1[1][1] * in2[1][0] + in1[1][2] * in2[2][0];
386 out[1][1] = in1[1][0] * in2[0][1] + in1[1][1] * in2[1][1] + in1[1][2] * in2[2][1];
387 out[1][2] = in1[1][0] * in2[0][2] + in1[1][1] * in2[1][2] + in1[1][2] * in2[2][2];
388 out[2][0] = in1[2][0] * in2[0][0] + in1[2][1] * in2[1][0] + in1[2][2] * in2[2][0];
389 out[2][1] = in1[2][0] * in2[0][1] + in1[2][1] * in2[1][1] + in1[2][2] * in2[2][1];
390 out[2][2] = in1[2][0] * in2[0][2] + in1[2][1] * in2[1][2] + in1[2][2] * in2[2][2];
399 void R_ConcatTransforms (float in1[3][4], float in2[3][4], float out[3][4])
401 out[0][0] = in1[0][0] * in2[0][0] + in1[0][1] * in2[1][0] + in1[0][2] * in2[2][0];
402 out[0][1] = in1[0][0] * in2[0][1] + in1[0][1] * in2[1][1] + in1[0][2] * in2[2][1];
403 out[0][2] = in1[0][0] * in2[0][2] + in1[0][1] * in2[1][2] + in1[0][2] * in2[2][2];
404 out[0][3] = in1[0][0] * in2[0][3] + in1[0][1] * in2[1][3] + in1[0][2] * in2[2][3] + in1[0][3];
405 out[1][0] = in1[1][0] * in2[0][0] + in1[1][1] * in2[1][0] + in1[1][2] * in2[2][0];
406 out[1][1] = in1[1][0] * in2[0][1] + in1[1][1] * in2[1][1] + in1[1][2] * in2[2][1];
407 out[1][2] = in1[1][0] * in2[0][2] + in1[1][1] * in2[1][2] + in1[1][2] * in2[2][2];
408 out[1][3] = in1[1][0] * in2[0][3] + in1[1][1] * in2[1][3] + in1[1][2] * in2[2][3] + in1[1][3];
409 out[2][0] = in1[2][0] * in2[0][0] + in1[2][1] * in2[1][0] + in1[2][2] * in2[2][0];
410 out[2][1] = in1[2][0] * in2[0][1] + in1[2][1] * in2[1][1] + in1[2][2] * in2[2][1];
411 out[2][2] = in1[2][0] * in2[0][2] + in1[2][1] * in2[1][2] + in1[2][2] * in2[2][2];
412 out[2][3] = in1[2][0] * in2[0][3] + in1[2][1] * in2[1][3] + in1[2][2] * in2[2][3] + in1[2][3];
420 Returns mathematically correct (floor-based) quotient and remainder for
421 numer and denom, both of which should contain no fractional part. The
422 quotient must fit in 32 bits.
426 void FloorDivMod (double numer, double denom, int *quotient,
434 Sys_Error ("FloorDivMod: bad denominator %d\n", denom);
436 // if ((floor(numer) != numer) || (floor(denom) != denom))
437 // Sys_Error ("FloorDivMod: non-integer numer or denom %f %f\n",
444 x = floor(numer / denom);
446 r = (int)floor(numer - (x * denom));
451 // perform operations with positive values, and fix mod to make floor-based
453 x = floor(-numer / denom);
455 r = (int)floor(-numer - (x * denom));
470 GreatestCommonDivisor
473 int GreatestCommonDivisor (int i1, int i2)
479 return GreatestCommonDivisor (i2, i1 % i2);
485 return GreatestCommonDivisor (i1, i2 % i1);