made various things take const pointers (optimizer hint), commented out and/or remove...
[divverent/darkplaces.git] / mathlib.c
1 /*
2 Copyright (C) 1996-1997 Id Software, Inc.
3
4 This program is free software; you can redistribute it and/or
5 modify it under the terms of the GNU General Public License
6 as published by the Free Software Foundation; either version 2
7 of the License, or (at your option) any later version.
8
9 This program is distributed in the hope that it will be useful,
10 but WITHOUT ANY WARRANTY; without even the implied warranty of
11 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
12
13 See the GNU General Public License for more details.
14
15 You should have received a copy of the GNU General Public License
16 along with this program; if not, write to the Free Software
17 Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.
18
19 */
20 // mathlib.c -- math primitives
21
22 #include <math.h>
23 #include "quakedef.h"
24
25 void Sys_Error (char *error, ...);
26
27 vec3_t vec3_origin = {0,0,0};
28 float ixtable[4096];
29
30 /*-----------------------------------------------------------------*/
31
32 float m_bytenormals[NUMVERTEXNORMALS][3] =
33 {
34 {-0.525731, 0.000000, 0.850651}, {-0.442863, 0.238856, 0.864188}, 
35 {-0.295242, 0.000000, 0.955423}, {-0.309017, 0.500000, 0.809017}, 
36 {-0.162460, 0.262866, 0.951056}, {0.000000, 0.000000, 1.000000}, 
37 {0.000000, 0.850651, 0.525731}, {-0.147621, 0.716567, 0.681718}, 
38 {0.147621, 0.716567, 0.681718}, {0.000000, 0.525731, 0.850651}, 
39 {0.309017, 0.500000, 0.809017}, {0.525731, 0.000000, 0.850651}, 
40 {0.295242, 0.000000, 0.955423}, {0.442863, 0.238856, 0.864188}, 
41 {0.162460, 0.262866, 0.951056}, {-0.681718, 0.147621, 0.716567}, 
42 {-0.809017, 0.309017, 0.500000}, {-0.587785, 0.425325, 0.688191}, 
43 {-0.850651, 0.525731, 0.000000}, {-0.864188, 0.442863, 0.238856},
44 {-0.716567, 0.681718, 0.147621}, {-0.688191, 0.587785, 0.425325}, 
45 {-0.500000, 0.809017, 0.309017}, {-0.238856, 0.864188, 0.442863}, 
46 {-0.425325, 0.688191, 0.587785}, {-0.716567, 0.681718, -0.147621}, 
47 {-0.500000, 0.809017, -0.309017}, {-0.525731, 0.850651, 0.000000}, 
48 {0.000000, 0.850651, -0.525731}, {-0.238856, 0.864188, -0.442863}, 
49 {0.000000, 0.955423, -0.295242}, {-0.262866, 0.951056, -0.162460}, 
50 {0.000000, 1.000000, 0.000000}, {0.000000, 0.955423, 0.295242}, 
51 {-0.262866, 0.951056, 0.162460}, {0.238856, 0.864188, 0.442863}, 
52 {0.262866, 0.951056, 0.162460}, {0.500000, 0.809017, 0.309017}, 
53 {0.238856, 0.864188, -0.442863}, {0.262866, 0.951056, -0.162460}, 
54 {0.500000, 0.809017, -0.309017}, {0.850651, 0.525731, 0.000000}, 
55 {0.716567, 0.681718, 0.147621}, {0.716567, 0.681718, -0.147621}, 
56 {0.525731, 0.850651, 0.000000}, {0.425325, 0.688191, 0.587785}, 
57 {0.864188, 0.442863, 0.238856}, {0.688191, 0.587785, 0.425325}, 
58 {0.809017, 0.309017, 0.500000}, {0.681718, 0.147621, 0.716567}, 
59 {0.587785, 0.425325, 0.688191}, {0.955423, 0.295242, 0.000000}, 
60 {1.000000, 0.000000, 0.000000}, {0.951056, 0.162460, 0.262866},
61 {0.850651, -0.525731, 0.000000}, {0.955423, -0.295242, 0.000000}, 
62 {0.864188, -0.442863, 0.238856}, {0.951056, -0.162460, 0.262866}, 
63 {0.809017, -0.309017, 0.500000}, {0.681718, -0.147621, 0.716567},
64 {0.850651, 0.000000, 0.525731}, {0.864188, 0.442863, -0.238856},
65 {0.809017, 0.309017, -0.500000}, {0.951056, 0.162460, -0.262866}, 
66 {0.525731, 0.000000, -0.850651}, {0.681718, 0.147621, -0.716567}, 
67 {0.681718, -0.147621, -0.716567}, {0.850651, 0.000000, -0.525731},
68 {0.809017, -0.309017, -0.500000}, {0.864188, -0.442863, -0.238856}, 
69 {0.951056, -0.162460, -0.262866}, {0.147621, 0.716567, -0.681718}, 
70 {0.309017, 0.500000, -0.809017}, {0.425325, 0.688191, -0.587785}, 
71 {0.442863, 0.238856, -0.864188}, {0.587785, 0.425325, -0.688191}, 
72 {0.688191, 0.587785, -0.425325}, {-0.147621, 0.716567, -0.681718}, 
73 {-0.309017, 0.500000, -0.809017}, {0.000000, 0.525731, -0.850651}, 
74 {-0.525731, 0.000000, -0.850651}, {-0.442863, 0.238856, -0.864188}, 
75 {-0.295242, 0.000000, -0.955423}, {-0.162460, 0.262866, -0.951056}, 
76 {0.000000, 0.000000, -1.000000}, {0.295242, 0.000000, -0.955423}, 
77 {0.162460, 0.262866, -0.951056}, {-0.442863, -0.238856, -0.864188}, 
78 {-0.309017, -0.500000, -0.809017}, {-0.162460, -0.262866, -0.951056}, 
79 {0.000000, -0.850651, -0.525731}, {-0.147621, -0.716567, -0.681718}, 
80 {0.147621, -0.716567, -0.681718}, {0.000000, -0.525731, -0.850651}, 
81 {0.309017, -0.500000, -0.809017}, {0.442863, -0.238856, -0.864188}, 
82 {0.162460, -0.262866, -0.951056}, {0.238856, -0.864188, -0.442863}, 
83 {0.500000, -0.809017, -0.309017}, {0.425325, -0.688191, -0.587785}, 
84 {0.716567, -0.681718, -0.147621}, {0.688191, -0.587785, -0.425325}, 
85 {0.587785, -0.425325, -0.688191}, {0.000000, -0.955423, -0.295242},
86 {0.000000, -1.000000, 0.000000}, {0.262866, -0.951056, -0.162460}, 
87 {0.000000, -0.850651, 0.525731}, {0.000000, -0.955423, 0.295242}, 
88 {0.238856, -0.864188, 0.442863}, {0.262866, -0.951056, 0.162460}, 
89 {0.500000, -0.809017, 0.309017}, {0.716567, -0.681718, 0.147621}, 
90 {0.525731, -0.850651, 0.000000}, {-0.238856, -0.864188, -0.442863}, 
91 {-0.500000, -0.809017, -0.309017}, {-0.262866, -0.951056, -0.162460}, 
92 {-0.850651, -0.525731, 0.000000}, {-0.716567, -0.681718, -0.147621},
93 {-0.716567, -0.681718, 0.147621}, {-0.525731, -0.850651, 0.000000}, 
94 {-0.500000, -0.809017, 0.309017}, {-0.238856, -0.864188, 0.442863},
95 {-0.262866, -0.951056, 0.162460}, {-0.864188, -0.442863, 0.238856}, 
96 {-0.809017, -0.309017, 0.500000}, {-0.688191, -0.587785, 0.425325},
97 {-0.681718, -0.147621, 0.716567}, {-0.442863, -0.238856, 0.864188}, 
98 {-0.587785, -0.425325, 0.688191}, {-0.309017, -0.500000, 0.809017}, 
99 {-0.147621, -0.716567, 0.681718}, {-0.425325, -0.688191, 0.587785}, 
100 {-0.162460, -0.262866, 0.951056}, {0.442863, -0.238856, 0.864188}, 
101 {0.162460, -0.262866, 0.951056}, {0.309017, -0.500000, 0.809017}, 
102 {0.147621, -0.716567, 0.681718}, {0.000000, -0.525731, 0.850651}, 
103 {0.425325, -0.688191, 0.587785}, {0.587785, -0.425325, 0.688191}, 
104 {0.688191, -0.587785, 0.425325}, {-0.955423, 0.295242, 0.000000}, 
105 {-0.951056, 0.162460, 0.262866}, {-1.000000, 0.000000, 0.000000}, 
106 {-0.850651, 0.000000, 0.525731}, {-0.955423, -0.295242, 0.000000}, 
107 {-0.951056, -0.162460, 0.262866}, {-0.864188, 0.442863, -0.238856}, 
108 {-0.951056, 0.162460, -0.262866}, {-0.809017, 0.309017, -0.500000}, 
109 {-0.864188, -0.442863, -0.238856}, {-0.951056, -0.162460, -0.262866},
110 {-0.809017, -0.309017, -0.500000}, {-0.681718, 0.147621, -0.716567}, 
111 {-0.681718, -0.147621, -0.716567}, {-0.850651, 0.000000, -0.525731}, 
112 {-0.688191, 0.587785, -0.425325}, {-0.587785, 0.425325, -0.688191}, 
113 {-0.425325, 0.688191, -0.587785}, {-0.425325, -0.688191, -0.587785}, 
114 {-0.587785, -0.425325, -0.688191}, {-0.688191, -0.587785, -0.425325}, 
115 };
116
117 qbyte NormalToByte(const vec3_t n)
118 {
119         int i, best;
120         float bestdistance, distance;
121
122         best = 0;
123         bestdistance = DotProduct (n, m_bytenormals[0]);
124         for (i = 1;i < NUMVERTEXNORMALS;i++)
125         {
126                 distance = DotProduct (n, m_bytenormals[i]);
127                 if (distance > bestdistance)
128                 {
129                         bestdistance = distance;
130                         best = i;
131                 }
132         }
133         return best;
134 }
135
136 // note: uses byte partly to force unsigned for the validity check
137 void ByteToNormal(qbyte num, vec3_t n)
138 {
139         if (num < NUMVERTEXNORMALS)
140                 VectorCopy(m_bytenormals[num], n);
141         else
142                 VectorClear(n); // FIXME: complain?
143 }
144
145 float Q_RSqrt(float number)
146 {
147         float y;
148
149         if (number == 0.0f)
150                 return 0.0f;
151
152         *((int *)&y) = 0x5f3759df - ((* (int *) &number) >> 1);
153         return y * (1.5f - (number * 0.5f * y * y));
154 }
155
156 #if 0
157 // LordHavoc: no longer used at all
158 void ProjectPointOnPlane( vec3_t dst, const vec3_t p, const vec3_t normal )
159 {
160 #if 0
161         // LordHavoc: the old way...
162         float d;
163         vec3_t n;
164         float inv_denom;
165
166         inv_denom = 1.0F / DotProduct( normal, normal );
167
168         d = DotProduct( normal, p ) * inv_denom;
169
170         n[0] = normal[0] * inv_denom;
171         n[1] = normal[1] * inv_denom;
172         n[2] = normal[2] * inv_denom;
173
174         dst[0] = p[0] - d * n[0];
175         dst[1] = p[1] - d * n[1];
176         dst[2] = p[2] - d * n[2];
177 #else
178         // LordHavoc: optimized to death and beyond
179         float d;
180
181         // LordHavoc: the normal is a unit vector by definition,
182         //            therefore inv_denom was pointless.
183         d = DotProduct(normal, p);
184         dst[0] = p[0] - d * normal[0];
185         dst[1] = p[1] - d * normal[1];
186         dst[2] = p[2] - d * normal[2];
187 #endif
188 }
189 #endif
190
191 // assumes "src" is normalized
192 void PerpendicularVector( vec3_t dst, const vec3_t src )
193 {
194 #if 0
195         // LordHavoc: the old way...
196         int     pos;
197         int i;
198         float minelem, d;
199         vec3_t tempvec;
200
201         // find the smallest magnitude axially aligned vector
202         minelem = 1.0F;
203         for ( pos = 0, i = 0; i < 3; i++ )
204         {
205                 if ( fabs( src[i] ) < minelem )
206                 {
207                         pos = i;
208                         minelem = fabs( src[i] );
209                 }
210         }
211         VectorClear(tempvec);
212         tempvec[pos] = 1.0F;
213
214         // project the point onto the plane defined by src
215         ProjectPointOnPlane( dst, tempvec, src );
216
217         // normalize the result
218         VectorNormalize(dst);
219 #else
220         // LordHavoc: optimized to death and beyond
221         int     pos;
222         float minelem;
223
224         if (src[0])
225         {
226                 dst[0] = 0;
227                 if (src[1])
228                 {
229                         dst[1] = 0;
230                         if (src[2])
231                         {
232                                 dst[2] = 0;
233                                 pos = 0;
234                                 minelem = fabs(src[0]);
235                                 if (fabs(src[1]) < minelem)
236                                 {
237                                         pos = 1;
238                                         minelem = fabs(src[1]);
239                                 }
240                                 if (fabs(src[2]) < minelem)
241                                         pos = 2;
242
243                                 dst[pos] = 1;
244                                 dst[0] -= src[pos] * src[0];
245                                 dst[1] -= src[pos] * src[1];
246                                 dst[2] -= src[pos] * src[2];
247
248                                 // normalize the result
249                                 VectorNormalize(dst);
250                         }
251                         else
252                                 dst[2] = 1;
253                 }
254                 else
255                 {
256                         dst[1] = 1;
257                         dst[2] = 0;
258                 }
259         }
260         else
261         {
262                 dst[0] = 1;
263                 dst[1] = 0;
264                 dst[2] = 0;
265         }
266 #endif
267 }
268
269
270 // LordHavoc: like AngleVectors, but taking a forward vector instead of angles, useful!
271 void VectorVectors(const vec3_t forward, vec3_t right, vec3_t up)
272 {
273         float d;
274
275         right[0] = forward[2];
276         right[1] = -forward[0];
277         right[2] = forward[1];
278
279         d = DotProduct(forward, right);
280         right[0] -= d * forward[0];
281         right[1] -= d * forward[1];
282         right[2] -= d * forward[2];
283         VectorNormalizeFast(right);
284         CrossProduct(right, forward, up);
285 }
286
287 void VectorVectorsDouble(const double *forward, double *right, double *up)
288 {
289         double d;
290
291         right[0] = forward[2];
292         right[1] = -forward[0];
293         right[2] = forward[1];
294
295         d = DotProduct(forward, right);
296         right[0] -= d * forward[0];
297         right[1] -= d * forward[1];
298         right[2] -= d * forward[2];
299         VectorNormalize(right);
300         CrossProduct(right, forward, up);
301 }
302
303 void RotatePointAroundVector( vec3_t dst, const vec3_t dir, const vec3_t point, float degrees )
304 {
305 #if 0
306         // LordHavoc: the old way, cryptic brute force...
307         float   m[3][3];
308         float   im[3][3];
309         float   zrot[3][3];
310         float   tmpmat[3][3];
311         float   rot[3][3];
312         int     i;
313         vec3_t vr, vup, vf;
314
315         vf[0] = dir[0];
316         vf[1] = dir[1];
317         vf[2] = dir[2];
318
319         PerpendicularVector( vr, dir );
320         CrossProduct( vr, vf, vup );
321
322         m[0][0] = vr[0];
323         m[1][0] = vr[1];
324         m[2][0] = vr[2];
325
326         m[0][1] = vup[0];
327         m[1][1] = vup[1];
328         m[2][1] = vup[2];
329
330         m[0][2] = vf[0];
331         m[1][2] = vf[1];
332         m[2][2] = vf[2];
333
334         memcpy( im, m, sizeof( im ) );
335
336         im[0][1] = m[1][0];
337         im[0][2] = m[2][0];
338         im[1][0] = m[0][1];
339         im[1][2] = m[2][1];
340         im[2][0] = m[0][2];
341         im[2][1] = m[1][2];
342
343         memset( zrot, 0, sizeof( zrot ) );
344         zrot[0][0] = zrot[1][1] = zrot[2][2] = 1.0F;
345
346         zrot[0][0] = cos( DEG2RAD( degrees ) );
347         zrot[0][1] = sin( DEG2RAD( degrees ) );
348         zrot[1][0] = -sin( DEG2RAD( degrees ) );
349         zrot[1][1] = cos( DEG2RAD( degrees ) );
350
351         R_ConcatRotations( m, zrot, tmpmat );
352         R_ConcatRotations( tmpmat, im, rot );
353
354         for ( i = 0; i < 3; i++ )
355                 dst[i] = rot[i][0] * point[0] + rot[i][1] * point[1] + rot[i][2] * point[2];
356 #elif 0
357         // LordHavoc: on the path to unintelligible code...
358 //      float   m[3][3];
359 //      float   im[3][3];
360 //      float   zrot[3][3];
361         float   tmpmat[3][3];
362 //      float   rot[3][3];
363         float   angle, c, s;
364 //      int     i;
365         vec3_t vr, vu, vf;
366
367         angle = DEG2RAD(degrees);
368
369         c = cos(angle);
370         s = sin(angle);
371
372         vf[0] = dir[0];
373         vf[1] = dir[1];
374         vf[2] = dir[2];
375
376         PerpendicularVector(vr, dir);
377         CrossProduct(vr, vf, vu);
378
379 //      m   [0][0] = vr[0];m   [0][1] = vu[0];m   [0][2] = vf[0];
380 //      m   [1][0] = vr[1];m   [1][1] = vu[1];m   [1][2] = vf[1];
381 //      m   [2][0] = vr[2];m   [2][1] = vu[2];m   [2][2] = vf[2];
382 //      im  [0][0] = vr[0];im  [0][1] = vr[1];im  [0][2] = vr[2];
383 //      im  [1][0] = vu[0];im  [1][1] = vu[1];im  [1][2] = vu[2];
384 //      im  [2][0] = vf[0];im  [2][1] = vf[1];im  [2][2] = vf[2];
385 //      zrot[0][0] =     c;zrot[0][1] =     s;zrot[0][2] =     0;
386 //      zrot[1][0] =    -s;zrot[1][1] =     c;zrot[1][2] =     0;
387 //      zrot[2][0] =     0;zrot[2][1] =     0;zrot[2][2] =     1;
388
389 //      tmpmat[0][0] = m[0][0] * zrot[0][0] + m[0][1] * zrot[1][0] + m[0][2] * zrot[2][0];
390 //      tmpmat[0][1] = m[0][0] * zrot[0][1] + m[0][1] * zrot[1][1] + m[0][2] * zrot[2][1];
391 //      tmpmat[0][2] = m[0][0] * zrot[0][2] + m[0][1] * zrot[1][2] + m[0][2] * zrot[2][2];
392 //      tmpmat[1][0] = m[1][0] * zrot[0][0] + m[1][1] * zrot[1][0] + m[1][2] * zrot[2][0];
393 //      tmpmat[1][1] = m[1][0] * zrot[0][1] + m[1][1] * zrot[1][1] + m[1][2] * zrot[2][1];
394 //      tmpmat[1][2] = m[1][0] * zrot[0][2] + m[1][1] * zrot[1][2] + m[1][2] * zrot[2][2];
395 //      tmpmat[2][0] = m[2][0] * zrot[0][0] + m[2][1] * zrot[1][0] + m[2][2] * zrot[2][0];
396 //      tmpmat[2][1] = m[2][0] * zrot[0][1] + m[2][1] * zrot[1][1] + m[2][2] * zrot[2][1];
397 //      tmpmat[2][2] = m[2][0] * zrot[0][2] + m[2][1] * zrot[1][2] + m[2][2] * zrot[2][2];
398
399         tmpmat[0][0] = vr[0] *  c + vu[0] * -s;
400         tmpmat[0][1] = vr[0] *  s + vu[0] *  c;
401 //      tmpmat[0][2] =                           vf[0];
402         tmpmat[1][0] = vr[1] *  c + vu[1] * -s;
403         tmpmat[1][1] = vr[1] *  s + vu[1] *  c;
404 //      tmpmat[1][2] =                           vf[1];
405         tmpmat[2][0] = vr[2] *  c + vu[2] * -s;
406         tmpmat[2][1] = vr[2] *  s + vu[2] *  c;
407 //      tmpmat[2][2] =                           vf[2];
408
409 //      rot[0][0] = tmpmat[0][0] * vr[0] + tmpmat[0][1] * vu[0] + tmpmat[0][2] * vf[0];
410 //      rot[0][1] = tmpmat[0][0] * vr[1] + tmpmat[0][1] * vu[1] + tmpmat[0][2] * vf[1];
411 //      rot[0][2] = tmpmat[0][0] * vr[2] + tmpmat[0][1] * vu[2] + tmpmat[0][2] * vf[2];
412 //      rot[1][0] = tmpmat[1][0] * vr[0] + tmpmat[1][1] * vu[0] + tmpmat[1][2] * vf[0];
413 //      rot[1][1] = tmpmat[1][0] * vr[1] + tmpmat[1][1] * vu[1] + tmpmat[1][2] * vf[1];
414 //      rot[1][2] = tmpmat[1][0] * vr[2] + tmpmat[1][1] * vu[2] + tmpmat[1][2] * vf[2];
415 //      rot[2][0] = tmpmat[2][0] * vr[0] + tmpmat[2][1] * vu[0] + tmpmat[2][2] * vf[0];
416 //      rot[2][1] = tmpmat[2][0] * vr[1] + tmpmat[2][1] * vu[1] + tmpmat[2][2] * vf[1];
417 //      rot[2][2] = tmpmat[2][0] * vr[2] + tmpmat[2][1] * vu[2] + tmpmat[2][2] * vf[2];
418
419 //      rot[0][0] = tmpmat[0][0] * vr[0] + tmpmat[0][1] * vu[0] + vf[0] * vf[0];
420 //      rot[0][1] = tmpmat[0][0] * vr[1] + tmpmat[0][1] * vu[1] + vf[0] * vf[1];
421 //      rot[0][2] = tmpmat[0][0] * vr[2] + tmpmat[0][1] * vu[2] + vf[0] * vf[2];
422 //      rot[1][0] = tmpmat[1][0] * vr[0] + tmpmat[1][1] * vu[0] + vf[1] * vf[0];
423 //      rot[1][1] = tmpmat[1][0] * vr[1] + tmpmat[1][1] * vu[1] + vf[1] * vf[1];
424 //      rot[1][2] = tmpmat[1][0] * vr[2] + tmpmat[1][1] * vu[2] + vf[1] * vf[2];
425 //      rot[2][0] = tmpmat[2][0] * vr[0] + tmpmat[2][1] * vu[0] + vf[2] * vf[0];
426 //      rot[2][1] = tmpmat[2][0] * vr[1] + tmpmat[2][1] * vu[1] + vf[2] * vf[1];
427 //      rot[2][2] = tmpmat[2][0] * vr[2] + tmpmat[2][1] * vu[2] + vf[2] * vf[2];
428
429 //      dst[0] = rot[0][0] * point[0] + rot[0][1] * point[1] + rot[0][2] * point[2];
430 //      dst[1] = rot[1][0] * point[0] + rot[1][1] * point[1] + rot[1][2] * point[2];
431 //      dst[2] = rot[2][0] * point[0] + rot[2][1] * point[1] + rot[2][2] * point[2];
432
433         dst[0] = (tmpmat[0][0] * vr[0] + tmpmat[0][1] * vu[0] + vf[0] * vf[0]) * point[0]
434                + (tmpmat[0][0] * vr[1] + tmpmat[0][1] * vu[1] + vf[0] * vf[1]) * point[1]
435                + (tmpmat[0][0] * vr[2] + tmpmat[0][1] * vu[2] + vf[0] * vf[2]) * point[2];
436         dst[1] = (tmpmat[1][0] * vr[0] + tmpmat[1][1] * vu[0] + vf[1] * vf[0]) * point[0]
437                + (tmpmat[1][0] * vr[1] + tmpmat[1][1] * vu[1] + vf[1] * vf[1]) * point[1]
438                + (tmpmat[1][0] * vr[2] + tmpmat[1][1] * vu[2] + vf[1] * vf[2]) * point[2];
439         dst[2] = (tmpmat[2][0] * vr[0] + tmpmat[2][1] * vu[0] + vf[2] * vf[0]) * point[0]
440                + (tmpmat[2][0] * vr[1] + tmpmat[2][1] * vu[1] + vf[2] * vf[1]) * point[1]
441                + (tmpmat[2][0] * vr[2] + tmpmat[2][1] * vu[2] + vf[2] * vf[2]) * point[2];
442 #else
443         // LordHavoc: optimized to death and beyond, cryptic in an entirely new way
444         float   t0, t1;
445         float   angle, c, s;
446         vec3_t  vr, vu, vf;
447
448         angle = DEG2RAD(degrees);
449
450         c = cos(angle);
451         s = sin(angle);
452
453         vf[0] = dir[0];
454         vf[1] = dir[1];
455         vf[2] = dir[2];
456
457 //      PerpendicularVector(vr, dir);
458 //      CrossProduct(vr, vf, vu);
459         VectorVectors(vf, vr, vu);
460
461         t0 = vr[0] *  c + vu[0] * -s;
462         t1 = vr[0] *  s + vu[0] *  c;
463         dst[0] = (t0 * vr[0] + t1 * vu[0] + vf[0] * vf[0]) * point[0]
464                + (t0 * vr[1] + t1 * vu[1] + vf[0] * vf[1]) * point[1]
465                + (t0 * vr[2] + t1 * vu[2] + vf[0] * vf[2]) * point[2];
466
467         t0 = vr[1] *  c + vu[1] * -s;
468         t1 = vr[1] *  s + vu[1] *  c;
469         dst[1] = (t0 * vr[0] + t1 * vu[0] + vf[1] * vf[0]) * point[0]
470                + (t0 * vr[1] + t1 * vu[1] + vf[1] * vf[1]) * point[1]
471                + (t0 * vr[2] + t1 * vu[2] + vf[1] * vf[2]) * point[2];
472
473         t0 = vr[2] *  c + vu[2] * -s;
474         t1 = vr[2] *  s + vu[2] *  c;
475         dst[2] = (t0 * vr[0] + t1 * vu[0] + vf[2] * vf[0]) * point[0]
476                + (t0 * vr[1] + t1 * vu[1] + vf[2] * vf[1]) * point[1]
477                + (t0 * vr[2] + t1 * vu[2] + vf[2] * vf[2]) * point[2];
478 #endif
479 }
480
481 /*-----------------------------------------------------------------*/
482
483
484 // LordHavoc note 1:
485 // BoxOnPlaneSide did a switch on a 'signbits' value and had optimized
486 // assembly in an attempt to accelerate it further, very inefficient
487 // considering that signbits of the frustum planes only changed each
488 // frame, and the world planes changed only at load time.
489 // So, to optimize it further I took the obvious route of storing a function
490 // pointer in the plane struct itself, and shrunk each of the individual
491 // cases to a single return statement.
492 // LordHavoc note 2:
493 // realized axial cases would be a nice speedup for world geometry, although
494 // never useful for the frustum planes.
495 int BoxOnPlaneSideX (vec3_t emins, vec3_t emaxs, mplane_t *p) {return p->dist <= emins[0] ? 1 : (p->dist >= emaxs[0] ? 2 : 3);}
496 int BoxOnPlaneSideY (vec3_t emins, vec3_t emaxs, mplane_t *p) {return p->dist <= emins[1] ? 1 : (p->dist >= emaxs[1] ? 2 : 3);}
497 int BoxOnPlaneSideZ (vec3_t emins, vec3_t emaxs, mplane_t *p) {return p->dist <= emins[2] ? 1 : (p->dist >= emaxs[2] ? 2 : 3);}
498 int BoxOnPlaneSide0 (vec3_t emins, vec3_t emaxs, mplane_t *p) {return (((p->normal[0]*emaxs[0] + p->normal[1]*emaxs[1] + p->normal[2]*emaxs[2]) >= p->dist) | (((p->normal[0]*emins[0] + p->normal[1]*emins[1] + p->normal[2]*emins[2]) < p->dist) << 1));}
499 int BoxOnPlaneSide1 (vec3_t emins, vec3_t emaxs, mplane_t *p) {return (((p->normal[0]*emins[0] + p->normal[1]*emaxs[1] + p->normal[2]*emaxs[2]) >= p->dist) | (((p->normal[0]*emaxs[0] + p->normal[1]*emins[1] + p->normal[2]*emins[2]) < p->dist) << 1));}
500 int BoxOnPlaneSide2 (vec3_t emins, vec3_t emaxs, mplane_t *p) {return (((p->normal[0]*emaxs[0] + p->normal[1]*emins[1] + p->normal[2]*emaxs[2]) >= p->dist) | (((p->normal[0]*emins[0] + p->normal[1]*emaxs[1] + p->normal[2]*emins[2]) < p->dist) << 1));}
501 int BoxOnPlaneSide3 (vec3_t emins, vec3_t emaxs, mplane_t *p) {return (((p->normal[0]*emins[0] + p->normal[1]*emins[1] + p->normal[2]*emaxs[2]) >= p->dist) | (((p->normal[0]*emaxs[0] + p->normal[1]*emaxs[1] + p->normal[2]*emins[2]) < p->dist) << 1));}
502 int BoxOnPlaneSide4 (vec3_t emins, vec3_t emaxs, mplane_t *p) {return (((p->normal[0]*emaxs[0] + p->normal[1]*emaxs[1] + p->normal[2]*emins[2]) >= p->dist) | (((p->normal[0]*emins[0] + p->normal[1]*emins[1] + p->normal[2]*emaxs[2]) < p->dist) << 1));}
503 int BoxOnPlaneSide5 (vec3_t emins, vec3_t emaxs, mplane_t *p) {return (((p->normal[0]*emins[0] + p->normal[1]*emaxs[1] + p->normal[2]*emins[2]) >= p->dist) | (((p->normal[0]*emaxs[0] + p->normal[1]*emins[1] + p->normal[2]*emaxs[2]) < p->dist) << 1));}
504 int BoxOnPlaneSide6 (vec3_t emins, vec3_t emaxs, mplane_t *p) {return (((p->normal[0]*emaxs[0] + p->normal[1]*emins[1] + p->normal[2]*emins[2]) >= p->dist) | (((p->normal[0]*emins[0] + p->normal[1]*emaxs[1] + p->normal[2]*emaxs[2]) < p->dist) << 1));}
505 int BoxOnPlaneSide7 (vec3_t emins, vec3_t emaxs, mplane_t *p) {return (((p->normal[0]*emins[0] + p->normal[1]*emins[1] + p->normal[2]*emins[2]) >= p->dist) | (((p->normal[0]*emaxs[0] + p->normal[1]*emaxs[1] + p->normal[2]*emaxs[2]) < p->dist) << 1));}
506
507 void BoxOnPlaneSideClassify(mplane_t *p)
508 {
509         switch(p->type)
510         {
511         case 0: // x axis
512                 p->BoxOnPlaneSideFunc = BoxOnPlaneSideX;
513                 break;
514         case 1: // y axis
515                 p->BoxOnPlaneSideFunc = BoxOnPlaneSideY;
516                 break;
517         case 2: // z axis
518                 p->BoxOnPlaneSideFunc = BoxOnPlaneSideZ;
519                 break;
520         default:
521                 if (p->normal[2] < 0) // 4
522                 {
523                         if (p->normal[1] < 0) // 2
524                         {
525                                 if (p->normal[0] < 0) // 1
526                                         p->BoxOnPlaneSideFunc = BoxOnPlaneSide7;
527                                 else
528                                         p->BoxOnPlaneSideFunc = BoxOnPlaneSide6;
529                         }
530                         else
531                         {
532                                 if (p->normal[0] < 0) // 1
533                                         p->BoxOnPlaneSideFunc = BoxOnPlaneSide5;
534                                 else
535                                         p->BoxOnPlaneSideFunc = BoxOnPlaneSide4;
536                         }
537                 }
538                 else
539                 {
540                         if (p->normal[1] < 0) // 2
541                         {
542                                 if (p->normal[0] < 0) // 1
543                                         p->BoxOnPlaneSideFunc = BoxOnPlaneSide3;
544                                 else
545                                         p->BoxOnPlaneSideFunc = BoxOnPlaneSide2;
546                         }
547                         else
548                         {
549                                 if (p->normal[0] < 0) // 1
550                                         p->BoxOnPlaneSideFunc = BoxOnPlaneSide1;
551                                 else
552                                         p->BoxOnPlaneSideFunc = BoxOnPlaneSide0;
553                         }
554                 }
555                 break;
556         }
557 }
558
559 void PlaneClassify(mplane_t *p)
560 {
561         if (p->normal[0] == 1)
562                 p->type = 0;
563         else if (p->normal[1] == 1)
564                 p->type = 1;
565         else if (p->normal[2] == 1)
566                 p->type = 2;
567         else
568                 p->type = 3;
569         BoxOnPlaneSideClassify(p);
570 }
571
572 void AngleVectors (const vec3_t angles, vec3_t forward, vec3_t right, vec3_t up)
573 {
574         double angle, sr, sp, sy, cr, cp, cy;
575
576         angle = angles[YAW] * (M_PI*2 / 360);
577         sy = sin(angle);
578         cy = cos(angle);
579         angle = angles[PITCH] * (M_PI*2 / 360);
580         sp = sin(angle);
581         cp = cos(angle);
582         if (forward)
583         {
584                 forward[0] = cp*cy;
585                 forward[1] = cp*sy;
586                 forward[2] = -sp;
587         }
588         if (right || up)
589         {
590                 angle = angles[ROLL] * (M_PI*2 / 360);
591                 sr = sin(angle);
592                 cr = cos(angle);
593                 if (right)
594                 {
595                         right[0] = -1*(sr*sp*cy+cr*-sy);
596                         right[1] = -1*(sr*sp*sy+cr*cy);
597                         right[2] = -1*(sr*cp);
598                 }
599                 if (up)
600                 {
601                         up[0] = (cr*sp*cy+-sr*-sy);
602                         up[1] = (cr*sp*sy+-sr*cy);
603                         up[2] = cr*cp;
604                 }
605         }
606 }
607
608 void AngleVectorsFLU (const vec3_t angles, vec3_t forward, vec3_t left, vec3_t up)
609 {
610         double angle, sr, sp, sy, cr, cp, cy;
611
612         angle = angles[YAW] * (M_PI*2 / 360);
613         sy = sin(angle);
614         cy = cos(angle);
615         angle = angles[PITCH] * (M_PI*2 / 360);
616         sp = sin(angle);
617         cp = cos(angle);
618         if (forward)
619         {
620                 forward[0] = cp*cy;
621                 forward[1] = cp*sy;
622                 forward[2] = -sp;
623         }
624         if (left || up)
625         {
626                 angle = angles[ROLL] * (M_PI*2 / 360);
627                 sr = sin(angle);
628                 cr = cos(angle);
629                 if (left)
630                 {
631                         left[0] = sr*sp*cy+cr*-sy;
632                         left[1] = sr*sp*sy+cr*cy;
633                         left[2] = sr*cp;
634                 }
635                 if (up)
636                 {
637                         up[0] = cr*sp*cy+-sr*-sy;
638                         up[1] = cr*sp*sy+-sr*cy;
639                         up[2] = cr*cp;
640                 }
641         }
642 }
643
644 void AngleMatrix (const vec3_t angles, const vec3_t translate, vec_t matrix[][4])
645 {
646         double angle, sr, sp, sy, cr, cp, cy;
647
648         angle = angles[YAW] * (M_PI*2 / 360);
649         sy = sin(angle);
650         cy = cos(angle);
651         angle = angles[PITCH] * (M_PI*2 / 360);
652         sp = sin(angle);
653         cp = cos(angle);
654         angle = angles[ROLL] * (M_PI*2 / 360);
655         sr = sin(angle);
656         cr = cos(angle);
657         matrix[0][0] = cp*cy;
658         matrix[0][1] = sr*sp*cy+cr*-sy;
659         matrix[0][2] = cr*sp*cy+-sr*-sy;
660         matrix[0][3] = translate[0];
661         matrix[1][0] = cp*sy;
662         matrix[1][1] = sr*sp*sy+cr*cy;
663         matrix[1][2] = cr*sp*sy+-sr*cy;
664         matrix[1][3] = translate[1];
665         matrix[2][0] = -sp;
666         matrix[2][1] = sr*cp;
667         matrix[2][2] = cr*cp;
668         matrix[2][3] = translate[2];
669 }
670
671 // LordHavoc: changed CrossProduct to a #define
672 /*
673 void CrossProduct (vec3_t v1, vec3_t v2, vec3_t cross)
674 {
675         cross[0] = v1[1]*v2[2] - v1[2]*v2[1];
676         cross[1] = v1[2]*v2[0] - v1[0]*v2[2];
677         cross[2] = v1[0]*v2[1] - v1[1]*v2[0];
678 }
679 */
680
681 /*
682 // LordHavoc: fixme: do more research on gcc assembly so that qftol_minushalf and result will not be considered unused
683 static double qftol_minushalf = -0.5;
684
685 int qftol(double v)
686 {
687         int result;
688 #ifdef _MSC_VER
689         __asm
690         {
691                 fld             v
692                 fadd    qftol_minushalf
693                 fistp   result
694         }
695 #else // gcc hopefully
696         asm("fldl v\n\tfaddl qftol_minushalf\n\tfistpl result");
697 #endif
698         return result;
699 }
700 */
701
702 // LordHavoc: renamed these to Length, and made the normal ones #define
703 float VectorNormalizeLength (vec3_t v)
704 {
705         float   length, ilength;
706
707         length = v[0]*v[0] + v[1]*v[1] + v[2]*v[2];
708         length = sqrt (length);         // FIXME
709
710         if (length)
711         {
712                 ilength = 1/length;
713                 v[0] *= ilength;
714                 v[1] *= ilength;
715                 v[2] *= ilength;
716         }
717                 
718         return length;
719
720 }
721
722 /*
723 float VectorNormalizeLength2 (vec3_t v, vec3_t dest) // LordHavoc: added to allow copying while doing the calculation...
724 {
725         float   length, ilength;
726
727         length = v[0]*v[0] + v[1]*v[1] + v[2]*v[2];
728         length = sqrt (length);         // FIXME
729
730         if (length)
731         {
732                 ilength = 1/length;
733                 dest[0] = v[0] * ilength;
734                 dest[1] = v[1] * ilength;
735                 dest[2] = v[2] * ilength;
736         }
737         else
738                 dest[0] = dest[1] = dest[2] = 0;
739                 
740         return length;
741 }
742 */
743
744 /*
745 ================
746 R_ConcatRotations
747 ================
748 */
749 void R_ConcatRotations (const float in1[3][3], const float in2[3][3], float out[3][3])
750 {
751         out[0][0] = in1[0][0] * in2[0][0] + in1[0][1] * in2[1][0] + in1[0][2] * in2[2][0];
752         out[0][1] = in1[0][0] * in2[0][1] + in1[0][1] * in2[1][1] + in1[0][2] * in2[2][1];
753         out[0][2] = in1[0][0] * in2[0][2] + in1[0][1] * in2[1][2] + in1[0][2] * in2[2][2];
754         out[1][0] = in1[1][0] * in2[0][0] + in1[1][1] * in2[1][0] + in1[1][2] * in2[2][0];
755         out[1][1] = in1[1][0] * in2[0][1] + in1[1][1] * in2[1][1] + in1[1][2] * in2[2][1];
756         out[1][2] = in1[1][0] * in2[0][2] + in1[1][1] * in2[1][2] + in1[1][2] * in2[2][2];
757         out[2][0] = in1[2][0] * in2[0][0] + in1[2][1] * in2[1][0] + in1[2][2] * in2[2][0];
758         out[2][1] = in1[2][0] * in2[0][1] + in1[2][1] * in2[1][1] + in1[2][2] * in2[2][1];
759         out[2][2] = in1[2][0] * in2[0][2] + in1[2][1] * in2[1][2] + in1[2][2] * in2[2][2];
760 }
761
762
763 /*
764 ================
765 R_ConcatTransforms
766 ================
767 */
768 void R_ConcatTransforms (const float in1[3][4], const float in2[3][4], float out[3][4])
769 {
770         out[0][0] = in1[0][0] * in2[0][0] + in1[0][1] * in2[1][0] + in1[0][2] * in2[2][0];
771         out[0][1] = in1[0][0] * in2[0][1] + in1[0][1] * in2[1][1] + in1[0][2] * in2[2][1];
772         out[0][2] = in1[0][0] * in2[0][2] + in1[0][1] * in2[1][2] + in1[0][2] * in2[2][2];
773         out[0][3] = in1[0][0] * in2[0][3] + in1[0][1] * in2[1][3] + in1[0][2] * in2[2][3] + in1[0][3];
774         out[1][0] = in1[1][0] * in2[0][0] + in1[1][1] * in2[1][0] + in1[1][2] * in2[2][0];
775         out[1][1] = in1[1][0] * in2[0][1] + in1[1][1] * in2[1][1] + in1[1][2] * in2[2][1];
776         out[1][2] = in1[1][0] * in2[0][2] + in1[1][1] * in2[1][2] + in1[1][2] * in2[2][2];
777         out[1][3] = in1[1][0] * in2[0][3] + in1[1][1] * in2[1][3] + in1[1][2] * in2[2][3] + in1[1][3];
778         out[2][0] = in1[2][0] * in2[0][0] + in1[2][1] * in2[1][0] + in1[2][2] * in2[2][0];
779         out[2][1] = in1[2][0] * in2[0][1] + in1[2][1] * in2[1][1] + in1[2][2] * in2[2][1];
780         out[2][2] = in1[2][0] * in2[0][2] + in1[2][1] * in2[1][2] + in1[2][2] * in2[2][2];
781         out[2][3] = in1[2][0] * in2[0][3] + in1[2][1] * in2[1][3] + in1[2][2] * in2[2][3] + in1[2][3];
782 }
783
784
785 void Mathlib_Init(void)
786 {
787         int a;
788
789         // LordHavoc: setup 1.0f / N table for quick recipricols of integers
790         ixtable[0] = 0;
791         for (a = 1;a < 4096;a++)
792                 ixtable[a] = 1.0f / a;
793 }