svx_tree_generic_func.h (7070B)
1 /* Copyright (C) 2018, 2020-2025 |Méso|Star> (contact@meso-star.com) 2 * Copyright (C) 2018 Université Paul Sabatier 3 * 4 * This program is free software: you can redistribute it and/or modify 5 * it under the terms of the GNU General Public License as published by 6 * the Free Software Foundation, either version 3 of the License, or 7 * (at your option) any later version. 8 * 9 * This program is distributed in the hope that it will be useful, 10 * but WITHOUT ANY WARRANTY; without even the implied warranty of 11 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 12 * GNU General Public License for more details. 13 * 14 * You should have received a copy of the GNU General Public License 15 * along with this program. If not, see <http://www.gnu.org/licenses/>. */ 16 17 #ifndef TREE_DIMENSION 18 19 #ifndef SVX_TREE_GENERIC_FUNC_H 20 #define SVX_TREE_GENERIC_FUNC_H 21 22 #include "svx.h" 23 #include "svx_tree.h" 24 #include "svx_tree_builder.h" /* For the TREE_DEPTH_MAX constant */ 25 26 #endif /* SVX_TREE_GENERIC_FUNC_H */ 27 #else /*!TREE_DIMENSION */ 28 29 #ifdef COMPILER_GCC 30 #pragma GCC push_options 31 #pragma GCC optimize("unroll-loops") 32 #endif 33 34 #if TREE_DIMENSION == 1 35 #define TREE_FUNC(Func) CONCAT(bintree_, Func) 36 #define SVX_TREE_TYPE SVX_BINTREE 37 #elif TREE_DIMENSION == 3 38 #define TREE_FUNC(Func) CONCAT(octree_, Func) 39 #define SVX_TREE_TYPE SVX_OCTREE 40 #else 41 #error "Invalid TREE_DIMENSION value" 42 #endif 43 44 #define NCHILDREN BIT(TREE_DIMENSION) 45 46 static res_T 47 TREE_FUNC(for_each_leaf) 48 (struct svx_tree* oct, svx_leaf_function_T func, void* ctx) 49 { 50 struct stack_entry { 51 struct buffer_index inode; 52 size_t depth; 53 double low[TREE_DIMENSION]; 54 double hsz[TREE_DIMENSION]; /* Half size */ 55 } stack[TREE_DEPTH_MAX*NCHILDREN]; 56 int istack; 57 struct svx_voxel leaf = SVX_VOXEL_NULL; 58 size_t ileaf = 0; 59 int i; 60 61 if(!oct || !func || oct->type != SVX_TREE_TYPE) return RES_BAD_ARG; 62 63 stack[0].depth = 0; 64 stack[0].inode = oct->root; 65 66 FOR_EACH(i, 0, TREE_DIMENSION) { 67 stack[0].low[i] = oct->tree_low[oct->frame[i]]; 68 stack[0].hsz[i] = 69 (oct->tree_upp[oct->frame[i]] - oct->tree_low[oct->frame[i]])*0.5; 70 } 71 istack = 1; 72 73 do { 74 const struct stack_entry entry = stack[--istack]; 75 const size_t child_depth = entry.depth + 1; 76 double child_hsz[TREE_DIMENSION]; 77 double mid[TREE_DIMENSION]; /* Middle point of the current node */ 78 struct buffer_xnode* node; 79 int ichild; 80 81 node = buffer_get_node(&oct->buffer, entry.inode); 82 83 FOR_EACH(i, 0, TREE_DIMENSION) { 84 mid[i] = entry.low[i] + entry.hsz[i]; 85 child_hsz[i] = entry.hsz[i] * 0.5; 86 } 87 88 FOR_EACH(ichild, 0, NCHILDREN) { 89 const uint8_t ichild_flag = (uint8_t)BIT(ichild); 90 double low[TREE_DIMENSION]; 91 92 if((node->is_valid & ichild_flag) == 0) continue; /* Empty node */ 93 94 FOR_EACH(i, 0, TREE_DIMENSION) { 95 low[i] = ichild & BIT(TREE_DIMENSION-1-i) ? mid[i] : entry.low[i]; 96 } 97 98 if(node->is_leaf & ichild_flag) { 99 struct buffer_index iattr; 100 101 iattr = buffer_get_child_attr_index 102 (&oct->buffer, entry.inode, ichild); 103 104 FOR_EACH(i, 0, TREE_DIMENSION) { 105 leaf.lower[oct->frame[i]] = low[i]; 106 leaf.upper[oct->frame[i]] = low[i] + entry.hsz[i]; 107 } 108 leaf.data = buffer_get_attr(&oct->buffer, iattr); 109 leaf.id = buffer_absolute_attr_index(&oct->buffer, iattr); 110 leaf.depth = child_depth; 111 leaf.is_leaf = 1; 112 113 func(&leaf, ileaf++, ctx); 114 } else { 115 struct stack_entry* top = stack + istack; 116 117 top->inode = buffer_get_child_node_index 118 (&oct->buffer, entry.inode, ichild); 119 FOR_EACH(i, 0, TREE_DIMENSION) { 120 top->low[i] = low[i]; 121 top->hsz[i] = child_hsz[i]; 122 } 123 top->depth = child_depth; 124 ++istack; 125 } 126 } 127 } while(istack); 128 129 return RES_OK; 130 } 131 132 static res_T 133 TREE_FUNC(at) 134 (struct svx_tree* tree, 135 const double position[3], 136 svx_at_filter_T filter, 137 void* context, 138 struct svx_voxel* voxel) 139 { 140 struct buffer_index inode; 141 struct buffer_index iattr; 142 struct svx_voxel vox = SVX_VOXEL_NULL; 143 double scale_exp2; 144 double low[TREE_DIMENSION]; 145 double pos[TREE_DIMENSION]; 146 int i; 147 res_T res = RES_OK; 148 149 if(!tree || !position || !voxel || tree->type != SVX_TREE_TYPE) { 150 res = RES_BAD_ARG; 151 goto error; 152 } 153 154 *voxel = SVX_VOXEL_NULL; 155 156 /* The position is outside the octree */ 157 FOR_EACH(i, 0, TREE_DIMENSION) { 158 if(position[tree->frame[i]] > tree->upper[tree->frame[i]] 159 || position[tree->frame[i]] < tree->lower[tree->frame[i]]) 160 goto exit; 161 } 162 163 FOR_EACH(i, 0, TREE_DIMENSION) { 164 /* Transform the position in the normalized octree space, 165 * i.e. octree lies in [0, 1]^2 */ 166 pos[i] = (position[tree->frame[i]] - tree->tree_low[tree->frame[i]]) 167 / tree->tree_size[tree->frame[i]]; 168 /* Initialized the lower left corner of the current node */ 169 low[i] = 0; 170 } 171 172 /* Root voxel */ 173 vox.depth = 0; 174 vox.is_leaf = 0; 175 FOR_EACH(i, 0, TREE_DIMENSION) { 176 vox.lower[tree->frame[i]] = tree->lower[tree->frame[i]]; 177 vox.upper[tree->frame[i]] = tree->upper[tree->frame[i]]; 178 } 179 if(filter) { 180 vox.data = tree->root_attr; 181 vox.id = buffer_absolute_attr_index(&tree->buffer, tree->buffer.attr_head); 182 if(!filter(&vox, position, context)) { *voxel = vox; goto exit; } 183 } 184 185 scale_exp2 = 0.5; 186 inode = tree->root; 187 for(;;) { 188 struct buffer_xnode* node = buffer_get_node(&tree->buffer, inode); 189 int ichild; 190 uint8_t ichild_flag; 191 double mid[TREE_DIMENSION]; 192 193 ++vox.depth; 194 195 /* Compute the middle point of the node */ 196 FOR_EACH(i, 0, TREE_DIMENSION) mid[i] = low[i] + scale_exp2; 197 198 /* Define the child of the current node into which pos `lies' and compute 199 * its lower left corner */ 200 ichild = 0; 201 FOR_EACH(i, 0, TREE_DIMENSION) { 202 if(pos[i] > mid[i]) { ichild |= BIT(TREE_DIMENSION-1-i); low[i] = mid[i]; } 203 } 204 205 ichild_flag = (uint8_t)BIT(ichild); 206 if((node->is_valid & ichild_flag) == 0) break; /* Empty node */ 207 208 vox.is_leaf = (node->is_leaf & ichild_flag) != 0; 209 if(filter || vox.is_leaf) { 210 iattr = buffer_get_child_attr_index(&tree->buffer, inode, ichild); 211 212 /* Setup the voxel */ 213 FOR_EACH(i, 0, TREE_DIMENSION) { 214 const int iaxis = tree->frame[i]; 215 vox.lower[iaxis] = low[i] * tree->tree_size[iaxis] + tree->tree_low[iaxis]; 216 vox.upper[iaxis] = vox.lower[iaxis] + tree->tree_size[iaxis] * scale_exp2; 217 } 218 vox.data = buffer_get_attr(&tree->buffer, iattr); 219 vox.id = buffer_absolute_attr_index(&tree->buffer, iattr); 220 vox.is_leaf = (node->is_leaf & ichild_flag) != 0; 221 222 if(vox.is_leaf || !filter(&vox, position, context)) { 223 *voxel = vox; 224 break; 225 } 226 } 227 228 inode = buffer_get_child_node_index(&tree->buffer, inode, ichild); 229 scale_exp2 *= 0.5; 230 } 231 232 exit: 233 return res; 234 error: 235 goto exit; 236 } 237 238 #undef TREE_FUNC 239 #undef SVX_TREE_TYPE 240 #undef TREE_DIMENSION 241 #undef NCHILDREN 242 243 #ifdef COMPILER_GCC 244 #pragma GCC pop_options 245 #endif 246 247 #endif /* !TREE_DIMENSION */