35 const unsigned& level,
36 const std::uint8_t& mask,
43 float closest_distance = std::numeric_limits<float>::max();
44 unsigned closest_index = 0;
45 uint3 closest = make_uint3(0, 0, 0);
47 for (
unsigned i = 0; i < 8; ++i) {
48 if ((mask & (1 << i)) == 0)
51 const uint3 child = make_uint3((index.x << 1) + (i & 1),
52 (index.y << 1) + ((i >> 1) & 1),
53 (index.z << 1) + ((i >> 2) & 1));
56 const unsigned voxel_width_scale_factor = 1 << (level + 2);
57 const float3 voxel_center =
58 make_float3(minp.x + (maxp.x - minp.x) * (2 * child.x + 1) / voxel_width_scale_factor,
59 minp.y + (maxp.y - minp.y) * (2 * child.y + 1) / voxel_width_scale_factor,
60 minp.z + (maxp.z - minp.z) * (2 * child.z + 1) / voxel_width_scale_factor);
63 const float3 dist = make_float3(
64 voxel_center.x - query.x, voxel_center.y - query.y, voxel_center.z - query.z);
66 const float distance_to_query = dist.x * dist.x + dist.y * dist.y + dist.z * dist.z;
69 if (distance_to_query < closest_distance) {
70 closest_distance = distance_to_query;
76 return {closest, 1 << closest_index};
82findNode(
const float3 minp,
const float3 maxp,
const float3 query,
const T nodes)
85 const auto code =
CalcMorton(minp, maxp)(query);
88 bool voxel_traversal =
false;
90 std::uint8_t mask_pos;
93 const auto node = nodes[node_idx];
94 const std::uint8_t mask = node & 0xFF;
102 const auto nearest_voxel =
nearestVoxel(query, level, mask, minp, maxp, index);
103 index = nearest_voxel.first;
104 mask_pos = nearest_voxel.second;
109 if (!(mask & mask_pos))
112 index.x >>= remaining_depth;
113 index.y >>= remaining_depth;
114 index.z >>= remaining_depth;
116 voxel_traversal =
true;
117 const auto nearest_voxel =
nearestVoxel(query, level, mask, minp, maxp, index);
118 index = nearest_voxel.first;
119 mask_pos = nearest_voxel.second;
122 node_idx = (node >> 8) +
getBitsNum(mask & (mask_pos - 1));
__host__ __device__ __forceinline__ std::pair< uint3, std::uint8_t > nearestVoxel(const float3 query, const unsigned &level, const std::uint8_t &mask, const float3 &minp, const float3 &maxp, const uint3 &index)