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.
174 void BOPS_Error (void)
176 Sys_Error ("BoxOnPlaneSide: Bad signbits");
186 Returns 1, 2, or 1 + 2
189 int BoxOnPlaneSide (vec3_t emins
, vec3_t emaxs
, mplane_t
*p
)
194 #if 0 // this is done by the BOX_ON_PLANE_SIDE macro before calling this
199 if (p
->dist
<= emins
[p
->type
])
201 if (p
->dist
>= emaxs
[p
->type
])
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];
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];
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];
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];
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];
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];
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];
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];
243 dist1
= dist2
= 0; // shut up compiler
252 for (i
=0 ; i
<3 ; i
++)
254 if (plane
->normal
[i
] < 0)
256 corners
[0][i
] = emins
[i
];
257 corners
[1][i
] = emaxs
[i
];
261 corners
[1][i
] = emins
[i
];
262 corners
[0][i
] = emaxs
[i
];
265 dist
= DotProduct (plane
->normal
, corners
[0]) - plane
->dist
;
266 dist2
= DotProduct (plane
->normal
, corners
[1]) - plane
->dist
;
276 if (dist1
>= p
->dist
)
283 Sys_Error ("BoxOnPlaneSide: sides==0");
292 void AngleVectors (vec3_t angles
, vec3_t forward
, vec3_t right
, vec3_t up
)
295 float sr
, sp
, sy
, cr
, cp
, cy
;
297 angle
= angles
[YAW
] * (M_PI
*2 / 360);
300 angle
= angles
[PITCH
] * (M_PI
*2 / 360);
303 angle
= angles
[ROLL
] * (M_PI
*2 / 360);
310 right
[0] = (-1*sr
*sp
*cy
+-1*cr
*-sy
);
311 right
[1] = (-1*sr
*sp
*sy
+-1*cr
*cy
);
313 up
[0] = (cr
*sp
*cy
+-sr
*-sy
);
314 up
[1] = (cr
*sp
*sy
+-sr
*cy
);
318 int VectorCompare (vec3_t v1
, vec3_t v2
)
322 for (i
=0 ; i
<3 ; i
++)
329 void VectorMA (vec3_t veca
, float scale
, vec3_t vecb
, vec3_t vecc
)
331 vecc
[0] = veca
[0] + scale
*vecb
[0];
332 vecc
[1] = veca
[1] + scale
*vecb
[1];
333 vecc
[2] = veca
[2] + scale
*vecb
[2];
337 vec_t
_DotProduct (vec3_t v1
, vec3_t v2
)
339 return v1
[0]*v2
[0] + v1
[1]*v2
[1] + v1
[2]*v2
[2];
342 void _VectorSubtract (vec3_t veca
, vec3_t vecb
, vec3_t out
)
344 out
[0] = veca
[0]-vecb
[0];
345 out
[1] = veca
[1]-vecb
[1];
346 out
[2] = veca
[2]-vecb
[2];
349 void _VectorAdd (vec3_t veca
, vec3_t vecb
, vec3_t out
)
351 out
[0] = veca
[0]+vecb
[0];
352 out
[1] = veca
[1]+vecb
[1];
353 out
[2] = veca
[2]+vecb
[2];
356 void _VectorCopy (vec3_t in
, vec3_t out
)
363 void CrossProduct (vec3_t v1
, vec3_t v2
, vec3_t cross
)
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];
370 double sqrt(double x
);
372 vec_t
Length(vec3_t v
)
378 for (i
=0 ; i
< 3 ; i
++)
380 length
= sqrt (length
); // FIXME
385 float VectorNormalize (vec3_t v
)
387 float length
, ilength
;
389 length
= v
[0]*v
[0] + v
[1]*v
[1] + v
[2]*v
[2];
390 length
= sqrt (length
); // FIXME
404 void VectorInverse (vec3_t v
)
411 void VectorScale (vec3_t in
, vec_t scale
, vec3_t out
)
413 out
[0] = in
[0]*scale
;
414 out
[1] = in
[1]*scale
;
415 out
[2] = in
[2]*scale
;
433 void R_ConcatRotations (float in1
[3][3], float in2
[3][3], float out
[3][3])
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];
461 void R_ConcatTransforms (float in1
[3][4], float in2
[3][4], float out
[3][4])
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];
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.
500 void FloorDivMod (double numer
, double denom
, int *quotient
,
508 Sys_Error ("FloorDivMod: bad denominator %d\n", denom
);
510 // if ((floor(numer) != numer) || (floor(denom) != denom))
511 // Sys_Error ("FloorDivMod: non-integer numer or denom %f %f\n",
518 x
= floor(numer
/ denom
);
520 r
= (int)floor(numer
- (x
* denom
));
525 // perform operations with positive values, and fix mod to make floor-based
527 x
= floor(-numer
/ denom
);
529 r
= (int)floor(-numer
- (x
* denom
));
544 GreatestCommonDivisor
547 int GreatestCommonDivisor (int i1
, int i2
)
553 return GreatestCommonDivisor (i2
, i1
% i2
);
559 return GreatestCommonDivisor (i1
, i2
% i1
);
566 // TODO: move to nonintel.c
572 Inverts an 8.24 value to a 16.16 value
576 fixed16_t
Invert24To16(fixed16_t val
)
582 (((double)0x10000 * (double)0x1000000 / (double)val
) + 0.5);