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