things!

2026-09-11 02:21:21 +03:00
parent 2e890fb5b7
commit adfae10938
17 changed files with 949 additions and 119 deletions
@@ -0,0 +1,80 @@
+/*
+    Fixed point implementation of 3 dimensional vectors.
+    SSE2 extension availability is assumed, making it more viable.
+    For mul and div we don't use com_fixed functions to make fewer bitshifts.
+*/
+
+#ifndef COM_VEC_H
+#define COM_VEC_H
+
+#include "def.h"
+#include "fixed.h"
+#include <stdint.h>
+
+typedef union {
+  com_fixed_t com_def_alignedas(16) a[3];
+  com_def_alignedas(16) struct {
+    com_fixed_t x;
+    com_fixed_t y;
+    com_fixed_t z;
+  } s;
+} com_vec_t;
+
+static inline com_vec_t com_vec_add(com_vec_t a, com_vec_t b) {
+  return (com_vec_t){.s = {.x = com_fixed_add(a.s.x, b.s.x),
+                           .y = com_fixed_add(a.s.y, b.s.y),
+                           .z = com_fixed_add(a.s.z, b.s.z)}};
+}
+
+static inline com_vec_t com_vec_sub(com_vec_t a, com_vec_t b) {
+  return (com_vec_t){.s = {.x = com_fixed_sub(a.s.x, b.s.x),
+                           .y = com_fixed_sub(a.s.y, b.s.y),
+                           .z = com_fixed_sub(a.s.z, b.s.z)}};
+}
+
+static inline com_vec_t com_vec_mul(com_vec_t a, com_vec_t b) {
+  return (com_vec_t){.s = {.x = com_fixed_mul(a.s.x, b.s.x),
+                           .y = com_fixed_mul(a.s.y, b.s.y),
+                           .z = com_fixed_mul(a.s.z, b.s.z)}};
+}
+
+/* Note: this does not clamp for over/underflow cases */
+static inline com_vec_t com_vec_div(com_vec_t a, com_vec_t b) {
+  return (com_vec_t){.s = {.x = com_fixed_div(a.s.x, b.s.x),
+                           .y = com_fixed_div(a.s.y, b.s.y),
+                           .z = com_fixed_div(a.s.z, b.s.z)}};
+}
+
+/* Scale vector by a fixed point number */
+static inline com_vec_t com_vec_scl(com_vec_t a, com_fixed_t b) {
+  return (com_vec_t){.s = {.x = com_fixed_mul(a.s.x, b),
+                           .y = com_fixed_mul(a.s.y, b),
+                           .z = com_fixed_mul(a.s.z, b)}};
+}
+
+/* Shows how much given vectors are correlated in direction to each other */
+/* Resulted range depends on input, it's in -1 to 1 for normalized
+ * input and otherwise is -ab to +ab */
+static inline com_fixed_t com_vec_dot(com_vec_t a, com_vec_t b) {
+  return (((int64_t)a.s.x * b.s.x) + ((int64_t)a.s.y * b.s.y) +
+          ((int64_t)a.s.z * b.s.z)) >>
+         COM_FIXED_FRACBITS;
+}
+
+/* Cross product produces a perpendicular for normalized vectors, or 0 for
+ * parallel vectors */
+static inline com_vec_t com_vec_crs(com_vec_t a, com_vec_t b) {
+  int64_t const cx = ((int64_t)a.s.y * b.s.z) - ((int64_t)a.s.z - b.s.y);
+  int64_t const cy = ((int64_t)a.s.z * b.s.x) - ((int64_t)a.s.x - b.s.z);
+  int64_t const cz = ((int64_t)a.s.x * b.s.y) - ((int64_t)a.s.y - b.s.x);
+  return (com_vec_t){.s = {.x = (com_fixed_t)(cx >> COM_FIXED_FRACBITS),
+                           .y = (com_fixed_t)(cy >> COM_FIXED_FRACBITS),
+                           .z = (com_fixed_t)(cz >> COM_FIXED_FRACBITS)}};
+}
+
+static inline com_vec_t com_vec_nrm(com_vec_t a) {
+  com_fixed_t const n = com_fixed_sqrt(com_vec_dot(a, a));
+  return com_vec_scl(a, com_fixed_div(COM_FIXED_FRACUNIT, n));
+}
+
+#endif