implement pathfinder

liblocky/CMakeLists.txt

@@ -12,6 +12,7 @@ target_sources(liblocky
   PRIVATE
     block.c
     extractor.c
-    sensor.c
     image.c
+    pathfinder.c
+    sensor.c
 )

liblocky/liblocky.h

@@ -10,6 +10,7 @@
 
 /* ---- Sensor ----
  * calculates correl from samples sequencially */
+
 typedef struct blky_sensor_t {
   double sum, avg, var;
   uint64_t time;
@@ -43,6 +44,7 @@ blky_block_estimate(
 
 /* ---- Extractor ----
  * extracts all features from a sequence of multiple frames */
+
 typedef struct blky_extractor_t {
   // must be filled before init()
   uint32_t block_num_x;
@@ -95,9 +97,75 @@ blky_extractor_feed(
     const uint8_t* img, uint32_t w, uint32_t h, const double verts[8]);
 
 
+/* ---- pathfinder ---- */
+typedef struct blky_pathfinder_step_t {
+  struct blky_pathfinder_step_t* next;
+  uint32_t indices[1];
+} blky_pathfinder_step_t;
+
+typedef struct blky_pathfinder_t {
+  // must be filled before init()
+  uint32_t block_num;
+  uint32_t step_branch;
+  uint64_t seed;
+  uint8_t  hopping_algo;
+# define BLKY_PATHFINDER_HOPPING_ALGO_XORSHFIT 0
+
+  // internal state
+  blky_pathfinder_step_t* step_first;
+  blky_pathfinder_step_t* step_last;
+
+  uint64_t steps;
+  uint64_t step_bytes;
+
+  double* probs;
+  double* probs_prev;
+} blky_pathfinder_t;
+
+void
+blky_pathfinder_init(
+    blky_pathfinder_t* pf);
+
+void
+blky_pathfinder_deinit(
+    blky_pathfinder_t* pf);
+
+void
+blky_pathfinder_feed(
+    blky_pathfinder_t* pf,
+    const double*      probs);
+
+
 /* ---- Image utility ---- */
 void blky_image_convert_to_normalized_coord(
     const double verts[8], double* x, double* y);
 
 uint64_t blky_image_offset(
     uint32_t w, uint32_t h, const double verts[8], double x, double y);
+
+
+/* ---- numeric utility ---- */
+static inline uint64_t blky_numeric_xorrshift64_rev(uint64_t x, uint8_t n) {
+  for (int16_t i = 64-n; i >= 0; --i) {
+    x ^= (1 << i) & (x >> n);
+  }
+  return x;
+}
+static inline uint64_t blky_numeric_xorlshift64_rev(uint64_t x, uint8_t n) {
+  for (uint8_t i = n; i < 64; ++i) {
+    x ^= (1 << i) & (x << n);
+  }
+  return x;
+}
+static inline uint64_t blky_numeric_xorshift64(uint64_t x) {
+  x ^= x << 13;
+  x ^= x >> 7;
+  x ^= x << 17;
+  return x;
+}
+static inline uint64_t blky_numeric_xorshift64_rev(uint64_t x) {
+  x = blky_numeric_xorlshift64_rev(x, 13);
+  x = blky_numeric_xorrshift64_rev(x, 7);
+  x = blky_numeric_xorlshift64_rev(x, 17);
+  return x;
+}

liblocky/pathfinder.c

@@ -0,0 +1,68 @@
+#include "liblocky.h"
+
+#include <assert.h>
+#include <stdlib.h>
+
+
+void blky_pathfinder_init(blky_pathfinder_t* pf) {
+  assert(pf->block_num   > 0);
+  assert(pf->step_branch > 0);
+  assert(pf->seed        > 0);
+
+  assert(pf->block_num >= pf->step_branch);
+  assert(pf->hopping_algo == BLKY_PATHFINDER_HOPPING_ALGO_XORSHFIT);
+
+  pf->step_first = NULL;
+  pf->step_last  = NULL;
+
+  pf->step_bytes =
+    sizeof(blky_pathfinder_step_t) +
+    sizeof(uint32_t)*(pf->block_num-1);
+
+  pf->probs = calloc(sizeof(*pf->probs), pf->block_num);
+  assert(pf->probs);
+
+  pf->probs_prev = calloc(sizeof(*pf->probs_prev), pf->block_num);
+  assert(pf->probs_prev);
+}
+
+void blky_pathfinder_deinit(blky_pathfinder_t* pf) {
+  free(pf->probs);
+  free(pf->probs_prev);
+}
+
+void blky_pathfinder_feed(blky_pathfinder_t* pf, const double* probs) {
+  blky_pathfinder_step_t* step = NULL;
+  if (++pf->steps > 1) {
+    step = calloc(pf->step_bytes, 1);
+    assert(step);
+  }
+
+  for (uint32_t bi = 0; bi < pf->block_num; ++bi) {
+    const double   prob  = probs[bi];
+    const uint64_t pbase = blky_numeric_xorshift64_rev(bi);
+    for (uint32_t si = 0; si < pf->step_branch; ++si) {
+      const uint64_t prev_seed = pbase - si*pf->block_num/pf->step_branch;
+      const uint32_t prev_bi   = (uint32_t) (prev_seed % pf->block_num);
+      const double   prev_prob = pf->probs_prev[prev_bi];
+      const double   sum       = prev_prob + prob;
+      if (pf->probs[bi] < sum) {
+        pf->probs[bi] = sum;
+        if (step) step->indices[prev_bi] = bi;
+      }
+    }
+  }
+
+  if (step) {
+    if (pf->step_last) {
+      pf->step_last->next = step;
+      pf->step_last       = step;
+    } else {
+      pf->step_first = pf->step_last = step;
+    }
+  }
+
+  double* temp = pf->probs;
+  pf->probs      = pf->probs_prev;
+  pf->probs_prev = temp;
+}