1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
|
#include <math.h>
#include "shared/shared.h"
void AngleVectors(vec3_t angles, vec3_t forward, vec3_t right, vec3_t up)
{
float angle;
float sr, sp, sy, cr, cp, cy;
angle = DEG2RAD(angles[YAW]);
sy = sin(angle);
cy = cos(angle);
angle = DEG2RAD(angles[PITCH]);
sp = sin(angle);
cp = cos(angle);
angle = DEG2RAD(angles[ROLL]);
sr = sin(angle);
cr = cos(angle);
if (forward) {
forward[0] = cp * cy;
forward[1] = cp * sy;
forward[2] = -sp;
}
if (right) {
right[0] = (-1 * sr * sp * cy + -1 * cr * -sy);
right[1] = (-1 * sr * sp * sy + -1 * cr * cy);
right[2] = -1 * sr * cp;
}
if (up) {
up[0] = (cr * sp * cy + -sr * -sy);
up[1] = (cr * sp * sy + -sr * cy);
up[2] = cr * cp;
}
}
static float inv_sqrt(float n)
{
float x2 = n * 0.5F;
union {
float f;
int i;
} y;
/* magic number for doubles: 0x5fe6eb50c7b537a9 */
y.f = n;
y.i = 0x5f375a86 - (y.i >> 1);
y.f = y.f * (1.5F - (x2 * y.f * y.f));
y.f = y.f * (1.5F - (x2 * y.f * y.f));
return y.f;
}
void VectorNormalize(vec3_t v)
{
VectorScale(v, inv_sqrt(VectorLengthSquared(v)), v);
}
void VectorNormalize2(vec3_t v, vec3_t out)
{
VectorScale(v, inv_sqrt(VectorLengthSquared(v)), out);
}
vec_t VectorNormalizeLen(vec3_t v)
{
float length = VectorLength(v);
if (length)
VectorScale(v, 1 / length, v);
return length;
}
mat4_t mul_mat4(const mat4_t *restrict a, const mat4_t *restrict b)
{
mat4_t p = { 0 };
for (int i = 0; i < 4; i++)
for (int j = 0; j < 4; j++)
for (int k = 0; k < 4; k++)
p.i[i][j] += a->i[k][j] * b->i[i][k];
return p;
}
|
