Vec2d impls

3 files changed, 208 insertions, 0 deletions

diff --git a/src/common.h b/src/common.h
index 1add4db..ad7a288 100644
--- a/src/common.h
+++ b/src/common.h
@@ -2,11 +2,16 @@
 
 #include <stdint.h>
 #include <stddef.h>
+#include <math.h> // IWYU pragma: keep
 
 #define KiB(n) ((u64)n<<10)
 #define MiB(n) ((u64)n<<20)
 #define GiB(n) ((u64)n<<30)
 
+#define PI 3.14159265358979323846264338327950
+#define F64_EPSILON = 1e-9
+
+#define F64_EQ(x, y, eps) (fabs((x) - (y)) <= (eps))
 #define MAX(n, m) ((n > m) ? (n) : (m))
 #define MIN(n, m) ((n < m) ? (n) : (m))
 #define ALIGN_UP_POW2(n, m) (((u64)(n) + (u64)(m) - 1) & (~((u64)(m) - 1)))
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);
+}
diff --git a/src/vec2.h b/src/vec2.h
new file mode 100644
index 0000000..fa106dc
--- /dev/null
+++ b/src/vec2.h
@@ -0,0 +1,118 @@
+#pragma once
+
+#include "common.h"
+
+#include <assert.h>
+#include <math.h>
+
+typedef union {
+    struct { f64 x, y; };
+    f64 d[2];
+} vec2d_t;
+
+typedef struct {
+    f64 *xs;
+    f64 *ys;
+    u64 size;
+} vec2d_soa_t;
+
+#define VEC2D_ZERO (vec2d_t){.x=0.0, .y=0.0}
+#define VEC2D_UNIF(n) (vec2d_t){.x=(n), .y=(n)}
+#define VEC2D_FROM(ix, iy) (vec2d_t){.x=(ix), .y=(iy)}
+
+   void   vec2d_soa_add_n(vec2d_soa_t       *restrict out ,
+                          vec2d_soa_t const *restrict a   ,
+                          vec2d_soa_t const *restrict b   );
+
+   void   vec2d_soa_sub_n(vec2d_soa_t       *restrict out ,
+                          vec2d_soa_t const *restrict lhs ,
+                          vec2d_soa_t const *restrict rhs );
+
+   void vec2d_soa_scale_n(vec2d_soa_t       *restrict out ,
+                          vec2d_soa_t const *restrict in  ,
+                                  f64       scalar        );
+
+   void  vec2d_soa_norm_n(vec2d_soa_t       *restrict out ,
+                          vec2d_soa_t const *restrict in  );
+
+vec2d_t vec2d_soa_average(vec2d_soa_t const *vs);
+
+// -------------------------------------------
+// vec2d_t inline operations
+// -------------------------------------------
+
+static inline
+b8 vec2d_epsilon_eq(vec2d_t v1, vec2d_t v2, f64 epsilon) {
+    return F64_EQ(v1.x, v2.x, epsilon) && F64_EQ(v1.y, v2.y, epsilon);
+}
+
+static inline
+vec2d_t vec2d_add(vec2d_t v1, vec2d_t v2) {
+    return (vec2d_t){
+        .x = v1.x + v2.x,
+        .y = v1.y + v2.y,
+    };
+}
+
+static inline
+vec2d_t vec2d_sub(vec2d_t lhs, vec2d_t rhs) {
+    return (vec2d_t){
+        .x = lhs.x - rhs.x,
+        .y = lhs.y - rhs.y,
+    };
+}
+
+static inline
+vec2d_t vec2d_scale(vec2d_t v, f64 scalar) {
+    return (vec2d_t){
+        .x = v.x * scalar,
+        .y = v.y * scalar,
+    };
+}
+
+static inline
+f64 vec2d_length_squared(vec2d_t v) {
+    return v.x * v.x + v.y * v.y;
+}
+
+static inline
+f64 vec2d_length(vec2d_t v) {
+    return sqrt(vec2d_length_squared(v));
+}
+
+static inline
+vec2d_t vec2d_normalize(vec2d_t v) {
+    f64 lsquared = vec2d_length_squared(v);
+
+    if (F64_EQ(lsquared, 0.0, 1e-9)) {
+        return VEC2D_ZERO;
+    }
+
+    return vec2d_scale(v, 1.0 / sqrt(lsquared));
+}
+
+static inline
+f64 vec2d_dot(vec2d_t v1, vec2d_t v2) {
+    return (v1.x * v2.x) + (v1.y * v2.y);
+}
+
+// -------------------------------------------
+// vec2d_soa_t inline operations
+// -------------------------------------------
+
+static inline
+vec2d_t vec2d_soa_get(const vec2d_soa_t *vs, u64 i) {
+    assert(i < vs->size && "Input index >= vs.size");
+    return (vec2d_t){
+        .x = vs->xs[i],
+        .y = vs->ys[i],
+    };
+}
+
+static inline
+void vec2d_soa_set(vec2d_soa_t *vs, u64 i, vec2d_t p) {
+    assert(i < vs->size && "Input index >= vs.size");
+    vs->xs[i] = p.x;
+    vs->ys[i] = p.y;
+}
+