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
24 #include "mathlib.h"
25 #include "model.h"
26 #include "sys.h"
27
28 #ifdef SERVERONLY
29 #include "qwsvdef.h"
30 #else
31 #include "quakedef.h"
32 #endif
33
34 vec3_t vec3_origin = { 0, 0, 0 };
35 int nanmask = 255 << 23;
36
37 /*-----------------------------------------------------------------*/
38
39 #define DEG2RAD( a ) ( a * M_PI ) / 180.0F
40
41 void
ProjectPointOnPlane(vec3_t dst,const vec3_t p,const vec3_t normal)42 ProjectPointOnPlane(vec3_t dst, const vec3_t p, const vec3_t normal)
43 {
44 vec3_t n;
45 float inv_denom = 1.0F / DotProduct(normal, normal);
46 float d = DotProduct(normal, p) * inv_denom;
47
48 n[0] = normal[0] * inv_denom;
49 n[1] = normal[1] * inv_denom;
50 n[2] = normal[2] * inv_denom;
51
52 dst[0] = p[0] - d * n[0];
53 dst[1] = p[1] - d * n[1];
54 dst[2] = p[2] - d * n[2];
55 }
56
57 /*
58 * assumes "src" is normalized
59 */
60 void
PerpendicularVector(vec3_t dst,const vec3_t src)61 PerpendicularVector(vec3_t dst, const vec3_t src)
62 {
63 int pos;
64 int i;
65 float minelem = 1.0F;
66 vec3_t tempvec;
67
68 /*
69 ** find the smallest magnitude axially aligned vector
70 */
71 for (pos = 0, i = 0; i < 3; i++) {
72 if (fabs(src[i]) < minelem) {
73 pos = i;
74 minelem = fabs(src[i]);
75 }
76 }
77 tempvec[0] = tempvec[1] = tempvec[2] = 0.0F;
78 tempvec[pos] = 1.0F;
79
80 /*
81 ** project the point onto the plane defined by src
82 */
83 ProjectPointOnPlane(dst, tempvec, src);
84
85 /*
86 ** normalize the result
87 */
88 VectorNormalize(dst);
89 }
90
91
92 void
RotatePointAroundVector(vec3_t dst,const vec3_t dir,const vec3_t point,float degrees)93 RotatePointAroundVector(vec3_t dst, const vec3_t dir, const vec3_t point,
94 float degrees)
95 {
96 float m[3][3];
97 float im[3][3];
98 float zrot[3][3];
99 float tmpmat[3][3];
100 float rot[3][3];
101 int i;
102 vec3_t vr, vup, vf;
103
104 vf[0] = dir[0];
105 vf[1] = dir[1];
106 vf[2] = dir[2];
107
108 PerpendicularVector(vr, dir);
109 CrossProduct(vr, vf, vup);
110
111 m[0][0] = vr[0];
112 m[1][0] = vr[1];
113 m[2][0] = vr[2];
114
115 m[0][1] = vup[0];
116 m[1][1] = vup[1];
117 m[2][1] = vup[2];
118
119 m[0][2] = vf[0];
120 m[1][2] = vf[1];
121 m[2][2] = vf[2];
122
123 memcpy(im, m, sizeof(im));
124
125 im[0][1] = m[1][0];
126 im[0][2] = m[2][0];
127 im[1][0] = m[0][1];
128 im[1][2] = m[2][1];
129 im[2][0] = m[0][2];
130 im[2][1] = m[1][2];
131
132 memset(zrot, 0, sizeof(zrot));
133 zrot[0][0] = zrot[1][1] = zrot[2][2] = 1.0F;
134
135 zrot[0][0] = cos(DEG2RAD(degrees));
136 zrot[0][1] = sin(DEG2RAD(degrees));
137 zrot[1][0] = -sin(DEG2RAD(degrees));
138 zrot[1][1] = cos(DEG2RAD(degrees));
139
140 R_ConcatRotations(m, zrot, tmpmat);
141 R_ConcatRotations(tmpmat, im, rot);
142
143 for (i = 0; i < 3; i++) {
144 dst[i] =
145 rot[i][0] * point[0] + rot[i][1] * point[1] +
146 rot[i][2] * point[2];
147 }
148 }
149
150 /*-----------------------------------------------------------------*/
151
152 float
anglemod(float a)153 anglemod(float a)
154 {
155 a = (360.0 / 65536) * ((int)(a * (65536 / 360.0)) & 65535);
156 return a;
157 }
158
159 int
SignbitsForPlane(const mplane_t * plane)160 SignbitsForPlane(const mplane_t *plane)
161 {
162 int i;
163 /* for fast box on planeside test */
164 int bits = 0;
165 for (i = 0; i < 3; i++)
166 {
167 if (plane->normal[i] < 0)
168 bits |= 1 << i;
169 }
170 return bits;
171 }
172
173 /*
174 ==================
175 BOPS_Error
176
177 Split out like this for ASM to call.
178 ==================
179 */
BOPS_Error(void)180 void BOPS_Error(void)
181 {
182 Sys_Error("%s: Bad signbits", __func__);
183 }
184
185 /*
186 ==================
187 BoxOnPlaneSide
188
189 Returns PSIDE_FRONT, PSIDE_BACK, or PSIDE_BOTH (PSIDE_FRONT | PSIDE_BACK)
190 ==================
191 */
192 int
BoxOnPlaneSide(const vec3_t mins,const vec3_t maxs,const mplane_t * p)193 BoxOnPlaneSide(const vec3_t mins, const vec3_t maxs, const mplane_t *p)
194 {
195 float dist1 = 0.0f, dist2 = 0.0f;
196 int sides;
197
198 /* general case */
199 switch (p->signbits)
200 {
201 case 0:
202 dist1 =
203 p->normal[0] * maxs[0] + p->normal[1] * maxs[1] +
204 p->normal[2] * maxs[2];
205 dist2 =
206 p->normal[0] * mins[0] + p->normal[1] * mins[1] +
207 p->normal[2] * mins[2];
208 break;
209 case 1:
210 dist1 =
211 p->normal[0] * mins[0] + p->normal[1] * maxs[1] +
212 p->normal[2] * maxs[2];
213 dist2 =
214 p->normal[0] * maxs[0] + p->normal[1] * mins[1] +
215 p->normal[2] * mins[2];
216 break;
217 case 2:
218 dist1 =
219 p->normal[0] * maxs[0] + p->normal[1] * mins[1] +
220 p->normal[2] * maxs[2];
221 dist2 =
222 p->normal[0] * mins[0] + p->normal[1] * maxs[1] +
223 p->normal[2] * mins[2];
224 break;
225 case 3:
226 dist1 =
227 p->normal[0] * mins[0] + p->normal[1] * mins[1] +
228 p->normal[2] * maxs[2];
229 dist2 =
230 p->normal[0] * maxs[0] + p->normal[1] * maxs[1] +
231 p->normal[2] * mins[2];
232 break;
233 case 4:
234 dist1 =
235 p->normal[0] * maxs[0] + p->normal[1] * maxs[1] +
236 p->normal[2] * mins[2];
237 dist2 =
238 p->normal[0] * mins[0] + p->normal[1] * mins[1] +
239 p->normal[2] * maxs[2];
240 break;
241 case 5:
242 dist1 =
243 p->normal[0] * mins[0] + p->normal[1] * maxs[1] +
244 p->normal[2] * mins[2];
245 dist2 =
246 p->normal[0] * maxs[0] + p->normal[1] * mins[1] +
247 p->normal[2] * maxs[2];
248 break;
249 case 6:
250 dist1 =
251 p->normal[0] * maxs[0] + p->normal[1] * mins[1] +
252 p->normal[2] * mins[2];
253 dist2 =
254 p->normal[0] * mins[0] + p->normal[1] * maxs[1] +
255 p->normal[2] * maxs[2];
256 break;
257 case 7:
258 dist1 =
259 p->normal[0] * mins[0] + p->normal[1] * mins[1] +
260 p->normal[2] * mins[2];
261 dist2 =
262 p->normal[0] * maxs[0] + p->normal[1] * maxs[1] +
263 p->normal[2] * maxs[2];
264 break;
265 default:
266 BOPS_Error();
267 break;
268 }
269
270 sides = 0;
271 if (dist1 >= p->dist)
272 sides = PSIDE_FRONT;
273 if (dist2 < p->dist)
274 sides |= PSIDE_BACK;
275
276 #ifdef PARANOID
277 if (sides == 0)
278 Sys_Error("%s: sides == 0", __func__);
279 #endif
280
281 return sides;
282 }
283
284 void
AngleVectors(const vec3_t angles,vec3_t forward,vec3_t right,vec3_t up)285 AngleVectors(const vec3_t angles, vec3_t forward, vec3_t right, vec3_t up)
286 {
287 float sr, sp, cr, cp;
288
289 float angle = angles[YAW] * (M_PI * 2 / 360);
290 float sy = sin(angle);
291 float cy = cos(angle);
292 angle = angles[PITCH] * (M_PI * 2 / 360);
293 sp = sin(angle);
294 cp = cos(angle);
295 angle = angles[ROLL] * (M_PI * 2 / 360);
296 sr = sin(angle);
297 cr = cos(angle);
298
299 forward[0] = cp * cy;
300 forward[1] = cp * sy;
301 forward[2] = -sp;
302 right[0] = (-1 * sr * sp * cy + -1 * cr * -sy);
303 right[1] = (-1 * sr * sp * sy + -1 * cr * cy);
304 right[2] = -1 * sr * cp;
305 up[0] = (cr * sp * cy + -sr * -sy);
306 up[1] = (cr * sp * sy + -sr * cy);
307 up[2] = cr * cp;
308 }
309
310 int
VectorCompare(vec3_t v1,vec3_t v2)311 VectorCompare(vec3_t v1, vec3_t v2)
312 {
313 int i;
314
315 for (i = 0; i < 3; i++)
316 if (v1[i] != v2[i])
317 return 0;
318
319 return 1;
320 }
321
VectorMA(const vec3_t veca,const float scale,const vec3_t vecb,vec3_t vecc)322 void VectorMA(const vec3_t veca, const float scale, const vec3_t vecb, vec3_t vecc)
323 {
324 vecc[0] = veca[0] + scale * vecb[0];
325 vecc[1] = veca[1] + scale * vecb[1];
326 vecc[2] = veca[2] + scale * vecb[2];
327 }
328
329
_DotProduct(vec3_t v1,vec3_t v2)330 vec_t _DotProduct(vec3_t v1, vec3_t v2)
331 {
332 return v1[0] * v2[0] + v1[1] * v2[1] + v1[2] * v2[2];
333 }
334
_VectorSubtract(vec3_t veca,vec3_t vecb,vec3_t out)335 void _VectorSubtract(vec3_t veca, vec3_t vecb, vec3_t out)
336 {
337 out[0] = veca[0] - vecb[0];
338 out[1] = veca[1] - vecb[1];
339 out[2] = veca[2] - vecb[2];
340 }
341
_VectorAdd(vec3_t veca,vec3_t vecb,vec3_t out)342 void _VectorAdd(vec3_t veca, vec3_t vecb, vec3_t out)
343 {
344 out[0] = veca[0] + vecb[0];
345 out[1] = veca[1] + vecb[1];
346 out[2] = veca[2] + vecb[2];
347 }
348
_VectorCopy(vec3_t in,vec3_t out)349 void _VectorCopy(vec3_t in, vec3_t out)
350 {
351 out[0] = in[0];
352 out[1] = in[1];
353 out[2] = in[2];
354 }
355
CrossProduct(const vec3_t v1,const vec3_t v2,vec3_t cross)356 void CrossProduct(const vec3_t v1, const vec3_t v2, vec3_t cross)
357 {
358 #if defined(__ARM_NEON__) && !defined(__APPLE__)
359 asm volatile (
360 "flds s3, [%0] \n\t" //d1[1]={x0}
361 "add %0, %0, #4 \n\t" //
362 "vld1.32 {d0}, [%0] \n\t" //d0={y0,z0}
363 "vmov.f32 s2, s1 \n\t" //d1[0]={z0}
364
365 "flds s5, [%1] \n\t" //d2[1]={x1}
366 "add %1, %1, #4 \n\t" //
367 "vld1.32 {d3}, [%1] \n\t" //d3={y1,z1}
368 "vmov.f32 s4, s7 \n\t" //d2[0]=d3[1]
369
370 "vmul.f32 d4, d0, d2 \n\t" //d4=d0*d2
371 "vmls.f32 d4, d1, d3 \n\t" //d4-=d1*d3
372
373 "vmul.f32 d5, d3, d1[1] \n\t" //d5=d3*d1[1]
374 "vmls.f32 d5, d0, d2[1] \n\t" //d5-=d0*d2[1]
375
376 "vst1.32 d4, [%2] \n\t" //
377 "fsts s10, [%2, #8] \n\t" //
378
379 : "+&r"(v1), "+&r"(v2), "+&r"(cross):
380 : "d0", "d1", "d2", "d3", "d4", "d5", "memory"
381 );
382 #else
383 cross[0] = v1[1] * v2[2] - v1[2] * v2[1];
384 cross[1] = v1[2] * v2[0] - v1[0] * v2[2];
385 cross[2] = v1[0] * v2[1] - v1[1] * v2[0];
386 #endif
387 }
388
389 double sqrt(double x);
390
Length(vec3_t v)391 vec_t Length(vec3_t v)
392 {
393 int i;
394 float length;
395
396 length = 0;
397 for (i = 0; i < 3; i++)
398 length += v[i] * v[i];
399 length = sqrt(length); // FIXME
400
401 return length;
402 }
403
VectorNormalize(vec3_t v)404 float VectorNormalize(vec3_t v)
405 {
406 float length = v[0] * v[0] + v[1] * v[1] + v[2] * v[2];
407 length = sqrt(length); // FIXME
408
409 if (length)
410 {
411 float ilength = 1 / length;
412 v[0] *= ilength;
413 v[1] *= ilength;
414 v[2] *= ilength;
415 }
416
417 return length;
418 }
419
VectorInverse(vec3_t v)420 void VectorInverse(vec3_t v)
421 {
422 v[0] = -v[0];
423 v[1] = -v[1];
424 v[2] = -v[2];
425 }
426
VectorScale(const vec3_t in,const vec_t scale,vec3_t out)427 void VectorScale(const vec3_t in, const vec_t scale, vec3_t out)
428 {
429 out[0] = in[0] * scale;
430 out[1] = in[1] * scale;
431 out[2] = in[2] * scale;
432 }
433
434
Q_log2(int val)435 int Q_log2(int val)
436 {
437 int answer = 0;
438
439 while ((val >>= 1) != 0)
440 answer++;
441 return answer;
442 }
443
444
445 /*
446 ================
447 R_ConcatRotations
448 ================
449 */
R_ConcatRotations(float in1[3][3],float in2[3][3],float out[3][3])450 void R_ConcatRotations(float in1[3][3], float in2[3][3], float out[3][3])
451 {
452 out[0][0] = in1[0][0] * in2[0][0] + in1[0][1] * in2[1][0] +
453 in1[0][2] * in2[2][0];
454 out[0][1] = in1[0][0] * in2[0][1] + in1[0][1] * in2[1][1] +
455 in1[0][2] * in2[2][1];
456 out[0][2] = in1[0][0] * in2[0][2] + in1[0][1] * in2[1][2] +
457 in1[0][2] * in2[2][2];
458 out[1][0] = in1[1][0] * in2[0][0] + in1[1][1] * in2[1][0] +
459 in1[1][2] * in2[2][0];
460 out[1][1] = in1[1][0] * in2[0][1] + in1[1][1] * in2[1][1] +
461 in1[1][2] * in2[2][1];
462 out[1][2] = in1[1][0] * in2[0][2] + in1[1][1] * in2[1][2] +
463 in1[1][2] * in2[2][2];
464 out[2][0] = in1[2][0] * in2[0][0] + in1[2][1] * in2[1][0] +
465 in1[2][2] * in2[2][0];
466 out[2][1] = in1[2][0] * in2[0][1] + in1[2][1] * in2[1][1] +
467 in1[2][2] * in2[2][1];
468 out[2][2] = in1[2][0] * in2[0][2] + in1[2][1] * in2[1][2] +
469 in1[2][2] * in2[2][2];
470 }
471
472
473 /*
474 ================
475 R_ConcatTransforms
476 ================
477 */
478 void
R_ConcatTransforms(float in1[3][4],float in2[3][4],float out[3][4])479 R_ConcatTransforms(float in1[3][4], float in2[3][4], float out[3][4])
480 {
481 out[0][0] = in1[0][0] * in2[0][0] + in1[0][1] * in2[1][0] +
482 in1[0][2] * in2[2][0];
483 out[0][1] = in1[0][0] * in2[0][1] + in1[0][1] * in2[1][1] +
484 in1[0][2] * in2[2][1];
485 out[0][2] = in1[0][0] * in2[0][2] + in1[0][1] * in2[1][2] +
486 in1[0][2] * in2[2][2];
487 out[0][3] = in1[0][0] * in2[0][3] + in1[0][1] * in2[1][3] +
488 in1[0][2] * in2[2][3] + in1[0][3];
489 out[1][0] = in1[1][0] * in2[0][0] + in1[1][1] * in2[1][0] +
490 in1[1][2] * in2[2][0];
491 out[1][1] = in1[1][0] * in2[0][1] + in1[1][1] * in2[1][1] +
492 in1[1][2] * in2[2][1];
493 out[1][2] = in1[1][0] * in2[0][2] + in1[1][1] * in2[1][2] +
494 in1[1][2] * in2[2][2];
495 out[1][3] = in1[1][0] * in2[0][3] + in1[1][1] * in2[1][3] +
496 in1[1][2] * in2[2][3] + in1[1][3];
497 out[2][0] = in1[2][0] * in2[0][0] + in1[2][1] * in2[1][0] +
498 in1[2][2] * in2[2][0];
499 out[2][1] = in1[2][0] * in2[0][1] + in1[2][1] * in2[1][1] +
500 in1[2][2] * in2[2][1];
501 out[2][2] = in1[2][0] * in2[0][2] + in1[2][1] * in2[1][2] +
502 in1[2][2] * in2[2][2];
503 out[2][3] = in1[2][0] * in2[0][3] + in1[2][1] * in2[1][3] +
504 in1[2][2] * in2[2][3] + in1[2][3];
505 }
506
507
508 /*
509 ===================
510 FloorDivMod
511
512 Returns mathematically correct (floor-based) quotient and remainder for
513 numer and denom, both of which should contain no fractional part. The
514 quotient must fit in 32 bits.
515 ====================
516 */
517
518 void
FloorDivMod(double numer,double denom,int * quotient,int * rem)519 FloorDivMod(double numer, double denom, int *quotient, int *rem)
520 {
521 int q, r;
522 double x;
523
524 #ifndef PARANOID
525 if (denom <= 0.0)
526 Sys_Error("%s: bad denominator %lf", __func__, denom);
527
528 #if 0
529 if ((floor(numer) != numer) || (floor(denom) != denom))
530 Sys_Error ("%s: non-integer numer or denom %f %f", __func__
531 numer, denom);
532 #endif
533 #endif
534
535 if (numer >= 0.0)
536 {
537 x = floor(numer / denom);
538 q = (int)x;
539 r = (int)floor(numer - (x * denom));
540 }
541 else
542 {
543 /* perform operations with positive values,
544 * and fix mod to make floor-based */
545 x = floor(-numer / denom);
546 q = -(int)x;
547 r = (int)floor(-numer - (x * denom));
548 if (r != 0)
549 {
550 q--;
551 r = (int)denom - r;
552 }
553 }
554
555 *quotient = q;
556 *rem = r;
557 }
558
559
560 /*
561 ===================
562 GreatestCommonDivisor
563 ====================
564 */
565 int
GreatestCommonDivisor(int i1,int i2)566 GreatestCommonDivisor(int i1, int i2)
567 {
568 if (i1 > i2)
569 {
570 if (i2 == 0)
571 return (i1);
572 return GreatestCommonDivisor(i2, i1 % i2);
573 }
574 else
575 {
576 if (i1 == 0)
577 return (i2);
578 return GreatestCommonDivisor(i1, i2 % i1);
579 }
580 }
581
582
583 /*
584 ===================
585 Invert24To16
586
587 Inverts an 8.24 value to a 16.16 value
588 ====================
589 */
590
Invert24To16(fixed16_t val)591 fixed16_t Invert24To16(fixed16_t val)
592 {
593 if (val < 256)
594 return (0xFFFFFFFF);
595
596 return (fixed16_t)
597 (((double)0x10000 * (double)0x1000000 / (double)val) + 0.5);
598 }
599