1 /*
2 Copyright (C) 1997-2001 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 //
21 // mathlib.c
22 //
23
24 #include "shared.h"
25
26 vec2_t vec2Origin = {
27 0, 0
28 };
29
30 vec3_t vec3Origin = {
31 0, 0, 0
32 };
33
34 vec4_t vec4Origin = {
35 0, 0, 0, 0
36 };
37
38 /*
39 =============================================================================
40
41 MATHLIB
42
43 =============================================================================
44 */
45
46 vec3_t m_byteDirs[NUMVERTEXNORMALS] = {
47 {-0.525731f, 0.000000f, 0.850651f}, {-0.442863f, 0.238856f, 0.864188f}, {-0.295242f, 0.000000f, 0.955423f},
48 {-0.309017f, 0.500000f, 0.809017f}, {-0.162460f, 0.262866f, 0.951056f}, {0.000000f, 0.000000f, 1.000000f},
49 {0.000000f, 0.850651f, 0.525731f}, {-0.147621f, 0.716567f, 0.681718f}, {0.147621f, 0.716567f, 0.681718f},
50 {0.000000f, 0.525731f, 0.850651f}, {0.309017f, 0.500000f, 0.809017f}, {0.525731f, 0.000000f, 0.850651f},
51 {0.295242f, 0.000000f, 0.955423f}, {0.442863f, 0.238856f, 0.864188f}, {0.162460f, 0.262866f, 0.951056f},
52 {-0.681718f, 0.147621f, 0.716567f}, {-0.809017f, 0.309017f, 0.500000f}, {-0.587785f, 0.425325f, 0.688191f},
53 {-0.850651f, 0.525731f, 0.000000f}, {-0.864188f, 0.442863f, 0.238856f}, {-0.716567f, 0.681718f, 0.147621f},
54 {-0.688191f, 0.587785f, 0.425325f}, {-0.500000f, 0.809017f, 0.309017f}, {-0.238856f, 0.864188f, 0.442863f},
55 {-0.425325f, 0.688191f, 0.587785f}, {-0.716567f, 0.681718f, -0.147621f}, {-0.500000f, 0.809017f, -0.309017f},
56 {-0.525731f, 0.850651f, 0.000000f}, {0.000000f, 0.850651f, -0.525731f}, {-0.238856f, 0.864188f, -0.442863f},
57 {0.000000f, 0.955423f, -0.295242f}, {-0.262866f, 0.951056f, -0.162460f}, {0.000000f, 1.000000f, 0.000000f},
58 {0.000000f, 0.955423f, 0.295242f}, {-0.262866f, 0.951056f, 0.162460f}, {0.238856f, 0.864188f, 0.442863f},
59 {0.262866f, 0.951056f, 0.162460f}, {0.500000f, 0.809017f, 0.309017f}, {0.238856f, 0.864188f, -0.442863f},
60 {0.262866f, 0.951056f, -0.162460f}, {0.500000f, 0.809017f, -0.309017f}, {0.850651f, 0.525731f, 0.000000f},
61 {0.716567f, 0.681718f, 0.147621f}, {0.716567f, 0.681718f, -0.147621f}, {0.525731f, 0.850651f, 0.000000f},
62 {0.425325f, 0.688191f, 0.587785f}, {0.864188f, 0.442863f, 0.238856f}, {0.688191f, 0.587785f, 0.425325f},
63 {0.809017f, 0.309017f, 0.500000f}, {0.681718f, 0.147621f, 0.716567f}, {0.587785f, 0.425325f, 0.688191f},
64 {0.955423f, 0.295242f, 0.000000f}, {1.000000f, 0.000000f, 0.000000f}, {0.951056f, 0.162460f, 0.262866f},
65 {0.850651f, -0.525731f, 0.000000f}, {0.955423f, -0.295242f, 0.000000f}, {0.864188f, -0.442863f, 0.238856f},
66 {0.951056f, -0.162460f, 0.262866f}, {0.809017f, -0.309017f, 0.500000f}, {0.681718f, -0.147621f, 0.716567f},
67 {0.850651f, 0.000000f, 0.525731f}, {0.864188f, 0.442863f, -0.238856f}, {0.809017f, 0.309017f, -0.500000f},
68 {0.951056f, 0.162460f, -0.262866f}, {0.525731f, 0.000000f, -0.850651f}, {0.681718f, 0.147621f, -0.716567f},
69 {0.681718f, -0.147621f, -0.716567f}, {0.850651f, 0.000000f, -0.525731f}, {0.809017f, -0.309017f, -0.500000f},
70 {0.864188f, -0.442863f, -0.238856f}, {0.951056f, -0.162460f, -0.262866f}, {0.147621f, 0.716567f, -0.681718f},
71 {0.309017f, 0.500000f, -0.809017f}, {0.425325f, 0.688191f, -0.587785f}, {0.442863f, 0.238856f, -0.864188f},
72 {0.587785f, 0.425325f, -0.688191f}, {0.688191f, 0.587785f, -0.425325f}, {-0.147621f, 0.716567f, -0.681718f},
73 {-0.309017f, 0.500000f, -0.809017f}, {0.000000f, 0.525731f, -0.850651f}, {-0.525731f, 0.000000f, -0.850651f},
74 {-0.442863f, 0.238856f, -0.864188f}, {-0.295242f, 0.000000f, -0.955423f}, {-0.162460f, 0.262866f, -0.951056f},
75 {0.000000f, 0.000000f, -1.000000f}, {0.295242f, 0.000000f, -0.955423f}, {0.162460f, 0.262866f, -0.951056f},
76 {-0.442863f, -0.238856f, -0.864188f}, {-0.309017f, -0.500000f, -0.809017f}, {-0.162460f, -0.262866f, -0.951056f},
77 {0.000000f, -0.850651f, -0.525731f}, {-0.147621f, -0.716567f, -0.681718f}, {0.147621f, -0.716567f, -0.681718f},
78 {0.000000f, -0.525731f, -0.850651f}, {0.309017f, -0.500000f, -0.809017f}, {0.442863f, -0.238856f, -0.864188f},
79 {0.162460f, -0.262866f, -0.951056f}, {0.238856f, -0.864188f, -0.442863f}, {0.500000f, -0.809017f, -0.309017f},
80 {0.425325f, -0.688191f, -0.587785f}, {0.716567f, -0.681718f, -0.147621f}, {0.688191f, -0.587785f, -0.425325f},
81 {0.587785f, -0.425325f, -0.688191f}, {0.000000f, -0.955423f, -0.295242f}, {0.000000f, -1.000000f, 0.000000f},
82 {0.262866f, -0.951056f, -0.162460f}, {0.000000f, -0.850651f, 0.525731f}, {0.000000f, -0.955423f, 0.295242f},
83 {0.238856f, -0.864188f, 0.442863f}, {0.262866f, -0.951056f, 0.162460f}, {0.500000f, -0.809017f, 0.309017f},
84 {0.716567f, -0.681718f, 0.147621f}, {0.525731f, -0.850651f, 0.000000f}, {-0.238856f, -0.864188f, -0.442863f},
85 {-0.500000f, -0.809017f, -0.309017f}, {-0.262866f, -0.951056f, -0.162460f}, {-0.850651f, -0.525731f, 0.000000f},
86 {-0.716567f, -0.681718f, -0.147621f}, {-0.716567f, -0.681718f, 0.147621f}, {-0.525731f, -0.850651f, 0.000000f},
87 {-0.500000f, -0.809017f, 0.309017f}, {-0.238856f, -0.864188f, 0.442863f}, {-0.262866f, -0.951056f, 0.162460f},
88 {-0.864188f, -0.442863f, 0.238856f}, {-0.809017f, -0.309017f, 0.500000f}, {-0.688191f, -0.587785f, 0.425325f},
89 {-0.681718f, -0.147621f, 0.716567f}, {-0.442863f, -0.238856f, 0.864188f}, {-0.587785f, -0.425325f, 0.688191f},
90 {-0.309017f, -0.500000f, 0.809017f}, {-0.147621f, -0.716567f, 0.681718f}, {-0.425325f, -0.688191f, 0.587785f},
91 {-0.162460f, -0.262866f, 0.951056f}, {0.442863f, -0.238856f, 0.864188f}, {0.162460f, -0.262866f, 0.951056f},
92 {0.309017f, -0.500000f, 0.809017f}, {0.147621f, -0.716567f, 0.681718f}, {0.000000f, -0.525731f, 0.850651f},
93 {0.425325f, -0.688191f, 0.587785f}, {0.587785f, -0.425325f, 0.688191f}, {0.688191f, -0.587785f, 0.425325f},
94 {-0.955423f, 0.295242f, 0.000000f}, {-0.951056f, 0.162460f, 0.262866f}, {-1.000000f, 0.000000f, 0.000000f},
95 {-0.850651f, 0.000000f, 0.525731f}, {-0.955423f, -0.295242f, 0.000000f}, {-0.951056f, -0.162460f, 0.262866f},
96 {-0.864188f, 0.442863f, -0.238856f}, {-0.951056f, 0.162460f, -0.262866f}, {-0.809017f, 0.309017f, -0.500000f},
97 {-0.864188f, -0.442863f, -0.238856f}, {-0.951056f, -0.162460f, -0.262866f}, {-0.809017f, -0.309017f, -0.500000f},
98 {-0.681718f, 0.147621f, -0.716567f}, {-0.681718f, -0.147621f, -0.716567f}, {-0.850651f, 0.000000f, -0.525731f},
99 {-0.688191f, 0.587785f, -0.425325f}, {-0.587785f, 0.425325f, -0.688191f}, {-0.425325f, 0.688191f, -0.587785f},
100 {-0.425325f, -0.688191f, -0.587785f}, {-0.587785f, -0.425325f, -0.688191f}, {-0.688191f, -0.587785f, -0.425325f}
101 };
102
103 /*
104 =================
105 DirToByte
106
107 This isn't a real cheap function to call!
108 =================
109 */
DirToByte(vec3_t dirVec)110 byte DirToByte (vec3_t dirVec)
111 {
112 byte i, best;
113 float d, bestDot;
114
115 if (!dirVec)
116 return 0;
117
118 best = 0;
119 bestDot = 0;
120 for (i=0 ; i<NUMVERTEXNORMALS ; i++) {
121 d = DotProduct (dirVec, m_byteDirs[i]);
122 if (d > bestDot) {
123 bestDot = d;
124 best = i;
125 }
126 }
127
128 return best;
129 }
130
131
132 /*
133 =================
134 ByteToDir
135 =================
136 */
ByteToDir(byte dirByte,vec3_t dirVec)137 void ByteToDir (byte dirByte, vec3_t dirVec)
138 {
139 if (dirByte >= NUMVERTEXNORMALS) {
140 Vec3Clear (dirVec);
141 return;
142 }
143
144 Vec3Copy (m_byteDirs[dirByte], dirVec);
145 }
146
147 // ===========================================================================
148
149 /*
150 ==============
151 FloatToByte
152 ==============
153 */
FloatToByte(float x)154 byte FloatToByte (float x)
155 {
156 union {
157 float f;
158 uint32 i;
159 } f2i;
160
161 // Shift float to have 8bit fraction at base of number
162 f2i.f = x + 32768.0f;
163 f2i.i &= 0x7FFFFF;
164
165 // Then read as integer and kill float bits...
166 return (byte)min(f2i.i, 255);
167 }
168
169
170 /*
171 ===============
172 ColorNormalizef
173 ===============
174 */
ColorNormalizef(const float * in,float * out)175 float ColorNormalizef (const float *in, float *out)
176 {
177 float f = max (max (in[0], in[1]), in[2]);
178
179 if (f > 1.0f) {
180 f = 1.0f / f;
181 out[0] = in[0] * f;
182 out[1] = in[1] * f;
183 out[2] = in[2] * f;
184 }
185 else {
186 out[0] = in[0];
187 out[1] = in[1];
188 out[2] = in[2];
189 }
190
191 return f;
192 }
193
194
195 /*
196 ===============
197 ColorNormalizeb
198 ===============
199 */
ColorNormalizeb(const float * in,byte * out)200 float ColorNormalizeb (const float *in, byte *out)
201 {
202 float f = max (max (in[0], in[1]), in[2]);
203
204 if (f > 1.0f) {
205 f = 1.0f / f;
206 out[0] = FloatToByte (in[0] * f);
207 out[1] = FloatToByte (in[1] * f);
208 out[2] = FloatToByte (in[2] * f);
209 }
210 else {
211 out[0] = FloatToByte (in[0]);
212 out[1] = FloatToByte (in[1]);
213 out[2] = FloatToByte (in[2]);
214 }
215
216 return f;
217 }
218
219 // ===========================================================================
220
221 /*
222 ===============
223 Q_ftol
224 ===============
225 */
226 #ifdef id386
Q_ftol(float f)227 __declspec_naked long Q_ftol (float f)
228 {
229 static int tmp;
230 __asm {
231 fld dword ptr [esp+4]
232 fistp tmp
233 mov eax, tmp
234 ret
235 }
236 }
237 #endif // id386
238
239
240 /*
241 ===============
242 Q_FastSqrt
243
244 5% margin of error
245 ===============
246 */
247 #ifdef id386
Q_FastSqrt(float value)248 float Q_FastSqrt (float value)
249 {
250 float result;
251 __asm {
252 mov eax, value
253 sub eax, 0x3f800000
254 sar eax, 1
255 add eax, 0x3f800000
256 mov result, eax
257 }
258 return result;
259 }
260 #endif // id386
261
262
263 /*
264 ===============
265 Q_RSqrtf
266
267 1/sqrt, faster but not as precise
268 ===============
269 */
Q_RSqrtf(float number)270 float Q_RSqrtf (float number)
271 {
272 float y;
273
274 if (number == 0.0f)
275 return 0.0f;
276 *((int *)&y) = 0x5f3759df - ((* (int *) &number) >> 1);
277 return y * (1.5f - (number * 0.5f * y * y));
278 }
279
280
281 /*
282 ===============
283 Q_RSqrtd
284
285 1/sqrt, faster but not as precise
286 ===============
287 */
Q_RSqrtd(double number)288 double Q_RSqrtd (double number)
289 {
290 double y;
291
292 if (number == 0.0)
293 return 0.0;
294 *((int *)&y) = 0x5f3759df - ((* (int *) &number) >> 1);
295 return y * (1.5f - (number * 0.5 * y * y));
296 }
297
298
299 /*
300 ===============
301 Q_log2
302 ===============
303 */
Q_log2(int val)304 int Q_log2 (int val)
305 {
306 int answer = 0;
307 while (val >>= 1)
308 answer++;
309 return answer;
310 }
311
312 /*
313 ===============
314 Q_NearestPow
315 ===============
316 */
Q_NearestPow(int * var,qBool oneLess)317 void Q_NearestPow (int *var, qBool oneLess)
318 {
319 int i;
320
321 for (i=1 ; i<*var ; i<<=1) ;
322
323 if (oneLess) {
324 // find the nearest power of two below input value
325 if (i > *var)
326 i >>= 1;
327 }
328
329 *var = i;
330 }
331
332 // ===========================================================================
333
334 /*
335 ====================
336 Q_CalcFovY
337
338 Calculates aspect based on fovX and the screen dimensions
339 ====================
340 */
Q_CalcFovY(float fovX,float width,float height)341 float Q_CalcFovY (float fovX, float width, float height)
342 {
343 if (fovX < 1 || fovX > 179)
344 Com_Printf (PRNT_ERROR, "Bad fov: %f\n", fovX);
345
346 return (float)(atan (height / (width / tan (fovX / 360.0f * M_PI)))) * ((180.0f / M_PI) * 2);
347 }
348
349
350 // ===========================================================================
351
352 /*
353 ===============
354 NormToLatLong
355 ===============
356 */
NormToLatLong(vec3_t normal,byte latLong[2])357 void NormToLatLong (vec3_t normal, byte latLong[2])
358 {
359 if (normal[0] == 0 && normal[1] == 0) {
360 if (normal[2] > 0) {
361 latLong[0] = 0;
362 latLong[1] = 0;
363 }
364 else {
365 latLong[0] = 128;
366 latLong[1] = 0;
367 }
368 }
369 else {
370 int angle;
371
372 angle = (int)(acos (normal[2]) * 255.0 / (M_PI * 2.0f)) & 0xff;
373 latLong[0] = angle;
374 angle = (int)(atan2 (normal[1], normal[0]) * 255.0 / (M_PI * 2.0f)) & 0xff;
375 latLong[1] = angle;
376 }
377 }
378
379
380 /*
381 ===============
382 MakeNormalVectorsf
383 ===============
384 */
MakeNormalVectorsf(vec3_t forward,vec3_t right,vec3_t up)385 void MakeNormalVectorsf (vec3_t forward, vec3_t right, vec3_t up)
386 {
387 float d;
388
389 // This rotate and negate guarantees a vector not colinear with the original
390 Vec3Set (right, forward[2], -forward[0], forward[1]);
391
392 d = DotProduct (right, forward);
393 Vec3MA (right, -d, forward, right);
394 VectorNormalizef (right, right);
395 CrossProduct (right, forward, up);
396 }
397
398
399 /*
400 ===============
401 MakeNormalVectorsd
402 ===============
403 */
MakeNormalVectorsd(dvec3_t forward,dvec3_t right,dvec3_t up)404 void MakeNormalVectorsd (dvec3_t forward, dvec3_t right, dvec3_t up)
405 {
406 double d;
407
408 // This rotate and negate guarantees a vector not colinear with the original
409 Vec3Set (right, forward[2], -forward[0], forward[1]);
410
411 d = DotProduct (right, forward);
412 Vec3MA (right, -d, forward, right);
413 VectorNormalized (right, right);
414 CrossProduct (right, forward, up);
415 }
416
417
418 /*
419 ===============
420 PerpendicularVector
421
422 Assumes "src" is normalized
423 ===============
424 */
PerpendicularVector(vec3_t src,vec3_t dst)425 void PerpendicularVector (vec3_t src, vec3_t dst)
426 {
427 int pos = 5;
428 float minElem = 1.0f;
429 vec3_t tempVec;
430
431 // Find the smallest magnitude axially aligned vector
432 if (fabs(src[0]) < minElem) {
433 pos = 0;
434 minElem = (float)fabs (src[0]);
435 }
436 if (fabs(src[1]) < minElem) {
437 pos = 1;
438 minElem = (float)fabs (src[1]);
439 }
440 if (fabs(src[2]) < minElem) {
441 pos = 2;
442 minElem = (float)fabs (src[2]);
443 }
444
445 assert (pos != 5);
446
447 Vec3Clear (tempVec);
448 tempVec[pos] = 1.0F;
449
450 // Project the point onto the plane defined by src
451 ProjectPointOnPlane (dst, tempVec, src);
452
453 // Normalize the result
454 VectorNormalizef (dst, dst);
455 }
456
457
458 /*
459 ===============
460 RotatePointAroundVector
461 ===============
462 */
RotatePointAroundVector(vec3_t dst,vec3_t dir,vec3_t point,float degrees)463 void RotatePointAroundVector (vec3_t dst, vec3_t dir, vec3_t point, float degrees)
464 {
465 float t0, t1;
466 float c, s;
467 vec3_t vr, vu, vf;
468
469 s = DEG2RAD (degrees);
470 c = (float)cos (s);
471 s = (float)sin (s);
472
473 Vec3Copy (dir, vf);
474 MakeNormalVectorsf (vf, vr, vu);
475
476 t0 = vr[0] * c + vu[0] * -s;
477 t1 = vr[0] * s + vu[0] * c;
478 dst[0] = (t0 * vr[0] + t1 * vu[0] + vf[0] * vf[0]) * point[0]
479 + (t0 * vr[1] + t1 * vu[1] + vf[0] * vf[1]) * point[1]
480 + (t0 * vr[2] + t1 * vu[2] + vf[0] * vf[2]) * point[2];
481
482 t0 = vr[1] * c + vu[1] * -s;
483 t1 = vr[1] * s + vu[1] * c;
484 dst[1] = (t0 * vr[0] + t1 * vu[0] + vf[1] * vf[0]) * point[0]
485 + (t0 * vr[1] + t1 * vu[1] + vf[1] * vf[1]) * point[1]
486 + (t0 * vr[2] + t1 * vu[2] + vf[1] * vf[2]) * point[2];
487
488 t0 = vr[2] * c + vu[2] * -s;
489 t1 = vr[2] * s + vu[2] * c;
490 dst[2] = (t0 * vr[0] + t1 * vu[0] + vf[2] * vf[0]) * point[0]
491 + (t0 * vr[1] + t1 * vu[1] + vf[2] * vf[1]) * point[1]
492 + (t0 * vr[2] + t1 * vu[2] + vf[2] * vf[2]) * point[2];
493 }
494
495
496 /*
497 ===============
498 VectorNormalizef
499 ===============
500 */
VectorNormalizef(vec3_t in,vec3_t out)501 float VectorNormalizef (vec3_t in, vec3_t out)
502 {
503 float length, invLength;
504
505 length = (float)Vec3Length (in);
506
507 if (length) {
508 invLength = 1.0f/length;
509 Vec3Scale (in, invLength, out);
510 }
511 else {
512 Vec3Clear (out);
513 }
514
515 return length;
516 }
517
518
519 /*
520 ===============
521 VectorNormalized
522 ===============
523 */
VectorNormalized(dvec3_t in,dvec3_t out)524 double VectorNormalized (dvec3_t in, dvec3_t out)
525 {
526 double length, invLength;
527
528 length = Vec3Length (in);
529
530 if (length) {
531 invLength = 1.0f/length;
532 Vec3Scale (in, invLength, out);
533 }
534 else {
535 Vec3Clear (out);
536 }
537
538 return length;
539 }
540
541
542 /*
543 ===============
544 VectorNormalizeFastf
545 ===============
546 */
VectorNormalizeFastf(vec3_t v)547 float VectorNormalizeFastf (vec3_t v)
548 {
549 float invLength = Q_RSqrtf (DotProduct(v,v));
550
551 v[0] *= invLength;
552 v[1] *= invLength;
553 v[2] *= invLength;
554
555 if (invLength != 0)
556 return 1.0f / invLength;
557
558 return 0.0f;
559 }
560
561
562 /*
563 ===============
564 VectorNormalizeFastd
565 ===============
566 */
VectorNormalizeFastd(dvec3_t v)567 double VectorNormalizeFastd (dvec3_t v)
568 {
569 double invLength = Q_RSqrtd (DotProduct(v,v));
570
571 v[0] *= invLength;
572 v[1] *= invLength;
573 v[2] *= invLength;
574
575 if (invLength != 0)
576 return 1.0 / invLength;
577
578 return 0.0;
579 }
580