Vec2d impls

1 files changed, 85 insertions, 0 deletions

diff --git a/src/vec2.c b/src/vec2.c
new file mode 100644
index 0000000..1e09c8e
--- /dev/null
+++ b/src/vec2.c
@@ -0,0 +1,85 @@
+#include "vec2.h"
+#include "common.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);
+}