blob: 9e779b1b41b2b75c2b8c1886c9557154c50c589a (
plain)
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
|
#include "common.h"
#include "vec2.h"
#include <assert.h>
#include <math.h>
void vec2d_soa_add_n(vec2d_soa_t *restrict out, const vec2d_soa_t *restrict a, const vec2d_soa_t *restrict b) {
assert(out->size > 0 && "Out should have size > 0" );
assert(out->size == a->size && "Input should have same size as output");
assert(out->size == b->size && "Input should have same size as output");
f64 *restrict out_x = out->xs;
f64 *restrict out_y = out->ys;
f64 const *restrict a_x = a->xs;
f64 const *restrict a_y = a->ys;
f64 const *restrict b_x = b->xs;
f64 const *restrict b_y = b->ys;
for (u64 i = 0; i < out->size; i++) out_x[i] = a_x[i] + b_x[i];
for (u64 i = 0; i < out->size; i++) out_y[i] = a_y[i] + b_y[i];
}
void vec2d_soa_sub_n(vec2d_soa_t *restrict out, const vec2d_soa_t *restrict lhs, const vec2d_soa_t *restrict rhs) {
assert(out->size > 0 && "Out should have size > 0" );
assert(out->size == lhs->size && "Input should have same size as output");
assert(out->size == rhs->size && "Input should have same size as output");
f64 *restrict out_x = out->xs;
f64 *restrict out_y = out->ys;
f64 const *restrict lhs_x = lhs->xs;
f64 const *restrict lhs_y = lhs->ys;
f64 const *restrict rhs_x = rhs->xs;
f64 const *restrict rhs_y = rhs->ys;
for (u64 i = 0; i < out->size; i++) out_x[i] = lhs_x[i] - rhs_x[i];
for (u64 i = 0; i < out->size; i++) out_y[i] = lhs_y[i] - rhs_y[i];
}
void vec2d_soa_scale_n(vec2d_soa_t *restrict out, const vec2d_soa_t *restrict in, f64 scalar) {
assert(out->size > 0 && "Out should have size > 0" );
assert(out->size == in->size && "Input should have same size as output");
f64 *restrict out_x = out->xs;
f64 *restrict out_y = out->ys;
f64 const *restrict in_x = in->xs;
f64 const *restrict in_y = in->ys;
for (u64 i = 0; i < out->size; i++) out_x[i] = in_x[i] * scalar;
for (u64 i = 0; i < out->size; i++) out_y[i] = in_y[i] * scalar;
}
void vec2d_soa_norm_n(vec2d_soa_t *restrict out, vec2d_soa_t const *restrict in) {
assert(out->size > 0 && "Out should have size > 0" );
assert(out->size == in->size && "Input should have same size as output");
f64 *restrict out_x = out->xs;
f64 *restrict out_y = out->ys;
f64 const *restrict in_x = in->xs;
f64 const *restrict in_y = in->ys;
for (u64 i = 0; i < out->size; i++) {
f64 lsquared = (in_x[i] * in_x[i]) +
(in_y[i] * in_y[i]);
if (F64_EQ(lsquared, 0.0, 1e-9)) { out_x[i] = 0.0;
out_y[i] = 0.0;
continue; }
f64 mag = sqrt(lsquared);
out_x[i] = in_x[i] / mag;
out_y[i] = in_y[i] / mag;
}
}
vec2d_t vec2d_soa_average(const vec2d_soa_t *vs) {
assert(vs->size != 0 && "Input should have size > 0");
f64 sum_x = 0.0, sum_y = 0.0;
for (u64 i = 0; i < vs->size; i++) {
sum_x += vs->xs[i];
sum_y += vs->ys[i];
}
return vec2d_scale(VEC2D_FROM(sum_x, sum_y), 1.0 / vs->size);
}
|
