Implement D-infinity flow routing

src/CMakeLists.txt

@@ -36,6 +36,8 @@ add_library(topotoolbox
   helpers/stat_func.h
   helpers/deque.c
   helpers/deque.h
+  helpers/edgeset.c
+  dinf.c
 )
 
 # Define the include directory

src/dinf.c

@@ -0,0 +1,165 @@
+#define TOPOTOOLBOX_BUILD
+
+#include <assert.h>
+#include <math.h>
+#include <stddef.h>
+#include <stdint.h>
+
+#include "topotoolbox.h"
+
+#define SQRT2 1.41421356237309504880f
+#define PI 3.14159265358979323846f
+
+static float replicate_boundaries(float *dem, ptrdiff_t i, ptrdiff_t j,
+                                  ptrdiff_t ei, ptrdiff_t ej,
+                                  ptrdiff_t dims[2]) {
+  float z = dem[j * dims[0] + i];
+  if (i + ei < 0 || i + ei >= dims[0]) {
+    if (j + ej < 0 || j + ej >= dims[1]) {
+      // Corners: z = dem[i, j]
+    } else {
+      // Top and bottom boundaries
+      z = dem[(j + ej) * dims[0] + i];
+    }
+  } else if (j + ej < 0 || j + ej >= dims[1]) {
+    // Left and right boundaries. The ei value is valid because we
+    // skipped the previous branch.
+    z = dem[j * dims[0] + i + ei];
+  } else {
+    // Interior
+    z = dem[(j + ej) * dims[0] + i + ei];
+  }
+  return z;
+}
+
+typedef struct {
+  int n;
+  float a;
+} facet;
+
+static facet dinf_flow_prop(float v1, float v2, int order) {
+  facet out = {-1, 0};
+  if (v1 == 0.0 && v2 == 0.0) {
+    return out;
+  }
+  // Compute the angle
+  //
+  // Despite appearances, this is correct. One must rotate the
+  // coordinate axes to line up the (v1, v2) vector with the (ei, ej)
+  // vectors. This depends on the memory order of the array. Then we
+  // work in units of PI/4 so that we only have to do this division
+  // once. We shift the angles from [-PI,PI] to [0,2PI] with the mod(x
+  // + 8, 8) (8 is 2PI in units of PI/4).
+  if (order) {
+    out.a = fmodf(atan2f(-v2, v1) / (PI / 4) + 8, 8);
+  } else {
+    out.a = fmodf(atan2f(-v1, v2) / (PI / 4) + 8, 8);
+  }
+
+  // The result is a number between 0 and 8. The integer part is the
+  // facet number. The fractional part is 1 - a, the correct flow
+  // proportion proportion.
+  out.n = (int)floorf(out.a);
+  out.a = (out.n + 1) - out.a;
+
+  return out;
+}
+
+TOPOTOOLBOX_API
+void flow_routing_dinf_directions(uint8_t *direction, float *prop, float *dem,
+                                  ptrdiff_t dims[2], int order) {
+  // Compute and store the direction bitmap and the flow proportions
+  // rather than the 2D flow vectors.
+  ptrdiff_t e[2][8] = {{0, -1, -1, -1, 0, 1, 1, 1},
+                       {1, 1, 0, -1, -1, -1, 0, 1}};
+
+  for (ptrdiff_t j = 0; j < dims[1]; j++) {
+    for (ptrdiff_t i = 0; i < dims[0]; i++) {
+      ptrdiff_t idx0 = j * dims[0] + i;
+      float z0 = dem[idx0];
+
+      direction[idx0] = 0;
+      prop[idx0] = 0.0;
+
+      if (isnan(z0)) {
+        prop[idx0] = NAN;
+        continue;
+      }
+
+      float maxslope = 0.0;
+      for (int n = 0; n < 8; n++) {
+        ptrdiff_t i1 = e[order & 1][(n + (order & 1)) & 7];
+        ptrdiff_t j1 = e[(order ^ 1) & 1][(n + (order & 1)) & 7];
+        float z1 = replicate_boundaries(dem, i, j, i1, j1, dims);
+
+        ptrdiff_t i2 = e[order & 1][(n + 1 - (order & 1)) & 7];
+        ptrdiff_t j2 = e[(order ^ 1) & 1][(n + 1 - (order & 1)) & 7];
+        float z2 = replicate_boundaries(dem, i, j, i2, j2, dims);
+
+        float s1 = j1 * (z2 - z0) - j2 * (z1 - z0);
+        float s2 = i1 * (z0 - z2) - i2 * (z0 - z1);
+
+        float mag = sqrtf(s1 * s1 + s2 * s2);
+        float d1 = sqrtf((float)(i1 * i1 + j1 * j1));
+        float d2 = sqrtf((float)(i2 * i2 + j2 * j2));
+
+        if (i1 * s2 - j1 * s1 < 0) {
+          mag = (s1 * i1 + s2 * j1) / d1;
+          s1 = mag * i1 / d1;
+          s2 = mag * j1 / d1;
+        } else if (j2 * s1 - i2 * s2 < 0) {
+          mag = (s1 * i2 + s2 * j2) / d2;
+          s1 = mag * i2 / d2;
+          s2 = mag * j2 / d2;
+        }
+
+        if (mag > maxslope) {
+          facet flowdir = dinf_flow_prop(s1, s2, order);
+
+          prop[idx0] = flowdir.a;
+
+          direction[idx0] = 0;
+          if (flowdir.a > 0) {
+            direction[idx0] |= 1 << (flowdir.n & 7);
+          }
+
+          if (flowdir.a < 1) {
+            direction[idx0] |= 1 << ((flowdir.n + 1) & 7);
+          }
+          maxslope = mag;
+        }
+      }
+    }
+  }
+}
+
+TOPOTOOLBOX_API
+void flow_routing_dinf_weights(float *weight, uint8_t *direction, float *prop,
+                               ptrdiff_t dims[2]) {
+  ptrdiff_t e = 0;
+  for (ptrdiff_t j = 0; j < dims[1]; j++) {
+    for (ptrdiff_t i = 0; i < dims[0]; i++) {
+      ptrdiff_t idx = j * dims[0] + i;
+      float a = prop[idx];
+
+      if (direction[idx] == 129) {
+        // Swap the order of the edges
+        if (a < 1) {
+          weight[e++] = 1 - a;
+        }
+
+        if (a > 0) {
+          weight[e++] = a;
+        }
+      } else if (direction[idx] > 0) {
+        if (a > 0) {
+          weight[e++] = a;
+        }
+
+        if (a < 1) {
+          weight[e++] = 1 - a;
+        }
+      }
+    }
+  }
+}

