svx_tree_builder.h (18113B)
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_BUILDER_H 20 #define SVX_TREE_BUILDER_H 21 22 #include "svx_buffer.h" 23 #include <rsys/double3.h> 24 #include <rsys/morton.h> 25 26 #define TREE_DEPTH_MAX 16 /* Maximum depth of a tree */ 27 28 struct voxel { 29 uint64_t mcode; /* Morton code of the voxel */ 30 void* data; /* Data of the voxel */ 31 }; 32 static const struct voxel VOXEL_NULL = {0, NULL}; 33 34 #endif /* SVX_TREE_BUILDER_H */ 35 #else /* TREE_DIMENSION */ 36 37 #if (TREE_DIMENSION<=0) || (TREE_DIMENSION>3) 38 #error "Invalid TREE_DIMENSION value" 39 #endif 40 41 #if TREE_DIMENSION == 1 42 #define XD(Name) CONCAT(bintree_, Name) 43 #elif TREE_DIMENSION == 2 44 #define XD(Name) CONCAT(quadtree_, Name) 45 #elif TREE_DIMENSION == 3 46 #define XD(Name) CONCAT(octree_, Name) 47 #else 48 #error "Invalid TREE_DIMENSION value" 49 #endif 50 51 #define NCHILDREN BIT(TREE_DIMENSION) 52 53 #ifdef COMPILER_CL 54 #pragma warning(push) 55 #pragma warning(disable:4324) /* Structure was padded due to alignment */ 56 #endif 57 58 struct XD(node) { 59 struct buffer_index ichild_node; /* Index of the 1st child node */ 60 struct buffer_index ichild_attr; /* Index of the 1st child attr */ 61 uint8_t is_valid; /* Mask defining whether the children are valid or not */ 62 uint8_t is_leaf; /* Mask defining whether the children are leaves or not */ 63 ALIGN(16) char data[NCHILDREN][SVX_MAX_SIZEOF_VOXEL]; /* Data of the leaves */ 64 }; 65 66 #ifdef COMPILER_CL 67 #pragma warning(pop) 68 #endif 69 70 /* Stacked children of a tree node */ 71 struct XD(stack) { 72 struct XD(node) nodes[NCHILDREN]; /* List of registered children */ 73 uint8_t mask; /* Mask of valid children nodes (0 = empty) */ 74 }; 75 76 struct XD(builder) { 77 struct XD(stack) stacks[TREE_DEPTH_MAX]; 78 struct buffer* buffer; 79 80 double lower[3]; /* Tree lower bound in world space */ 81 double upper[3]; /* Tree upper bound in world space */ 82 double voxsz[3]; /* Size of the finest voxel in world space */ 83 enum svx_axis frame[3]; 84 85 const struct svx_voxel_desc* desc; 86 size_t nleaves; /* Number of emitted leaves */ 87 88 int tree_depth; /* Maximum tree desc */ 89 int non_empty_lvl; /* Index of the 1st non empty level */ 90 91 uint64_t mcode; /* Morton code of the voxel */ 92 }; 93 94 /******************************************************************************* 95 * Stack functions 96 ******************************************************************************/ 97 static INLINE void 98 XD(stack_clear)(struct XD(stack)* stack) 99 { 100 int inode; 101 FOR_EACH(inode, 0, NCHILDREN) { 102 int ichild; 103 stack->nodes[inode].is_leaf = 0; 104 stack->nodes[inode].is_valid = 0; 105 stack->nodes[inode].ichild_node = BUFFER_INDEX_NULL; 106 stack->nodes[inode].ichild_attr = BUFFER_INDEX_NULL; 107 FOR_EACH(ichild, 0, NCHILDREN) { 108 memset(stack->nodes[inode].data[ichild], 0, SVX_MAX_SIZEOF_VOXEL); 109 } 110 } 111 stack->mask = 0; 112 } 113 114 /* Build a parent tree node from the registered stack nodes */ 115 static INLINE void 116 XD(stack_setup_node) 117 (struct XD(stack)* stack, 118 struct XD(node)* node, 119 const double node_low[3], /* World space node lower bound */ 120 const double node_sz[3], /* World space node size */ 121 const size_t node_depth, /* Depth of the node */ 122 const enum svx_axis frame[3], 123 const struct svx_voxel_desc* desc) 124 { 125 double child_sz[3]; 126 double grandchild_sz[3]; 127 int ichild; 128 int i; 129 ASSERT(stack && node && check_svx_voxel_desc(desc)); 130 ASSERT(node_low && node_sz); 131 ASSERT(node_sz[0] > 0 && node_sz[1] > 0 && node_sz[2] > 0); 132 ASSERT(frame); 133 134 node->ichild_node = BUFFER_INDEX_NULL; 135 node->ichild_attr = BUFFER_INDEX_NULL; 136 node->is_valid = stack->mask; 137 node->is_leaf = 0; 138 139 if(stack->mask == 0) return; /* Empty stack */ 140 141 d3_splat(child_sz, INF); 142 d3_splat(grandchild_sz, INF); 143 FOR_EACH(i, 0, TREE_DIMENSION) { 144 child_sz[frame[i]] = node_sz[frame[i]] * 0.5f; 145 grandchild_sz[frame[i]] = node_sz[frame[i]] * 0.25f; 146 } 147 148 /* Try to merge the child's leaves */ 149 FOR_EACH(ichild, 0, NCHILDREN) { 150 const void* data[NCHILDREN]; 151 struct svx_voxel voxels[NCHILDREN]; 152 double child_low[3]; 153 const uint8_t ichild_flag = (uint8_t)BIT(ichild); 154 struct XD(node)* child = stack->nodes + ichild; 155 int igrandchild; 156 size_t ngrandchildren = 0; /* #active grand children */ 157 158 d3_splat(child_low, -INF); 159 FOR_EACH(i, 0, TREE_DIMENSION) { 160 const int iaxis = frame[i]; 161 child_low[iaxis] = node_low[iaxis]; 162 if(ichild & (NCHILDREN >> (1+i))) child_low[iaxis] += child_sz[iaxis]; 163 } 164 165 if(!(stack->mask & ichild_flag)) continue; /* Empty child */ 166 167 /* Fetch the grandchildren data */ 168 FOR_EACH(igrandchild, 0, NCHILDREN) { 169 struct svx_voxel* vox = &voxels[igrandchild]; 170 const uint8_t igrandchild_flag = (uint8_t)BIT(igrandchild); 171 172 if(!(child->is_valid & igrandchild_flag)) continue; /* Empty grandchild */ 173 174 voxels[ngrandchildren].data = child->data[igrandchild]; 175 voxels[ngrandchildren].depth = node_depth + 2; 176 voxels[ngrandchildren].id = SIZE_MAX; 177 voxels[ngrandchildren].is_leaf = (child->is_leaf & igrandchild_flag)!=0; 178 voxels[ngrandchildren].data = child->data[igrandchild]; 179 data[ngrandchildren] = child->data[igrandchild]; 180 181 d3_splat(vox->lower,-INF); 182 d3_splat(vox->upper, INF); 183 FOR_EACH(i, 0, TREE_DIMENSION) { 184 const int iaxis = frame[i]; 185 vox->lower[iaxis] = child_low[iaxis]; 186 if(igrandchild & (NCHILDREN >> (1+i))) { 187 vox->lower[iaxis] += grandchild_sz[iaxis]; 188 } 189 vox->upper[iaxis] = vox->lower[iaxis] + grandchild_sz[iaxis]; 190 } 191 192 ++ngrandchildren; 193 } 194 195 desc->merge(node->data[ichild], data, ngrandchildren, desc->context); 196 197 if(child->is_leaf == (BIT(NCHILDREN)-1)/*all active bitmask*/ 198 && desc->challenge_merge(voxels, ngrandchildren, desc->context)) { 199 /* The node becomes a leaf: the children does not exist anymore */ 200 node->is_leaf |= ichild_flag; 201 stack->mask ^= ichild_flag; 202 } 203 } 204 } 205 206 static res_T 207 XD(stack_write) 208 (struct XD(stack)* stack, /* Node to write */ 209 struct buffer* buf, /* Buffer where nodes are written */ 210 struct buffer_index* out_index, /* Index of the first written node */ 211 size_t* out_nleaves) /* #writen leaves */ 212 { 213 struct buffer_index nodes_id = BUFFER_INDEX_NULL; 214 struct XD(node)* node = NULL; 215 size_t nleaves = 0; 216 int inode; 217 res_T res = RES_OK; 218 ASSERT(stack && buf && out_index && out_nleaves); 219 220 /* No registered nodes, this means that the nodes were merged in an higher 221 * level */ 222 if(!stack->mask) goto exit; 223 224 /* Write the attrib of the children */ 225 FOR_EACH(inode, 0, NCHILDREN) { 226 char* data = NULL; 227 size_t nattrs = 0; 228 size_t nvoxs = 0; 229 int ichild = 0; 230 231 if((stack->mask & BIT(inode)) == 0) continue; /* Empty node */ 232 233 node = stack->nodes + inode; 234 235 nattrs = (size_t)popcount(node->is_valid); 236 ASSERT(nattrs > 0); 237 238 res = buffer_alloc_attrs(buf, nattrs, &node->ichild_attr); 239 if(res != RES_OK) goto error; 240 241 data = buffer_get_attr(buf, node->ichild_attr); 242 nvoxs = 0; 243 FOR_EACH(ichild, 0, NCHILDREN) { 244 if(!(node->is_valid & BIT(ichild))) continue; 245 memcpy(data + nvoxs*buf->voxsize, node->data[ichild], buf->voxsize); 246 ++nvoxs; 247 } 248 ASSERT(nvoxs == nattrs); 249 nleaves += (size_t)popcount(node->is_leaf); 250 } 251 252 do { 253 struct buffer_index index = BUFFER_INDEX_NULL; 254 struct buffer_xnode* xnodes = NULL; 255 const size_t nnodes = (size_t)popcount(stack->mask); 256 size_t ixnode = 0; 257 258 /* Alloc the tree nodes */ 259 res = buffer_alloc_nodes(buf, (size_t)nnodes, &nodes_id); 260 if(res != RES_OK) goto error; 261 xnodes = buffer_get_node(buf, nodes_id); 262 263 FOR_EACH(inode, 0, NCHILDREN) { 264 uint16_t attr_offset = UINT16_MAX; 265 uint16_t node_offset = UINT16_MAX; 266 267 if((stack->mask & BIT(inode)) == 0) continue; /* Empty node */ 268 node = stack->nodes + inode; 269 270 /* Setup the offset toward the children and children attribs */ 271 if(node->ichild_node.ipage == nodes_id.ipage) { 272 node_offset = node->ichild_node.inode; 273 } 274 /* The page id of the children is not the same as that of node */ 275 if(node_offset > BUFFER_XNODE_MAX_CHILDREN_OFFSET) { 276 res = buffer_alloc_far_index(buf, &index); 277 if(res != RES_OK) break; 278 *buffer_get_far_index(buf, index) = node->ichild_node; 279 node_offset = BUFFER_XNODE_FLAG_FAR_INDEX | index.inode; 280 } 281 282 /* Setup the offset toward the children attribs */ 283 if(node->ichild_attr.ipage == nodes_id.ipage) { 284 attr_offset = node->ichild_attr.inode; 285 } 286 287 /* The page id of the attribs is not tthe same as that of node */ 288 if(attr_offset > BUFFER_XNODE_FLAG_FAR_INDEX) { 289 res = buffer_alloc_far_index(buf, &index); 290 if(res != RES_OK) break; 291 *buffer_get_far_index(buf, index) = node->ichild_attr; 292 attr_offset = BUFFER_XNODE_FLAG_FAR_INDEX | index.inode; 293 } 294 295 xnodes[ixnode].node_offset = node_offset; 296 xnodes[ixnode].attr_offset = attr_offset; 297 xnodes[ixnode].is_valid = node->is_valid; 298 xnodes[ixnode].is_leaf = node->is_leaf; 299 ++ixnode; 300 } 301 /* inode < NCHILDREN <=> not enough memory in the current page. A far index 302 * could not be stored in the same page of its associated node. The write 303 * process was stoped. Rewrite the whole stacked nodes in a new page. */ 304 } while(inode < NCHILDREN); 305 306 307 exit: 308 /* Return the index toward the first writen nodes */ 309 *out_index = nodes_id; 310 *out_nleaves = nleaves; 311 return res; 312 error: 313 goto exit; 314 } 315 316 317 /******************************************************************************* 318 * Builder functions 319 ******************************************************************************/ 320 static res_T 321 XD(builder_init) 322 (struct XD(builder)* bldr, 323 const size_t definition, 324 const double lower[3], /* Lower bound of the tree */ 325 const double upper[3], /* Upper bound of the tree */ 326 const enum svx_axis frame[3], 327 const struct svx_voxel_desc* desc, 328 struct buffer* buffer) 329 { 330 int ilvl, i; 331 res_T res = RES_OK; 332 ASSERT(bldr && IS_POW2(definition) && check_svx_voxel_desc(desc)); 333 ASSERT(lower && upper && frame); 334 memset(bldr, 0, sizeof(struct XD(builder))); 335 336 /* Compute the maximum depth of the tree */ 337 bldr->tree_depth = log2i((int)definition); 338 if(bldr->tree_depth > TREE_DEPTH_MAX) { 339 res = RES_MEM_ERR; 340 goto error; 341 } 342 343 /* Init the per tree level stack */ 344 FOR_EACH(ilvl, 0, bldr->tree_depth) { 345 XD(stack_clear)(&bldr->stacks[ilvl]); 346 } 347 348 buffer_clear(buffer); 349 bldr->nleaves = 0; 350 bldr->desc = desc; 351 bldr->buffer = buffer; 352 bldr->non_empty_lvl = bldr->tree_depth - 1; 353 bldr->frame[0] = frame[0]; 354 bldr->frame[1] = frame[1]; 355 bldr->frame[2] = frame[2]; 356 d3_splat(bldr->lower,-INF); 357 d3_splat(bldr->upper, INF); 358 d3_splat(bldr->voxsz, INF); 359 360 FOR_EACH(i, 0, TREE_DIMENSION) { 361 const int iaxis = frame[i]; 362 bldr->lower[iaxis] = lower[iaxis]; 363 bldr->upper[iaxis] = upper[iaxis]; 364 bldr->voxsz[iaxis] = (upper[iaxis] - lower[iaxis]) / (double)definition; 365 } 366 367 exit: 368 return res; 369 error: 370 goto exit; 371 } 372 373 static res_T 374 XD(builder_add_voxel) 375 (struct XD(builder)* bldr, 376 const struct voxel* vox) 377 { 378 uint64_t mcode_xor; 379 int inode; 380 int ichild; 381 uint8_t ichild_flag; 382 res_T res = RES_OK; 383 ASSERT(bldr && vox && vox->mcode >= bldr->mcode); 384 385 /* Define if the bits in [4 .. 63] of the previous and the next Morton 386 * codes are the same */ 387 mcode_xor = bldr->mcode ^ vox->mcode; 388 389 /* The next voxel is not in the current node */ 390 if(mcode_xor >= NCHILDREN) { 391 size_t ilvl; 392 393 inode = (bldr->mcode >> TREE_DIMENSION) & (NCHILDREN-1); 394 bldr->stacks[0].mask |= (uint8_t)BIT(inode); 395 396 /* Flush the stack of the tree level that does not contain the next voxel. 397 * The last tree level is actually the level that contains the root node 398 * while the penultimate describes the root node itself. These 2 levels 399 * contain all the voxels and can be skipped */ 400 FOR_EACH(ilvl, 0, (size_t)bldr->tree_depth-2/*The 2 last voxels contain all voxels*/) { 401 uint32_t parent_coords[3] = {UINT32_MAX, UINT32_MAX, UINT32_MAX}; 402 double parent_sz[3]; 403 double parent_low[3]; 404 size_t parent_depth; 405 uint64_t parent_mcode; 406 struct XD(node)* parent_node; 407 uint64_t mcode_max_lvl; 408 size_t nleaves; 409 int i; 410 411 /* Compute the maximum morton code value for the current tree level */ 412 mcode_max_lvl = 413 NCHILDREN/*#children*/ 414 * (1lu<<(TREE_DIMENSION*(ilvl+1)))/*#voxels per children*/; 415 416 if(mcode_xor < mcode_max_lvl) break; 417 418 /* Compute the parent node attribute */ 419 parent_mcode = bldr->mcode >> (TREE_DIMENSION * (ilvl+2)); 420 d3_splat(parent_sz, INF); 421 d3_splat(parent_low, -INF); 422 FOR_EACH(i, 0, TREE_DIMENSION) { 423 const int iaxis = bldr->frame[i]; 424 parent_sz[iaxis] = bldr->voxsz[iaxis] * (double)(1 << (ilvl+2)); 425 parent_coords[iaxis] = morton3D_decode_u21 426 (parent_mcode >> (TREE_DIMENSION-1-i)); 427 parent_low[iaxis] = 428 parent_coords[iaxis] * parent_sz[iaxis] + bldr->lower[iaxis]; 429 } 430 431 parent_depth = (size_t)bldr->tree_depth - 2 - ilvl; 432 ASSERT(parent_depth <= (size_t)bldr->tree_depth-2); 433 434 /* Retrieve the node index of the next level */ 435 inode = (int)parent_mcode & (NCHILDREN-1); 436 437 /* The next voxel is not in the ilvl^th stack. Setup the parent node of the 438 * nodes registered into the stack */ 439 parent_node = &bldr->stacks[ilvl+1].nodes[inode]; 440 XD(stack_setup_node)(&bldr->stacks[ilvl], parent_node, parent_low, 441 parent_sz, parent_depth, bldr->frame, bldr->desc); 442 bldr->stacks[ilvl+1].mask |= (uint8_t)BIT(inode); 443 444 /* Write the nodes of the stack of the current tree level into the buf */ 445 res = XD(stack_write) 446 (&bldr->stacks[ilvl], bldr->buffer, &parent_node->ichild_node, &nleaves); 447 if(res != RES_OK) goto error; 448 449 bldr->nleaves += nleaves; 450 if(nleaves) bldr->non_empty_lvl = MMIN(bldr->non_empty_lvl, (int)ilvl); 451 452 /* Reset the current stack */ 453 XD(stack_clear)(&bldr->stacks[ilvl]); 454 } 455 } 456 457 /* Retrieve the index of the current voxel and of its parent */ 458 ichild = vox->mcode & (NCHILDREN-1); 459 inode = (vox->mcode >> TREE_DIMENSION) & (NCHILDREN-1); 460 ichild_flag = (uint8_t)BIT(ichild); 461 462 /* Register the voxel */ 463 memcpy(bldr->stacks[0].nodes[inode].data[ichild], vox->data, bldr->desc->size); 464 bldr->stacks[0].nodes[inode].is_valid |= ichild_flag; 465 bldr->stacks[0].nodes[inode].is_leaf |= ichild_flag; 466 467 /* Update morton code of the last registered voxel */ 468 bldr->mcode = vox->mcode; 469 470 exit: 471 return res; 472 error: 473 goto exit; 474 } 475 476 static INLINE res_T 477 XD(builder_finalize) 478 (struct XD(builder)* bldr, 479 struct buffer_index* root_id, 480 void* root_data) 481 { 482 const void* data[NCHILDREN]; 483 size_t inode; 484 size_t nleaves; 485 int ilvl; 486 int ichild; 487 size_t nchildren; 488 res_T res = RES_OK; 489 ASSERT(bldr); 490 491 inode = (bldr->mcode >> TREE_DIMENSION) & (NCHILDREN-1); 492 bldr->stacks[0].mask |= (uint8_t)BIT(inode); 493 494 /* Flush the stacked nodes */ 495 FOR_EACH(ilvl, 0, bldr->tree_depth-1) { 496 uint32_t parent_coords[3]; 497 double parent_sz[3]; 498 double parent_low[3]; 499 size_t parent_depth; 500 uint64_t parent_mcode; 501 struct XD(node)* parent_node; 502 int i; 503 504 if(bldr->stacks[ilvl].mask == 0) continue; 505 506 /* Compute the parent node attribute */ 507 parent_mcode = bldr->mcode >> (TREE_DIMENSION * (ilvl+2)); 508 d3_splat(parent_sz, INF); 509 d3_splat(parent_low,-INF); 510 FOR_EACH(i, 0, TREE_DIMENSION) { 511 const int iaxis = bldr->frame[i]; 512 parent_coords[iaxis] = morton3D_decode_u21 513 (parent_mcode >> (TREE_DIMENSION-1-i)); 514 parent_sz[iaxis] = bldr->voxsz[iaxis] * (double)(1 << (ilvl+2)); 515 parent_low[iaxis] = parent_coords[iaxis] * parent_sz[iaxis]; 516 } 517 518 parent_depth = (size_t)(bldr->tree_depth - 2 - ilvl); 519 ASSERT(parent_depth <= (size_t)bldr->tree_depth-2); 520 521 /* Retrieve the node index of the next level */ 522 inode = (int)parent_mcode & (NCHILDREN-1); 523 524 /* Setup the parent node of the nodes registered into the current stack */ 525 parent_node = &bldr->stacks[ilvl+1].nodes[inode]; /* Fetch the parent node */ 526 XD(stack_setup_node)(&bldr->stacks[ilvl], parent_node, parent_low, 527 parent_sz, parent_depth, bldr->frame, bldr->desc); 528 bldr->stacks[ilvl+1].mask |= (uint8_t)BIT(inode); 529 530 /* Write the stacked nodes of the current level */ 531 res = XD(stack_write) 532 (&bldr->stacks[ilvl], bldr->buffer, &parent_node->ichild_node, &nleaves); 533 if(res != RES_OK) goto error; 534 bldr->nleaves += nleaves; 535 } 536 537 ilvl = bldr->tree_depth-1; /* Root level */ 538 539 /* Write the root node */ 540 res = XD(stack_write)(&bldr->stacks[ilvl], bldr->buffer, root_id, &nleaves); 541 if(res != RES_OK) goto error; 542 bldr->nleaves += nleaves; 543 544 /* Setup the root attribs */ 545 nchildren = 0; 546 ASSERT(bldr->stacks[ilvl].mask == 1); /* Only the root node is active */ 547 FOR_EACH(ichild, 0, NCHILDREN) { 548 const int ichild_flag = BIT(ichild); 549 if(!(bldr->stacks[ilvl].nodes[0].is_valid & ichild_flag)) continue; 550 551 data[nchildren] = bldr->stacks[ilvl].nodes[0].data[ichild]; 552 ++nchildren; 553 } 554 bldr->desc->merge(root_data, data, nchildren, bldr->desc->context); 555 556 exit: 557 return res; 558 error: 559 goto exit; 560 } 561 562 #undef TREE_DIMENSION 563 #undef NCHILDREN 564 #undef XD 565 566 #endif /* !TREE_DIMENSION */