feat: hillclimbing distance metric

dijkstra3d.hpp

@@ -344,6 +344,138 @@ std::vector<OUT> dijkstra3d(
   return path;
 }
 
+// The original dijkstra function used its distance
+// metric as the value of the destination field.
+// However, it was pointed out that a better metric
+// for many use cases might be the hillclimbing distance 
+// between the two with adjustable parameters alpha and beta:
+// sqrt(alpha^2 ||Xs - Xf||^2 + beta^2 (Is - If)^2)
+// only supports 26 connectivity for now
+template <typename T, typename OUT = uint32_t>
+std::vector<OUT> distance_metric_dijkstra3d(
+    T* field, 
+    const size_t sx, const size_t sy, const size_t sz, 
+    const size_t source, const size_t target,
+    const float alpha = 1,
+    const float beta = 1
+  ) {
+
+  if (source == target) {
+    return std::vector<OUT>{ static_cast<OUT>(source) };
+  }
+
+  const size_t voxels = sx * sy * sz;
+  const size_t sxy = sx * sy;
+
+  const libdivide::divider<size_t> fast_sx(sx); 
+  const libdivide::divider<size_t> fast_sxy(sxy); 
+
+  const bool power_of_two = !((sx & (sx - 1)) || (sy & (sy - 1))); 
+  const int xshift = std::log2(sx); // must use log2 here, not lg/lg2 to avoid fp errors
+  const int yshift = std::log2(sy);
+
+  float *dist = new float[voxels]();
+  OUT *parents = new OUT[voxels]();
+  fill(dist, +INFINITY, voxels);
+  dist[source] = -0;
+
+  int neighborhood[NHOOD_SIZE];
+
+  std::priority_queue<HeapNode<OUT>, std::vector<HeapNode<OUT>>, HeapNodeCompare<OUT>> queue;
+  queue.emplace(0.0, source);
+
+  size_t loc;
+  float delta;
+  size_t neighboridx;
+
+  int x, y, z;
+  int nx, ny, nz;
+  bool target_reached = false;
+
+  while (!queue.empty()) {
+    loc = queue.top().value;
+    queue.pop();
+
+    if (std::signbit(dist[loc])) {
+      continue;
+    }
+
+    // As early as possible, start fetching the
+    // data from RAM b/c the annotated lines below
+    // have 30-50% cache miss.
+    DIJKSTRA_3D_PREFETCH_26WAY(field, loc)
+    DIJKSTRA_3D_PREFETCH_26WAY(dist, loc)
+
+    if (power_of_two) {
+      z = loc >> (xshift + yshift);
+      y = (loc - (z << (xshift + yshift))) >> xshift;
+      x = loc - ((y + (z << yshift)) << xshift);
+    }
+    else {
+      z = loc / fast_sxy;
+      y = (loc - (z * sxy)) / fast_sx;
+      x = loc - sx * (y + z * sy);
+    }
+
+    for (int dz = -1; dz <= 1; dz++) {
+      nz = z + dz;
+      if (nz < 0 || nz >= sz) {
+        continue;
+      }
+      for (int dy = -1; dy <= 1; dy++) {
+        ny = y + dy;
+        if (ny < 0 || ny >= sy) {
+          continue;
+        }
+        for (int dx = -1; dx <= 1; dx++) {
+          nx = x + dx;
+          if (nx < 0 || nx >= sx) {
+            continue;
+          }
+
+          neighboridx = loc + (dx + sx * (dy + sy * dz));
+          delta = std::sqrt(
+              sq(alpha) * (sq(x - nx) + sq(y - ny) + sq(z - nz)) 
+            + sq(beta) * sq(field[loc] - field[neighboridx])
+          );
+
+          // Visited nodes are negative and thus the current node
+          // will always be less than as field is filled with non-negative
+          // integers.
+          if (dist[loc] + delta < dist[neighboridx]) { // high cache miss
+            dist[neighboridx] = dist[loc] + delta;
+            parents[neighboridx] = loc + 1; // +1 to avoid 0 ambiguity
+
+            // Dijkstra, Edgar. "Go To Statement Considered Harmful".
+            // Communications of the ACM. Vol. 11. No. 3 March 1968. pp. 147-148
+            if (neighboridx == target) {
+              target_reached = true;
+              goto OUTSIDE;
+            }
+
+            queue.emplace(dist[neighboridx], neighboridx);
+          }
+        }
+
+        dist[loc] *= -1;
+      }
+    }
+  }
+
+  OUTSIDE:
+  delete []dist;
+
+  std::vector<OUT> path;
+  // if voxel graph supplied, it's possible 
+  // to never reach target.
+  if (target_reached) { 
+    path = query_shortest_path<OUT>(parents, target);
+  }
+  delete [] parents;
+
+  return path;
+}
+
 // helper function for bidirectional_dijkstra
 template <typename T, typename OUT>
 inline void bidirectional_core(