2 Copyright (C) 1996-1997 Id Software, Inc.
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.
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.
13 See the GNU General Public License for more details.
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.
20 // mathlib.c -- math primitives
26 vec3_t vec3_origin = {0,0,0};
29 /*-----------------------------------------------------------------*/
31 float m_bytenormals[NUMVERTEXNORMALS][3] =
33 {-0.525731f, 0.000000f, 0.850651f}, {-0.442863f, 0.238856f, 0.864188f},
34 {-0.295242f, 0.000000f, 0.955423f}, {-0.309017f, 0.500000f, 0.809017f},
35 {-0.162460f, 0.262866f, 0.951056f}, {0.000000f, 0.000000f, 1.000000f},
36 {0.000000f, 0.850651f, 0.525731f}, {-0.147621f, 0.716567f, 0.681718f},
37 {0.147621f, 0.716567f, 0.681718f}, {0.000000f, 0.525731f, 0.850651f},
38 {0.309017f, 0.500000f, 0.809017f}, {0.525731f, 0.000000f, 0.850651f},
39 {0.295242f, 0.000000f, 0.955423f}, {0.442863f, 0.238856f, 0.864188f},
40 {0.162460f, 0.262866f, 0.951056f}, {-0.681718f, 0.147621f, 0.716567f},
41 {-0.809017f, 0.309017f, 0.500000f}, {-0.587785f, 0.425325f, 0.688191f},
42 {-0.850651f, 0.525731f, 0.000000f}, {-0.864188f, 0.442863f, 0.238856f},
43 {-0.716567f, 0.681718f, 0.147621f}, {-0.688191f, 0.587785f, 0.425325f},
44 {-0.500000f, 0.809017f, 0.309017f}, {-0.238856f, 0.864188f, 0.442863f},
45 {-0.425325f, 0.688191f, 0.587785f}, {-0.716567f, 0.681718f, -0.147621f},
46 {-0.500000f, 0.809017f, -0.309017f}, {-0.525731f, 0.850651f, 0.000000f},
47 {0.000000f, 0.850651f, -0.525731f}, {-0.238856f, 0.864188f, -0.442863f},
48 {0.000000f, 0.955423f, -0.295242f}, {-0.262866f, 0.951056f, -0.162460f},
49 {0.000000f, 1.000000f, 0.000000f}, {0.000000f, 0.955423f, 0.295242f},
50 {-0.262866f, 0.951056f, 0.162460f}, {0.238856f, 0.864188f, 0.442863f},
51 {0.262866f, 0.951056f, 0.162460f}, {0.500000f, 0.809017f, 0.309017f},
52 {0.238856f, 0.864188f, -0.442863f}, {0.262866f, 0.951056f, -0.162460f},
53 {0.500000f, 0.809017f, -0.309017f}, {0.850651f, 0.525731f, 0.000000f},
54 {0.716567f, 0.681718f, 0.147621f}, {0.716567f, 0.681718f, -0.147621f},
55 {0.525731f, 0.850651f, 0.000000f}, {0.425325f, 0.688191f, 0.587785f},
56 {0.864188f, 0.442863f, 0.238856f}, {0.688191f, 0.587785f, 0.425325f},
57 {0.809017f, 0.309017f, 0.500000f}, {0.681718f, 0.147621f, 0.716567f},
58 {0.587785f, 0.425325f, 0.688191f}, {0.955423f, 0.295242f, 0.000000f},
59 {1.000000f, 0.000000f, 0.000000f}, {0.951056f, 0.162460f, 0.262866f},
60 {0.850651f, -0.525731f, 0.000000f}, {0.955423f, -0.295242f, 0.000000f},
61 {0.864188f, -0.442863f, 0.238856f}, {0.951056f, -0.162460f, 0.262866f},
62 {0.809017f, -0.309017f, 0.500000f}, {0.681718f, -0.147621f, 0.716567f},
63 {0.850651f, 0.000000f, 0.525731f}, {0.864188f, 0.442863f, -0.238856f},
64 {0.809017f, 0.309017f, -0.500000f}, {0.951056f, 0.162460f, -0.262866f},
65 {0.525731f, 0.000000f, -0.850651f}, {0.681718f, 0.147621f, -0.716567f},
66 {0.681718f, -0.147621f, -0.716567f}, {0.850651f, 0.000000f, -0.525731f},
67 {0.809017f, -0.309017f, -0.500000f}, {0.864188f, -0.442863f, -0.238856f},
68 {0.951056f, -0.162460f, -0.262866f}, {0.147621f, 0.716567f, -0.681718f},
69 {0.309017f, 0.500000f, -0.809017f}, {0.425325f, 0.688191f, -0.587785f},
70 {0.442863f, 0.238856f, -0.864188f}, {0.587785f, 0.425325f, -0.688191f},
71 {0.688191f, 0.587785f, -0.425325f}, {-0.147621f, 0.716567f, -0.681718f},
72 {-0.309017f, 0.500000f, -0.809017f}, {0.000000f, 0.525731f, -0.850651f},
73 {-0.525731f, 0.000000f, -0.850651f}, {-0.442863f, 0.238856f, -0.864188f},
74 {-0.295242f, 0.000000f, -0.955423f}, {-0.162460f, 0.262866f, -0.951056f},
75 {0.000000f, 0.000000f, -1.000000f}, {0.295242f, 0.000000f, -0.955423f},
76 {0.162460f, 0.262866f, -0.951056f}, {-0.442863f, -0.238856f, -0.864188f},
77 {-0.309017f, -0.500000f, -0.809017f}, {-0.162460f, -0.262866f, -0.951056f},
78 {0.000000f, -0.850651f, -0.525731f}, {-0.147621f, -0.716567f, -0.681718f},
79 {0.147621f, -0.716567f, -0.681718f}, {0.000000f, -0.525731f, -0.850651f},
80 {0.309017f, -0.500000f, -0.809017f}, {0.442863f, -0.238856f, -0.864188f},
81 {0.162460f, -0.262866f, -0.951056f}, {0.238856f, -0.864188f, -0.442863f},
82 {0.500000f, -0.809017f, -0.309017f}, {0.425325f, -0.688191f, -0.587785f},
83 {0.716567f, -0.681718f, -0.147621f}, {0.688191f, -0.587785f, -0.425325f},
84 {0.587785f, -0.425325f, -0.688191f}, {0.000000f, -0.955423f, -0.295242f},
85 {0.000000f, -1.000000f, 0.000000f}, {0.262866f, -0.951056f, -0.162460f},
86 {0.000000f, -0.850651f, 0.525731f}, {0.000000f, -0.955423f, 0.295242f},
87 {0.238856f, -0.864188f, 0.442863f}, {0.262866f, -0.951056f, 0.162460f},
88 {0.500000f, -0.809017f, 0.309017f}, {0.716567f, -0.681718f, 0.147621f},
89 {0.525731f, -0.850651f, 0.000000f}, {-0.238856f, -0.864188f, -0.442863f},
90 {-0.500000f, -0.809017f, -0.309017f}, {-0.262866f, -0.951056f, -0.162460f},
91 {-0.850651f, -0.525731f, 0.000000f}, {-0.716567f, -0.681718f, -0.147621f},
92 {-0.716567f, -0.681718f, 0.147621f}, {-0.525731f, -0.850651f, 0.000000f},
93 {-0.500000f, -0.809017f, 0.309017f}, {-0.238856f, -0.864188f, 0.442863f},
94 {-0.262866f, -0.951056f, 0.162460f}, {-0.864188f, -0.442863f, 0.238856f},
95 {-0.809017f, -0.309017f, 0.500000f}, {-0.688191f, -0.587785f, 0.425325f},
96 {-0.681718f, -0.147621f, 0.716567f}, {-0.442863f, -0.238856f, 0.864188f},
97 {-0.587785f, -0.425325f, 0.688191f}, {-0.309017f, -0.500000f, 0.809017f},
98 {-0.147621f, -0.716567f, 0.681718f}, {-0.425325f, -0.688191f, 0.587785f},
99 {-0.162460f, -0.262866f, 0.951056f}, {0.442863f, -0.238856f, 0.864188f},
100 {0.162460f, -0.262866f, 0.951056f}, {0.309017f, -0.500000f, 0.809017f},
101 {0.147621f, -0.716567f, 0.681718f}, {0.000000f, -0.525731f, 0.850651f},
102 {0.425325f, -0.688191f, 0.587785f}, {0.587785f, -0.425325f, 0.688191f},
103 {0.688191f, -0.587785f, 0.425325f}, {-0.955423f, 0.295242f, 0.000000f},
104 {-0.951056f, 0.162460f, 0.262866f}, {-1.000000f, 0.000000f, 0.000000f},
105 {-0.850651f, 0.000000f, 0.525731f}, {-0.955423f, -0.295242f, 0.000000f},
106 {-0.951056f, -0.162460f, 0.262866f}, {-0.864188f, 0.442863f, -0.238856f},
107 {-0.951056f, 0.162460f, -0.262866f}, {-0.809017f, 0.309017f, -0.500000f},
108 {-0.864188f, -0.442863f, -0.238856f}, {-0.951056f, -0.162460f, -0.262866f},
109 {-0.809017f, -0.309017f, -0.500000f}, {-0.681718f, 0.147621f, -0.716567f},
110 {-0.681718f, -0.147621f, -0.716567f}, {-0.850651f, 0.000000f, -0.525731f},
111 {-0.688191f, 0.587785f, -0.425325f}, {-0.587785f, 0.425325f, -0.688191f},
112 {-0.425325f, 0.688191f, -0.587785f}, {-0.425325f, -0.688191f, -0.587785f},
113 {-0.587785f, -0.425325f, -0.688191f}, {-0.688191f, -0.587785f, -0.425325f},
117 unsigned char NormalToByte(const vec3_t n)
120 float bestdistance, distance;
123 bestdistance = DotProduct (n, m_bytenormals[0]);
124 for (i = 1;i < NUMVERTEXNORMALS;i++)
126 distance = DotProduct (n, m_bytenormals[i]);
127 if (distance > bestdistance)
129 bestdistance = distance;
136 // note: uses byte partly to force unsigned for the validity check
137 void ByteToNormal(unsigned char num, vec3_t n)
139 if (num < NUMVERTEXNORMALS)
140 VectorCopy(m_bytenormals[num], n);
142 VectorClear(n); // FIXME: complain?
145 // assumes "src" is normalized
146 void PerpendicularVector( vec3_t dst, const vec3_t src )
148 // LadyHavoc: optimized to death and beyond
162 minelem = fabs(src[0]);
163 if (fabs(src[1]) < minelem)
166 minelem = fabs(src[1]);
168 if (fabs(src[2]) < minelem)
172 dst[0] -= src[pos] * src[0];
173 dst[1] -= src[pos] * src[1];
174 dst[2] -= src[pos] * src[2];
176 // normalize the result
177 VectorNormalize(dst);
198 // LadyHavoc: like AngleVectors, but taking a forward vector instead of angles, useful!
199 void VectorVectors(const vec3_t forward, vec3_t right, vec3_t up)
201 // NOTE: this is consistent to AngleVectors applied to AnglesFromVectors
202 if (forward[0] == 0 && forward[1] == 0)
206 VectorSet(right, 0, -1, 0);
207 VectorSet(up, -1, 0, 0);
211 VectorSet(right, 0, -1, 0);
212 VectorSet(up, 1, 0, 0);
217 right[0] = forward[1];
218 right[1] = -forward[0];
220 VectorNormalize(right);
222 up[0] = (-forward[2]*forward[0]);
223 up[1] = (-forward[2]*forward[1]);
224 up[2] = (forward[0]*forward[0] + forward[1]*forward[1]);
229 void VectorVectorsDouble(const double *forward, double *right, double *up)
231 if (forward[0] == 0 && forward[1] == 0)
235 VectorSet(right, 0, -1, 0);
236 VectorSet(up, -1, 0, 0);
240 VectorSet(right, 0, -1, 0);
241 VectorSet(up, 1, 0, 0);
246 right[0] = forward[1];
247 right[1] = -forward[0];
249 VectorNormalize(right);
251 up[0] = (-forward[2]*forward[0]);
252 up[1] = (-forward[2]*forward[1]);
253 up[2] = (forward[0]*forward[0] + forward[1]*forward[1]);
258 void RotatePointAroundVector( vec3_t dst, const vec3_t dir, const vec3_t point, float degrees )
264 angle = DEG2RAD(degrees);
268 VectorVectors(vf, vr, vu);
270 t0 = vr[0] * c + vu[0] * -s;
271 t1 = vr[0] * s + vu[0] * c;
272 dst[0] = (t0 * vr[0] + t1 * vu[0] + vf[0] * vf[0]) * point[0]
273 + (t0 * vr[1] + t1 * vu[1] + vf[0] * vf[1]) * point[1]
274 + (t0 * vr[2] + t1 * vu[2] + vf[0] * vf[2]) * point[2];
276 t0 = vr[1] * c + vu[1] * -s;
277 t1 = vr[1] * s + vu[1] * c;
278 dst[1] = (t0 * vr[0] + t1 * vu[0] + vf[1] * vf[0]) * point[0]
279 + (t0 * vr[1] + t1 * vu[1] + vf[1] * vf[1]) * point[1]
280 + (t0 * vr[2] + t1 * vu[2] + vf[1] * vf[2]) * point[2];
282 t0 = vr[2] * c + vu[2] * -s;
283 t1 = vr[2] * s + vu[2] * c;
284 dst[2] = (t0 * vr[0] + t1 * vu[0] + vf[2] * vf[0]) * point[0]
285 + (t0 * vr[1] + t1 * vu[1] + vf[2] * vf[1]) * point[1]
286 + (t0 * vr[2] + t1 * vu[2] + vf[2] * vf[2]) * point[2];
289 /*-----------------------------------------------------------------*/
291 // returns the smallest integer greater than or equal to "value", or 0 if "value" is too big
292 unsigned int CeilPowerOf2(unsigned int value)
294 unsigned int ceilvalue;
296 if (value > (1U << (sizeof(int) * 8 - 1)))
300 while (ceilvalue < value)
307 /*-----------------------------------------------------------------*/
310 void PlaneClassify(mplane_t *p)
312 // for optimized plane comparisons
313 if (p->normal[0] == 1)
315 else if (p->normal[1] == 1)
317 else if (p->normal[2] == 1)
321 // for BoxOnPlaneSide
323 if (p->normal[0] < 0) // 1
325 if (p->normal[1] < 0) // 2
327 if (p->normal[2] < 0) // 4
331 int BoxOnPlaneSide(const vec3_t emins, const vec3_t emaxs, const mplane_t *p)
334 return ((emaxs[p->type] >= p->dist) | ((emins[p->type] < p->dist) << 1));
338 case 0: 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));
339 case 1: 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));
340 case 2: 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));
341 case 3: 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));
342 case 4: 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));
343 case 5: 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));
344 case 6: 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));
345 case 7: 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));
350 int BoxOnPlaneSide_Separate(const vec3_t emins, const vec3_t emaxs, const vec3_t normal, const vec_t dist)
352 switch((normal[0] < 0) | ((normal[1] < 0) << 1) | ((normal[2] < 0) << 2))
355 case 0: return (((normal[0] * emaxs[0] + normal[1] * emaxs[1] + normal[2] * emaxs[2]) >= dist) | (((normal[0] * emins[0] + normal[1] * emins[1] + normal[2] * emins[2]) < dist) << 1));
356 case 1: return (((normal[0] * emins[0] + normal[1] * emaxs[1] + normal[2] * emaxs[2]) >= dist) | (((normal[0] * emaxs[0] + normal[1] * emins[1] + normal[2] * emins[2]) < dist) << 1));
357 case 2: return (((normal[0] * emaxs[0] + normal[1] * emins[1] + normal[2] * emaxs[2]) >= dist) | (((normal[0] * emins[0] + normal[1] * emaxs[1] + normal[2] * emins[2]) < dist) << 1));
358 case 3: return (((normal[0] * emins[0] + normal[1] * emins[1] + normal[2] * emaxs[2]) >= dist) | (((normal[0] * emaxs[0] + normal[1] * emaxs[1] + normal[2] * emins[2]) < dist) << 1));
359 case 4: return (((normal[0] * emaxs[0] + normal[1] * emaxs[1] + normal[2] * emins[2]) >= dist) | (((normal[0] * emins[0] + normal[1] * emins[1] + normal[2] * emaxs[2]) < dist) << 1));
360 case 5: return (((normal[0] * emins[0] + normal[1] * emaxs[1] + normal[2] * emins[2]) >= dist) | (((normal[0] * emaxs[0] + normal[1] * emins[1] + normal[2] * emaxs[2]) < dist) << 1));
361 case 6: return (((normal[0] * emaxs[0] + normal[1] * emins[1] + normal[2] * emins[2]) >= dist) | (((normal[0] * emins[0] + normal[1] * emaxs[1] + normal[2] * emaxs[2]) < dist) << 1));
362 case 7: return (((normal[0] * emins[0] + normal[1] * emins[1] + normal[2] * emins[2]) >= dist) | (((normal[0] * emaxs[0] + normal[1] * emaxs[1] + normal[2] * emaxs[2]) < dist) << 1));
367 void BoxPlaneCorners(const vec3_t emins, const vec3_t emaxs, const mplane_t *p, vec3_t outnear, vec3_t outfar)
371 outnear[0] = outnear[1] = outnear[2] = outfar[0] = outfar[1] = outfar[2] = 0;
372 outnear[p->type] = emins[p->type];
373 outfar[p->type] = emaxs[p->type];
379 case 0: outnear[0] = emaxs[0];outnear[1] = emaxs[1];outnear[2] = emaxs[2];outfar[0] = emins[0];outfar[1] = emins[1];outfar[2] = emins[2];break;
380 case 1: outnear[0] = emins[0];outnear[1] = emaxs[1];outnear[2] = emaxs[2];outfar[0] = emaxs[0];outfar[1] = emins[1];outfar[2] = emins[2];break;
381 case 2: outnear[0] = emaxs[0];outnear[1] = emins[1];outnear[2] = emaxs[2];outfar[0] = emins[0];outfar[1] = emaxs[1];outfar[2] = emins[2];break;
382 case 3: outnear[0] = emins[0];outnear[1] = emins[1];outnear[2] = emaxs[2];outfar[0] = emaxs[0];outfar[1] = emaxs[1];outfar[2] = emins[2];break;
383 case 4: outnear[0] = emaxs[0];outnear[1] = emaxs[1];outnear[2] = emins[2];outfar[0] = emins[0];outfar[1] = emins[1];outfar[2] = emaxs[2];break;
384 case 5: outnear[0] = emins[0];outnear[1] = emaxs[1];outnear[2] = emins[2];outfar[0] = emaxs[0];outfar[1] = emins[1];outfar[2] = emaxs[2];break;
385 case 6: outnear[0] = emaxs[0];outnear[1] = emins[1];outnear[2] = emins[2];outfar[0] = emins[0];outfar[1] = emaxs[1];outfar[2] = emaxs[2];break;
386 case 7: outnear[0] = emins[0];outnear[1] = emins[1];outnear[2] = emins[2];outfar[0] = emaxs[0];outfar[1] = emaxs[1];outfar[2] = emaxs[2];break;
390 void BoxPlaneCorners_Separate(const vec3_t emins, const vec3_t emaxs, const vec3_t normal, vec3_t outnear, vec3_t outfar)
392 switch((normal[0] < 0) | ((normal[1] < 0) << 1) | ((normal[2] < 0) << 2))
395 case 0: outnear[0] = emaxs[0];outnear[1] = emaxs[1];outnear[2] = emaxs[2];outfar[0] = emins[0];outfar[1] = emins[1];outfar[2] = emins[2];break;
396 case 1: outnear[0] = emins[0];outnear[1] = emaxs[1];outnear[2] = emaxs[2];outfar[0] = emaxs[0];outfar[1] = emins[1];outfar[2] = emins[2];break;
397 case 2: outnear[0] = emaxs[0];outnear[1] = emins[1];outnear[2] = emaxs[2];outfar[0] = emins[0];outfar[1] = emaxs[1];outfar[2] = emins[2];break;
398 case 3: outnear[0] = emins[0];outnear[1] = emins[1];outnear[2] = emaxs[2];outfar[0] = emaxs[0];outfar[1] = emaxs[1];outfar[2] = emins[2];break;
399 case 4: outnear[0] = emaxs[0];outnear[1] = emaxs[1];outnear[2] = emins[2];outfar[0] = emins[0];outfar[1] = emins[1];outfar[2] = emaxs[2];break;
400 case 5: outnear[0] = emins[0];outnear[1] = emaxs[1];outnear[2] = emins[2];outfar[0] = emaxs[0];outfar[1] = emins[1];outfar[2] = emaxs[2];break;
401 case 6: outnear[0] = emaxs[0];outnear[1] = emins[1];outnear[2] = emins[2];outfar[0] = emins[0];outfar[1] = emaxs[1];outfar[2] = emaxs[2];break;
402 case 7: outnear[0] = emins[0];outnear[1] = emins[1];outnear[2] = emins[2];outfar[0] = emaxs[0];outfar[1] = emaxs[1];outfar[2] = emaxs[2];break;
406 void BoxPlaneCornerDistances(const vec3_t emins, const vec3_t emaxs, const mplane_t *p, vec_t *outneardist, vec_t *outfardist)
410 *outneardist = emins[p->type] - p->dist;
411 *outfardist = emaxs[p->type] - p->dist;
417 case 0: *outneardist = p->normal[0] * emaxs[0] + p->normal[1] * emaxs[1] + p->normal[2] * emaxs[2] - p->dist;*outfardist = p->normal[0] * emins[0] + p->normal[1] * emins[1] + p->normal[2] * emins[2] - p->dist;break;
418 case 1: *outneardist = p->normal[0] * emins[0] + p->normal[1] * emaxs[1] + p->normal[2] * emaxs[2] - p->dist;*outfardist = p->normal[0] * emaxs[0] + p->normal[1] * emins[1] + p->normal[2] * emins[2] - p->dist;break;
419 case 2: *outneardist = p->normal[0] * emaxs[0] + p->normal[1] * emins[1] + p->normal[2] * emaxs[2] - p->dist;*outfardist = p->normal[0] * emins[0] + p->normal[1] * emaxs[1] + p->normal[2] * emins[2] - p->dist;break;
420 case 3: *outneardist = p->normal[0] * emins[0] + p->normal[1] * emins[1] + p->normal[2] * emaxs[2] - p->dist;*outfardist = p->normal[0] * emaxs[0] + p->normal[1] * emaxs[1] + p->normal[2] * emins[2] - p->dist;break;
421 case 4: *outneardist = p->normal[0] * emaxs[0] + p->normal[1] * emaxs[1] + p->normal[2] * emins[2] - p->dist;*outfardist = p->normal[0] * emins[0] + p->normal[1] * emins[1] + p->normal[2] * emaxs[2] - p->dist;break;
422 case 5: *outneardist = p->normal[0] * emins[0] + p->normal[1] * emaxs[1] + p->normal[2] * emins[2] - p->dist;*outfardist = p->normal[0] * emaxs[0] + p->normal[1] * emins[1] + p->normal[2] * emaxs[2] - p->dist;break;
423 case 6: *outneardist = p->normal[0] * emaxs[0] + p->normal[1] * emins[1] + p->normal[2] * emins[2] - p->dist;*outfardist = p->normal[0] * emins[0] + p->normal[1] * emaxs[1] + p->normal[2] * emaxs[2] - p->dist;break;
424 case 7: *outneardist = p->normal[0] * emins[0] + p->normal[1] * emins[1] + p->normal[2] * emins[2] - p->dist;*outfardist = p->normal[0] * emaxs[0] + p->normal[1] * emaxs[1] + p->normal[2] * emaxs[2] - p->dist;break;
428 void BoxPlaneCornerDistances_Separate(const vec3_t emins, const vec3_t emaxs, const vec3_t normal, vec_t *outneardist, vec_t *outfardist)
430 switch((normal[0] < 0) | ((normal[1] < 0) << 1) | ((normal[2] < 0) << 2))
433 case 0: *outneardist = normal[0] * emaxs[0] + normal[1] * emaxs[1] + normal[2] * emaxs[2];*outfardist = normal[0] * emins[0] + normal[1] * emins[1] + normal[2] * emins[2];break;
434 case 1: *outneardist = normal[0] * emins[0] + normal[1] * emaxs[1] + normal[2] * emaxs[2];*outfardist = normal[0] * emaxs[0] + normal[1] * emins[1] + normal[2] * emins[2];break;
435 case 2: *outneardist = normal[0] * emaxs[0] + normal[1] * emins[1] + normal[2] * emaxs[2];*outfardist = normal[0] * emins[0] + normal[1] * emaxs[1] + normal[2] * emins[2];break;
436 case 3: *outneardist = normal[0] * emins[0] + normal[1] * emins[1] + normal[2] * emaxs[2];*outfardist = normal[0] * emaxs[0] + normal[1] * emaxs[1] + normal[2] * emins[2];break;
437 case 4: *outneardist = normal[0] * emaxs[0] + normal[1] * emaxs[1] + normal[2] * emins[2];*outfardist = normal[0] * emins[0] + normal[1] * emins[1] + normal[2] * emaxs[2];break;
438 case 5: *outneardist = normal[0] * emins[0] + normal[1] * emaxs[1] + normal[2] * emins[2];*outfardist = normal[0] * emaxs[0] + normal[1] * emins[1] + normal[2] * emaxs[2];break;
439 case 6: *outneardist = normal[0] * emaxs[0] + normal[1] * emins[1] + normal[2] * emins[2];*outfardist = normal[0] * emins[0] + normal[1] * emaxs[1] + normal[2] * emaxs[2];break;
440 case 7: *outneardist = normal[0] * emins[0] + normal[1] * emins[1] + normal[2] * emins[2];*outfardist = normal[0] * emaxs[0] + normal[1] * emaxs[1] + normal[2] * emaxs[2];break;
444 void AngleVectors (const vec3_t angles, vec3_t forward, vec3_t right, vec3_t up)
446 double angle, sr, sp, sy, cr, cp, cy;
448 angle = angles[YAW] * (M_PI*2 / 360);
451 angle = angles[PITCH] * (M_PI*2 / 360);
464 angle = angles[ROLL] * (M_PI*2 / 360);
469 right[0] = -1*(sr*sp*cy+cr*-sy);
470 right[1] = -1*(sr*sp*sy+cr*cy);
471 right[2] = -1*(sr*cp);
475 up[0] = (cr*sp*cy+-sr*-sy);
476 up[1] = (cr*sp*sy+-sr*cy);
498 void AngleVectorsFLU (const vec3_t angles, vec3_t forward, vec3_t left, vec3_t up)
500 double angle, sr, sp, sy, cr, cp, cy;
502 angle = angles[YAW] * (M_PI*2 / 360);
505 angle = angles[PITCH] * (M_PI*2 / 360);
518 angle = angles[ROLL] * (M_PI*2 / 360);
523 left[0] = sr*sp*cy+cr*-sy;
524 left[1] = sr*sp*sy+cr*cy;
529 up[0] = cr*sp*cy+-sr*-sy;
530 up[1] = cr*sp*sy+-sr*cy;
552 void AngleVectorsDuke3DFLU (const vec3_t angles, vec3_t forward, vec3_t left, vec3_t up, double maxShearAngle)
554 double angle, sr, sy, cr, cy;
555 double sxx, sxz, szx, szz;
556 double cosMaxShearAngle = cos(maxShearAngle * (M_PI*2 / 360));
557 double tanMaxShearAngle = tan(maxShearAngle * (M_PI*2 / 360));
559 angle = angles[YAW] * (M_PI*2 / 360);
562 angle = angles[PITCH] * (M_PI*2 / 360);
564 // We will calculate a shear matrix pitch = [[sxx sxz][szx szz]].
566 if (fabs(cos(angle)) > cosMaxShearAngle)
568 // Pure shear. Keep the original sign of the coefficients.
573 // Covering angle per screen coordinate:
574 // d/dt arctan((sxz + t*szz) / (sxx + t*szx)) @ t=0
575 // d_angle = det(S) / (sxx*sxx + szx*szx)
576 // = 1 / (1 + tan^2 angle)
581 // A mix of shear and rotation. Implementation-wise, we're
582 // looking at a capsule, and making the screen surface
583 // tangential to it... and if we get here, we're looking at the
584 // two half-spheres of the capsule (and the cylinder part is
586 double x, y, h, t, d, f;
587 h = tanMaxShearAngle;
590 t = h * fabs(y) + sqrt(1 - (h * x) * (h * x));
592 sxz = y * t - h * (y > 0 ? 1.0 : -1.0);
595 // BUT: keep the amount of a sphere we see in pitch direction
597 // Covering angle per screen coordinate:
598 // d_angle = det(S) / (sxx*sxx + szx*szx)
599 d = (sxx * szz - sxz * szx) / (sxx * sxx + szx * szx);
600 f = cosMaxShearAngle * cosMaxShearAngle / d;
615 angle = angles[ROLL] * (M_PI*2 / 360);
620 left[0] = sr*sxz*cy+cr*-sy;
621 left[1] = sr*sxz*sy+cr*cy;
626 up[0] = cr*sxz*cy+-sr*-sy;
627 up[1] = cr*sxz*sy+-sr*cy;
649 // LadyHavoc: calculates pitch/yaw/roll angles from forward and up vectors
650 void AnglesFromVectors (vec3_t angles, const vec3_t forward, const vec3_t up, qboolean flippitch)
652 if (forward[0] == 0 && forward[1] == 0)
656 angles[PITCH] = -M_PI * 0.5;
657 angles[YAW] = up ? atan2(-up[1], -up[0]) : 0;
661 angles[PITCH] = M_PI * 0.5;
662 angles[YAW] = up ? atan2(up[1], up[0]) : 0;
668 angles[YAW] = atan2(forward[1], forward[0]);
669 angles[PITCH] = -atan2(forward[2], sqrt(forward[0]*forward[0] + forward[1]*forward[1]));
670 // note: we know that angles[PITCH] is in ]-pi/2..pi/2[ due to atan2(anything, positive)
673 vec_t cp = cos(angles[PITCH]), sp = sin(angles[PITCH]);
674 // note: we know cp > 0, due to the range angles[pitch] is in
675 vec_t cy = cos(angles[YAW]), sy = sin(angles[YAW]);
683 angles[ROLL] = -atan2(DotProduct(up, tleft), DotProduct(up, tup));
684 // for up == '0 0 1', this is
685 // angles[ROLL] = -atan2(0, cp);
691 // so no up vector is equivalent to '1 0 0'!
694 // now convert radians to degrees, and make all values positive
695 VectorScale(angles, 180.0 / M_PI, angles);
698 if (angles[PITCH] < 0) angles[PITCH] += 360;
699 if (angles[YAW] < 0) angles[YAW] += 360;
700 if (angles[ROLL] < 0) angles[ROLL] += 360;
705 vec3_t tforward, tleft, tup, nforward, nup;
706 VectorCopy(forward, nforward);
707 VectorNormalize(nforward);
711 VectorNormalize(nup);
712 AngleVectors(angles, tforward, tleft, tup);
713 if (VectorDistance(tforward, nforward) > 0.01 || VectorDistance(tup, nup) > 0.01)
715 Con_Printf("vectoangles('%f %f %f', '%f %f %f') = %f %f %f\n", nforward[0], nforward[1], nforward[2], nup[0], nup[1], nup[2], angles[0], angles[1], angles[2]);
716 Con_Printf("^3But that is '%f %f %f', '%f %f %f'\n", tforward[0], tforward[1], tforward[2], tup[0], tup[1], tup[2]);
721 AngleVectors(angles, tforward, tleft, tup);
722 if (VectorDistance(tforward, nforward) > 0.01)
724 Con_Printf("vectoangles('%f %f %f') = %f %f %f\n", nforward[0], nforward[1], nforward[2], angles[0], angles[1], angles[2]);
725 Con_Printf("^3But that is '%f %f %f'\n", tforward[0], tforward[1], tforward[2]);
733 void AngleMatrix (const vec3_t angles, const vec3_t translate, vec_t matrix[][4])
735 double angle, sr, sp, sy, cr, cp, cy;
737 angle = angles[YAW] * (M_PI*2 / 360);
740 angle = angles[PITCH] * (M_PI*2 / 360);
743 angle = angles[ROLL] * (M_PI*2 / 360);
746 matrix[0][0] = cp*cy;
747 matrix[0][1] = sr*sp*cy+cr*-sy;
748 matrix[0][2] = cr*sp*cy+-sr*-sy;
749 matrix[0][3] = translate[0];
750 matrix[1][0] = cp*sy;
751 matrix[1][1] = sr*sp*sy+cr*cy;
752 matrix[1][2] = cr*sp*sy+-sr*cy;
753 matrix[1][3] = translate[1];
755 matrix[2][1] = sr*cp;
756 matrix[2][2] = cr*cp;
757 matrix[2][3] = translate[2];
762 // LadyHavoc: renamed this to Length, and made the normal one a #define
763 float VectorNormalizeLength (vec3_t v)
765 float length, ilength;
767 length = v[0]*v[0] + v[1]*v[1] + v[2]*v[2];
768 length = sqrt (length);
788 void R_ConcatRotations (const float in1[3*3], const float in2[3*3], float out[3*3])
790 out[0*3+0] = in1[0*3+0] * in2[0*3+0] + in1[0*3+1] * in2[1*3+0] + in1[0*3+2] * in2[2*3+0];
791 out[0*3+1] = in1[0*3+0] * in2[0*3+1] + in1[0*3+1] * in2[1*3+1] + in1[0*3+2] * in2[2*3+1];
792 out[0*3+2] = in1[0*3+0] * in2[0*3+2] + in1[0*3+1] * in2[1*3+2] + in1[0*3+2] * in2[2*3+2];
793 out[1*3+0] = in1[1*3+0] * in2[0*3+0] + in1[1*3+1] * in2[1*3+0] + in1[1*3+2] * in2[2*3+0];
794 out[1*3+1] = in1[1*3+0] * in2[0*3+1] + in1[1*3+1] * in2[1*3+1] + in1[1*3+2] * in2[2*3+1];
795 out[1*3+2] = in1[1*3+0] * in2[0*3+2] + in1[1*3+1] * in2[1*3+2] + in1[1*3+2] * in2[2*3+2];
796 out[2*3+0] = in1[2*3+0] * in2[0*3+0] + in1[2*3+1] * in2[1*3+0] + in1[2*3+2] * in2[2*3+0];
797 out[2*3+1] = in1[2*3+0] * in2[0*3+1] + in1[2*3+1] * in2[1*3+1] + in1[2*3+2] * in2[2*3+1];
798 out[2*3+2] = in1[2*3+0] * in2[0*3+2] + in1[2*3+1] * in2[1*3+2] + in1[2*3+2] * in2[2*3+2];
807 void R_ConcatTransforms (const float in1[3*4], const float in2[3*4], float out[3*4])
809 out[0*4+0] = in1[0*4+0] * in2[0*4+0] + in1[0*4+1] * in2[1*4+0] + in1[0*4+2] * in2[2*4+0];
810 out[0*4+1] = in1[0*4+0] * in2[0*4+1] + in1[0*4+1] * in2[1*4+1] + in1[0*4+2] * in2[2*4+1];
811 out[0*4+2] = in1[0*4+0] * in2[0*4+2] + in1[0*4+1] * in2[1*4+2] + in1[0*4+2] * in2[2*4+2];
812 out[0*4+3] = in1[0*4+0] * in2[0*4+3] + in1[0*4+1] * in2[1*4+3] + in1[0*4+2] * in2[2*4+3] + in1[0*4+3];
813 out[1*4+0] = in1[1*4+0] * in2[0*4+0] + in1[1*4+1] * in2[1*4+0] + in1[1*4+2] * in2[2*4+0];
814 out[1*4+1] = in1[1*4+0] * in2[0*4+1] + in1[1*4+1] * in2[1*4+1] + in1[1*4+2] * in2[2*4+1];
815 out[1*4+2] = in1[1*4+0] * in2[0*4+2] + in1[1*4+1] * in2[1*4+2] + in1[1*4+2] * in2[2*4+2];
816 out[1*4+3] = in1[1*4+0] * in2[0*4+3] + in1[1*4+1] * in2[1*4+3] + in1[1*4+2] * in2[2*4+3] + in1[1*4+3];
817 out[2*4+0] = in1[2*4+0] * in2[0*4+0] + in1[2*4+1] * in2[1*4+0] + in1[2*4+2] * in2[2*4+0];
818 out[2*4+1] = in1[2*4+0] * in2[0*4+1] + in1[2*4+1] * in2[1*4+1] + in1[2*4+2] * in2[2*4+1];
819 out[2*4+2] = in1[2*4+0] * in2[0*4+2] + in1[2*4+1] * in2[1*4+2] + in1[2*4+2] * in2[2*4+2];
820 out[2*4+3] = in1[2*4+0] * in2[0*4+3] + in1[2*4+1] * in2[1*4+3] + in1[2*4+2] * in2[2*4+3] + in1[2*4+3];
823 float RadiusFromBounds (const vec3_t mins, const vec3_t maxs)
826 VectorMultiply(mins, mins, m1);
827 VectorMultiply(maxs, maxs, m2);
828 return sqrt(max(m1[0], m2[0]) + max(m1[1], m2[1]) + max(m1[2], m2[2]));
831 float RadiusFromBoundsAndOrigin (const vec3_t mins, const vec3_t maxs, const vec3_t origin)
834 VectorSubtract(mins, origin, m1);VectorMultiply(m1, m1, m1);
835 VectorSubtract(maxs, origin, m2);VectorMultiply(m2, m2, m2);
836 return sqrt(max(m1[0], m2[0]) + max(m1[1], m2[1]) + max(m1[2], m2[2]));
839 static void Math_RandomSeed_UnitTests(void);
840 void Mathlib_Init(void)
844 // LadyHavoc: setup 1.0f / N table for quick recipricols of integers
846 for (a = 1;a < 4096;a++)
847 ixtable[a] = 1.0f / a;
849 Math_RandomSeed_UnitTests();
852 #include "matrixlib.h"
854 void Matrix4x4_Print(const matrix4x4_t *in)
856 Con_Printf("%f %f %f %f\n%f %f %f %f\n%f %f %f %f\n%f %f %f %f\n"
857 , in->m[0][0], in->m[0][1], in->m[0][2], in->m[0][3]
858 , in->m[1][0], in->m[1][1], in->m[1][2], in->m[1][3]
859 , in->m[2][0], in->m[2][1], in->m[2][2], in->m[2][3]
860 , in->m[3][0], in->m[3][1], in->m[3][2], in->m[3][3]);
863 int Math_atov(const char *s, prvm_vec3_t out)
869 for (i = 0;i < 3;i++)
871 while (*s == ' ' || *s == '\t')
874 if (out[i] == 0 && *s != '-' && *s != '+' && (*s < '0' || *s > '9'))
875 break; // not a number
876 while (*s && *s != ' ' && *s !='\t' && *s != '\'')
884 void BoxFromPoints(vec3_t mins, vec3_t maxs, int numpoints, vec_t *point3f)
887 VectorCopy(point3f, mins);
888 VectorCopy(point3f, maxs);
889 for (i = 1, point3f += 3;i < numpoints;i++, point3f += 3)
891 mins[0] = min(mins[0], point3f[0]);maxs[0] = max(maxs[0], point3f[0]);
892 mins[1] = min(mins[1], point3f[1]);maxs[1] = max(maxs[1], point3f[1]);
893 mins[2] = min(mins[2], point3f[2]);maxs[2] = max(maxs[2], point3f[2]);
897 // LadyHavoc: this has to be done right or you get severe precision breakdown
898 int LoopingFrameNumberFromDouble(double t, int loopframes)
901 return (int)(t - floor(t/loopframes)*loopframes);
906 static unsigned int mul_Lecuyer[4] = { 0x12e15e35, 0xb500f16e, 0x2e714eb2, 0xb37916a5 };
908 static void mul128(const unsigned int a[], const unsigned int b[], unsigned int dest[4])
910 #if 0 //defined(__GNUC__) && defined(__x86_64__)
911 unsigned __int128 ia = ((__int128)a[0] << 96) | ((__int128)a[1] << 64) | ((__int128)a[2] << 32) | (a[3]);
912 unsigned __int128 ib = ((__int128)b[0] << 96) | ((__int128)b[1] << 64) | ((__int128)b[2] << 32) | (b[3]);
913 unsigned __int128 id = ia * ib;
914 dest[0] = (id >> 96) & 0xffffffff;
915 dest[1] = (id >> 64) & 0xffffffff;
916 dest[2] = (id >> 32) & 0xffffffff;
917 dest[3] = (id) & 0xffffffff;
919 unsigned long long t[4];
921 // this multiply chain is relatively straightforward - a[] is repeatedly
922 // added with shifts based on b[] and the results stored into uint64,
923 // but due to C limitations (no access to carry flag) we have to resolve
924 // carries in a really lame way which wastes a fair number of ops
925 // (repeatedly iterating MSB to LSB, rather than LSB to MSB with carry),
926 // an alternative would be to use 16bit multiplies and resolve carries
927 // only at the end, but that would be twice as many multiplies...
929 // note: >> 32 is a function call in win32 MSVS2015 debug builds.
930 t[0] = (unsigned long long)a[0] * b[3];
931 t[1] = (unsigned long long)a[1] * b[3];
932 t[2] = (unsigned long long)a[2] * b[3];
933 t[3] = (unsigned long long)a[3] * b[3];
948 t[0] += (unsigned long long)a[1] * b[2];
949 t[1] += (unsigned long long)a[2] * b[2];
950 t[2] += (unsigned long long)a[3] * b[2];
958 t[0] += (unsigned long long)a[2] * b[1];
959 t[1] += (unsigned long long)a[3] * b[1];
962 t[0] += (unsigned long long)a[3] * b[0];
964 dest[0] = t[0] & 0xffffffff;
965 dest[1] = t[1] & 0xffffffff;
966 dest[2] = t[2] & 0xffffffff;
967 dest[3] = t[3] & 0xffffffff;
971 static void testmul128(unsigned int a0, unsigned int a1, unsigned int a2, unsigned int a3, unsigned int b0, unsigned int b1, unsigned int b2, unsigned int b3, unsigned int x0, unsigned int x1, unsigned int x2, unsigned int x3)
975 unsigned int expected[4];
976 unsigned int result[4];
989 mul128(a, b, result);
990 if (result[0] != expected[0]
991 || result[1] != expected[1]
992 || result[2] != expected[2]
993 || result[3] != expected[3])
994 Con_Printf("testmul128(\na = %08x %08x %08x %08x,\nb = %08x %08x %08x %08x,\nx = %08x %08x %08x %08x) instead computed\nc = %08x %08x %08x %08x\n", a[0], a[1], a[2], a[3], b[0], b[1], b[2], b[3], expected[0], expected[1], expected[2], expected[3], result[0], result[1], result[2], result[3]);
997 void Math_RandomSeed_UnitTests(void)
1000 0x00000000, 0x00000000, 0x00000000, 0x00000001,
1001 0x00000000, 0x00000000, 0x00000000, 0x00000001,
1002 0x00000000, 0x00000000, 0x00000000, 0x00000001);
1004 0x00000000, 0x00000000, 0x00000000, 0x00000001,
1005 0x00000000, 0x00000000, 0x00000001, 0x00000000,
1006 0x00000000, 0x00000000, 0x00000001, 0x00000000);
1008 0x00000000, 0x00000000, 0x00000001, 0x00000000,
1009 0x00000000, 0x00000000, 0x00000000, 0x00000001,
1010 0x00000000, 0x00000000, 0x00000001, 0x00000000);
1012 0x00000000, 0x00000000, 0x00000000, 0x00000001,
1013 0x00000001, 0x00000001, 0x00000001, 0x00000001,
1014 0x00000001, 0x00000001, 0x00000001, 0x00000001);
1016 0x00000000, 0x00000000, 0x00000000, 0x00000002,
1017 0xffffffff, 0xffffffff, 0xffffffff, 0xffffffff,
1018 0xffffffff, 0xffffffff, 0xffffffff, 0xfffffffe);
1020 0xffffffff, 0xffffffff, 0xffffffff, 0xffffffff,
1021 0x00000000, 0x00000000, 0x00000000, 0x00000002,
1022 0xffffffff, 0xffffffff, 0xffffffff, 0xfffffffe);
1024 0x00000000, 0x00000000, 0xffffffff, 0xffffffff,
1025 0x00000000, 0x00000000, 0x00000002, 0x00000000,
1026 0x00000001, 0xffffffff, 0xfffffffe, 0x00000000);
1028 0x00000000, 0x00000000, 0x00000002, 0x00000000,
1029 0x00000000, 0x00000000, 0xffffffff, 0xffffffff,
1030 0x00000001, 0xffffffff, 0xfffffffe, 0x00000000);
1033 void Math_RandomSeed_Reset(randomseed_t *r)
1041 void Math_RandomSeed_FromInts(randomseed_t *r, unsigned int s0, unsigned int s1, unsigned int s2, unsigned int s3)
1046 r->s[3] = s3 | 1; // the Lehmer RNG requires that the seed be odd
1049 unsigned long long Math_rand64(randomseed_t *r)
1052 mul128(r->s, mul_Lecuyer, o);
1057 return ((unsigned long long)o[3] << 32) + o[2];
1060 float Math_randomf(randomseed_t *r)
1062 unsigned long long n = Math_rand64(r);
1063 return n * (0.25f / 0x80000000 / 0x80000000);
1066 float Math_crandomf(randomseed_t *r)
1068 // do this with a signed number and double the result, so we make use of all parts of the cow
1069 long long n = (long long)Math_rand64(r);
1070 return n * (0.5f / 0x80000000 / 0x80000000);
1073 float Math_randomrangef(randomseed_t *r, float minf, float maxf)
1075 return Math_randomf(r) * (maxf - minf) + minf;
1078 int Math_randomrangei(randomseed_t *r, int mini, int maxi)
1080 unsigned long long n = Math_rand64(r);
1081 return (int)(((n >> 33) * (maxi - mini) + mini) >> 31);