Sergey-project/vec.cpp at master · Solnec/Sergey-project · GitHub

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
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
#include "pch.h"
#include "vec.h"


float DotProduct(const vec3_t vec1, const vec3_t vec2)
{
	return vec1[0] * vec2[0] + vec1[1] * vec2[1] + vec1[2] * vec2[2];
}

//-------------------------------------------------------------//
void VecAdd(vec3_t in, vec3_t added)
{
	in[0] += added[0];
	in[1] += added[1];
	in[2] += added[2];
}

//-------------------------------------------------------------//
void VecSub(vec3_t in, vec3_t added)
{
	in[0] -= added[0];
	in[1] -= added[1];
	in[2] -= added[2];
}

//-------------------------------------------------------------//
void VectorSubtract(const vec3_t vec1, const vec3_t vec2, vec3_t vecOut)
{
	vecOut[0] = vec1[0] - vec2[0];
	vecOut[1] = vec1[1] - vec2[1];
	vecOut[2] = vec1[2] - vec2[2];
}

//-------------------------------------------------------------//
void VecDivByNum(vec3_t in, float numDiv)
{
	in[0] = in[0] / numDiv;
	in[1] = in[1] / numDiv;
	in[2] = in[2] / numDiv;
}

//-------------------------------------------------------------//
float VecDistance(vec3_t &start, vec3_t &end)
{
	float crd0 = (end[0] - start[0]) * (end[0] - start[0]);
	float crd1 = (end[1] - start[1]) * (end[1] - start[1]);
	float crd2 = (end[2] - start[2]) * (end[2] - start[2]);

	float distance = sqrt(crd0 + crd1 + crd2);

	return distance;
}

//-------------------------------------------------------------//
float VecDistance2(vec3_t& start, vec3_t& end)
{
	float crd0 = (end[0] - start[0]) * (end[0] - start[0]);
	float crd1 = (end[1] - start[1]) * (end[1] - start[1]);

	float distance = sqrt(crd0 + crd1);

	return distance;
}

//-------------------------------------------------------------//
void VecNull(vec3_t in)
{
	in[0] = 0;
	in[1] = 0;
	in[2] = 0;
}

//-------------------------------------------------------------//
float VecMult(vec3_t out, vec3_t in)
{
	float result = 0;
	result = out[0] * in[0] + out[1] * in[1] + out[2] * in[2];
	return result;
}

//-------------------------------------------------------------//
void VecCopy(vec3_t out, vec3_t in)
{
	out[0] = in[0];
	out[1] = in[1];
	out[2] = in[2];
}

//-------------------------------------------------------------//
void VecFill(vec3_t in, float x, float y, float z)
{
	in[0] = x;
	in[1] = y;
	in[2] = z;

}

//-------------------------------------------------------------//
void AngleVectors(float yaw, float pitch, float roll, vec3_t forward, vec3_t right, vec3_t up)
{
	float		angle;
	static float		sr, sp, sy, cr, cp, cy;
	// static to help MS compiler fp bugs

	float angleDegrees = (float)(PI * 2 / 360);

	angle = yaw * angleDegrees;
	sy = sinf(angle);
	cy = cosf(angle);
	angle = pitch * angleDegrees;
	sp = sinf(angle);
	cp = cosf(angle);
	angle = roll * angleDegrees;
	sr = sinf(angle);
	cr = cosf(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;
	}
}