1 /*
2 ===========================================================================
3 Copyright (C) 1999-2005 Id Software, Inc.
4
5 This file is part of Quake III Arena source code.
6
7 Quake III Arena source code is free software; you can redistribute it
8 and/or modify it under the terms of the GNU General Public License as
9 published by the Free Software Foundation; either version 2 of the License,
10 or (at your option) any later version.
11
12 Quake III Arena source code is distributed in the hope that it will be
13 useful, but WITHOUT ANY WARRANTY; without even the implied warranty of
14 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
15 GNU General Public License for more details.
16
17 You should have received a copy of the GNU General Public License
18 along with Foobar; if not, write to the Free Software
19 Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
20 ===========================================================================
21 */
22 //
23 // q_math.c -- stateless support routines that are included in each code module
24 #include "q_shared.h"
25
26 vec3_t vec3_origin = { 0, 0, 0 };
27 vec3_t axisDefault[3] = { {1, 0, 0}
28 , {0, 1, 0}
29 , {0, 0, 1}
30 };
31 matrix_t matrixIdentity = { 1, 0, 0, 0,
32 0, 1, 0, 0,
33 0, 0, 1, 0,
34 0, 0, 0, 1
35 };
36
37 quat_t quatIdentity = { 0, 0, 0, 1 };
38
39 vec4_t colorBlack = { 0, 0, 0, 1 };
40 vec4_t colorRed = { 1, 0, 0, 1 };
41 vec4_t colorGreen = { 0, 1, 0, 1 };
42 vec4_t colorBlue = { 0, 0, 1, 1 };
43 vec4_t colorYellow = { 1, 1, 0, 1 };
44 vec4_t colorMagenta = { 1, 0, 1, 1 };
45 vec4_t colorCyan = { 0, 1, 1, 1 };
46 vec4_t colorWhite = { 1, 1, 1, 1 };
47 vec4_t colorLtGrey = { 0.75, 0.75, 0.75, 1 };
48 vec4_t colorMdGrey = { 0.5, 0.5, 0.5, 1 };
49 vec4_t colorDkGrey = { 0.25, 0.25, 0.25, 1 };
50
51 vec4_t g_color_table[8] = {
52 {0.0, 0.0, 0.0, 1.0}
53 ,
54 {1.0, 0.0, 0.0, 1.0}
55 ,
56 {0.0, 1.0, 0.0, 1.0}
57 ,
58 {1.0, 1.0, 0.0, 1.0}
59 ,
60 {0.0, 0.0, 1.0, 1.0}
61 ,
62 {0.0, 1.0, 1.0, 1.0}
63 ,
64 {1.0, 0.0, 1.0, 1.0}
65 ,
66 {1.0, 1.0, 1.0, 1.0}
67 ,
68 };
69
70 vec3_t bytedirs[NUMVERTEXNORMALS] = {
71 {-0.525731f, 0.000000f, 0.850651f}
72 , {-0.442863f, 0.238856f, 0.864188f}
73 ,
74 {-0.295242f, 0.000000f, 0.955423f}
75 , {-0.309017f, 0.500000f, 0.809017f}
76 ,
77 {-0.162460f, 0.262866f, 0.951056f}
78 , {0.000000f, 0.000000f, 1.000000f}
79 ,
80 {0.000000f, 0.850651f, 0.525731f}
81 , {-0.147621f, 0.716567f, 0.681718f}
82 ,
83 {0.147621f, 0.716567f, 0.681718f}
84 , {0.000000f, 0.525731f, 0.850651f}
85 ,
86 {0.309017f, 0.500000f, 0.809017f}
87 , {0.525731f, 0.000000f, 0.850651f}
88 ,
89 {0.295242f, 0.000000f, 0.955423f}
90 , {0.442863f, 0.238856f, 0.864188f}
91 ,
92 {0.162460f, 0.262866f, 0.951056f}
93 , {-0.681718f, 0.147621f, 0.716567f}
94 ,
95 {-0.809017f, 0.309017f, 0.500000f}
96 , {-0.587785f, 0.425325f, 0.688191f}
97 ,
98 {-0.850651f, 0.525731f, 0.000000f}
99 , {-0.864188f, 0.442863f, 0.238856f}
100 ,
101 {-0.716567f, 0.681718f, 0.147621f}
102 , {-0.688191f, 0.587785f, 0.425325f}
103 ,
104 {-0.500000f, 0.809017f, 0.309017f}
105 , {-0.238856f, 0.864188f, 0.442863f}
106 ,
107 {-0.425325f, 0.688191f, 0.587785f}
108 , {-0.716567f, 0.681718f, -0.147621f}
109 ,
110 {-0.500000f, 0.809017f, -0.309017f}
111 , {-0.525731f, 0.850651f, 0.000000f}
112 ,
113 {0.000000f, 0.850651f, -0.525731f}
114 , {-0.238856f, 0.864188f, -0.442863f}
115 ,
116 {0.000000f, 0.955423f, -0.295242f}
117 , {-0.262866f, 0.951056f, -0.162460f}
118 ,
119 {0.000000f, 1.000000f, 0.000000f}
120 , {0.000000f, 0.955423f, 0.295242f}
121 ,
122 {-0.262866f, 0.951056f, 0.162460f}
123 , {0.238856f, 0.864188f, 0.442863f}
124 ,
125 {0.262866f, 0.951056f, 0.162460f}
126 , {0.500000f, 0.809017f, 0.309017f}
127 ,
128 {0.238856f, 0.864188f, -0.442863f}
129 , {0.262866f, 0.951056f, -0.162460f}
130 ,
131 {0.500000f, 0.809017f, -0.309017f}
132 , {0.850651f, 0.525731f, 0.000000f}
133 ,
134 {0.716567f, 0.681718f, 0.147621f}
135 , {0.716567f, 0.681718f, -0.147621f}
136 ,
137 {0.525731f, 0.850651f, 0.000000f}
138 , {0.425325f, 0.688191f, 0.587785f}
139 ,
140 {0.864188f, 0.442863f, 0.238856f}
141 , {0.688191f, 0.587785f, 0.425325f}
142 ,
143 {0.809017f, 0.309017f, 0.500000f}
144 , {0.681718f, 0.147621f, 0.716567f}
145 ,
146 {0.587785f, 0.425325f, 0.688191f}
147 , {0.955423f, 0.295242f, 0.000000f}
148 ,
149 {1.000000f, 0.000000f, 0.000000f}
150 , {0.951056f, 0.162460f, 0.262866f}
151 ,
152 {0.850651f, -0.525731f, 0.000000f}
153 , {0.955423f, -0.295242f, 0.000000f}
154 ,
155 {0.864188f, -0.442863f, 0.238856f}
156 , {0.951056f, -0.162460f, 0.262866f}
157 ,
158 {0.809017f, -0.309017f, 0.500000f}
159 , {0.681718f, -0.147621f, 0.716567f}
160 ,
161 {0.850651f, 0.000000f, 0.525731f}
162 , {0.864188f, 0.442863f, -0.238856f}
163 ,
164 {0.809017f, 0.309017f, -0.500000f}
165 , {0.951056f, 0.162460f, -0.262866f}
166 ,
167 {0.525731f, 0.000000f, -0.850651f}
168 , {0.681718f, 0.147621f, -0.716567f}
169 ,
170 {0.681718f, -0.147621f, -0.716567f}
171 , {0.850651f, 0.000000f, -0.525731f}
172 ,
173 {0.809017f, -0.309017f, -0.500000f}
174 , {0.864188f, -0.442863f, -0.238856f}
175 ,
176 {0.951056f, -0.162460f, -0.262866f}
177 , {0.147621f, 0.716567f, -0.681718f}
178 ,
179 {0.309017f, 0.500000f, -0.809017f}
180 , {0.425325f, 0.688191f, -0.587785f}
181 ,
182 {0.442863f, 0.238856f, -0.864188f}
183 , {0.587785f, 0.425325f, -0.688191f}
184 ,
185 {0.688191f, 0.587785f, -0.425325f}
186 , {-0.147621f, 0.716567f, -0.681718f}
187 ,
188 {-0.309017f, 0.500000f, -0.809017f}
189 , {0.000000f, 0.525731f, -0.850651f}
190 ,
191 {-0.525731f, 0.000000f, -0.850651f}
192 , {-0.442863f, 0.238856f, -0.864188f}
193 ,
194 {-0.295242f, 0.000000f, -0.955423f}
195 , {-0.162460f, 0.262866f, -0.951056f}
196 ,
197 {0.000000f, 0.000000f, -1.000000f}
198 , {0.295242f, 0.000000f, -0.955423f}
199 ,
200 {0.162460f, 0.262866f, -0.951056f}
201 , {-0.442863f, -0.238856f, -0.864188f}
202 ,
203 {-0.309017f, -0.500000f, -0.809017f}
204 , {-0.162460f, -0.262866f, -0.951056f}
205 ,
206 {0.000000f, -0.850651f, -0.525731f}
207 , {-0.147621f, -0.716567f, -0.681718f}
208 ,
209 {0.147621f, -0.716567f, -0.681718f}
210 , {0.000000f, -0.525731f, -0.850651f}
211 ,
212 {0.309017f, -0.500000f, -0.809017f}
213 , {0.442863f, -0.238856f, -0.864188f}
214 ,
215 {0.162460f, -0.262866f, -0.951056f}
216 , {0.238856f, -0.864188f, -0.442863f}
217 ,
218 {0.500000f, -0.809017f, -0.309017f}
219 , {0.425325f, -0.688191f, -0.587785f}
220 ,
221 {0.716567f, -0.681718f, -0.147621f}
222 , {0.688191f, -0.587785f, -0.425325f}
223 ,
224 {0.587785f, -0.425325f, -0.688191f}
225 , {0.000000f, -0.955423f, -0.295242f}
226 ,
227 {0.000000f, -1.000000f, 0.000000f}
228 , {0.262866f, -0.951056f, -0.162460f}
229 ,
230 {0.000000f, -0.850651f, 0.525731f}
231 , {0.000000f, -0.955423f, 0.295242f}
232 ,
233 {0.238856f, -0.864188f, 0.442863f}
234 , {0.262866f, -0.951056f, 0.162460f}
235 ,
236 {0.500000f, -0.809017f, 0.309017f}
237 , {0.716567f, -0.681718f, 0.147621f}
238 ,
239 {0.525731f, -0.850651f, 0.000000f}
240 , {-0.238856f, -0.864188f, -0.442863f}
241 ,
242 {-0.500000f, -0.809017f, -0.309017f}
243 , {-0.262866f, -0.951056f, -0.162460f}
244 ,
245 {-0.850651f, -0.525731f, 0.000000f}
246 , {-0.716567f, -0.681718f, -0.147621f}
247 ,
248 {-0.716567f, -0.681718f, 0.147621f}
249 , {-0.525731f, -0.850651f, 0.000000f}
250 ,
251 {-0.500000f, -0.809017f, 0.309017f}
252 , {-0.238856f, -0.864188f, 0.442863f}
253 ,
254 {-0.262866f, -0.951056f, 0.162460f}
255 , {-0.864188f, -0.442863f, 0.238856f}
256 ,
257 {-0.809017f, -0.309017f, 0.500000f}
258 , {-0.688191f, -0.587785f, 0.425325f}
259 ,
260 {-0.681718f, -0.147621f, 0.716567f}
261 , {-0.442863f, -0.238856f, 0.864188f}
262 ,
263 {-0.587785f, -0.425325f, 0.688191f}
264 , {-0.309017f, -0.500000f, 0.809017f}
265 ,
266 {-0.147621f, -0.716567f, 0.681718f}
267 , {-0.425325f, -0.688191f, 0.587785f}
268 ,
269 {-0.162460f, -0.262866f, 0.951056f}
270 , {0.442863f, -0.238856f, 0.864188f}
271 ,
272 {0.162460f, -0.262866f, 0.951056f}
273 , {0.309017f, -0.500000f, 0.809017f}
274 ,
275 {0.147621f, -0.716567f, 0.681718f}
276 , {0.000000f, -0.525731f, 0.850651f}
277 ,
278 {0.425325f, -0.688191f, 0.587785f}
279 , {0.587785f, -0.425325f, 0.688191f}
280 ,
281 {0.688191f, -0.587785f, 0.425325f}
282 , {-0.955423f, 0.295242f, 0.000000f}
283 ,
284 {-0.951056f, 0.162460f, 0.262866f}
285 , {-1.000000f, 0.000000f, 0.000000f}
286 ,
287 {-0.850651f, 0.000000f, 0.525731f}
288 , {-0.955423f, -0.295242f, 0.000000f}
289 ,
290 {-0.951056f, -0.162460f, 0.262866f}
291 , {-0.864188f, 0.442863f, -0.238856f}
292 ,
293 {-0.951056f, 0.162460f, -0.262866f}
294 , {-0.809017f, 0.309017f, -0.500000f}
295 ,
296 {-0.864188f, -0.442863f, -0.238856f}
297 , {-0.951056f, -0.162460f, -0.262866f}
298 ,
299 {-0.809017f, -0.309017f, -0.500000f}
300 , {-0.681718f, 0.147621f, -0.716567f}
301 ,
302 {-0.681718f, -0.147621f, -0.716567f}
303 , {-0.850651f, 0.000000f, -0.525731f}
304 ,
305 {-0.688191f, 0.587785f, -0.425325f}
306 , {-0.587785f, 0.425325f, -0.688191f}
307 ,
308 {-0.425325f, 0.688191f, -0.587785f}
309 , {-0.425325f, -0.688191f, -0.587785f}
310 ,
311 {-0.587785f, -0.425325f, -0.688191f}
312 , {-0.688191f, -0.587785f, -0.425325f}
313 };
314
315 //==============================================================
316
Q_rand(int * seed)317 int Q_rand(int *seed)
318 {
319 *seed = (69069 * *seed + 1);
320 return *seed;
321 }
322
Q_random(int * seed)323 float Q_random(int *seed)
324 {
325 return (Q_rand(seed) & 0xffff) / (float)0x10000;
326 }
327
Q_crandom(int * seed)328 float Q_crandom(int *seed)
329 {
330 return 2.0 * (Q_random(seed) - 0.5);
331 }
332
333 //=======================================================
334
ClampChar(int i)335 signed char ClampChar(int i)
336 {
337 if(i < -128)
338 {
339 return -128;
340 }
341 if(i > 127)
342 {
343 return 127;
344 }
345 return i;
346 }
347
ClampShort(int i)348 signed short ClampShort(int i)
349 {
350 if(i < -32768)
351 {
352 return -32768;
353 }
354 if(i > 0x7fff)
355 {
356 return 0x7fff;
357 }
358 return i;
359 }
360
361
362 // this isn't a real cheap function to call!
DirToByte(vec3_t dir)363 int DirToByte(vec3_t dir)
364 {
365 int i, best;
366 float d, bestd;
367
368 if(!dir)
369 {
370 return 0;
371 }
372
373 bestd = 0;
374 best = 0;
375 for(i = 0; i < NUMVERTEXNORMALS; i++)
376 {
377 d = DotProduct(dir, bytedirs[i]);
378 if(d > bestd)
379 {
380 bestd = d;
381 best = i;
382 }
383 }
384
385 return best;
386 }
387
ByteToDir(int b,vec3_t dir)388 void ByteToDir(int b, vec3_t dir)
389 {
390 if(b < 0 || b >= NUMVERTEXNORMALS)
391 {
392 VectorCopy(vec3_origin, dir);
393 return;
394 }
395 VectorCopy(bytedirs[b], dir);
396 }
397
398
ColorBytes3(float r,float g,float b)399 unsigned ColorBytes3(float r, float g, float b)
400 {
401 unsigned i;
402
403 ((byte *) & i)[0] = r * 255;
404 ((byte *) & i)[1] = g * 255;
405 ((byte *) & i)[2] = b * 255;
406
407 return i;
408 }
409
ColorBytes4(float r,float g,float b,float a)410 unsigned ColorBytes4(float r, float g, float b, float a)
411 {
412 unsigned i;
413
414 ((byte *) & i)[0] = r * 255;
415 ((byte *) & i)[1] = g * 255;
416 ((byte *) & i)[2] = b * 255;
417 ((byte *) & i)[3] = a * 255;
418
419 return i;
420 }
421
NormalizeColor(const vec3_t in,vec3_t out)422 float NormalizeColor(const vec3_t in, vec3_t out)
423 {
424 float max;
425
426 max = in[0];
427 if(in[1] > max)
428 {
429 max = in[1];
430 }
431 if(in[2] > max)
432 {
433 max = in[2];
434 }
435
436 if(!max)
437 {
438 VectorClear(out);
439 }
440 else
441 {
442 out[0] = in[0] / max;
443 out[1] = in[1] / max;
444 out[2] = in[2] / max;
445 }
446 return max;
447 }
448
ClampColor(vec4_t color)449 void ClampColor(vec4_t color)
450 {
451 int i;
452
453 for(i = 0; i < 4; i++)
454 {
455 if(color[i] < 0)
456 color[i] = 0;
457
458 if(color[i] > 1)
459 color[i] = 1;
460 }
461 }
462
463 /*
464 =====================
465 Q_acos
466
467 the msvc acos doesn't always return a value between -PI and PI:
468 int i;
469 i = 1065353246;
470 acos(*(float*) &i) == -1.#IND0
471 =====================
472 */
Q_acos(float c)473 float Q_acos(float c)
474 {
475 float angle;
476
477 angle = acos(c);
478
479 if(angle > M_PI)
480 {
481 return (float)M_PI;
482 }
483 if(angle < -M_PI)
484 {
485 return (float)M_PI;
486 }
487 return angle;
488 }
489
PlaneNormalize(vec4_t plane)490 vec_t PlaneNormalize(vec4_t plane)
491 {
492 vec_t length, ilength;
493
494 length = sqrt(plane[0] * plane[0] + plane[1] * plane[1] + plane[2] * plane[2]);
495 if(length == 0)
496 {
497 VectorClear(plane);
498 return 0;
499 }
500
501 ilength = 1.0 / length;
502 plane[0] = plane[0] * ilength;
503 plane[1] = plane[1] * ilength;
504 plane[2] = plane[2] * ilength;
505 plane[3] = plane[3] * ilength;
506
507 return length;
508 }
509
510 /*
511 =====================
512 PlaneFromPoints
513
514 Returns false if the triangle is degenrate.
515 =====================
516 */
PlaneFromPoints(vec4_t plane,const vec3_t a,const vec3_t b,const vec3_t c,qboolean cw)517 qboolean PlaneFromPoints(vec4_t plane, const vec3_t a, const vec3_t b, const vec3_t c, qboolean cw)
518 {
519 vec3_t d1, d2;
520
521 VectorSubtract(b, a, d1);
522 VectorSubtract(c, a, d2);
523
524 if(cw)
525 {
526 CrossProduct(d2, d1, plane);
527 }
528 else
529 {
530 CrossProduct(d1, d2, plane);
531 }
532
533 if(VectorNormalize(plane) == 0)
534 {
535 return qfalse;
536 }
537
538 plane[3] = DotProduct(a, plane);
539 return qtrue;
540 }
541
542 /*
543 ===============
544 RotatePointAroundVector
545 ===============
546 */
RotatePointAroundVector(vec3_t dst,const vec3_t dir,const vec3_t point,float degrees)547 void RotatePointAroundVector(vec3_t dst, const vec3_t dir, const vec3_t point, float degrees)
548 {
549 vec3_t m[3];
550 vec3_t im[3];
551 vec3_t zrot[3];
552 vec3_t tmpmat[3];
553 vec3_t rot[3];
554
555 vec3_t vr, vup, vf;
556
557 vf[0] = dir[0];
558 vf[1] = dir[1];
559 vf[2] = dir[2];
560
561 PerpendicularVector(vr, dir);
562 CrossProduct(vr, vf, vup);
563
564 m[0][0] = vr[0];
565 m[1][0] = vr[1];
566 m[2][0] = vr[2];
567
568 m[0][1] = vup[0];
569 m[1][1] = vup[1];
570 m[2][1] = vup[2];
571
572 m[0][2] = vf[0];
573 m[1][2] = vf[1];
574 m[2][2] = vf[2];
575
576 im[0][0] = m[0][0];
577 im[0][1] = m[1][0];
578 im[0][2] = m[2][0];
579
580 im[1][0] = m[0][1];
581 im[1][1] = m[1][1];
582 im[1][2] = m[2][1];
583
584 im[2][0] = m[0][2];
585 im[2][1] = m[1][2];
586 im[2][2] = m[2][2];
587
588 zrot[0][0] = (float)cos(DEG2RAD(degrees));
589 zrot[0][1] = (float)sin(DEG2RAD(degrees));
590 zrot[0][2] = 0;
591
592 zrot[1][0] = (float)-sin(DEG2RAD(degrees));
593 zrot[1][1] = (float)cos(DEG2RAD(degrees));
594 zrot[1][2] = 0;
595
596 zrot[2][0] = 0.0f;
597 zrot[2][1] = 0.0f;
598 zrot[2][2] = 1.0f;
599
600 AxisMultiply(m, zrot, tmpmat);
601 AxisMultiply(tmpmat, im, rot);
602
603 dst[0] = rot[0][0] * point[0] + rot[0][1] * point[1] + rot[0][2] * point[2];
604 dst[1] = rot[1][0] * point[0] + rot[1][1] * point[1] + rot[1][2] * point[2];
605 dst[2] = rot[2][0] * point[0] + rot[2][1] * point[1] + rot[2][2] * point[2];
606 }
607
608 /*
609 ===============
610 RotateAroundDirection
611 ===============
612 */
RotateAroundDirection(vec3_t axis[3],float yaw)613 void RotateAroundDirection(vec3_t axis[3], float yaw)
614 {
615 // create an arbitrary axis[1]
616 PerpendicularVector(axis[1], axis[0]);
617
618 // rotate it around axis[0] by yaw
619 if(yaw)
620 {
621 vec3_t temp;
622
623 VectorCopy(axis[1], temp);
624 RotatePointAroundVector(axis[1], axis[0], temp, yaw);
625 }
626
627 // cross to get axis[2]
628 CrossProduct(axis[0], axis[1], axis[2]);
629 }
630
631
632
vectoangles(const vec3_t value1,vec3_t angles)633 void vectoangles(const vec3_t value1, vec3_t angles)
634 {
635 float forward;
636 float yaw, pitch;
637
638 if(value1[1] == 0 && value1[0] == 0)
639 {
640 yaw = 0;
641 if(value1[2] > 0)
642 {
643 pitch = 90;
644 }
645 else
646 {
647 pitch = 270;
648 }
649 }
650 else
651 {
652 if(value1[0])
653 {
654 yaw = (atan2(value1[1], value1[0]) * 180 / M_PI);
655 }
656 else if(value1[1] > 0)
657 {
658 yaw = 90;
659 }
660 else
661 {
662 yaw = 270;
663 }
664 if(yaw < 0)
665 {
666 yaw += 360;
667 }
668
669 forward = sqrt(value1[0] * value1[0] + value1[1] * value1[1]);
670 pitch = (atan2(value1[2], forward) * 180 / M_PI);
671 if(pitch < 0)
672 {
673 pitch += 360;
674 }
675 }
676
677 angles[PITCH] = -pitch;
678 angles[YAW] = yaw;
679 angles[ROLL] = 0;
680 }
681
682
683 /*
684 =================
685 AnglesToAxis
686 =================
687 */
AnglesToAxis(const vec3_t angles,vec3_t axis[3])688 void AnglesToAxis(const vec3_t angles, vec3_t axis[3])
689 {
690 vec3_t right;
691
692 // angle vectors returns "right" instead of "y axis"
693 AngleVectors(angles, axis[0], right, axis[2]);
694 VectorSubtract(vec3_origin, right, axis[1]);
695 }
696
AxisClear(vec3_t axis[3])697 void AxisClear(vec3_t axis[3])
698 {
699 axis[0][0] = 1;
700 axis[0][1] = 0;
701 axis[0][2] = 0;
702 axis[1][0] = 0;
703 axis[1][1] = 1;
704 axis[1][2] = 0;
705 axis[2][0] = 0;
706 axis[2][1] = 0;
707 axis[2][2] = 1;
708 }
709
AxisCopy(vec3_t in[3],vec3_t out[3])710 void AxisCopy(vec3_t in[3], vec3_t out[3])
711 {
712 VectorCopy(in[0], out[0]);
713 VectorCopy(in[1], out[1]);
714 VectorCopy(in[2], out[2]);
715 }
716
ProjectPointOnPlane(vec3_t dst,const vec3_t p,const vec3_t normal)717 void ProjectPointOnPlane(vec3_t dst, const vec3_t p, const vec3_t normal)
718 {
719 float d;
720 vec3_t n;
721 float inv_denom;
722
723 inv_denom = DotProduct(normal, normal);
724 inv_denom = 1.0f / inv_denom;
725
726 d = DotProduct(normal, p) * inv_denom;
727
728 n[0] = normal[0] * inv_denom;
729 n[1] = normal[1] * inv_denom;
730 n[2] = normal[2] * inv_denom;
731
732 dst[0] = p[0] - d * n[0];
733 dst[1] = p[1] - d * n[1];
734 dst[2] = p[2] - d * n[2];
735 }
736
737 /*
738 ================
739 MakeNormalVectors
740
741 Given a normalized forward vector, create two
742 other perpendicular vectors
743 ================
744 */
MakeNormalVectors(const vec3_t forward,vec3_t right,vec3_t up)745 void MakeNormalVectors(const vec3_t forward, vec3_t right, vec3_t up)
746 {
747 float d;
748
749 // this rotate and negate guarantees a vector
750 // not colinear with the original
751 right[1] = -forward[0];
752 right[2] = forward[1];
753 right[0] = forward[2];
754
755 d = DotProduct(right, forward);
756 VectorMA(right, -d, forward, right);
757 VectorNormalize(right);
758 CrossProduct(right, forward, up);
759 }
760
761
VectorRotate(vec3_t in,vec3_t matrix[3],vec3_t out)762 void VectorRotate(vec3_t in, vec3_t matrix[3], vec3_t out)
763 {
764 out[0] = DotProduct(in, matrix[0]);
765 out[1] = DotProduct(in, matrix[1]);
766 out[2] = DotProduct(in, matrix[2]);
767 }
768
769 //============================================================================
770
771 #if !idppc
772 /*
773 ** float q_rsqrt( float number )
774 */
Q_rsqrt(float number)775 float Q_rsqrt(float number)
776 {
777 union
778 {
779 float f;
780 int i;
781 } t;
782 float x2, y;
783 const float threehalfs = 1.5F;
784
785 x2 = number * 0.5F;
786 t.f = number;
787 t.i = 0x5f3759df - (t.i >> 1); // what the fuck?
788 y = t.f;
789 y = y * (threehalfs - (x2 * y * y)); // 1st iteration
790 // y = y * ( threehalfs - ( x2 * y * y ) ); // 2nd iteration, this can be removed
791
792 return y;
793 }
794
Q_fabs(float f)795 float Q_fabs(float f)
796 {
797 int tmp = *(int *)&f;
798
799 tmp &= 0x7FFFFFFF;
800 return *(float *)&tmp;
801 }
802 #endif
803
Q_recip(vec_t in)804 vec_t Q_recip(vec_t in)
805 {
806 return ((float)(1.0f / (in)));
807 }
808
809 /*
810 ================
811 Q_isnan
812
813 Don't pass doubles to this
814 ================
815 */
Q_isnan(float x)816 int Q_isnan(float x)
817 {
818 union
819 {
820 float f;
821 unsigned int i;
822 } t;
823
824 t.f = x;
825 t.i &= 0x7FFFFFFF;
826 t.i = 0x7F800000 - t.i;
827
828 return (int)((unsigned int)t.i >> 31);
829 }
830
831 //============================================================
832
833 /*
834 ===============
835 LerpAngle
836
837 ===============
838 */
LerpAngle(float from,float to,float frac)839 float LerpAngle(float from, float to, float frac)
840 {
841 float a;
842
843 if(to - from > 180)
844 {
845 to -= 360;
846 }
847 if(to - from < -180)
848 {
849 to += 360;
850 }
851 a = from + frac * (to - from);
852
853 return a;
854 }
855
856
857 /*
858 =================
859 AngleSubtract
860
861 Always returns a value from -180 to 180
862 =================
863 */
AngleSubtract(float a1,float a2)864 float AngleSubtract(float a1, float a2)
865 {
866 float a;
867
868 a = a1 - a2;
869 while(a > 180)
870 {
871 a -= 360;
872 }
873 while(a < -180)
874 {
875 a += 360;
876 }
877 return a;
878 }
879
880
AnglesSubtract(vec3_t v1,vec3_t v2,vec3_t v3)881 void AnglesSubtract(vec3_t v1, vec3_t v2, vec3_t v3)
882 {
883 v3[0] = AngleSubtract(v1[0], v2[0]);
884 v3[1] = AngleSubtract(v1[1], v2[1]);
885 v3[2] = AngleSubtract(v1[2], v2[2]);
886 }
887
888
AngleMod(float a)889 float AngleMod(float a)
890 {
891 a = (360.0 / 65536) * ((int)(a * (65536 / 360.0)) & 65535);
892 return a;
893 }
894
895
896 /*
897 =================
898 AngleNormalize360
899
900 returns angle normalized to the range [0 <= angle < 360]
901 =================
902 */
AngleNormalize360(float angle)903 float AngleNormalize360(float angle)
904 {
905 return (360.0 / 65536) * ((int)(angle * (65536 / 360.0)) & 65535);
906 }
907
908
909 /*
910 =================
911 AngleNormalize180
912
913 returns angle normalized to the range [-180 < angle <= 180]
914 =================
915 */
AngleNormalize180(float angle)916 float AngleNormalize180(float angle)
917 {
918 angle = AngleNormalize360(angle);
919 if(angle > 180.0)
920 {
921 angle -= 360.0;
922 }
923 return angle;
924 }
925
926
927 /*
928 =================
929 AngleDelta
930
931 returns the normalized delta from angle1 to angle2
932 =================
933 */
AngleDelta(float angle1,float angle2)934 float AngleDelta(float angle1, float angle2)
935 {
936 return AngleNormalize180(angle1 - angle2);
937 }
938
939
940 /*
941 =================
942 AngleBetweenVectors
943
944 returns the angle between two vectors normalized to the range [0 <= angle <= 180]
945 =================
946 */
AngleBetweenVectors(const vec3_t a,const vec3_t b)947 float AngleBetweenVectors(const vec3_t a, const vec3_t b)
948 {
949 vec_t alen, blen;
950
951 alen = VectorLength(a);
952 blen = VectorLength(b);
953
954 // complete dot product of two vectors a, b is |a| * |b| * cos(angle)
955 // this results in: angle = acos( (a * b) / (|a| * |b|) )
956 return RAD2DEG(Q_acos(DotProduct(a, b) / (alen * blen)));
957 }
958
959 //============================================================
960
961
962 /*
963 =================
964 SetPlaneSignbits
965 =================
966 */
SetPlaneSignbits(cplane_t * out)967 void SetPlaneSignbits(cplane_t * out)
968 {
969 int bits, j;
970
971 // for fast box on planeside test
972 bits = 0;
973 for(j = 0; j < 3; j++)
974 {
975 if(out->normal[j] < 0)
976 {
977 bits |= 1 << j;
978 }
979 }
980 out->signbits = bits;
981 }
982
983
984 /*
985 ==================
986 BoxOnPlaneSide
987
988 Returns 1, 2, or 1 + 2
989 ==================
990 */
991 #if !id386
BoxOnPlaneSide(vec3_t emins,vec3_t emaxs,struct cplane_s * p)992 int BoxOnPlaneSide(vec3_t emins, vec3_t emaxs, struct cplane_s *p)
993 {
994 float dist1, dist2;
995 int sides;
996
997 // fast axial cases
998 if(p->type < 3)
999 {
1000 if(p->dist <= emins[p->type])
1001 return 1;
1002 if(p->dist >= emaxs[p->type])
1003 return 2;
1004 return 3;
1005 }
1006
1007 // general case
1008 switch (p->signbits)
1009 {
1010 case 0:
1011 dist1 = p->normal[0] * emaxs[0] + p->normal[1] * emaxs[1] + p->normal[2] * emaxs[2];
1012 dist2 = p->normal[0] * emins[0] + p->normal[1] * emins[1] + p->normal[2] * emins[2];
1013 break;
1014 case 1:
1015 dist1 = p->normal[0] * emins[0] + p->normal[1] * emaxs[1] + p->normal[2] * emaxs[2];
1016 dist2 = p->normal[0] * emaxs[0] + p->normal[1] * emins[1] + p->normal[2] * emins[2];
1017 break;
1018 case 2:
1019 dist1 = p->normal[0] * emaxs[0] + p->normal[1] * emins[1] + p->normal[2] * emaxs[2];
1020 dist2 = p->normal[0] * emins[0] + p->normal[1] * emaxs[1] + p->normal[2] * emins[2];
1021 break;
1022 case 3:
1023 dist1 = p->normal[0] * emins[0] + p->normal[1] * emins[1] + p->normal[2] * emaxs[2];
1024 dist2 = p->normal[0] * emaxs[0] + p->normal[1] * emaxs[1] + p->normal[2] * emins[2];
1025 break;
1026 case 4:
1027 dist1 = p->normal[0] * emaxs[0] + p->normal[1] * emaxs[1] + p->normal[2] * emins[2];
1028 dist2 = p->normal[0] * emins[0] + p->normal[1] * emins[1] + p->normal[2] * emaxs[2];
1029 break;
1030 case 5:
1031 dist1 = p->normal[0] * emins[0] + p->normal[1] * emaxs[1] + p->normal[2] * emins[2];
1032 dist2 = p->normal[0] * emaxs[0] + p->normal[1] * emins[1] + p->normal[2] * emaxs[2];
1033 break;
1034 case 6:
1035 dist1 = p->normal[0] * emaxs[0] + p->normal[1] * emins[1] + p->normal[2] * emins[2];
1036 dist2 = p->normal[0] * emins[0] + p->normal[1] * emaxs[1] + p->normal[2] * emaxs[2];
1037 break;
1038 case 7:
1039 dist1 = p->normal[0] * emins[0] + p->normal[1] * emins[1] + p->normal[2] * emins[2];
1040 dist2 = p->normal[0] * emaxs[0] + p->normal[1] * emaxs[1] + p->normal[2] * emaxs[2];
1041 break;
1042 default:
1043 dist1 = dist2 = 0; // shut up compiler
1044 break;
1045 }
1046
1047 sides = 0;
1048 if(dist1 >= p->dist)
1049 sides = 1;
1050 if(dist2 < p->dist)
1051 sides |= 2;
1052
1053 return sides;
1054 }
1055 #elif __GNUC__
1056 // use matha.s
1057 #else
1058 #pragma warning( disable: 4035 )
BoxOnPlaneSide(vec3_t emins,vec3_t emaxs,struct cplane_s * p)1059 __declspec( naked ) int BoxOnPlaneSide (vec3_t emins, vec3_t emaxs, struct cplane_s *p)
1060 {
1061 static int bops_initialized;
1062 static int Ljmptab[8];
1063
1064 __asm {
1065
1066 push ebx
1067
1068 cmp bops_initialized, 1
1069 je initialized
1070 mov bops_initialized, 1
1071
1072 mov Ljmptab[0*4], offset Lcase0
1073 mov Ljmptab[1*4], offset Lcase1
1074 mov Ljmptab[2*4], offset Lcase2
1075 mov Ljmptab[3*4], offset Lcase3
1076 mov Ljmptab[4*4], offset Lcase4
1077 mov Ljmptab[5*4], offset Lcase5
1078 mov Ljmptab[6*4], offset Lcase6
1079 mov Ljmptab[7*4], offset Lcase7
1080
1081 initialized:
1082
1083 mov edx,dword ptr[4+12+esp]
1084 mov ecx,dword ptr[4+4+esp]
1085 xor eax,eax
1086 mov ebx,dword ptr[4+8+esp]
1087 mov al,byte ptr[17+edx]
1088 cmp al,8
1089 jge Lerror
1090 fld dword ptr[0+edx]
1091 fld st(0)
1092 jmp dword ptr[Ljmptab+eax*4]
1093 Lcase0:
1094 fmul dword ptr[ebx]
1095 fld dword ptr[0+4+edx]
1096 fxch st(2)
1097 fmul dword ptr[ecx]
1098 fxch st(2)
1099 fld st(0)
1100 fmul dword ptr[4+ebx]
1101 fld dword ptr[0+8+edx]
1102 fxch st(2)
1103 fmul dword ptr[4+ecx]
1104 fxch st(2)
1105 fld st(0)
1106 fmul dword ptr[8+ebx]
1107 fxch st(5)
1108 faddp st(3),st(0)
1109 fmul dword ptr[8+ecx]
1110 fxch st(1)
1111 faddp st(3),st(0)
1112 fxch st(3)
1113 faddp st(2),st(0)
1114 jmp LSetSides
1115 Lcase1:
1116 fmul dword ptr[ecx]
1117 fld dword ptr[0+4+edx]
1118 fxch st(2)
1119 fmul dword ptr[ebx]
1120 fxch st(2)
1121 fld st(0)
1122 fmul dword ptr[4+ebx]
1123 fld dword ptr[0+8+edx]
1124 fxch st(2)
1125 fmul dword ptr[4+ecx]
1126 fxch st(2)
1127 fld st(0)
1128 fmul dword ptr[8+ebx]
1129 fxch st(5)
1130 faddp st(3),st(0)
1131 fmul dword ptr[8+ecx]
1132 fxch st(1)
1133 faddp st(3),st(0)
1134 fxch st(3)
1135 faddp st(2),st(0)
1136 jmp LSetSides
1137 Lcase2:
1138 fmul dword ptr[ebx]
1139 fld dword ptr[0+4+edx]
1140 fxch st(2)
1141 fmul dword ptr[ecx]
1142 fxch st(2)
1143 fld st(0)
1144 fmul dword ptr[4+ecx]
1145 fld dword ptr[0+8+edx]
1146 fxch st(2)
1147 fmul dword ptr[4+ebx]
1148 fxch st(2)
1149 fld st(0)
1150 fmul dword ptr[8+ebx]
1151 fxch st(5)
1152 faddp st(3),st(0)
1153 fmul dword ptr[8+ecx]
1154 fxch st(1)
1155 faddp st(3),st(0)
1156 fxch st(3)
1157 faddp st(2),st(0)
1158 jmp LSetSides
1159 Lcase3:
1160 fmul dword ptr[ecx]
1161 fld dword ptr[0+4+edx]
1162 fxch st(2)
1163 fmul dword ptr[ebx]
1164 fxch st(2)
1165 fld st(0)
1166 fmul dword ptr[4+ecx]
1167 fld dword ptr[0+8+edx]
1168 fxch st(2)
1169 fmul dword ptr[4+ebx]
1170 fxch st(2)
1171 fld st(0)
1172 fmul dword ptr[8+ebx]
1173 fxch st(5)
1174 faddp st(3),st(0)
1175 fmul dword ptr[8+ecx]
1176 fxch st(1)
1177 faddp st(3),st(0)
1178 fxch st(3)
1179 faddp st(2),st(0)
1180 jmp LSetSides
1181 Lcase4:
1182 fmul dword ptr[ebx]
1183 fld dword ptr[0+4+edx]
1184 fxch st(2)
1185 fmul dword ptr[ecx]
1186 fxch st(2)
1187 fld st(0)
1188 fmul dword ptr[4+ebx]
1189 fld dword ptr[0+8+edx]
1190 fxch st(2)
1191 fmul dword ptr[4+ecx]
1192 fxch st(2)
1193 fld st(0)
1194 fmul dword ptr[8+ecx]
1195 fxch st(5)
1196 faddp st(3),st(0)
1197 fmul dword ptr[8+ebx]
1198 fxch st(1)
1199 faddp st(3),st(0)
1200 fxch st(3)
1201 faddp st(2),st(0)
1202 jmp LSetSides
1203 Lcase5:
1204 fmul dword ptr[ecx]
1205 fld dword ptr[0+4+edx]
1206 fxch st(2)
1207 fmul dword ptr[ebx]
1208 fxch st(2)
1209 fld st(0)
1210 fmul dword ptr[4+ebx]
1211 fld dword ptr[0+8+edx]
1212 fxch st(2)
1213 fmul dword ptr[4+ecx]
1214 fxch st(2)
1215 fld st(0)
1216 fmul dword ptr[8+ecx]
1217 fxch st(5)
1218 faddp st(3),st(0)
1219 fmul dword ptr[8+ebx]
1220 fxch st(1)
1221 faddp st(3),st(0)
1222 fxch st(3)
1223 faddp st(2),st(0)
1224 jmp LSetSides
1225 Lcase6:
1226 fmul dword ptr[ebx]
1227 fld dword ptr[0+4+edx]
1228 fxch st(2)
1229 fmul dword ptr[ecx]
1230 fxch st(2)
1231 fld st(0)
1232 fmul dword ptr[4+ecx]
1233 fld dword ptr[0+8+edx]
1234 fxch st(2)
1235 fmul dword ptr[4+ebx]
1236 fxch st(2)
1237 fld st(0)
1238 fmul dword ptr[8+ecx]
1239 fxch st(5)
1240 faddp st(3),st(0)
1241 fmul dword ptr[8+ebx]
1242 fxch st(1)
1243 faddp st(3),st(0)
1244 fxch st(3)
1245 faddp st(2),st(0)
1246 jmp LSetSides
1247 Lcase7:
1248 fmul dword ptr[ecx]
1249 fld dword ptr[0+4+edx]
1250 fxch st(2)
1251 fmul dword ptr[ebx]
1252 fxch st(2)
1253 fld st(0)
1254 fmul dword ptr[4+ecx]
1255 fld dword ptr[0+8+edx]
1256 fxch st(2)
1257 fmul dword ptr[4+ebx]
1258 fxch st(2)
1259 fld st(0)
1260 fmul dword ptr[8+ecx]
1261 fxch st(5)
1262 faddp st(3),st(0)
1263 fmul dword ptr[8+ebx]
1264 fxch st(1)
1265 faddp st(3),st(0)
1266 fxch st(3)
1267 faddp st(2),st(0)
1268 LSetSides:
1269 faddp st(2),st(0)
1270 fcomp dword ptr[12+edx]
1271 xor ecx,ecx
1272 fnstsw ax
1273 fcomp dword ptr[12+edx]
1274 and ah,1
1275 xor ah,1
1276 add cl,ah
1277 fnstsw ax
1278 and ah,1
1279 add ah,ah
1280 add cl,ah
1281 pop ebx
1282 mov eax,ecx
1283 ret
1284 Lerror:
1285 int 3
1286 }
1287 }
1288 #pragma warning( default: 4035 )
1289 #endif
1290
1291 /*
1292 ==================
1293 BoxOnPlaneSide2
1294
1295 Returns 1, 2, or 1 + 2
1296 ==================
1297 */
BoxOnPlaneSide2(vec3_t mins,vec3_t maxs,vec4_t plane)1298 int BoxOnPlaneSide2(vec3_t mins, vec3_t maxs, vec4_t plane)
1299 {
1300 int i;
1301 float dist1, dist2;
1302 int sides;
1303 vec3_t corners[2];
1304
1305 for(i = 0; i < 3; i++)
1306 {
1307 if(plane[i] < 0)
1308 {
1309 corners[0][i] = mins[i];
1310 corners[1][i] = maxs[i];
1311 }
1312 else
1313 {
1314 corners[1][i] = mins[i];
1315 corners[0][i] = maxs[i];
1316 }
1317 }
1318
1319 dist1 = DotProduct(plane, corners[0]) - plane[3];
1320 dist2 = DotProduct(plane, corners[1]) - plane[3];
1321
1322 sides = 0;
1323 if(dist1 >= 0)
1324 sides = 1;
1325 if(dist2 < 0)
1326 sides |= 2;
1327
1328 return sides;
1329 }
1330
1331 /*
1332 =================
1333 RadiusFromBounds
1334 =================
1335 */
RadiusFromBounds(const vec3_t mins,const vec3_t maxs)1336 float RadiusFromBounds(const vec3_t mins, const vec3_t maxs)
1337 {
1338 int i;
1339 vec3_t corner;
1340 float a, b;
1341
1342 for(i = 0; i < 3; i++)
1343 {
1344 a = fabs(mins[i]);
1345 b = fabs(maxs[i]);
1346 corner[i] = a > b ? a : b;
1347 }
1348
1349 return VectorLength(corner);
1350 }
1351
1352
ClearBounds(vec3_t mins,vec3_t maxs)1353 void ClearBounds(vec3_t mins, vec3_t maxs)
1354 {
1355 mins[0] = mins[1] = mins[2] = 99999;
1356 maxs[0] = maxs[1] = maxs[2] = -99999;
1357 }
1358
AddPointToBounds(const vec3_t v,vec3_t mins,vec3_t maxs)1359 void AddPointToBounds(const vec3_t v, vec3_t mins, vec3_t maxs)
1360 {
1361 if(v[0] < mins[0])
1362 {
1363 mins[0] = v[0];
1364 }
1365 if(v[0] > maxs[0])
1366 {
1367 maxs[0] = v[0];
1368 }
1369
1370 if(v[1] < mins[1])
1371 {
1372 mins[1] = v[1];
1373 }
1374 if(v[1] > maxs[1])
1375 {
1376 maxs[1] = v[1];
1377 }
1378
1379 if(v[2] < mins[2])
1380 {
1381 mins[2] = v[2];
1382 }
1383 if(v[2] > maxs[2])
1384 {
1385 maxs[2] = v[2];
1386 }
1387 }
1388
BoundsIntersect(const vec3_t mins,const vec3_t maxs,const vec3_t mins2,const vec3_t maxs2)1389 qboolean BoundsIntersect(const vec3_t mins, const vec3_t maxs, const vec3_t mins2, const vec3_t maxs2)
1390 {
1391 if(maxs[0] < mins2[0] ||
1392 maxs[1] < mins2[1] || maxs[2] < mins2[2] || mins[0] > maxs2[0] || mins[1] > maxs2[1] || mins[2] > maxs2[2])
1393 {
1394 return qfalse;
1395 }
1396
1397 return qtrue;
1398 }
1399
BoundsIntersectSphere(const vec3_t mins,const vec3_t maxs,const vec3_t origin,vec_t radius)1400 qboolean BoundsIntersectSphere(const vec3_t mins, const vec3_t maxs, const vec3_t origin, vec_t radius)
1401 {
1402 if(origin[0] - radius > maxs[0] ||
1403 origin[0] + radius < mins[0] ||
1404 origin[1] - radius > maxs[1] ||
1405 origin[1] + radius < mins[1] || origin[2] - radius > maxs[2] || origin[2] + radius < mins[2])
1406 {
1407 return qfalse;
1408 }
1409
1410 return qtrue;
1411 }
1412
BoundsIntersectPoint(const vec3_t mins,const vec3_t maxs,const vec3_t origin)1413 qboolean BoundsIntersectPoint(const vec3_t mins, const vec3_t maxs, const vec3_t origin)
1414 {
1415 if(origin[0] > maxs[0] ||
1416 origin[0] < mins[0] || origin[1] > maxs[1] || origin[1] < mins[1] || origin[2] > maxs[2] || origin[2] < mins[2])
1417 {
1418 return qfalse;
1419 }
1420
1421 return qtrue;
1422 }
1423
VectorNormalize(vec3_t v)1424 vec_t VectorNormalize(vec3_t v)
1425 {
1426 // NOTE: TTimo - Apple G4 altivec source uses double?
1427 float length, ilength;
1428
1429 length = v[0] * v[0] + v[1] * v[1] + v[2] * v[2];
1430 length = sqrt(length);
1431
1432 if(length)
1433 {
1434 ilength = 1 / length;
1435 v[0] *= ilength;
1436 v[1] *= ilength;
1437 v[2] *= ilength;
1438 }
1439
1440 return length;
1441 }
1442
VectorNormalize2(const vec3_t v,vec3_t out)1443 vec_t VectorNormalize2(const vec3_t v, vec3_t out)
1444 {
1445 float length, ilength;
1446
1447 length = v[0] * v[0] + v[1] * v[1] + v[2] * v[2];
1448 length = sqrt(length);
1449
1450 if(length)
1451 {
1452 ilength = 1 / length;
1453 out[0] = v[0] * ilength;
1454 out[1] = v[1] * ilength;
1455 out[2] = v[2] * ilength;
1456 }
1457 else
1458 {
1459 VectorClear(out);
1460 }
1461
1462 return length;
1463
1464 }
1465
_VectorMA(const vec3_t veca,float scale,const vec3_t vecb,vec3_t vecc)1466 void _VectorMA(const vec3_t veca, float scale, const vec3_t vecb, vec3_t vecc)
1467 {
1468 vecc[0] = veca[0] + scale * vecb[0];
1469 vecc[1] = veca[1] + scale * vecb[1];
1470 vecc[2] = veca[2] + scale * vecb[2];
1471 }
1472
1473
_DotProduct(const vec3_t v1,const vec3_t v2)1474 vec_t _DotProduct(const vec3_t v1, const vec3_t v2)
1475 {
1476 return v1[0] * v2[0] + v1[1] * v2[1] + v1[2] * v2[2];
1477 }
1478
_VectorSubtract(const vec3_t veca,const vec3_t vecb,vec3_t out)1479 void _VectorSubtract(const vec3_t veca, const vec3_t vecb, vec3_t out)
1480 {
1481 out[0] = veca[0] - vecb[0];
1482 out[1] = veca[1] - vecb[1];
1483 out[2] = veca[2] - vecb[2];
1484 }
1485
_VectorAdd(const vec3_t veca,const vec3_t vecb,vec3_t out)1486 void _VectorAdd(const vec3_t veca, const vec3_t vecb, vec3_t out)
1487 {
1488 out[0] = veca[0] + vecb[0];
1489 out[1] = veca[1] + vecb[1];
1490 out[2] = veca[2] + vecb[2];
1491 }
1492
_VectorCopy(const vec3_t in,vec3_t out)1493 void _VectorCopy(const vec3_t in, vec3_t out)
1494 {
1495 out[0] = in[0];
1496 out[1] = in[1];
1497 out[2] = in[2];
1498 }
1499
_VectorScale(const vec3_t in,vec_t scale,vec3_t out)1500 void _VectorScale(const vec3_t in, vec_t scale, vec3_t out)
1501 {
1502 out[0] = in[0] * scale;
1503 out[1] = in[1] * scale;
1504 out[2] = in[2] * scale;
1505 }
1506
Vector4Scale(const vec4_t in,vec_t scale,vec4_t out)1507 void Vector4Scale(const vec4_t in, vec_t scale, vec4_t out)
1508 {
1509 out[0] = in[0] * scale;
1510 out[1] = in[1] * scale;
1511 out[2] = in[2] * scale;
1512 out[3] = in[3] * scale;
1513 }
1514
NearestPowerOfTwo(int val)1515 int NearestPowerOfTwo(int val)
1516 {
1517 int answer;
1518
1519 for(answer = 1; answer < val; answer <<= 1)
1520 ;
1521 return answer;
1522 }
1523
Q_log2(int val)1524 int Q_log2(int val)
1525 {
1526 int answer;
1527
1528 answer = 0;
1529 while((val >>= 1) != 0)
1530 {
1531 answer++;
1532 }
1533 return answer;
1534 }
1535
1536
1537
1538 /*
1539 =================
1540 PlaneTypeForNormal
1541 =================
1542 */
1543 /*
1544 int PlaneTypeForNormal (vec3_t normal) {
1545 if ( normal[0] == 1.0 )
1546 return PLANE_X;
1547 if ( normal[1] == 1.0 )
1548 return PLANE_Y;
1549 if ( normal[2] == 1.0 )
1550 return PLANE_Z;
1551
1552 return PLANE_NON_AXIAL;
1553 }
1554 */
1555
1556
1557
AxisMultiply(float in1[3][3],float in2[3][3],float out[3][3])1558 void AxisMultiply(float in1[3][3], float in2[3][3], float out[3][3])
1559 {
1560 out[0][0] = in1[0][0] * in2[0][0] + in1[0][1] * in2[1][0] + in1[0][2] * in2[2][0];
1561 out[0][1] = in1[0][0] * in2[0][1] + in1[0][1] * in2[1][1] + in1[0][2] * in2[2][1];
1562 out[0][2] = in1[0][0] * in2[0][2] + in1[0][1] * in2[1][2] + in1[0][2] * in2[2][2];
1563 out[1][0] = in1[1][0] * in2[0][0] + in1[1][1] * in2[1][0] + in1[1][2] * in2[2][0];
1564 out[1][1] = in1[1][0] * in2[0][1] + in1[1][1] * in2[1][1] + in1[1][2] * in2[2][1];
1565 out[1][2] = in1[1][0] * in2[0][2] + in1[1][1] * in2[1][2] + in1[1][2] * in2[2][2];
1566 out[2][0] = in1[2][0] * in2[0][0] + in1[2][1] * in2[1][0] + in1[2][2] * in2[2][0];
1567 out[2][1] = in1[2][0] * in2[0][1] + in1[2][1] * in2[1][1] + in1[2][2] * in2[2][1];
1568 out[2][2] = in1[2][0] * in2[0][2] + in1[2][1] * in2[1][2] + in1[2][2] * in2[2][2];
1569 }
1570
1571
AngleVectors(const vec3_t angles,vec3_t forward,vec3_t right,vec3_t up)1572 void AngleVectors(const vec3_t angles, vec3_t forward, vec3_t right, vec3_t up)
1573 {
1574 float angle;
1575 static float sr, sp, sy, cr, cp, cy;
1576
1577 // static to help MS compiler fp bugs
1578
1579 angle = angles[YAW] * (M_PI * 2 / 360);
1580 sy = sin(angle);
1581 cy = cos(angle);
1582 angle = angles[PITCH] * (M_PI * 2 / 360);
1583 sp = sin(angle);
1584 cp = cos(angle);
1585 angle = angles[ROLL] * (M_PI * 2 / 360);
1586 sr = sin(angle);
1587 cr = cos(angle);
1588
1589 if(forward)
1590 {
1591 forward[0] = cp * cy;
1592 forward[1] = cp * sy;
1593 forward[2] = -sp;
1594 }
1595 if(right)
1596 {
1597 right[0] = (-1 * sr * sp * cy + -1 * cr * -sy);
1598 right[1] = (-1 * sr * sp * sy + -1 * cr * cy);
1599 right[2] = -1 * sr * cp;
1600 }
1601 if(up)
1602 {
1603 up[0] = (cr * sp * cy + -sr * -sy);
1604 up[1] = (cr * sp * sy + -sr * cy);
1605 up[2] = cr * cp;
1606 }
1607 }
1608
1609 /*
1610 ** assumes "src" is normalized
1611 */
PerpendicularVector(vec3_t dst,const vec3_t src)1612 void PerpendicularVector(vec3_t dst, const vec3_t src)
1613 {
1614 int pos;
1615 float minelem;
1616
1617 if(src[0])
1618 {
1619 dst[0] = 0;
1620 if(src[1])
1621 {
1622 dst[1] = 0;
1623 if(src[2])
1624 {
1625 dst[2] = 0;
1626 pos = 0;
1627 minelem = fabs(src[0]);
1628 if(Q_fabs(src[1]) < minelem)
1629 {
1630 pos = 1;
1631 minelem = fabs(src[1]);
1632 }
1633
1634 if(Q_fabs(src[2]) < minelem)
1635 pos = 2;
1636
1637 dst[pos] = 1;
1638 dst[0] -= src[pos] * src[0];
1639 dst[1] -= src[pos] * src[1];
1640 dst[2] -= src[pos] * src[2];
1641
1642 VectorNormalize(dst);
1643 }
1644 else
1645 {
1646 dst[2] = 1;
1647 }
1648 }
1649 else
1650 {
1651 dst[1] = 1;
1652 dst[2] = 0;
1653 }
1654 }
1655 else
1656 {
1657 dst[0] = 1;
1658 dst[1] = 0;
1659 dst[2] = 0;
1660 }
1661 }
1662
MatrixIdentity(matrix_t m)1663 void MatrixIdentity(matrix_t m)
1664 {
1665 m[ 0] = 1; m[ 4] = 0; m[ 8] = 0; m[12] = 0;
1666 m[ 1] = 0; m[ 5] = 1; m[ 9] = 0; m[13] = 0;
1667 m[ 2] = 0; m[ 6] = 0; m[10] = 1; m[14] = 0;
1668 m[ 3] = 0; m[ 7] = 0; m[11] = 0; m[15] = 1;
1669 }
1670
MatrixClear(matrix_t m)1671 void MatrixClear(matrix_t m)
1672 {
1673 m[ 0] = 0; m[ 4] = 0; m[ 8] = 0; m[12] = 0;
1674 m[ 1] = 0; m[ 5] = 0; m[ 9] = 0; m[13] = 0;
1675 m[ 2] = 0; m[ 6] = 0; m[10] = 0; m[14] = 0;
1676 m[ 3] = 0; m[ 7] = 0; m[11] = 0; m[15] = 0;
1677 }
1678
MatrixCopy(const matrix_t in,matrix_t out)1679 void MatrixCopy(const matrix_t in, matrix_t out)
1680 {
1681 #if id386_sse && defined __GNUC__
1682 asm volatile
1683 (
1684 "movups (%%edx), %%xmm0\n"
1685 "movups 0x10(%%edx), %%xmm1\n"
1686 "movups 0x20(%%edx), %%xmm2\n"
1687 "movups 0x30(%%edx), %%xmm3\n"
1688
1689 "movups %%xmm0, (%%eax)\n"
1690 "movups %%xmm1, 0x10(%%eax)\n"
1691 "movups %%xmm2, 0x20(%%eax)\n"
1692 "movups %%xmm3, 0x30(%%eax)\n"
1693 :
1694 : "a"( out ), "d"( in )
1695 : "memory"
1696 );
1697 #elif id386_3dnow && defined __GNUC__
1698 femms();
1699 asm volatile
1700 (
1701 "movq (%%edx), %%mm0\n"
1702 "movq 8(%%edx), %%mm1\n"
1703 "movq 16(%%edx), %%mm2\n"
1704 "movq 24(%%edx), %%mm3\n"
1705 "movq 32(%%edx), %%mm4\n"
1706 "movq 40(%%edx), %%mm5\n"
1707 "movq 48(%%edx), %%mm6\n"
1708 "movq 56(%%edx), %%mm7\n"
1709
1710 "movq %%mm0, (%%eax)\n"
1711 "movq %%mm1, 8(%%eax)\n"
1712 "movq %%mm2, 16(%%eax)\n"
1713 "movq %%mm3, 24(%%eax)\n"
1714 "movq %%mm4, 32(%%eax)\n"
1715 "movq %%mm5, 40(%%eax)\n"
1716 "movq %%mm6, 48(%%eax)\n"
1717 "movq %%mm7, 56(%%eax)\n"
1718 :
1719 : "a"( out ), "d"( in )
1720 : "memory"
1721 );
1722 femms();
1723 #else
1724 out[ 0] = in[ 0]; out[ 4] = in[ 4]; out[ 8] = in[ 8]; out[12] = in[12];
1725 out[ 1] = in[ 1]; out[ 5] = in[ 5]; out[ 9] = in[ 9]; out[13] = in[13];
1726 out[ 2] = in[ 2]; out[ 6] = in[ 6]; out[10] = in[10]; out[14] = in[14];
1727 out[ 3] = in[ 3]; out[ 7] = in[ 7]; out[11] = in[11]; out[15] = in[15];
1728 #endif
1729 }
1730
MatrixTransposeIntoXMM(const matrix_t m)1731 void MatrixTransposeIntoXMM(const matrix_t m)
1732 {
1733 #if id386_sse && defined __GNUC__
1734 asm volatile
1735 ( // reg[0] | reg[1] | reg[2] | reg[3]
1736 // load transpose into XMM registers
1737 "movlps (%%eax), %%xmm4\n" // m[0][0] | m[0][1] | - | -
1738 "movhps 16(%%eax), %%xmm4\n" // m[0][0] | m[0][1] | m[1][0] | m[1][1]
1739
1740 "movlps 32(%%eax), %%xmm3\n" // m[2][0] | m[2][1] | - | -
1741 "movhps 48(%%eax), %%xmm3\n" // m[2][0] | m[2][1] | m[3][0] | m[3][1]
1742
1743 "movups %%xmm4, %%xmm5\n" // m[0][0] | m[0][1] | m[1][0] | m[1][1]
1744
1745 // 0x88 = 10 00 | 10 00 <-> 00 10 | 00 10 xmm4[00] xmm4[10] xmm3[00] xmm3[10]
1746 "shufps $0x88, %%xmm3, %%xmm4\n" // m[0][0] | m[1][0] | m[2][0] | m[3][0]
1747
1748 // 0xDD = 11 01 | 11 01 <-> 01 11 | 01 11 xmm5[01] xmm5[11] xmm3[01] xmm3[11]
1749 "shufps $0xDD, %%xmm3, %%xmm5\n" // m[0][1] | m[1][1] | m[2][1] | m[3][1]
1750
1751 "movlps 8(%%eax), %%xmm6\n" // m[0][2] | m[0][3] | - | -
1752 "movhps 24(%%eax), %%xmm6\n" // m[0][2] | m[0][3] | m[1][2] | m[1][3]
1753
1754 "movlps 40(%%eax), %%xmm3\n" // m[2][2] | m[2][3] | - | -
1755 "movhps 56(%%eax), %%xmm3\n" // m[2][2] | m[2][3] | m[3][2] | m[3][3]
1756
1757 "movups %%xmm6, %%xmm7\n" // m[0][2] | m[0][3] | m[1][2] | m[1][3]
1758
1759 // 0x88 = 10 00 | 10 00 <-> 00 10 | 00 10 xmm6[00] xmm6[10] xmm3[00] xmm3[10]
1760 "shufps $0x88, %%xmm3, %%xmm6\n" // m[0][2] | m[1][2] | m[2][2] | m[3][2]
1761
1762 // 0xDD = 11 01 | 11 01 <-> 01 11 | 01 11 xmm7[01] xmm7[11] xmm3[01] xmm3[11]
1763 "shufps $0xDD, %%xmm3, %%xmm7\n" // m[0][3] | m[1][3] | m[2][3] | m[3][3]
1764 :
1765 : "a"( m )
1766 : "memory"
1767 );
1768 #endif
1769 }
1770
MatrixTranspose(const matrix_t in,matrix_t out)1771 void MatrixTranspose(const matrix_t in, matrix_t out)
1772 {
1773 #if id386_sse && defined __GNUC__
1774 // transpose the matrix into the xmm4-7
1775 MatrixTransposeIntoXMM(in);
1776
1777 asm volatile
1778 (
1779 "movups %%xmm4, (%%eax)\n"
1780 "movups %%xmm5, 0x10(%%eax)\n"
1781 "movups %%xmm6, 0x20(%%eax)\n"
1782 "movups %%xmm7, 0x30(%%eax)\n"
1783 :
1784 : "a"( out )
1785 : "memory"
1786 );
1787 #else
1788 out[ 0] = in[ 0]; out[ 1] = in[ 4]; out[ 2] = in[ 8]; out[ 3] = in[12];
1789 out[ 4] = in[ 1]; out[ 5] = in[ 5]; out[ 6] = in[ 9]; out[ 7] = in[13];
1790 out[ 8] = in[ 2]; out[ 9] = in[ 6]; out[10] = in[10]; out[11] = in[14];
1791 out[12] = in[ 3]; out[13] = in[ 7]; out[14] = in[11]; out[15] = in[15];
1792 #endif
1793 }
1794
MatrixSetupXRotation(matrix_t m,vec_t degrees)1795 void MatrixSetupXRotation(matrix_t m, vec_t degrees)
1796 {
1797 vec_t a = DEG2RAD(degrees);
1798
1799 m[ 0] = 1; m[ 4] = 0; m[ 8] = 0; m[12] = 0;
1800 m[ 1] = 0; m[ 5] = cos(a); m[ 9] =-sin(a); m[13] = 0;
1801 m[ 2] = 0; m[ 6] = sin(a); m[10] = cos(a); m[14] = 0;
1802 m[ 3] = 0; m[ 7] = 0; m[11] = 0; m[15] = 1;
1803 }
1804
MatrixSetupYRotation(matrix_t m,vec_t degrees)1805 void MatrixSetupYRotation(matrix_t m, vec_t degrees)
1806 {
1807 vec_t a = DEG2RAD(degrees);
1808
1809 m[ 0] = cos(a); m[ 4] = 0; m[ 8] = sin(a); m[12] = 0;
1810 m[ 1] = 0; m[ 5] = 1; m[ 9] = 0; m[13] = 0;
1811 m[ 2] =-sin(a); m[ 6] = 0; m[10] = cos(a); m[14] = 0;
1812 m[ 3] = 0; m[ 7] = 0; m[11] = 0; m[15] = 1;
1813 }
1814
MatrixSetupZRotation(matrix_t m,vec_t degrees)1815 void MatrixSetupZRotation(matrix_t m, vec_t degrees)
1816 {
1817 vec_t a = DEG2RAD(degrees);
1818
1819 m[ 0] = cos(a); m[ 4] =-sin(a); m[ 8] = 0; m[12] = 0;
1820 m[ 1] = sin(a); m[ 5] = cos(a); m[ 9] = 0; m[13] = 0;
1821 m[ 2] = 0; m[ 6] = 0; m[10] = 1; m[14] = 0;
1822 m[ 3] = 0; m[ 7] = 0; m[11] = 0; m[15] = 1;
1823 }
1824
MatrixSetupTranslation(matrix_t m,vec_t x,vec_t y,vec_t z)1825 void MatrixSetupTranslation(matrix_t m, vec_t x, vec_t y, vec_t z)
1826 {
1827 m[ 0] = 1; m[ 4] = 0; m[ 8] = 0; m[12] = x;
1828 m[ 1] = 0; m[ 5] = 1; m[ 9] = 0; m[13] = y;
1829 m[ 2] = 0; m[ 6] = 0; m[10] = 1; m[14] = z;
1830 m[ 3] = 0; m[ 7] = 0; m[11] = 0; m[15] = 1;
1831 }
1832
MatrixSetupScale(matrix_t m,vec_t x,vec_t y,vec_t z)1833 void MatrixSetupScale(matrix_t m, vec_t x, vec_t y, vec_t z)
1834 {
1835 m[ 0] = x; m[ 4] = 0; m[ 8] = 0; m[12] = 0;
1836 m[ 1] = 0; m[ 5] = y; m[ 9] = 0; m[13] = 0;
1837 m[ 2] = 0; m[ 6] = 0; m[10] = z; m[14] = 0;
1838 m[ 3] = 0; m[ 7] = 0; m[11] = 0; m[15] = 1;
1839 }
1840
MatrixSetupShear(matrix_t m,vec_t x,vec_t y)1841 void MatrixSetupShear(matrix_t m, vec_t x, vec_t y)
1842 {
1843 m[ 0] = 1; m[ 4] = x; m[ 8] = 0; m[12] = 0;
1844 m[ 1] = y; m[ 5] = 1; m[ 9] = 0; m[13] = 0;
1845 m[ 2] = 0; m[ 6] = 0; m[10] = 1; m[14] = 0;
1846 m[ 3] = 0; m[ 7] = 0; m[11] = 0; m[15] = 1;
1847 }
1848
MatrixMultiply(const matrix_t a,const matrix_t b,matrix_t out)1849 void MatrixMultiply(const matrix_t a, const matrix_t b, matrix_t out)
1850 {
1851 #if id386_sse && defined __GNUC__
1852 asm volatile
1853 (
1854 // load m2 into the xmm4-7
1855 "movups (%%edx), %%xmm4\n" // a[0][0] | a[0][1] | a[0][2] | a[0][3]
1856 "movups 16(%%edx), %%xmm5\n" // a[1][0] | a[1][1] | a[1][2] | a[1][3]
1857 "movups 32(%%edx), %%xmm6\n" // a[2][0] | a[2][1] | a[2][2] | a[2][3]
1858 "movups 48(%%edx), %%xmm7\n" // a[3][0] | a[3][1] | a[3][2] | a[3][3]
1859
1860
1861 // calculate first row of out
1862 "movups (%%eax), %%xmm0\n" // b[0][0] | b[0][1] | b[0][2] | b[0][3]
1863 "movups %%xmm0, %%xmm1\n" // b[0][0] | b[0][1] | b[0][2] | b[0][3]
1864 "movups %%xmm0, %%xmm2\n" // b[0][0] | b[0][1] | b[0][2] | b[0][3]
1865 "movups %%xmm0, %%xmm3\n" // b[0][0] | b[0][1] | b[0][2] | b[0][3]
1866
1867 // 0x00 = 00 00 | 00 00 <-> 00 00 | 00 00 xmmx[00] xmmx[00] xmmx[00] xmmx[00]
1868 "shufps $0x00, %%xmm0, %%xmm0\n" // b[0][0] | b[0][0] | b[0][0] | b[0][0]
1869
1870 // 0x55 = 01 01 | 01 01 <-> 01 01 | 01 01 xmm1[01] xmm1[01] xmm1[01] xmm1[01]
1871 "shufps $0x55, %%xmm1, %%xmm1\n" // b[0][1] | b[0][1] | b[0][1] | b[0][1]
1872
1873 // 0xAA = 10 10 | 10 10 <-> 10 10 | 10 10 xmm2[10] xmm2[10] xmm2[10] xmm2[10]
1874 "shufps $0xAA, %%xmm2, %%xmm2\n" // b[0][2] | b[0][2] | b[0][2] | b[0][2]
1875
1876 // 0xFF = 11 11 | 11 11 <-> 11 11 | 11 11 xmm3[11] xmm3[11] xmm3[11] xmm3[11]
1877 "shufps $0xFF, %%xmm3, %%xmm3\n" // b[0][3] | b[0][3] | b[0][3] | b[0][3]
1878
1879 "mulps %%xmm4, %%xmm0\n" // b[0][0]*a[0][0] | b[0][0]*a[0][1] | b[0][0]*a[0][2] | b[0][0]*a[0][3]
1880 "mulps %%xmm5, %%xmm1\n" // b[0][1]*a[1][0] | b[0][1]*a[1][1] | b[0][1]*a[1][2] | b[0][1]*a[1][3]
1881 "mulps %%xmm6, %%xmm2\n" // b[0][2]*a[2][0] | b[0][2]*a[2][1] | b[0][2]*a[2][2] | b[0][2]*a[2][3]
1882 "mulps %%xmm7, %%xmm3\n" // b[0][3]*a[3][0] | b[0][3]*a[3][1] | b[0][3]*a[3][2] | b[0][3]*a[3][3]
1883
1884 "addps %%xmm0, %%xmm1\n" // b[0][0]*a[0][0]+ | b[0][0]*a[0][1]+ | b[0][0]*a[0][2]+ | b[0][0]*a[0][3]+
1885 // b[0][1]*a[1][0] | b[0][1]*a[1][1] | b[0][1]*a[1][2] | b[0][1]*a[1][3]
1886
1887 "addps %%xmm1, %%xmm2\n" // b[0][0]*a[0][0]+ | b[0][0]*a[0][1]+ | b[0][0]*a[0][2]+ | b[0][0]*a[0][3]+
1888 // b[0][1]*a[1][0]+ | b[0][1]*a[1][1]+ | b[0][1]*a[1][2]+ | b[0][1]*a[1][3]+
1889 // b[0][2]*a[2][0] | b[0][2]*a[2][1] | b[0][2]*a[2][2] | b[0][2]*a[2][3]
1890
1891 "addps %%xmm2, %%xmm3\n" // b[0][0]*a[0][0]+ | b[0][0]*a[0][1]+ | b[0][0]*a[0][2]+ | b[0][0]*a[0][3]+
1892 // b[0][1]*a[1][0]+ | b[0][1]*a[1][1]+ | b[0][1]*a[1][2]+ | b[0][1]*a[1][3]+
1893 // b[0][2]*a[2][0]+ | b[0][2]*a[2][1]+ | b[0][2]*a[2][2]+ | b[0][2]*a[2][3]+
1894 // b[0][3]*a[3][0] | b[0][3]*a[3][1] | b[0][3]*a[3][2] | b[0][3]*a[3][3]
1895
1896 "movups %%xmm3, (%%ecx)\n"
1897
1898 // calculate second row of out
1899 "movups 16(%%eax), %%xmm0\n"
1900 "movups %%xmm0, %%xmm1\n"
1901 "movups %%xmm0, %%xmm2\n"
1902 "movups %%xmm0, %%xmm3\n"
1903
1904 "shufps $0x00, %%xmm0, %%xmm0\n"
1905 "shufps $0x55, %%xmm1, %%xmm1\n"
1906 "shufps $0xAA, %%xmm2, %%xmm2\n"
1907 "shufps $0xFF, %%xmm3, %%xmm3\n"
1908
1909 "mulps %%xmm4, %%xmm0\n"
1910 "mulps %%xmm5, %%xmm1\n"
1911 "mulps %%xmm6, %%xmm2\n"
1912 "mulps %%xmm7, %%xmm3\n"
1913
1914 "addps %%xmm0, %%xmm1\n"
1915 "addps %%xmm1, %%xmm2\n"
1916 "addps %%xmm2, %%xmm3\n"
1917
1918 "movups %%xmm3, 16(%%ecx)\n"
1919
1920 // calculate third row of out
1921 "movups 32(%%eax), %%xmm0\n"
1922 "movups %%xmm0, %%xmm1\n"
1923 "movups %%xmm0, %%xmm2\n"
1924 "movups %%xmm0, %%xmm3\n"
1925
1926 "shufps $0x00, %%xmm0, %%xmm0\n"
1927 "shufps $0x55, %%xmm1, %%xmm1\n"
1928 "shufps $0xAA, %%xmm2, %%xmm2\n"
1929 "shufps $0xFF, %%xmm3, %%xmm3\n"
1930
1931 "mulps %%xmm4, %%xmm0\n"
1932 "mulps %%xmm5, %%xmm1\n"
1933 "mulps %%xmm6, %%xmm2\n"
1934 "mulps %%xmm7, %%xmm3\n"
1935
1936 "addps %%xmm0, %%xmm1\n"
1937 "addps %%xmm1, %%xmm2\n"
1938 "addps %%xmm2, %%xmm3\n"
1939
1940 "movups %%xmm3, 32(%%ecx)\n"
1941
1942 // calculate fourth row of out
1943 "movups 48(%%eax), %%xmm0\n"
1944 "movups %%xmm0, %%xmm1\n"
1945 "movups %%xmm0, %%xmm2\n"
1946 "movups %%xmm0, %%xmm3\n"
1947
1948 "shufps $0x00, %%xmm0, %%xmm0\n"
1949 "shufps $0x55, %%xmm1, %%xmm1\n"
1950 "shufps $0xAA, %%xmm2, %%xmm2\n"
1951 "shufps $0xFF, %%xmm3, %%xmm3\n"
1952
1953 "mulps %%xmm4, %%xmm0\n"
1954 "mulps %%xmm5, %%xmm1\n"
1955 "mulps %%xmm6, %%xmm2\n"
1956 "mulps %%xmm7, %%xmm3\n"
1957
1958 "addps %%xmm0, %%xmm1\n"
1959 "addps %%xmm1, %%xmm2\n"
1960 "addps %%xmm2, %%xmm3\n"
1961
1962 "movups %%xmm3, 48(%%ecx)\n"
1963 :
1964 : "a"( b ), "d"( a ), "c"( out )
1965 : "memory"
1966 );
1967 #else
1968 out[ 0] = b[ 0]*a[ 0] + b[ 1]*a[ 4] + b[ 2]*a[ 8] + b[ 3]*a[12];
1969 out[ 1] = b[ 0]*a[ 1] + b[ 1]*a[ 5] + b[ 2]*a[ 9] + b[ 3]*a[13];
1970 out[ 2] = b[ 0]*a[ 2] + b[ 1]*a[ 6] + b[ 2]*a[10] + b[ 3]*a[14];
1971 out[ 3] = b[ 0]*a[ 3] + b[ 1]*a[ 7] + b[ 2]*a[11] + b[ 3]*a[15];
1972
1973 out[ 4] = b[ 4]*a[ 0] + b[ 5]*a[ 4] + b[ 6]*a[ 8] + b[ 7]*a[12];
1974 out[ 5] = b[ 4]*a[ 1] + b[ 5]*a[ 5] + b[ 6]*a[ 9] + b[ 7]*a[13];
1975 out[ 6] = b[ 4]*a[ 2] + b[ 5]*a[ 6] + b[ 6]*a[10] + b[ 7]*a[14];
1976 out[ 7] = b[ 4]*a[ 3] + b[ 5]*a[ 7] + b[ 6]*a[11] + b[ 7]*a[15];
1977
1978 out[ 8] = b[ 8]*a[ 0] + b[ 9]*a[ 4] + b[10]*a[ 8] + b[11]*a[12];
1979 out[ 9] = b[ 8]*a[ 1] + b[ 9]*a[ 5] + b[10]*a[ 9] + b[11]*a[13];
1980 out[10] = b[ 8]*a[ 2] + b[ 9]*a[ 6] + b[10]*a[10] + b[11]*a[14];
1981 out[11] = b[ 8]*a[ 3] + b[ 9]*a[ 7] + b[10]*a[11] + b[11]*a[15];
1982
1983 out[12] = b[12]*a[ 0] + b[13]*a[ 4] + b[14]*a[ 8] + b[15]*a[12];
1984 out[13] = b[12]*a[ 1] + b[13]*a[ 5] + b[14]*a[ 9] + b[15]*a[13];
1985 out[14] = b[12]*a[ 2] + b[13]*a[ 6] + b[14]*a[10] + b[15]*a[14];
1986 out[15] = b[12]*a[ 3] + b[13]*a[ 7] + b[14]*a[11] + b[15]*a[15];
1987 #endif
1988 }
1989
MatrixMultiply2(matrix_t m,matrix_t m2)1990 void MatrixMultiply2(matrix_t m, matrix_t m2)
1991 {
1992 matrix_t tmp;
1993
1994 MatrixCopy(m, tmp);
1995 MatrixMultiply(tmp, m2, m);
1996 }
1997
MatrixMultiplyRotation(matrix_t m,vec_t pitch,vec_t yaw,vec_t roll)1998 void MatrixMultiplyRotation(matrix_t m, vec_t pitch, vec_t yaw, vec_t roll)
1999 {
2000 matrix_t tmp, rot;
2001
2002 MatrixCopy(m, tmp);
2003 MatrixFromAngles(rot, pitch, yaw, roll);
2004
2005 MatrixMultiply(tmp, rot, m);
2006 }
2007
MatrixMultiplyZRotation(matrix_t m,vec_t degrees)2008 void MatrixMultiplyZRotation(matrix_t m, vec_t degrees)
2009 {
2010 matrix_t tmp, rot;
2011
2012 MatrixCopy(m, tmp);
2013 MatrixSetupZRotation(rot, degrees);
2014
2015 MatrixMultiply(tmp, rot, m);
2016 }
2017
MatrixMultiplyTranslation(matrix_t m,vec_t x,vec_t y,vec_t z)2018 void MatrixMultiplyTranslation(matrix_t m, vec_t x, vec_t y, vec_t z)
2019 {
2020 #if 1
2021 matrix_t tmp, trans;
2022
2023 MatrixCopy(m, tmp);
2024 MatrixSetupTranslation(trans, x, y, z);
2025 MatrixMultiply(tmp, trans, m);
2026 #else
2027 m[12] += m[ 0] * x + m[ 4] * y + m[ 8] * z;
2028 m[13] += m[ 1] * x + m[ 5] * y + m[ 9] * z;
2029 m[14] += m[ 2] * x + m[ 6] * y + m[10] * z;
2030 m[15] += m[ 3] * x + m[ 7] * y + m[11] * z;
2031 #endif
2032 }
2033
MatrixMultiplyScale(matrix_t m,vec_t x,vec_t y,vec_t z)2034 void MatrixMultiplyScale(matrix_t m, vec_t x, vec_t y, vec_t z)
2035 {
2036 #if 0
2037 matrix_t tmp, scale;
2038
2039 MatrixCopy(m, tmp);
2040 MatrixSetupScale(scale, x, y, z);
2041 MatrixMultiply(tmp, scale, m);
2042 #else
2043 m[ 0] *= x; m[ 4] *= y; m[ 8] *= z;
2044 m[ 1] *= x; m[ 5] *= y; m[ 9] *= z;
2045 m[ 2] *= x; m[ 6] *= y; m[10] *= z;
2046 m[ 3] *= x; m[ 7] *= y; m[11] *= z;
2047 #endif
2048 }
2049
MatrixMultiplyShear(matrix_t m,vec_t x,vec_t y)2050 void MatrixMultiplyShear(matrix_t m, vec_t x, vec_t y)
2051 {
2052 matrix_t tmp, shear;
2053
2054 MatrixCopy(m, tmp);
2055 MatrixSetupShear(shear, x, y);
2056 MatrixMultiply(tmp, shear, m);
2057 }
2058
MatrixToAngles(const matrix_t m,vec3_t angles)2059 void MatrixToAngles(const matrix_t m, vec3_t angles)
2060 {
2061 #if 1
2062 double theta;
2063 double cp;
2064 double sp;
2065
2066 sp = m[ 2];
2067
2068 // cap off our sin value so that we don't get any NANs
2069 if(sp > 1.0)
2070 {
2071 sp = 1.0;
2072 }
2073 else if(sp < -1.0)
2074 {
2075 sp = -1.0;
2076 }
2077
2078 theta = -asin(sp);
2079 cp = cos(theta);
2080
2081 if(cp > 8192 * FLT_EPSILON)
2082 {
2083 angles[PITCH] = RAD2DEG(theta);
2084 angles[YAW] = RAD2DEG(atan2(m[1], m[0]));
2085 angles[ROLL] = RAD2DEG(atan2(m[6], m[10]));
2086 }
2087 else
2088 {
2089 angles[PITCH] = RAD2DEG(theta);
2090 angles[YAW] = RAD2DEG(-atan2(m[4], m[5]));
2091 angles[ROLL] = 0;
2092 }
2093 #else
2094 double a;
2095 double ca;
2096
2097 a = asin(-m[2]);
2098 ca = cos(a);
2099
2100 if(fabs(ca) > 0.005) // Gimbal lock?
2101 {
2102 angles[PITCH] = RAD2DEG(atan2(m[6] / ca, m[10] / ca));
2103 angles[YAW] = RAD2DEG(a);
2104 angles[ROLL] = RAD2DEG(atan2(m[1] / ca, m[0] / ca));
2105 }
2106 else
2107 {
2108 // Gimbal lock has occurred
2109 angles[PITCH] = RAD2DEG(atan2(-m[9], m[5]));
2110 angles[YAW] = RAD2DEG(a);
2111 angles[ROLL] = 0;
2112 }
2113 #endif
2114 }
2115
2116
MatrixFromAngles(matrix_t m,vec_t pitch,vec_t yaw,vec_t roll)2117 void MatrixFromAngles(matrix_t m, vec_t pitch, vec_t yaw, vec_t roll)
2118 {
2119 static float sr, sp, sy, cr, cp, cy;
2120
2121 // static to help MS compiler fp bugs
2122 sp = sin(DEG2RAD(pitch));
2123 cp = cos(DEG2RAD(pitch));
2124
2125 sy = sin(DEG2RAD(yaw));
2126 cy = cos(DEG2RAD(yaw));
2127
2128 sr = sin(DEG2RAD(roll));
2129 cr = cos(DEG2RAD(roll));
2130
2131 m[ 0] = cp*cy; m[ 4] = (sr*sp*cy+cr*-sy); m[ 8] = (cr*sp*cy+-sr*-sy); m[12] = 0;
2132 m[ 1] = cp*sy; m[ 5] = (sr*sp*sy+cr*cy); m[ 9] = (cr*sp*sy+-sr*cy); m[13] = 0;
2133 m[ 2] = -sp; m[ 6] = sr*cp; m[10] = cr*cp; m[14] = 0;
2134 m[ 3] = 0; m[ 7] = 0; m[11] = 0; m[15] = 1;
2135 }
2136
MatrixFromVectorsFLU(matrix_t m,const vec3_t forward,const vec3_t left,const vec3_t up)2137 void MatrixFromVectorsFLU(matrix_t m, const vec3_t forward, const vec3_t left, const vec3_t up)
2138 {
2139 m[ 0] = forward[0]; m[ 4] = left[0]; m[ 8] = up[0]; m[12] = 0;
2140 m[ 1] = forward[1]; m[ 5] = left[1]; m[ 9] = up[1]; m[13] = 0;
2141 m[ 2] = forward[2]; m[ 6] = left[2]; m[10] = up[2]; m[14] = 0;
2142 m[ 3] = 0; m[ 7] = 0; m[11] = 0; m[15] = 1;
2143 }
2144
MatrixFromVectorsFRU(matrix_t m,const vec3_t forward,const vec3_t right,const vec3_t up)2145 void MatrixFromVectorsFRU(matrix_t m, const vec3_t forward, const vec3_t right, const vec3_t up)
2146 {
2147 m[ 0] = forward[0]; m[ 4] =-right[0]; m[ 8] = up[0]; m[12] = 0;
2148 m[ 1] = forward[1]; m[ 5] =-right[1]; m[ 9] = up[1]; m[13] = 0;
2149 m[ 2] = forward[2]; m[ 6] =-right[2]; m[10] = up[2]; m[14] = 0;
2150 m[ 3] = 0; m[ 7] = 0; m[11] = 0; m[15] = 1;
2151 }
2152
MatrixFromQuat(matrix_t m,const quat_t q)2153 void MatrixFromQuat(matrix_t m, const quat_t q)
2154 {
2155 #if 1
2156 /*
2157 From Quaternion to Matrix and Back
2158 February 27th 2005
2159 J.M.P. van Waveren
2160
2161 http://www.intel.com/cd/ids/developer/asmo-na/eng/293748.htm
2162 */
2163 float x2, y2, z2, w2;
2164 float yy2, xy2;
2165 float xz2, yz2, zz2;
2166 float wz2, wy2, wx2, xx2;
2167
2168 x2 = q[0] + q[0];
2169 y2 = q[1] + q[1];
2170 z2 = q[2] + q[2];
2171 w2 = q[3] + q[3];
2172
2173 yy2 = q[1] * y2;
2174 xy2 = q[0] * y2;
2175
2176 xz2 = q[0] * z2;
2177 yz2 = q[1] * z2;
2178 zz2 = q[2] * z2;
2179
2180 wz2 = q[3] * z2;
2181 wy2 = q[3] * y2;
2182 wx2 = q[3] * x2;
2183 xx2 = q[0] * x2;
2184
2185 m[ 0] = - yy2 - zz2 + 1.0f;
2186 m[ 1] = xy2 + wz2;
2187 m[ 2] = xz2 - wy2;
2188
2189 m[ 4] = xy2 - wz2;
2190 m[ 5] = - xx2 - zz2 + 1.0f;
2191 m[ 6] = yz2 + wx2;
2192
2193 m[ 8] = xz2 + wy2;
2194 m[ 9] = yz2 - wx2;
2195 m[10] = - xx2 - yy2 + 1.0f;
2196
2197 m[ 3] = m[ 7] = m[11] = m[12] = m[13] = m[14] = 0;
2198 m[15] = 1;
2199
2200 #else
2201 /*
2202 http://www.gamedev.net/reference/articles/article1691.asp#Q54
2203 Q54. How do I convert a quaternion to a rotation matrix?
2204
2205 Assuming that a quaternion has been created in the form:
2206
2207 Q = |X Y Z W|
2208
2209 Then the quaternion can then be converted into a 4x4 rotation
2210 matrix using the following expression (Warning: you might have to
2211 transpose this matrix if you (do not) follow the OpenGL order!):
2212
2213 ? 2 2 ?
2214 ? 1 - (2Y + 2Z ) 2XY - 2ZW 2XZ + 2YW ?
2215 ? ?
2216 ? 2 2 ?
2217 M = ? 2XY + 2ZW 1 - (2X + 2Z ) 2YZ - 2XW ?
2218 ? ?
2219 ? 2 2 ?
2220 ? 2XZ - 2YW 2YZ + 2XW 1 - (2X + 2Y ) ?
2221 ? ?
2222 */
2223
2224 // http://www.euclideanspace.com/maths/geometry/rotations/conversions/quaternionToMatrix/index.htm
2225
2226 float xx, xy, xz, xw, yy, yz, yw, zz, zw;
2227
2228 xx = q[0] * q[0];
2229 xy = q[0] * q[1];
2230 xz = q[0] * q[2];
2231 xw = q[0] * q[3];
2232 yy = q[1] * q[1];
2233 yz = q[1] * q[2];
2234 yw = q[1] * q[3];
2235 zz = q[2] * q[2];
2236 zw = q[2] * q[3];
2237
2238 m[ 0] = 1 - 2 * ( yy + zz );
2239 m[ 1] = 2 * ( xy + zw );
2240 m[ 2] = 2 * ( xz - yw );
2241 m[ 4] = 2 * ( xy - zw );
2242 m[ 5] = 1 - 2 * ( xx + zz );
2243 m[ 6] = 2 * ( yz + xw );
2244 m[ 8] = 2 * ( xz + yw );
2245 m[ 9] = 2 * ( yz - xw );
2246 m[10] = 1 - 2 * ( xx + yy );
2247
2248 m[ 3] = m[ 7] = m[11] = m[12] = m[13] = m[14] = 0;
2249 m[15] = 1;
2250 #endif
2251 }
2252
MatrixFromPlanes(matrix_t m,const vec4_t left,const vec4_t right,const vec4_t bottom,const vec4_t top,const vec4_t front,const vec4_t back)2253 void MatrixFromPlanes(matrix_t m, const vec4_t left, const vec4_t right, const vec4_t bottom, const vec4_t top, const vec4_t front, const vec4_t back)
2254 {
2255 m[ 0] = (right[0] - left[0]) / 2;
2256 m[ 1] = (top[0] - bottom[0]) / 2;
2257 m[ 2] = (back[0] - front[0]) / 2;
2258 m[ 3] = right[0] - (right[0] - left[0]) / 2;
2259 m[ 4] = (right[1] - left[1]) / 2;
2260 m[ 5] = (top[1] - bottom[1]) / 2;
2261 m[ 6] = (back[1] - front[1]) / 2;
2262 m[ 7] = right[1] - (right[1] - left[1]) / 2;
2263 m[ 8] = (right[1] - left[1]) / 2;
2264 m[ 9] = (top[1] - bottom[1]) / 2;
2265 m[10] = (back[1] - front[1]) / 2;
2266 m[11] = right[1] - (right[1] - left[1]) / 2;
2267 m[12] = (right[1] - left[1]) / 2;
2268 m[13] = (top[1] - bottom[1]) / 2;
2269 m[14] = (back[1] - front[1]) / 2;
2270 m[15] = right[1] - (right[1] - left[1]) / 2;
2271 }
2272
MatrixToVectorsFLU(const matrix_t m,vec3_t forward,vec3_t left,vec3_t up)2273 void MatrixToVectorsFLU(const matrix_t m, vec3_t forward, vec3_t left, vec3_t up)
2274 {
2275 if(forward)
2276 {
2277 forward[0] = m[ 0]; // cp*cy;
2278 forward[1] = m[ 1]; // cp*sy;
2279 forward[2] = m[ 2]; //-sp;
2280 }
2281
2282 if(left)
2283 {
2284 left[0] = m[ 4]; // sr*sp*cy+cr*-sy;
2285 left[1] = m[ 5]; // sr*sp*sy+cr*cy;
2286 left[2] = m[ 6]; // sr*cp;
2287 }
2288
2289 if(up)
2290 {
2291 up[0] = m[ 8]; // cr*sp*cy+-sr*-sy;
2292 up[1] = m[ 9]; // cr*sp*sy+-sr*cy;
2293 up[2] = m[10]; // cr*cp;
2294 }
2295 }
2296
MatrixToVectorsFRU(const matrix_t m,vec3_t forward,vec3_t right,vec3_t up)2297 void MatrixToVectorsFRU(const matrix_t m, vec3_t forward, vec3_t right, vec3_t up)
2298 {
2299 if(forward)
2300 {
2301 forward[0] = m[ 0];
2302 forward[1] = m[ 1];
2303 forward[2] = m[ 2];
2304 }
2305
2306 if(right)
2307 {
2308 right[0] =-m[ 4];
2309 right[1] =-m[ 5];
2310 right[2] =-m[ 6];
2311 }
2312
2313 if(up)
2314 {
2315 up[0] = m[ 8];
2316 up[1] = m[ 9];
2317 up[2] = m[10];
2318 }
2319 }
2320
MatrixSetupTransform(matrix_t m,const vec3_t forward,const vec3_t left,const vec3_t up,const vec3_t origin)2321 void MatrixSetupTransform(matrix_t m, const vec3_t forward, const vec3_t left, const vec3_t up, const vec3_t origin)
2322 {
2323 m[ 0] = forward[0]; m[ 4] = left[0]; m[ 8] = up[0]; m[12] = origin[0];
2324 m[ 1] = forward[1]; m[ 5] = left[1]; m[ 9] = up[1]; m[13] = origin[1];
2325 m[ 2] = forward[2]; m[ 6] = left[2]; m[10] = up[2]; m[14] = origin[2];
2326 m[ 3] = 0; m[ 7] = 0; m[11] = 0; m[15] = 1;
2327 }
2328
MatrixSetupTransformFromRotation(matrix_t m,const matrix_t rot,const vec3_t origin)2329 void MatrixSetupTransformFromRotation(matrix_t m, const matrix_t rot, const vec3_t origin)
2330 {
2331 m[ 0] = rot[ 0]; m[ 4] = rot[ 4]; m[ 8] = rot[ 8]; m[12] = origin[0];
2332 m[ 1] = rot[ 1]; m[ 5] = rot[ 5]; m[ 9] = rot[ 9]; m[13] = origin[1];
2333 m[ 2] = rot[ 2]; m[ 6] = rot[ 6]; m[10] = rot[10]; m[14] = origin[2];
2334 m[ 3] = 0; m[ 7] = 0; m[11] = 0; m[15] = 1;
2335 }
2336
MatrixSetupTransformFromQuat(matrix_t m,const quat_t quat,const vec3_t origin)2337 void MatrixSetupTransformFromQuat(matrix_t m, const quat_t quat, const vec3_t origin)
2338 {
2339 matrix_t rot;
2340
2341 MatrixFromQuat(rot, quat);
2342
2343 m[ 0] = rot[ 0]; m[ 4] = rot[ 4]; m[ 8] = rot[ 8]; m[12] = origin[0];
2344 m[ 1] = rot[ 1]; m[ 5] = rot[ 5]; m[ 9] = rot[ 9]; m[13] = origin[1];
2345 m[ 2] = rot[ 2]; m[ 6] = rot[ 6]; m[10] = rot[10]; m[14] = origin[2];
2346 m[ 3] = 0; m[ 7] = 0; m[11] = 0; m[15] = 1;
2347 }
2348
MatrixAffineInverse(const matrix_t in,matrix_t out)2349 void MatrixAffineInverse(const matrix_t in, matrix_t out)
2350 {
2351 #if 0
2352 MatrixCopy(in, out);
2353 MatrixInverse(out);
2354 #else
2355 // Tr3B - cleaned up
2356 out[ 0] = in[ 0]; out[ 4] = in[ 1]; out[ 8] = in[ 2];
2357 out[ 1] = in[ 4]; out[ 5] = in[ 5]; out[ 9] = in[ 6];
2358 out[ 2] = in[ 8]; out[ 6] = in[ 9]; out[10] = in[10];
2359 out[ 3] = 0; out[ 7] = 0; out[11] = 0; out[15] = 1;
2360
2361 out[12] = -( in[12] * out[ 0] + in[13] * out[ 4] + in[14] * out[ 8] );
2362 out[13] = -( in[12] * out[ 1] + in[13] * out[ 5] + in[14] * out[ 9] );
2363 out[14] = -( in[12] * out[ 2] + in[13] * out[ 6] + in[14] * out[10] );
2364 #endif
2365 }
2366
MatrixTransformNormal(const matrix_t m,const vec3_t in,vec3_t out)2367 void MatrixTransformNormal(const matrix_t m, const vec3_t in, vec3_t out)
2368 {
2369 out[ 0] = m[ 0] * in[ 0] + m[ 4] * in[ 1] + m[ 8] * in[ 2];
2370 out[ 1] = m[ 1] * in[ 0] + m[ 5] * in[ 1] + m[ 9] * in[ 2];
2371 out[ 2] = m[ 2] * in[ 0] + m[ 6] * in[ 1] + m[10] * in[ 2];
2372 }
2373
MatrixTransformPoint(const matrix_t m,const vec3_t in,vec3_t out)2374 void MatrixTransformPoint(const matrix_t m, const vec3_t in, vec3_t out)
2375 {
2376 out[ 0] = m[ 0] * in[ 0] + m[ 4] * in[ 1] + m[ 8] * in[ 2] + m[12];
2377 out[ 1] = m[ 1] * in[ 0] + m[ 5] * in[ 1] + m[ 9] * in[ 2] + m[13];
2378 out[ 2] = m[ 2] * in[ 0] + m[ 6] * in[ 1] + m[10] * in[ 2] + m[14];
2379 }
2380
MatrixTransform4(const matrix_t m,const vec4_t in,vec4_t out)2381 void MatrixTransform4(const matrix_t m, const vec4_t in, vec4_t out)
2382 {
2383 out[ 0] = m[ 0] * in[ 0] + m[ 4] * in[ 1] + m[ 8] * in[ 2] + m[12] * in[ 3];
2384 out[ 1] = m[ 1] * in[ 0] + m[ 5] * in[ 1] + m[ 9] * in[ 2] + m[13] * in[ 3];
2385 out[ 2] = m[ 2] * in[ 0] + m[ 6] * in[ 1] + m[10] * in[ 2] + m[14] * in[ 3];
2386 out[ 3] = m[ 3] * in[ 0] + m[ 7] * in[ 1] + m[11] * in[ 2] + m[15] * in[ 3];
2387 }
2388
QuatNormalize(quat_t q)2389 vec_t QuatNormalize(quat_t q)
2390 {
2391 float length, ilength;
2392
2393 length = q[0] * q[0] + q[1] * q[1] + q[2] * q[2] + q[3] * q[3];
2394 length = sqrt(length);
2395
2396 if(length)
2397 {
2398 ilength = 1 / length;
2399 q[0] *= ilength;
2400 q[1] *= ilength;
2401 q[2] *= ilength;
2402 q[3] *= ilength;
2403 }
2404
2405 return length;
2406 }
2407
QuatFromAngles(quat_t q,vec_t pitch,vec_t yaw,vec_t roll)2408 void QuatFromAngles(quat_t q, vec_t pitch, vec_t yaw, vec_t roll)
2409 {
2410 #if 1
2411 matrix_t tmp;
2412
2413 MatrixFromAngles(tmp, pitch, yaw, roll);
2414 QuatFromMatrix(q, tmp);
2415 #else
2416 static float sr, sp, sy, cr, cp, cy;
2417
2418 // static to help MS compiler fp bugs
2419 sp = sin(DEG2RAD(pitch));
2420 cp = cos(DEG2RAD(pitch));
2421
2422 sy = sin(DEG2RAD(yaw));
2423 cy = cos(DEG2RAD(yaw));
2424
2425 sr = sin(DEG2RAD(roll));
2426 cr = cos(DEG2RAD(roll));
2427
2428 q[0] = sr * cp * cy - cr * sp * sy; // x
2429 q[1] = cr * sp * cy + sr * cp * sy; // y
2430 q[2] = cr * cp * sy - sr * sp * cy; // z
2431 q[3] = cr * cp * cy + sr * sp * sy; // w
2432 #endif
2433 }
2434
QuatFromMatrix(quat_t q,const matrix_t m)2435 void QuatFromMatrix(quat_t q, const matrix_t m)
2436 {
2437 #if 1
2438 /*
2439 From Quaternion to Matrix and Back
2440 February 27th 2005
2441 J.M.P. van Waveren
2442
2443 http://www.intel.com/cd/ids/developer/asmo-na/eng/293748.htm
2444 */
2445 float t, s;
2446
2447 if(m[0] + m[5] + m[10] > 0.0f)
2448 {
2449 t = m[0] + m[5] + m[10] + 1.0f;
2450 s = 0.5f / sqrt(t);
2451
2452 q[3] = s * t;
2453 q[2] = (m[1] - m[4]) * s;
2454 q[1] = (m[8] - m[2]) * s;
2455 q[0] = (m[6] - m[9]) * s;
2456 }
2457 else if(m[0] > m[5] && m[0] > m[10])
2458 {
2459 t = m[0] - m[5] - m[10] + 1.0f;
2460 s = 0.5f / sqrt(t);
2461
2462 q[0] = s * t;
2463 q[1] = (m[1] + m[4]) * s;
2464 q[2] = (m[8] + m[2]) * s;
2465 q[3] = (m[6] - m[9]) * s;
2466 }
2467 else if(m[5] > m[10])
2468 {
2469 t = -m[0] + m[5] - m[10] + 1.0f;
2470 s = 0.5f / sqrt(t);
2471
2472 q[1] = s * t;
2473 q[0] = (m[1] + m[4]) * s;
2474 q[3] = (m[8] - m[2]) * s;
2475 q[2] = (m[6] + m[9]) * s;
2476 }
2477 else
2478 {
2479 t = -m[0] - m[5] + m[10] + 1.0f;
2480 s = 0.5f / sqrt(t);
2481
2482 q[2] = s * t;
2483 q[3] = (m[1] - m[4]) * s;
2484 q[0] = (m[8] + m[2]) * s;
2485 q[1] = (m[6] + m[9]) * s;
2486 }
2487
2488 #else
2489 float trace;
2490
2491 // http://www.euclideanspace.com/maths/geometry/rotations/conversions/matrixToQuaternion/index.htm
2492
2493 trace = 1.0f + m[0] + m[5] + m[10];
2494
2495 if(trace > 0.0f)
2496 {
2497 vec_t s = 0.5f / sqrt(trace);
2498
2499 q[0] = (m[6] - m[9]) * s;
2500 q[1] = (m[8] - m[2]) * s;
2501 q[2] = (m[1] - m[4]) * s;
2502 q[3] = 0.25f / s;
2503 }
2504 else
2505 {
2506 if(m[0] > m[5] && m[0] > m[10])
2507 {
2508 // column 0
2509 float s = sqrt(1.0f + m[0] - m[5] - m[10]) * 2.0f;
2510
2511 q[0] = 0.25f * s;
2512 q[1] = (m[4] + m[1]) / s;
2513 q[2] = (m[8] + m[2]) / s;
2514 q[3] = (m[9] - m[6]) / s;
2515 }
2516 else if(m[5] > m[10])
2517 {
2518 // column 1
2519 float s = sqrt(1.0f + m[5] - m[0] - m[10]) * 2.0f;
2520
2521 q[0] = (m[4] + m[1]) / s;
2522 q[1] = 0.25f * s;
2523 q[2] = (m[9] + m[6]) / s;
2524 q[3] = (m[8] - m[2]) / s;
2525 }
2526 else
2527 {
2528 // column 2
2529 float s = sqrt(1.0f + m[10] - m[0] - m[5]) * 2.0f;
2530
2531 q[0] = (m[8] + m[2]) / s;
2532 q[1] = (m[9] + m[6]) / s;
2533 q[2] = 0.25f * s;
2534 q[3] = (m[4] - m[1]) / s;
2535 }
2536 }
2537
2538 QuatNormalize(q);
2539 #endif
2540 }
2541
QuatToVectors(const quat_t q,vec3_t forward,vec3_t right,vec3_t up)2542 void QuatToVectors(const quat_t q, vec3_t forward, vec3_t right, vec3_t up)
2543 {
2544 matrix_t tmp;
2545
2546 MatrixFromQuat(tmp, q);
2547 MatrixToVectorsFRU(tmp, forward, right, up);
2548 }
2549
QuatToAxis(const quat_t q,vec3_t axis[3])2550 void QuatToAxis(const quat_t q, vec3_t axis[3])
2551 {
2552 matrix_t tmp;
2553
2554 MatrixFromQuat(tmp, q);
2555 MatrixToVectorsFLU(tmp, axis[0], axis[1], axis[2]);
2556 }
2557
QuatToAngles(const quat_t q,vec3_t angles)2558 void QuatToAngles(const quat_t q, vec3_t angles)
2559 {
2560 quat_t q2;
2561
2562 q2[0] = q[0] * q[0];
2563 q2[1] = q[1] * q[1];
2564 q2[2] = q[2] * q[2];
2565 q2[3] = q[3] * q[3];
2566
2567 angles[PITCH] = RAD2DEG(asin(-2 * (q[2] * q[0] - q[3] * q[1])));
2568 angles[YAW] = RAD2DEG(atan2(2 * (q[2] * q[3] + q[0] * q[1]), (q2[2] - q2[3] - q2[0] + q2[1])));
2569 angles[ROLL] = RAD2DEG(atan2(2 * (q[3] * q[0] + q[2] * q[1]), (-q2[2] - q2[3] + q2[0] + q2[1])));
2570 }
2571
2572
QuatMultiply0(quat_t qa,const quat_t qb)2573 void QuatMultiply0(quat_t qa, const quat_t qb)
2574 {
2575 quat_t tmp;
2576
2577 QuatCopy(qa, tmp);
2578 QuatMultiply1(tmp, qb, qa);
2579 }
2580
QuatMultiply1(const quat_t qa,const quat_t qb,quat_t qc)2581 void QuatMultiply1(const quat_t qa, const quat_t qb, quat_t qc)
2582 {
2583 /*
2584 from matrix and quaternion faq
2585 x = w1x2 + x1w2 + y1z2 - z1y2
2586 y = w1y2 + y1w2 + z1x2 - x1z2
2587 z = w1z2 + z1w2 + x1y2 - y1x2
2588
2589 w = w1w2 - x1x2 - y1y2 - z1z2
2590 */
2591
2592 qc[0] = qa[3] * qb[0] + qa[0] * qb[3] + qa[1] * qb[2] - qa[2] * qb[1];
2593 qc[1] = qa[3] * qb[1] + qa[1] * qb[3] + qa[2] * qb[0] - qa[0] * qb[2];
2594 qc[2] = qa[3] * qb[2] + qa[2] * qb[3] + qa[0] * qb[1] - qa[1] * qb[0];
2595 qc[3] = qa[3] * qb[3] - qa[0] * qb[0] - qa[1] * qb[1] - qa[2] * qb[2];
2596 }
2597
QuatMultiply2(const quat_t qa,const quat_t qb,quat_t qc)2598 void QuatMultiply2(const quat_t qa, const quat_t qb, quat_t qc)
2599 {
2600 qc[0] = qa[3] * qb[0] + qa[0] * qb[3] + qa[1] * qb[2] + qa[2] * qb[1];
2601 qc[1] = qa[3] * qb[1] - qa[1] * qb[3] - qa[2] * qb[0] + qa[0] * qb[2];
2602 qc[2] = qa[3] * qb[2] - qa[2] * qb[3] - qa[0] * qb[1] + qa[1] * qb[0];
2603 qc[3] = qa[3] * qb[3] - qa[0] * qb[0] - qa[1] * qb[1] + qa[2] * qb[2];
2604 }
2605
QuatMultiply3(const quat_t qa,const quat_t qb,quat_t qc)2606 void QuatMultiply3(const quat_t qa, const quat_t qb, quat_t qc)
2607 {
2608 qc[0] = qa[3] * qb[0] + qa[0] * qb[3] + qa[1] * qb[2] + qa[2] * qb[1];
2609 qc[1] = -qa[3] * qb[1] + qa[1] * qb[3] - qa[2] * qb[0] + qa[0] * qb[2];
2610 qc[2] = -qa[3] * qb[2] + qa[2] * qb[3] - qa[0] * qb[1] + qa[1] * qb[0];
2611 qc[3] = -qa[3] * qb[3] + qa[0] * qb[0] - qa[1] * qb[1] + qa[2] * qb[2];
2612 }
2613
QuatMultiply4(const quat_t qa,const quat_t qb,quat_t qc)2614 void QuatMultiply4(const quat_t qa, const quat_t qb, quat_t qc)
2615 {
2616 qc[0] = qa[3] * qb[0] - qa[0] * qb[3] - qa[1] * qb[2] - qa[2] * qb[1];
2617 qc[1] = -qa[3] * qb[1] - qa[1] * qb[3] + qa[2] * qb[0] - qa[0] * qb[2];
2618 qc[2] = -qa[3] * qb[2] - qa[2] * qb[3] + qa[0] * qb[1] - qa[1] * qb[0];
2619 qc[3] = -qa[3] * qb[3] - qa[0] * qb[0] + qa[1] * qb[1] - qa[2] * qb[2];
2620 }
2621
QuatSlerp(const quat_t from,const quat_t to,float frac,quat_t out)2622 void QuatSlerp(const quat_t from, const quat_t to, float frac, quat_t out)
2623 {
2624 #if 0
2625 quat_t to1;
2626 double omega, cosom, sinom, scale0, scale1;
2627
2628 cosom = from[0] * to[0] + from[1] * to[1] + from[2] * to[2] + from[3] * to[3];
2629
2630 if(cosom < 0.0)
2631 {
2632 cosom = -cosom;
2633
2634 QuatCopy(to, to1);
2635 QuatAntipodal(to1);
2636 }
2637 else
2638 {
2639 QuatCopy(to, to1);
2640 }
2641
2642 if((1.0 - cosom) > 0)
2643 {
2644 omega = acos(cosom);
2645 sinom = sin(omega);
2646 scale0 = sin((1.0 - frac) * omega) / sinom;
2647 scale1 = sin(frac * omega) / sinom;
2648 }
2649 else
2650 {
2651 scale0 = 1.0 - frac;
2652 scale1 = frac;
2653 }
2654
2655 out[0] = scale0 * from[0] + scale1 * to1[0];
2656 out[1] = scale0 * from[1] + scale1 * to1[1];
2657 out[2] = scale0 * from[2] + scale1 * to1[2];
2658 out[3] = scale0 * from[3] + scale1 * to1[3];
2659 #else
2660 /*
2661 Slerping Clock Cycles
2662 February 27th 2005
2663 J.M.P. van Waveren
2664
2665 http://www.intel.com/cd/ids/developer/asmo-na/eng/293747.htm
2666 */
2667 float cosom, absCosom, sinom, sinSqr, omega, scale0, scale1;
2668
2669 if(frac <= 0.0f)
2670 {
2671 QuatCopy(from, out);
2672 return;
2673 }
2674
2675 if(frac >= 1.0f)
2676 {
2677 QuatCopy(to, out);
2678 return;
2679 }
2680
2681 if(QuatCompare(from, to))
2682 {
2683 QuatCopy(from, out);
2684 return;
2685 }
2686
2687 cosom = from[0] * to[0] + from[1] * to[1] + from[2] * to[2] + from[3] * to[3];
2688 absCosom = fabs(cosom);
2689
2690 if((1.0f - absCosom) > 1e-6f)
2691 {
2692 sinSqr = 1.0f - absCosom * absCosom;
2693 sinom = 1.0f / sqrt(sinSqr);
2694 omega = atan2(sinSqr * sinom, absCosom);
2695
2696 scale0 = sin((1.0f - frac) * omega) * sinom;
2697 scale1 = sin(frac * omega) * sinom;
2698 }
2699 else
2700 {
2701 scale0 = 1.0f - frac;
2702 scale1 = frac;
2703 }
2704
2705 scale1 = (cosom >= 0.0f) ? scale1 : -scale1;
2706
2707 out[0] = scale0 * from[0] + scale1 * to[0];
2708 out[1] = scale0 * from[1] + scale1 * to[1];
2709 out[2] = scale0 * from[2] + scale1 * to[2];
2710 out[3] = scale0 * from[3] + scale1 * to[3];
2711 #endif
2712 }
2713
QuatTransformVector(const quat_t q,const vec3_t in,vec3_t out)2714 void QuatTransformVector(const quat_t q, const vec3_t in, vec3_t out)
2715 {
2716 matrix_t m;
2717
2718 MatrixFromQuat(m, q);
2719 MatrixTransformNormal(m, in, out);
2720 }
2721