SourcePP
Several modern C++20 libraries for sanely parsing Valve's formats.
Loading...
Searching...
No Matches
Math.h
Go to the documentation of this file.
1#pragma once
2
3#include <bit>
4#include <concepts>
5#include <cmath>
6#include <cstdint>
7#include <numbers>
8#include <type_traits>
9
10#include <half.hpp>
11
12// Numeric types are intentionally outside the sourcepp namespace
13using std::int8_t;
14using std::int16_t;
15using std::int32_t;
16using std::int64_t;
17using std::uint8_t;
18using std::uint16_t;
19using std::uint32_t;
20using std::uint64_t;
21using half_float::half;
22
23namespace sourcepp::math {
24
25template<std::floating_point F>
26constexpr F pi = std::numbers::pi_v<F>;
27constexpr auto pi_f32 = pi<float>;
28constexpr auto pi_f64 = pi<double>;
29
30template<typename T>
31concept Arithmetic = std::is_arithmetic_v<T> || std::same_as<T, half>;
32
33template<Arithmetic T>
34[[nodiscard]] constexpr T remap(T value, T l1, T h1, T l2, T h2) {
35 return l2 + (value - l1) * (h2 - l2) / (h1 - l1);
36}
37
38template<Arithmetic T>
39[[nodiscard]] constexpr T remap(T value, T h1, T h2) {
40 return value * h2 / h1;
41}
42
43[[nodiscard]] constexpr bool isPowerOf2(std::unsigned_integral auto n) {
44 return n && !(n & (n - 1));
45}
46
47template<std::unsigned_integral T>
48[[nodiscard]] constexpr T nearestPowerOf2(T n) {
49 if (isPowerOf2(n)) {
50 return n;
51 }
52 auto bigger = std::bit_ceil(n);
53 auto smaller = std::bit_floor(n);
54 return (n - smaller) < (bigger - n) ? smaller : bigger;
55}
56
57[[nodiscard]] constexpr uint32_t log2ceil(uint32_t value) {
58 return ((std::bit_cast<uint32_t>(static_cast<float>(value)) >> 23) & 0xff) - 127;
59}
60
61[[nodiscard]] constexpr uint16_t paddingForAlignment(uint16_t alignment, uint64_t n) {
62 if (const auto rest = n % alignment; rest > 0) {
63 return alignment - rest;
64 }
65 return 0;
66}
67
68template<uint8_t S, Arithmetic P>
69struct Vec {
70 static_assert(S >= 2, "Vectors must have at least two values!");
71
72 P values[S];
73
74 // By defining these constructors, the type becomes nontrivial...
75#if 1
76 constexpr Vec() = default;
77
78 template<std::convertible_to<P>... Vals>
79 requires (sizeof...(Vals) == S)
80 constexpr Vec(Vals... vals) // NOLINT(*-explicit-constructor)
81 : values{static_cast<P>(vals)...} {}
82#endif
83
84 using value_type = P;
85
86 [[nodiscard]] constexpr const P* data() const {
87 return this->values;
88 }
89
90 [[nodiscard]] constexpr P* data() {
91 return this->values;
92 }
93
94 [[nodiscard]] constexpr uint8_t size() const {
95 return S;
96 }
97
98 [[nodiscard]] constexpr P& operator[](uint8_t index) {
99 if (index < S) {
100 return this->values[index];
101 }
102 return this->operator[](index % S);
103 }
104
105 [[nodiscard]] constexpr P operator[](uint8_t index) const {
106 if (index < S) {
107 return this->values[index];
108 }
109 return this->operator[](index % S);
110 }
111
112 [[nodiscard]] constexpr Vec operator+() const {
113 return *this;
114 }
115
116 template<uint8_t SO, Arithmetic PO>
117 [[nodiscard]] constexpr Vec operator+(const Vec<SO, PO>& other) const {
118 auto out = *this;
119 for (uint8_t i = 0; i < (S > SO ? SO : S); i++) {
120 out[i] += static_cast<P>(other[i]);
121 }
122 return out;
123 }
124
125 template<uint8_t SO, Arithmetic PO>
126 constexpr void operator+=(const Vec<SO, PO>& other) {
127 for (uint8_t i = 0; i < (S > SO ? SO : S); i++) {
128 (*this)[i] += static_cast<P>(other[i]);
129 }
130 }
131
132 [[nodiscard]] constexpr Vec operator-() const {
133 auto out = *this;
134 for (uint8_t i = 0; i < S; i++) {
135 out[i] *= -1;
136 }
137 return out;
138 }
139
140 template<uint8_t SO, Arithmetic PO>
141 [[nodiscard]] constexpr Vec operator-(const Vec<SO, PO>& other) const {
142 auto out = *this;
143 for (uint8_t i = 0; i < (S > SO ? SO : S); i++) {
144 out[i] -= static_cast<P>(other[i]);
145 }
146 return out;
147 }
148
149 template<uint8_t SO, Arithmetic PO>
150 constexpr void operator-=(const Vec<SO, PO>& other) {
151 for (uint8_t i = 0; i < (S > SO ? SO : S); i++) {
152 (*this)[i] -= static_cast<P>(other[i]);
153 }
154 }
155
156 [[nodiscard]] constexpr Vec operator*(Arithmetic auto scalar) const {
157 auto out = *this;
158 for (uint8_t i = 0; i < S; i++) {
159 out[i] *= static_cast<P>(scalar);
160 }
161 return out;
162 }
163
164 constexpr void operator*=(Arithmetic auto scalar) {
165 for (uint8_t i = 0; i < S; i++) {
166 (*this)[i] *= static_cast<P>(scalar);
167 }
168 }
169
170 [[nodiscard]] constexpr Vec operator/(Arithmetic auto scalar) const {
171 auto out = *this;
172 for (uint8_t i = 0; i < S; i++) {
173 out[i] /= static_cast<P>(scalar);
174 }
175 return out;
176 }
177
178 constexpr void operator/=(Arithmetic auto scalar) {
179 for (uint8_t i = 0; i < S; i++) {
180 (*this)[i] /= static_cast<P>(scalar);
181 }
182 }
183
184 [[nodiscard]] constexpr Vec operator%(Arithmetic auto scalar) const {
185 auto out = *this;
186 for (uint8_t i = 0; i < S; i++) {
187 out[i] %= static_cast<P>(scalar);
188 }
189 return out;
190 }
191
192 constexpr void operator%=(Arithmetic auto scalar) {
193 for (uint8_t i = 0; i < S; i++) {
194 (*this)[i] %= static_cast<P>(scalar);
195 }
196 }
197
198 template<uint8_t SO, Arithmetic PO>
199 [[nodiscard]] constexpr bool operator==(const Vec<SO, PO>& other) const {
200 if constexpr (S != SO) {
201 return false;
202 } else {
203 for (uint8_t i = 0; i < S; i++) {
204 if ((*this)[i] != static_cast<P>(other[i])) {
205 return false;
206 }
207 }
208 return true;
209 }
210 }
211
212 template<uint8_t SO, Arithmetic PO = P>
213 [[nodiscard]] constexpr Vec<SO, PO> to() const {
214 Vec<SO, PO> out{};
215 for (uint8_t i = 0; i < (S > SO ? SO : S); i++) {
216 out[i] = static_cast<PO>((*this)[i]);
217 }
218 return out;
219 }
220
221 template<uint8_t SO, Arithmetic PO>
222 [[nodiscard]] constexpr Vec mul(const Vec<SO, PO>& other) const {
223 auto out = *this;
224 for (uint8_t i = 0; i < (S > SO ? SO : S); i++) {
225 out[i] *= static_cast<P>(other[i]);
226 }
227 return out;
228 }
229
230 template<uint8_t SO, Arithmetic PO>
231 [[nodiscard]] constexpr Vec div(const Vec<SO, PO>& other) const {
232 auto out = *this;
233 for (uint8_t i = 0; i < (S > SO ? SO : S); i++) {
234 out[i] /= static_cast<P>(other[i]);
235 }
236 return out;
237 }
238
239 template<uint8_t SO, Arithmetic PO>
240 [[nodiscard]] constexpr Vec mod(const Vec<SO, PO>& other) const {
241 auto out = *this;
242 for (uint8_t i = 0; i < (S > SO ? SO : S); i++) {
243 if constexpr ((std::floating_point<P> && std::floating_point<PO>) || std::floating_point<P>) {
244 out[i] = std::fmod(out[i], static_cast<P>(other[i]));
245 } else {
246 out[i] %= static_cast<P>(other[i]);
247 }
248 }
249 return out;
250 }
251
252 [[nodiscard]] constexpr float magf() const {
253 float out = 0.0;
254 for (uint8_t i = 0; i < S; i++) {
255 out += std::pow((*this)[i], 2);
256 }
257 return std::sqrt(out);
258 }
259
260 [[nodiscard]] constexpr double mag() const {
261 double out = 0.0;
262 for (uint8_t i = 0; i < S; i++) {
263 out += std::pow((*this)[i], 2);
264 }
265 return std::sqrt(out);
266 }
267
268 [[nodiscard]] constexpr P sum() const {
269 P out{};
270 for (uint8_t i = 0; i < S; i++) {
271 out += (*this)[i];
272 }
273 return out;
274 }
275
276 template<Arithmetic PO>
277 [[nodiscard]] constexpr Vec scale(const Vec<S, PO>& other) const {
278 Vec out;
279 for (uint8_t i = 0; i < S; i++) {
280 out[i] = (*this)[i] * static_cast<P>(other[i]);
281 }
282 return out;
283 }
284
285 template<Arithmetic PO>
286 [[nodiscard]] constexpr P dot(const Vec<S, PO>& other) const {
287 return this->scale(other).sum();
288 }
289
290 [[nodiscard]] constexpr Vec abs() const {
291 auto out = *this;
292 for (uint8_t i = 0; i < S; i++) {
293 out[i] = std::abs(out[i]);
294 }
295 return out;
296 }
297
298 [[nodiscard]] static constexpr Vec zero() {
299 return {};
300 }
301
302 [[nodiscard]] constexpr bool isZero() const {
303 return *this == zero();
304 }
305};
306static_assert(std::is_trivially_copyable_v<Vec<2, float>>);
307
308#define SOURCEPP_VEC_DEFINE(S) \
309 template<Arithmetic P> \
310 using Vec##S = Vec<S, P>; \
311 using Vec##S##i8 = Vec##S<int8_t>; \
312 using Vec##S##i16 = Vec##S<int16_t>; \
313 using Vec##S##i32 = Vec##S<int32_t>; \
314 using Vec##S##i64 = Vec##S<int64_t>; \
315 using Vec##S##i = Vec##S##i32; \
316 using Vec##S##ui8 = Vec##S<uint8_t>; \
317 using Vec##S##ui16 = Vec##S<uint16_t>; \
318 using Vec##S##ui32 = Vec##S<uint32_t>; \
319 using Vec##S##ui64 = Vec##S<uint64_t>; \
320 using Vec##S##ui = Vec##S##ui32; \
321 using Vec##S##f16 = Vec##S<half>; \
322 using Vec##S##f32 = Vec##S<float>; \
323 using Vec##S##f64 = Vec##S<double>; \
324 using Vec##S##f = Vec##S##f32
325
329
330#undef SOURCEPP_VEC_DEFINE
331
332using EulerAngles = Vec3f;
333
334using Quat = Vec4f;
335
338 uint16_t x : 16;
339 uint16_t y : 16;
340 uint16_t z : 15;
341 uint16_t wn : 1;
342
343 [[nodiscard]] Quat decompress() const {
344 // Convert from 16-bit (or 15-bit) integers to floating point values in the range [-1, 1]
345 const float fx = (static_cast<float>(this->x) / 32767.5f) - 1.f; // x / ((2^16 - 1) / 2) - 1
346 const float fy = (static_cast<float>(this->y) / 32767.5f) - 1.f; // y / ((2^16 - 1) / 2) - 1
347 const float fz = (static_cast<float>(this->z) / 16383.5f) - 1.f; // z / ((2^15 - 1) / 2) - 1
348
349 // Recalculate w from the constraint that x^2 + y^2 + z^2 + w^2 = 1
350 float fw = std::sqrt(1.f - fx * fx - fy * fy - fz * fz);
351
352 // Adjust w based on the stored sign bit
353 if (this->wn) {
354 fw = -fw;
355 }
356
357 return {fx, fy, fz, fw};
358 }
359};
360static_assert(std::is_trivially_copyable_v<QuatCompressed48>);
361
364 uint32_t x : 21;
365 uint32_t y : 21;
366 uint32_t z : 21;
367 uint32_t wn : 1;
368
369 [[nodiscard]] Quat decompress() const {
370 // Convert from 21-bit integers to floating point values in the range [-1, 1]
371 const double fx = (static_cast<double>(this->x) / 1048575.5) - 1.0f; // x / ((2^21 - 1) / 2) - 1
372 const double fy = (static_cast<double>(this->y) / 1048575.5) - 1.0f; // y / ((2^21 - 1) / 2) - 1
373 const double fz = (static_cast<double>(this->z) / 1048575.5) - 1.0f; // z / ((2^21 - 1) / 2) - 1
374
375 // Recalculate w from the constraint that x^2 + y^2 + z^2 + w^2 = 1
376 double fw = std::sqrt(1.0 - fx * fx - fy * fy - fz * fz);
377
378 // Adjust w based on the stored sign bit
379 if (this->wn) {
380 fw = -fw;
381 }
382
383 return {static_cast<float>(fx), static_cast<float>(fy), static_cast<float>(fz), static_cast<float>(fw)};
384 }
385};
386static_assert(std::is_trivially_copyable_v<QuatCompressed64>);
387
388template<uint8_t M, uint8_t N, Arithmetic P>
389class Mat {
390 static_assert(M >= 2, "Matrices must have at least two rows!");
391 static_assert(N >= 2, "Matrices must have at least two columns!");
392
393public:
394 [[nodiscard]] P* operator[](uint8_t i) { return this->data[i]; }
395
396 [[nodiscard]] const P* operator[](uint8_t i) const { return this->data[i]; }
397
398private:
399 P data[M][N];
400};
401static_assert(std::is_trivially_copyable_v<Mat<2, 2, float>>);
402
403#define SOURCEPP_MAT_DEFINE(M, N) \
404 template<Arithmetic P> \
405 using Mat##M##x##N = Mat<M, N, P>; \
406 using Mat##M##x##N##i8 = Mat##M##x##N<int8_t>; \
407 using Mat##M##x##N##i16 = Mat##M##x##N<int16_t>; \
408 using Mat##M##x##N##i32 = Mat##M##x##N<int32_t>; \
409 using Mat##M##x##N##i64 = Mat##M##x##N<int64_t>; \
410 using Mat##M##x##N##i = Mat##M##x##N##i32; \
411 using Mat##M##x##N##ui8 = Mat##M##x##N<uint8_t>; \
412 using Mat##M##x##N##ui16 = Mat##M##x##N<uint16_t>; \
413 using Mat##M##x##N##ui32 = Mat##M##x##N<uint32_t>; \
414 using Mat##M##x##N##ui64 = Mat##M##x##N<uint64_t>; \
415 using Mat##M##x##N##ui = Mat##M##x##N##ui32; \
416 using Mat##M##x##N##f16 = Mat##M##x##N<half>; \
417 using Mat##M##x##N##f32 = Mat##M##x##N<float>; \
418 using Mat##M##x##N##f64 = Mat##M##x##N<double>; \
419 using Mat##M##x##N##f = Mat##M##x##N##f32
420
430
431#undef SOURCEPP_MAT_DEFINE
432
433} // namespace sourcepp::math
#define SOURCEPP_VEC_DEFINE(S)
Definition: Math.h:308
#define SOURCEPP_MAT_DEFINE(M, N)
Definition: Math.h:403
P * operator[](uint8_t i)
Definition: Math.h:394
const P * operator[](uint8_t i) const
Definition: Math.h:396
constexpr T nearestPowerOf2(T n)
Definition: Math.h:48
constexpr bool isPowerOf2(std::unsigned_integral auto n)
Definition: Math.h:43
constexpr auto pi_f32
Definition: Math.h:27
constexpr auto pi_f64
Definition: Math.h:28
Vec4f Quat
Definition: Math.h:334
Vec3f EulerAngles
Definition: Math.h:332
constexpr uint16_t paddingForAlignment(uint16_t alignment, uint64_t n)
Definition: Math.h:61
constexpr T remap(T value, T l1, T h1, T l2, T h2)
Definition: Math.h:34
constexpr uint32_t log2ceil(uint32_t value)
Definition: Math.h:57
constexpr F pi
Definition: Math.h:26
Lower precision Quat compressed to 6 bytes.
Definition: Math.h:337
Lower precision Quat compressed to 8 bytes.
Definition: Math.h:363
constexpr float magf() const
Definition: Math.h:252
constexpr Vec< SO, PO > to() const
Definition: Math.h:213
constexpr Vec operator-(const Vec< SO, PO > &other) const
Definition: Math.h:141
constexpr Vec operator%(Arithmetic auto scalar) const
Definition: Math.h:184
constexpr void operator/=(Arithmetic auto scalar)
Definition: Math.h:178
constexpr Vec operator+(const Vec< SO, PO > &other) const
Definition: Math.h:117
constexpr P operator[](uint8_t index) const
Definition: Math.h:105
constexpr const P * data() const
Definition: Math.h:86
constexpr bool isZero() const
Definition: Math.h:302
constexpr void operator-=(const Vec< SO, PO > &other)
Definition: Math.h:150
constexpr P & operator[](uint8_t index)
Definition: Math.h:98
constexpr P dot(const Vec< S, PO > &other) const
Definition: Math.h:286
constexpr P * data()
Definition: Math.h:90
constexpr Vec scale(const Vec< S, PO > &other) const
Definition: Math.h:277
constexpr void operator*=(Arithmetic auto scalar)
Definition: Math.h:164
constexpr void operator%=(Arithmetic auto scalar)
Definition: Math.h:192
constexpr Vec operator-() const
Definition: Math.h:132
constexpr P sum() const
Definition: Math.h:268
constexpr bool operator==(const Vec< SO, PO > &other) const
Definition: Math.h:199
static constexpr Vec zero()
Definition: Math.h:298
constexpr Vec mod(const Vec< SO, PO > &other) const
Definition: Math.h:240
constexpr Vec operator/(Arithmetic auto scalar) const
Definition: Math.h:170
constexpr Vec abs() const
Definition: Math.h:290
constexpr double mag() const
Definition: Math.h:260
constexpr Vec div(const Vec< SO, PO > &other) const
Definition: Math.h:231
constexpr Vec()=default
constexpr Vec operator*(Arithmetic auto scalar) const
Definition: Math.h:156
constexpr uint8_t size() const
Definition: Math.h:94
constexpr void operator+=(const Vec< SO, PO > &other)
Definition: Math.h:126
constexpr Vec(Vals... vals)
Definition: Math.h:80
constexpr Vec operator+() const
Definition: Math.h:112
constexpr Vec mul(const Vec< SO, PO > &other) const
Definition: Math.h:222