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
|
#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;
}
}
vec_t VectorNormalize(vec3_t v)
{
float length = VectorLength(v);
if (length)
VectorScale(v, 1 / length, v);
return length;
}
vec_t VectorNormalize2(vec3_t v, vec3_t out)
{
float length, ilength;
length = v[0] * v[0] + v[1] * v[1] + v[2] * v[2];
length = sqrt(length); // FIXME
if (length) {
ilength = 1 / length;
out[0] = v[0] * ilength;
out[1] = v[1] * ilength;
out[2] = v[2] * ilength;
}
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;
}
|
