diff --git a/dijkstra3d.hpp b/dijkstra3d.hpp index d7c8447..a88534a 100644 --- a/dijkstra3d.hpp +++ b/dijkstra3d.hpp @@ -14,10 +14,11 @@ */ #include -#include #include #include #include +#include +#include #include #include @@ -77,11 +78,11 @@ inline std::vector query_shortest_path(const OUT* parents, const OUT target return path; } -inline void compute_neighborhood_helper( +void compute_neighborhood_helper_6( int *neighborhood, const int x, const int y, const int z, - const uint64_t sx, const uint64_t sy, const uint64_t sz, - const int connectivity = 26) { + const uint64_t sx, const uint64_t sy, const uint64_t sz +) { const int sxy = sx * sy; @@ -92,38 +93,55 @@ inline void compute_neighborhood_helper( neighborhood[3] = static_cast(sx) * (y < static_cast(sy) - 1); // +y neighborhood[4] = -sxy * static_cast(z > 0); // -z neighborhood[5] = sxy * (z < static_cast(sz) - 1); // +z +} + +void compute_neighborhood_helper_18( + int *neighborhood, + const int x, const int y, const int z, + const uint64_t sx, const uint64_t sy, const uint64_t sz +) { + // 6-hood + compute_neighborhood_helper_6(neighborhood, x,y,z, sx,sy,sz); // 18-hood // xy diagonals - neighborhood[6] = (connectivity > 6) * (neighborhood[0] + neighborhood[2]) * (neighborhood[0] && neighborhood[2]); // up-left - neighborhood[7] = (connectivity > 6) * (neighborhood[0] + neighborhood[3]) * (neighborhood[0] && neighborhood[3]); // up-right - neighborhood[8] = (connectivity > 6) * (neighborhood[1] + neighborhood[2]) * (neighborhood[1] && neighborhood[2]); // down-left - neighborhood[9] = (connectivity > 6) * (neighborhood[1] + neighborhood[3]) * (neighborhood[1] && neighborhood[3]); // down-right + neighborhood[6] = (neighborhood[0] + neighborhood[2]) * (neighborhood[0] && neighborhood[2]); // up-left + neighborhood[7] = (neighborhood[0] + neighborhood[3]) * (neighborhood[0] && neighborhood[3]); // up-right + neighborhood[8] = (neighborhood[1] + neighborhood[2]) * (neighborhood[1] && neighborhood[2]); // down-left + neighborhood[9] = (neighborhood[1] + neighborhood[3]) * (neighborhood[1] && neighborhood[3]); // down-right // yz diagonals - neighborhood[10] = (connectivity > 6) * (neighborhood[2] + neighborhood[4]) * (neighborhood[2] && neighborhood[4]); // up-left - neighborhood[11] = (connectivity > 6) * (neighborhood[2] + neighborhood[5]) * (neighborhood[2] && neighborhood[5]); // up-right - neighborhood[12] = (connectivity > 6) * (neighborhood[3] + neighborhood[4]) * (neighborhood[3] && neighborhood[4]); // down-left - neighborhood[13] = (connectivity > 6) * (neighborhood[3] + neighborhood[5]) * (neighborhood[3] && neighborhood[5]); // down-right + neighborhood[10] = (neighborhood[2] + neighborhood[4]) * (neighborhood[2] && neighborhood[4]); // up-left + neighborhood[11] = (neighborhood[2] + neighborhood[5]) * (neighborhood[2] && neighborhood[5]); // up-right + neighborhood[12] = (neighborhood[3] + neighborhood[4]) * (neighborhood[3] && neighborhood[4]); // down-left + neighborhood[13] = (neighborhood[3] + neighborhood[5]) * (neighborhood[3] && neighborhood[5]); // down-right // xz diagonals - neighborhood[14] = (connectivity > 6) * (neighborhood[0] + neighborhood[4]) * (neighborhood[0] && neighborhood[4]); // up-left - neighborhood[15] = (connectivity > 6) * (neighborhood[0] + neighborhood[5]) * (neighborhood[0] && neighborhood[5]); // up-right - neighborhood[16] = (connectivity > 6) * (neighborhood[1] + neighborhood[4]) * (neighborhood[1] && neighborhood[4]); // down-left - neighborhood[17] = (connectivity > 6) * (neighborhood[1] + neighborhood[5]) * (neighborhood[1] && neighborhood[5]); // down-right + neighborhood[14] = (neighborhood[0] + neighborhood[4]) * (neighborhood[0] && neighborhood[4]); // up-left + neighborhood[15] = (neighborhood[0] + neighborhood[5]) * (neighborhood[0] && neighborhood[5]); // up-right + neighborhood[16] = (neighborhood[1] + neighborhood[4]) * (neighborhood[1] && neighborhood[4]); // down-left + neighborhood[17] = (neighborhood[1] + neighborhood[5]) * (neighborhood[1] && neighborhood[5]); // down-right +} +void compute_neighborhood_helper_26( + int *neighborhood, + const int x, const int y, const int z, + const uint64_t sx, const uint64_t sy, const uint64_t sz +) { + compute_neighborhood_helper_18(neighborhood, x,y,z, sx,sy,sz); + // 26-hood // Now the eight corners of the cube - neighborhood[18] = (connectivity > 18) * (neighborhood[0] + neighborhood[2] + neighborhood[4]) * (neighborhood[2] && neighborhood[4]); - neighborhood[19] = (connectivity > 18) * (neighborhood[1] + neighborhood[2] + neighborhood[4]) * (neighborhood[2] && neighborhood[4]); - neighborhood[20] = (connectivity > 18) * (neighborhood[0] + neighborhood[3] + neighborhood[4]) * (neighborhood[3] && neighborhood[4]); - neighborhood[21] = (connectivity > 18) * (neighborhood[0] + neighborhood[2] + neighborhood[5]) * (neighborhood[2] && neighborhood[5]); - neighborhood[22] = (connectivity > 18) * (neighborhood[1] + neighborhood[3] + neighborhood[4]) * (neighborhood[3] && neighborhood[4]); - neighborhood[23] = (connectivity > 18) * (neighborhood[1] + neighborhood[2] + neighborhood[5]) * (neighborhood[2] && neighborhood[5]); - neighborhood[24] = (connectivity > 18) * (neighborhood[0] + neighborhood[3] + neighborhood[5]) * (neighborhood[3] && neighborhood[5]); - neighborhood[25] = (connectivity > 18) * (neighborhood[1] + neighborhood[3] + neighborhood[5]) * (neighborhood[3] && neighborhood[5]); + neighborhood[18] = (neighborhood[0] + neighborhood[2] + neighborhood[4]) * (neighborhood[2] && neighborhood[4]); + neighborhood[19] = (neighborhood[1] + neighborhood[2] + neighborhood[4]) * (neighborhood[2] && neighborhood[4]); + neighborhood[20] = (neighborhood[0] + neighborhood[3] + neighborhood[4]) * (neighborhood[3] && neighborhood[4]); + neighborhood[21] = (neighborhood[0] + neighborhood[2] + neighborhood[5]) * (neighborhood[2] && neighborhood[5]); + neighborhood[22] = (neighborhood[1] + neighborhood[3] + neighborhood[4]) * (neighborhood[3] && neighborhood[4]); + neighborhood[23] = (neighborhood[1] + neighborhood[2] + neighborhood[5]) * (neighborhood[2] && neighborhood[5]); + neighborhood[24] = (neighborhood[0] + neighborhood[3] + neighborhood[5]) * (neighborhood[3] && neighborhood[5]); + neighborhood[25] = (neighborhood[1] + neighborhood[3] + neighborhood[5]) * (neighborhood[3] && neighborhood[5]); } inline void compute_neighborhood( @@ -132,7 +150,15 @@ inline void compute_neighborhood( const uint64_t sx, const uint64_t sy, const uint64_t sz, const int connectivity = 26, const uint32_t* voxel_connectivity_graph = NULL) { - compute_neighborhood_helper(neighborhood, x, y, z, sx, sy, sz, connectivity); + if (connectivity == 26) { + compute_neighborhood_helper_26(neighborhood, x, y, z, sx, sy, sz); + } + else if (connectivity == 18) { + compute_neighborhood_helper_18(neighborhood, x, y, z, sx, sy, sz); + } + else { + compute_neighborhood_helper_6(neighborhood, x, y, z, sx, sy, sz); + } if (voxel_connectivity_graph == NULL) { return; @@ -275,12 +301,12 @@ std::vector dijkstra3d( 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); + std::unique_ptr dist(new float[voxels]()); + std::unique_ptr parents(new OUT[voxels]()); + fill(dist.get(), +INFINITY, voxels); dist[source] = -0; - int neighborhood[NHOOD_SIZE]; + int neighborhood[NHOOD_SIZE] = {}; std::priority_queue, std::vector>, HeapNodeCompare> queue; queue.emplace(0.0, source); @@ -349,15 +375,14 @@ std::vector dijkstra3d( } OUTSIDE: - delete []dist; + dist.reset(); std::vector path; // if voxel graph supplied, it's possible // to never reach target. if (target_reached) { - path = query_shortest_path(parents, target); + path = query_shortest_path(parents.get(), target); } - delete [] parents; return path; } @@ -388,12 +413,12 @@ std::vector dijkstra3d_anisotropy( 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); + std::unique_ptr dist(new float[voxels]()); + std::unique_ptr parents(new OUT[voxels]()); + fill(dist.get(), +INFINITY, voxels); dist[source] = -0; - int neighborhood[NHOOD_SIZE]; + int neighborhood[NHOOD_SIZE] = {}; float neighbor_multiplier[NHOOD_SIZE] = { wx, wx, wy, wy, wz, wz, // axial directions (6) @@ -474,15 +499,14 @@ std::vector dijkstra3d_anisotropy( } OUTSIDE: - delete []dist; + dist.reset(); std::vector path; // if voxel graph supplied, it's possible // to never reach target. if (target_reached) { - path = query_shortest_path(parents, target); + path = query_shortest_path(parents.get(), target); } - delete [] parents; return path; } @@ -518,12 +542,12 @@ std::vector binary_dijkstra3d( 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); + std::unique_ptr dist(new float[voxels]()); + std::unique_ptr parents(new OUT[voxels]()); + fill(dist.get(), +INFINITY, voxels); dist[source] = 0.0; - int neighborhood[NHOOD_SIZE]; + int neighborhood[NHOOD_SIZE] = {}; float neighbor_multiplier[NHOOD_SIZE] = { wx, wx, wy, wy, wz, wz, // axial directions (6) @@ -613,15 +637,14 @@ std::vector binary_dijkstra3d( } OUTSIDE: - delete []dist; + dist.reset(); std::vector path; // it's possible to never reach target. if (target_reached) { - path = query_shortest_path(parents, target); + path = query_shortest_path(parents.get(), target); } - delete [] parents; - + return path; } @@ -677,12 +700,12 @@ std::vector value_target_dijkstra3d( 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); + std::unique_ptr dist(new float[voxels]()); + std::unique_ptr parents(new OUT[voxels]()); + fill(dist.get(), +INFINITY, voxels); dist[source] = -0; - int neighborhood[NHOOD_SIZE]; + int neighborhood[NHOOD_SIZE] = {}; std::priority_queue, std::vector>, HeapNodeCompare> queue; queue.emplace(0.0, source); @@ -751,15 +774,14 @@ std::vector value_target_dijkstra3d( } OUTSIDE: - delete []dist; + dist.reset(); std::vector path; // if voxel graph supplied, it's possible // to never reach target. if (target_loc < voxels) { // voxels is an impossible target - path = query_shortest_path(parents, target_loc); + path = query_shortest_path(parents.get(), target_loc); } - delete [] parents; return path; } @@ -769,14 +791,14 @@ template inline void bidirectional_core( const size_t loc, T* field, float *dist, OUT* parents, - int *neighborhood, + int *neighborhood, int connectivity, std::priority_queue, std::vector>, HeapNodeCompare> &queue ) { float delta; size_t neighboridx; - for (int i = 0; i < NHOOD_SIZE; i++) { + for (int i = 0; i < connectivity; i++) { if (neighborhood[i] == 0) { continue; } @@ -833,7 +855,7 @@ std::vector bidirectional_dijkstra3d( dist_fwd[source] = 0; dist_rev[target] = 0; - int neighborhood[NHOOD_SIZE]; + int neighborhood[NHOOD_SIZE] = {}; std::priority_queue, std::vector>, HeapNodeCompare> queue_fwd; queue_fwd.emplace(dist_fwd[source], source); @@ -915,13 +937,13 @@ std::vector bidirectional_dijkstra3d( if (forward) { bidirectional_core( loc, field, dist_fwd, parents_fwd, - neighborhood, queue_fwd + neighborhood, connectivity, queue_fwd ); } else { bidirectional_core( loc, field, dist_rev, parents_rev, - neighborhood, queue_rev + neighborhood, connectivity, queue_rev ); } } @@ -984,9 +1006,9 @@ std::vector compass_guided_dijkstra3d( 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); + std::unique_ptr dist(new float[voxels]()); + std::unique_ptr parents(new OUT[voxels]()); + fill(dist.get(), +INFINITY, voxels); dist[source] = 0; // Normalizer value must be positive. @@ -1000,7 +1022,7 @@ std::vector compass_guided_dijkstra3d( } } - int neighborhood[NHOOD_SIZE]; + int neighborhood[NHOOD_SIZE] = {}; std::priority_queue, std::vector>, HeapNodeCompare> queue; queue.emplace(0.0, source); @@ -1101,13 +1123,12 @@ std::vector compass_guided_dijkstra3d( } OUTSIDE: - delete []dist; + dist.reset(); std::vector path; if (target_reached) { - path = query_shortest_path(parents, target); + path = query_shortest_path(parents.get(), target); } - delete [] parents; return path; } @@ -1139,9 +1160,9 @@ std::vector compass_guided_dijkstra3d_anisotropy_line_preference( 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); + std::unique_ptr dist(new float[voxels]()); + std::unique_ptr parents(new OUT[voxels]()); + fill(dist.get(), +INFINITY, voxels); dist[source] = 0; // Normalizer value must be positive. @@ -1155,7 +1176,7 @@ std::vector compass_guided_dijkstra3d_anisotropy_line_preference( } } - int neighborhood[NHOOD_SIZE]; + int neighborhood[NHOOD_SIZE] = {}; float neighbor_multiplier[NHOOD_SIZE] = { wx, wx, wy, wy, wz, wz, // axial directions (6) @@ -1277,13 +1298,12 @@ std::vector compass_guided_dijkstra3d_anisotropy_line_preference( } OUTSIDE: - delete []dist; + dist.reset(); std::vector path; if (target_reached) { - path = query_shortest_path(parents, target); + path = query_shortest_path(parents.get(), target); } - delete [] parents; return path; } @@ -1331,16 +1351,16 @@ OUT* parental_field3d( 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](); + std::unique_ptr dist(new float[voxels]()); if (parents == NULL) { parents = new OUT[voxels](); } - fill(dist, +INFINITY, voxels); + fill(dist.get(), +INFINITY, voxels); dist[source] = -0; - int neighborhood[NHOOD_SIZE]; + int neighborhood[NHOOD_SIZE] = {}; std::priority_queue, std::vector>, HeapNodeCompare> queue; queue.emplace(0.0, source); @@ -1396,8 +1416,6 @@ OUT* parental_field3d( dist[loc] *= -1; } - delete [] dist; - return parents; } @@ -1425,7 +1443,7 @@ float* distance_field3d( float *dist = new float[voxels](); fill(dist, +INFINITY, voxels); - int neighborhood[NHOOD_SIZE]; + int neighborhood[NHOOD_SIZE] = {}; std::priority_queue, std::vector>, HeapNodeCompare> queue; @@ -1684,7 +1702,7 @@ float* euclidean_distance_field3d( fill(dist, +INFINITY, voxels); - int neighborhood[NHOOD_SIZE]; + int neighborhood[NHOOD_SIZE] = {}; float neighbor_multiplier[NHOOD_SIZE] = { wx, wx, wy, wy, wz, wz, // axial directions (6) @@ -1817,7 +1835,8 @@ float* euclidean_distance_field3d( ); } - +#undef sq +#undef NHOOD_SIZE #undef DIJKSTRA_3D_PREFETCH_26WAY }; // namespace dijkstra3d diff --git a/dijkstra3d.pyx b/dijkstra3d.pyx index 2d49e4a..5f17e2e 100644 --- a/dijkstra3d.pyx +++ b/dijkstra3d.pyx @@ -618,7 +618,8 @@ def euclidean_distance_field( Parameters: data: Input weights in a 2D or 3D numpy array. - source: (x,y,z) coordinate of starting voxel + source: (x,y,z) coordinate or list of coordinates + of starting voxels anisotropy: (wx,wy,wz) weights for each axial direction. free_space_radius: (float, optional) if you know that the region surrounding the source is free space, we can use