diff --git a/dijkstra3d.hpp b/dijkstra3d.hpp index efa1286..3e3aa8c 100644 --- a/dijkstra3d.hpp +++ b/dijkstra3d.hpp @@ -344,6 +344,138 @@ std::vector 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 +std::vector 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{ static_cast(source) }; + } + + const size_t voxels = sx * sy * sz; + const size_t sxy = sx * sy; + + const libdivide::divider fast_sx(sx); + const libdivide::divider 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, std::vector>, HeapNodeCompare> 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 path; + // if voxel graph supplied, it's possible + // to never reach target. + if (target_reached) { + path = query_shortest_path(parents, target); + } + delete [] parents; + + return path; +} + // helper function for bidirectional_dijkstra template inline void bidirectional_core(