Initial revision
[divverent/darkplaces.git] / mathlib.c
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
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 */
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
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
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 */
174 /*
175 void BOPS_Error (void)
176 {
177         Sys_Error ("BoxOnPlaneSide:  Bad signbits");
178 }
179
180
181 #if     !id386
182
183 */
184 /*
185 ==================
186 BoxOnPlaneSide
187
188 Returns 1, 2, or 1 + 2
189 ==================
190 */
191 /*
192 int BoxOnPlaneSide (vec3_t emins, vec3_t emaxs, mplane_t *p)
193 {
194         float   dist1, dist2;
195         int             sides;
196
197 #if 0   // this is done by the BOX_ON_PLANE_SIDE macro before calling this
198                 // function
199 // fast axial cases
200         if (p->type < 3)
201         {
202                 if (p->dist <= emins[p->type])
203                         return 1;
204                 if (p->dist >= emaxs[p->type])
205                         return 2;
206                 return 3;
207         }
208 #endif
209         
210 // general case
211         switch (p->signbits)
212         {
213         case 0:
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];
216                 break;
217         case 1:
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];
220                 break;
221         case 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];
224                 break;
225         case 3:
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];
228                 break;
229         case 4:
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];
232                 break;
233         case 5:
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];
236                 break;
237         case 6:
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];
240                 break;
241         case 7:
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];
244                 break;
245         default:
246                 dist1 = dist2 = 0;              // shut up compiler
247                 BOPS_Error ();
248                 break;
249         }
250
251 #if 0
252         int             i;
253         vec3_t  corners[2];
254
255         for (i=0 ; i<3 ; i++)
256         {
257                 if (plane->normal[i] < 0)
258                 {
259                         corners[0][i] = emins[i];
260                         corners[1][i] = emaxs[i];
261                 }
262                 else
263                 {
264                         corners[1][i] = emins[i];
265                         corners[0][i] = emaxs[i];
266                 }
267         }
268         dist = DotProduct (plane->normal, corners[0]) - plane->dist;
269         dist2 = DotProduct (plane->normal, corners[1]) - plane->dist;
270         sides = 0;
271         if (dist1 >= 0)
272                 sides = 1;
273         if (dist2 < 0)
274                 sides |= 2;
275
276 #endif
277
278         sides = 0;
279         if (dist1 >= p->dist)
280                 sides = 1;
281         if (dist2 < p->dist)
282                 sides |= 2;
283
284 #ifdef PARANOID
285 if (sides == 0)
286         Sys_Error ("BoxOnPlaneSide: sides==0");
287 #endif
288
289         return sides;
290 }
291
292 #endif
293 */
294
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));}
303
304 void BoxOnPlaneSideClassify(mplane_t *p)
305 {
306         if (p->normal[2] < 0) // 4
307         {
308                 if (p->normal[1] < 0) // 2
309                 {
310                         if (p->normal[0] < 0) // 1
311                                 p->BoxOnPlaneSideFunc = BoxOnPlaneSide7;
312                         else
313                                 p->BoxOnPlaneSideFunc = BoxOnPlaneSide6;
314                 }
315                 else
316                 {
317                         if (p->normal[0] < 0) // 1
318                                 p->BoxOnPlaneSideFunc = BoxOnPlaneSide5;
319                         else
320                                 p->BoxOnPlaneSideFunc = BoxOnPlaneSide4;
321                 }
322         }
323         else
324         {
325                 if (p->normal[1] < 0) // 2
326                 {
327                         if (p->normal[0] < 0) // 1
328                                 p->BoxOnPlaneSideFunc = BoxOnPlaneSide3;
329                         else
330                                 p->BoxOnPlaneSideFunc = BoxOnPlaneSide2;
331                 }
332                 else
333                 {
334                         if (p->normal[0] < 0) // 1
335                                 p->BoxOnPlaneSideFunc = BoxOnPlaneSide1;
336                         else
337                                 p->BoxOnPlaneSideFunc = BoxOnPlaneSide0;
338                 }
339         }
340 }
341
342 void AngleVectors (vec3_t angles, vec3_t forward, vec3_t right, vec3_t up)
343 {
344         float           angle;
345         float           sr, sp, sy, cr, cp, cy;
346         
347         angle = angles[YAW] * (M_PI*2 / 360);
348         sy = sin(angle);
349         cy = cos(angle);
350         angle = angles[PITCH] * (M_PI*2 / 360);
351         sp = sin(angle);
352         cp = cos(angle);
353         angle = angles[ROLL] * (M_PI*2 / 360);
354         sr = sin(angle);
355         cr = cos(angle);
356
357         forward[0] = cp*cy;
358         forward[1] = cp*sy;
359         forward[2] = -sp;
360         right[0] = (-1*sr*sp*cy+-1*cr*-sy);
361         right[1] = (-1*sr*sp*sy+-1*cr*cy);
362         right[2] = -1*sr*cp;
363         up[0] = (cr*sp*cy+-sr*-sy);
364         up[1] = (cr*sp*sy+-sr*cy);
365         up[2] = cr*cp;
366 }
367
368 int VectorCompare (vec3_t v1, vec3_t v2)
369 {
370         int             i;
371         
372         for (i=0 ; i<3 ; i++)
373                 if (v1[i] != v2[i])
374                         return 0;
375                         
376         return 1;
377 }
378
379 void VectorMA (vec3_t veca, float scale, vec3_t vecb, vec3_t vecc)
380 {
381         vecc[0] = veca[0] + scale*vecb[0];
382         vecc[1] = veca[1] + scale*vecb[1];
383         vecc[2] = veca[2] + scale*vecb[2];
384 }
385
386
387 vec_t _DotProduct (vec3_t v1, vec3_t v2)
388 {
389         return v1[0]*v2[0] + v1[1]*v2[1] + v1[2]*v2[2];
390 }
391
392 void _VectorSubtract (vec3_t veca, vec3_t vecb, vec3_t out)
393 {
394         out[0] = veca[0]-vecb[0];
395         out[1] = veca[1]-vecb[1];
396         out[2] = veca[2]-vecb[2];
397 }
398
399 void _VectorAdd (vec3_t veca, vec3_t vecb, vec3_t out)
400 {
401         out[0] = veca[0]+vecb[0];
402         out[1] = veca[1]+vecb[1];
403         out[2] = veca[2]+vecb[2];
404 }
405
406 void _VectorCopy (vec3_t in, vec3_t out)
407 {
408         out[0] = in[0];
409         out[1] = in[1];
410         out[2] = in[2];
411 }
412
413 // LordHavoc: changed CrossProduct to a #define
414 /*
415 void CrossProduct (vec3_t v1, vec3_t v2, vec3_t cross)
416 {
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];
420 }
421 */
422
423 double sqrt(double x);
424
425 vec_t Length(vec3_t v)
426 {
427         int             i;
428         float   length;
429         
430         length = 0;
431         for (i=0 ; i< 3 ; i++)
432                 length += v[i]*v[i];
433         length = sqrt (length);         // FIXME
434
435         return length;
436 }
437
438 // LordHavoc: renamed these to Length, and made the normal ones #define
439 float VectorNormalizeLength (vec3_t v)
440 {
441         float   length, ilength;
442
443         length = v[0]*v[0] + v[1]*v[1] + v[2]*v[2];
444         length = sqrt (length);         // FIXME
445
446         if (length)
447         {
448                 ilength = 1/length;
449                 v[0] *= ilength;
450                 v[1] *= ilength;
451                 v[2] *= ilength;
452         }
453                 
454         return length;
455
456 }
457
458 float VectorNormalizeLength2 (vec3_t v, vec3_t dest) // LordHavoc: added to allow copying while doing the calculation...
459 {
460         float   length, ilength;
461
462         length = v[0]*v[0] + v[1]*v[1] + v[2]*v[2];
463         length = sqrt (length);         // FIXME
464
465         if (length)
466         {
467                 ilength = 1/length;
468                 dest[0] = v[0] * ilength;
469                 dest[1] = v[1] * ilength;
470                 dest[2] = v[2] * ilength;
471         }
472         else
473                 dest[0] = dest[1] = dest[2] = 0;
474                 
475         return length;
476
477 }
478
479 void VectorInverse (vec3_t v)
480 {
481         v[0] = -v[0];
482         v[1] = -v[1];
483         v[2] = -v[2];
484 }
485
486 void VectorScale (vec3_t in, vec_t scale, vec3_t out)
487 {
488         out[0] = in[0]*scale;
489         out[1] = in[1]*scale;
490         out[2] = in[2]*scale;
491 }
492
493
494 int Q_log2(int val)
495 {
496         int answer=0;
497         while (val>>=1)
498                 answer++;
499         return answer;
500 }
501
502
503 /*
504 ================
505 R_ConcatRotations
506 ================
507 */
508 void R_ConcatRotations (float in1[3][3], float in2[3][3], float out[3][3])
509 {
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];
519 }
520
521
522 /*
523 ================
524 R_ConcatTransforms
525 ================
526 */
527 void R_ConcatTransforms (float in1[3][4], float in2[3][4], float out[3][4])
528 {
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];
541 }
542
543
544 /*
545 ===================
546 FloorDivMod
547
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.
551 ====================
552 */
553
554 void FloorDivMod (double numer, double denom, int *quotient,
555                 int *rem)
556 {
557         int             q, r;
558         double  x;
559
560 #ifndef PARANOID
561         if (denom <= 0.0)
562                 Sys_Error ("FloorDivMod: bad denominator %d\n", denom);
563
564 //      if ((floor(numer) != numer) || (floor(denom) != denom))
565 //              Sys_Error ("FloorDivMod: non-integer numer or denom %f %f\n",
566 //                              numer, denom);
567 #endif
568
569         if (numer >= 0.0)
570         {
571
572                 x = floor(numer / denom);
573                 q = (int)x;
574                 r = (int)floor(numer - (x * denom));
575         }
576         else
577         {
578         //
579         // perform operations with positive values, and fix mod to make floor-based
580         //
581                 x = floor(-numer / denom);
582                 q = -(int)x;
583                 r = (int)floor(-numer - (x * denom));
584                 if (r != 0)
585                 {
586                         q--;
587                         r = (int)denom - r;
588                 }
589         }
590
591         *quotient = q;
592         *rem = r;
593 }
594
595
596 /*
597 ===================
598 GreatestCommonDivisor
599 ====================
600 */
601 int GreatestCommonDivisor (int i1, int i2)
602 {
603         if (i1 > i2)
604         {
605                 if (i2 == 0)
606                         return (i1);
607                 return GreatestCommonDivisor (i2, i1 % i2);
608         }
609         else
610         {
611                 if (i1 == 0)
612                         return (i2);
613                 return GreatestCommonDivisor (i1, i2 % i1);
614         }
615 }
616
617
618 #if     !id386
619
620 // TODO: move to nonintel.c
621
622 /*
623 ===================
624 Invert24To16
625
626 Inverts an 8.24 value to a 16.16 value
627 ====================
628 */
629
630 fixed16_t Invert24To16(fixed16_t val)
631 {
632         if (val < 256)
633                 return (0xFFFFFFFF);
634
635         return (fixed16_t)
636                         (((double)0x10000 * (double)0x1000000 / (double)val) + 0.5);
637 }
638
639 #endif