Vec2d impls

1 files changed, 118 insertions, 0 deletions

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;
+}
+