src/flow_routing.c

@@ -5,8 +5,11 @@
 #include <stddef.h>
 #include <stdint.h>
 
+#include "helpers/priority_queue.h"
 #include "topotoolbox.h"
 
+#define SQRT2 1.41421356237309504880f
+
 // Compute the steepest descent flow direction from the DEM with flow
 // routed over flat regions by the auxiliary topography in dist.
 uint8_t compute_flowdirection(ptrdiff_t i, ptrdiff_t j, float *dem, float *dist,
@@ -307,3 +310,206 @@ ptrdiff_t flow_routing_d8_edgelist(ptrdiff_t *source, ptrdiff_t *target,
   }
   return edge_count;
 }
+
+///////////////////////////////////
+// Topological sorting of edge sets
+
+static uint8_t next_neighbor(uint8_t d) {
+  uint8_t n = 0;
+  while (d >>= 1) {
+    n++;
+  }
+  return n;
+}
+
+TOPOTOOLBOX_API
+void flow_routing_tsort(ptrdiff_t *stream, ptrdiff_t *source, ptrdiff_t *target,
+                        float *sorted_weight, ptrdiff_t *stack,
+                        uint8_t *stackdir, uint8_t *direction, float *weight,
+                        ptrdiff_t *weightscan, uint8_t *visited,
+                        ptrdiff_t edge_count, ptrdiff_t dims[2], int order) {
+  ptrdiff_t e[2][8] = {{0, -1, -1, -1, 0, 1, 1, 1},
+                       {1, 1, 0, -1, -1, -1, 0, 1}};
+
+  for (ptrdiff_t j = 0; j < dims[1]; j++) {
+    for (ptrdiff_t i = 0; i < dims[0]; i++) {
+      visited[j * dims[0] + i] = 0;
+    }
+  }
+
+  ptrdiff_t stack_top = 0;
+  ptrdiff_t next_node = dims[0] * dims[1];
+
+  for (ptrdiff_t j = 0; j < dims[1]; j++) {
+    for (ptrdiff_t i = 0; i < dims[0]; i++) {
+      ptrdiff_t idx = j * dims[0] + i;
+
+      if (visited[idx]) continue;
+
+      visited[idx] = 1;
+
+      stack[stack_top] = idx;
+      stackdir[stack_top++] = direction[idx];
+
+      while (stack_top > 0) {
+        while (stackdir[stack_top - 1]) {
+          // We still have neighbors left to check
+          ptrdiff_t node = stack[stack_top - 1];
+          ptrdiff_t iu = node % dims[0];
+          ptrdiff_t ju = node / dims[0];
+
+          uint8_t n = next_neighbor(stackdir[stack_top - 1]);
+          stackdir[stack_top - 1] ^= (1 << n);
+
+          ptrdiff_t in = iu + e[order & 1][n];
+          ptrdiff_t jn = ju + e[(order ^ 1) & 1][n];
+
+          if (!visited[jn * dims[0] + in]) {
+            visited[jn * dims[0] + in] = 1;
+
+            // Push the neighbor and its outgoing edges on the stack
+            stack[stack_top] = jn * dims[0] + in;
+            stackdir[stack_top++] = direction[jn * dims[0] + in];
+          } else if (visited[jn * dims[0] + in] == 1) {
+            // This is a cycle. The neighbor is already on the stack
+            assert(0 && "flow_routing_tsort detected a cycle. This is a bug.");
+          }
+        }
+        // Pop the stack.
+        ptrdiff_t node = stack[--stack_top];
+        ptrdiff_t iu = node % dims[0];
+        ptrdiff_t ju = node / dims[0];
+
+        visited[node] = 2;
+
+        if (next_node <= 0) {
+          assert(0 && "flow_routing_tsort ran out of nodes. This is a bug.");
+        }
+        stream[--next_node] = node;
+
+        ptrdiff_t offset = weightscan[node];
+
+        // Add its edges to source and target
+        for (int n = 0; n < 8; n++) {
+          if (direction[node] & (1 << n)) {
+            ptrdiff_t in = iu + e[order & 1][n];
+            ptrdiff_t jn = ju + e[(order ^ 1) & 1][n];
+
+            if (edge_count <= 0) {
+              assert(0 &&
+                     "flow_routing_tsort ran out of nodes. This is a bug.");
+            }
+            source[--edge_count] = node;
+            target[edge_count] = jn * dims[0] + in;
+
+            sorted_weight[edge_count] = weight[offset++];
+          }
+        }
+      }
+    }
+  }
+}
+
+static uint8_t get_direction(ptrdiff_t i, ptrdiff_t j, int order) {
+  uint8_t e[2][9] = {{8, 16, 32, 4, 0, 64, 2, 1, 128},
+                     {8, 4, 2, 16, 0, 1, 32, 64, 128}};
+
+  return e[order][(j + 1) * 3 + (i + 1)];
+}
+
+TOPOTOOLBOX_API
+void resolve_flats_shortest_path(uint8_t *direction, uint8_t *resolved,
+                                 float *path_distance, ptrdiff_t *heap,
+                                 ptrdiff_t *back, float *dem, ptrdiff_t dims[2],
+                                 int order) {
+  ptrdiff_t e[2][8] = {{0, -1, -1, -1, 0, 1, 1, 1},
+                       {1, 1, 0, -1, -1, -1, 0, 1}};
+  float chamfer[8] = {1.0f, SQRT2, 1.0f, SQRT2, 1.0f, SQRT2, 1.0f, SQRT2};
+
+  PriorityQueue pq = pq_create(dims[0] * dims[1], heap, back, path_distance, 0);
+
+  // Initialize distances based on the resolved array.
+  for (ptrdiff_t j = 0; j < dims[1]; j++) {
+    for (ptrdiff_t i = 0; i < dims[0]; i++) {
+      ptrdiff_t idx = j * dims[0] + i;
+      if (resolved[idx] == 0) {
+        pq_insert(&pq, idx, INFINITY);
+      } else {
+        path_distance[idx] = -INFINITY;
+      }
+    }
+  }
+  // Identify presills, flats that border a resolved pixel with the same
+  // elevation
+  for (ptrdiff_t j = 0; j < dims[1]; j++) {
+    for (ptrdiff_t i = 0; i < dims[0]; i++) {
+      ptrdiff_t idx = j * dims[0] + i;
+      if (resolved[idx]) {
+        continue;
+      }
+      float z = dem[idx];
+      for (ptrdiff_t n = 0; n < 8; n++) {
+        ptrdiff_t in = i + e[order & 1][n];
+        ptrdiff_t jn = j + e[(order ^ 1) & 1][n];
+
+        if (in < 0 || in >= dims[0] || jn < 0 || jn >= dims[1]) {
+          // Pixel is on the boundary, call it a presill
+          pq_decrease_key(&pq, idx, 0.0);
+          break;
+        }
+
+        ptrdiff_t idxn = jn * dims[0] + in;
+
+        if (resolved[idxn] && z == dem[idxn]) {
+          // These are the presills, so their distance is 0
+          pq_decrease_key(&pq, idx, 0.0);
+
+          // Flow the presill towards the neighbor
+          direction[idx] = get_direction(in - i, jn - j, order);
+          break;
+        }
+      }
+    }
+  }
+
+  while (!pq_isempty(&pq)) {
+    ptrdiff_t idx = pq_deletemin(&pq);
+    float d = pq_get_priority(&pq, idx);
+
+    ptrdiff_t j = idx / dims[0];
+    ptrdiff_t i = idx % dims[0];
+
+    float z = dem[idx];
+
+    for (ptrdiff_t n = 0; n < 8; n++) {
+      ptrdiff_t in = i + e[order & 1][n];
+      ptrdiff_t jn = j + e[(order ^ 1) & 1][n];
+
+      // Skip any neighboring pixels that are outside the domain, are
+      // resolved or are not equal in elevation. The weird elevation
+      // comparison is in case the neighboring pixel is a NaN, in
+      // which case !(neighbor_z <= z) is true and we skip the pixel.
+      if (in < 0 || in >= dims[0] || jn < 0 || jn >= dims[1] ||
+          resolved[jn * dims[0] + in] || !(dem[jn * dims[0] + in] == z)) {
+        continue;
+      }
+      float proposal = d + chamfer[n];
+
+      if (proposal < pq_get_priority(&pq, jn * dims[0] + in)) {
+        pq_decrease_key(&pq, jn * dims[0] + in, proposal);
+
+        // Save the flow direction to the current pixel as the neighbor's flow
+        // direction
+        direction[jn * dims[0] + in] = get_direction(i - in, j - jn, order);
+      }
+    }
+  }
+}
+
+// Weights are 1 for shortest path flat resolution
+TOPOTOOLBOX_API
+void resolve_flats_shortest_path_weights(float *weight, ptrdiff_t count) {
+  for (ptrdiff_t e = 0; e < count; e++) {
+    weight[e] = 1.0;
+  }
+}