svx_octree.c (5665B)
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 #include "svx.h" 18 #include "svx_c.h" 19 #include "svx_device.h" 20 #include "svx_tree.h" 21 #include "svx_tree_builder.h" 22 #include "svx_tree_generic_func.h" 23 24 #include <rsys/double3.h> 25 #include <rsys/mem_allocator.h> 26 27 /* Generate the octree_builder API */ 28 #define TREE_DIMENSION 3 29 #include "svx_tree_builder.h" 30 31 /******************************************************************************* 32 * Exported functions 33 ******************************************************************************/ 34 res_T 35 svx_octree_create 36 (struct svx_device* dev, 37 const double lower[3], /* Lower bound of the octree */ 38 const double upper[3], /* Upper bound of the octree */ 39 const size_t nvoxels[3], /* # voxels along the 3 axis */ 40 const struct svx_voxel_desc* desc, /* Descriptor of a voxel */ 41 struct svx_tree** out_oct) 42 { 43 struct svx_tree* oct = NULL; 44 double vox_sz[3]; /* World space size of a voxel */ 45 struct octree_builder bldr; 46 struct voxel vox = VOXEL_NULL; 47 uint64_t mcode_max; 48 uint64_t mcode; 49 res_T res = RES_OK; 50 51 if(!dev || !lower || !upper || !nvoxels || !check_svx_voxel_desc(desc) 52 || !out_oct) { 53 res = RES_BAD_ARG; 54 goto error; 55 } 56 if(lower[0] >= upper[0] 57 || lower[1] >= upper[1] 58 || lower[2] >= upper[2]) { 59 log_err(dev, 60 "%s: the submitted AABB is degenerated.\n" 61 "\tlower={%g, %g, %g}, upper={%g, %g, %g}.\n", 62 FUNC_NAME, SPLIT3(lower), SPLIT3(upper)); 63 res = RES_BAD_ARG; 64 goto error; 65 } 66 if(!nvoxels[0] || !nvoxels[1] || !nvoxels[2]) { 67 log_err(dev, 68 "%s: the number of voxels along each axis cannot be null.\n" 69 "\t#voxels XYZ = {%lu, %lu, %lu}.\n", 70 FUNC_NAME, 71 (unsigned long)nvoxels[0], 72 (unsigned long)nvoxels[1], 73 (unsigned long)nvoxels[2]); 74 res = RES_BAD_ARG; 75 goto error; 76 } 77 78 res = tree_create(dev, SVX_OCTREE, desc->size, &oct); 79 if(res != RES_OK) goto error; 80 81 oct->frame[0] = SVX_AXIS_X; 82 oct->frame[1] = SVX_AXIS_Y; 83 oct->frame[2] = SVX_AXIS_Z; 84 85 /* Compute the octree definition */ 86 oct->definition = MMAX(nvoxels[0], 2); 87 oct->definition = MMAX(nvoxels[1], oct->definition); 88 oct->definition = MMAX(nvoxels[2], oct->definition); 89 oct->definition = round_up_pow2(oct->definition); 90 91 /* Setup the octree AABB in world space */ 92 d3_set(oct->lower, lower); 93 d3_set(oct->upper, upper); 94 95 /* Compute the voxel size in world space */ 96 vox_sz[0] = (upper[0] - lower[0]) / (double)nvoxels[0]; 97 vox_sz[1] = (upper[1] - lower[1]) / (double)nvoxels[1]; 98 vox_sz[2] = (upper[2] - lower[2]) / (double)nvoxels[2]; 99 100 /* Compute the octree AABB in world space */ 101 oct->tree_low[0] = lower[0]; 102 oct->tree_low[1] = lower[1]; 103 oct->tree_low[2] = lower[2]; 104 oct->tree_upp[0] = oct->tree_low[0] + (double)oct->definition * vox_sz[0]; 105 oct->tree_upp[1] = oct->tree_low[1] + (double)oct->definition * vox_sz[1]; 106 oct->tree_upp[2] = oct->tree_low[2] + (double)oct->definition * vox_sz[2]; 107 oct->tree_size[0] = oct->tree_upp[0] - oct->tree_low[0]; 108 oct->tree_size[1] = oct->tree_upp[1] - oct->tree_low[1]; 109 oct->tree_size[2] = oct->tree_upp[2] - oct->tree_low[2]; 110 111 /* Intialize the octree builder */ 112 res = octree_builder_init(&bldr, oct->definition, oct->tree_low, 113 oct->tree_upp, oct->frame, desc, &oct->buffer); 114 if(res != RES_OK) goto error; 115 116 vox.data = MEM_CALLOC(dev->allocator, 1, desc->size); 117 if(!vox.data) { 118 res = RES_MEM_ERR; 119 goto error; 120 } 121 122 mcode_max = oct->definition * oct->definition * oct->definition; 123 FOR_EACH(mcode, 0, mcode_max) { 124 size_t xyz[3]; 125 uint32_t ui3[3]; 126 127 morton_xyz_decode_u21(mcode, ui3); 128 129 /* Out of bound voxels */ 130 if(ui3[0] >= nvoxels[0] 131 || ui3[1] >= nvoxels[1] 132 || ui3[2] >= nvoxels[2]) 133 continue; 134 135 /* Retrieve the voxel data from the caller */ 136 xyz[0] = (size_t)ui3[0]; 137 xyz[1] = (size_t)ui3[1]; 138 xyz[2] = (size_t)ui3[2]; 139 desc->get(xyz, mcode, vox.data, desc->context); 140 vox.mcode = mcode; 141 142 /* Register the voxel against the octree */ 143 res = octree_builder_add_voxel(&bldr, &vox); 144 if(res != RES_OK) goto error; 145 } 146 147 res = octree_builder_finalize(&bldr, &oct->root, oct->root_attr); 148 if(res != RES_OK) goto error; 149 150 oct->nleaves = bldr.nleaves; 151 oct->depth = (size_t)(bldr.tree_depth - bldr.non_empty_lvl) 152 + 1 /* leaf level */; 153 ASSERT(bldr.tree_depth > bldr.non_empty_lvl); 154 155 #ifndef NDEBUG 156 { 157 size_t nleaves = 0; 158 CHK(buffer_check_tree(&oct->buffer, oct->root, 3, &nleaves) == RES_OK); 159 CHK(nleaves == oct->nleaves); 160 } 161 #endif 162 163 /* Adjust the octree definition with respect to the octree depth since finest 164 * levels might be removed during the build due to the merging process */ 165 oct->definition = oct->definition / ((size_t)1 << bldr.non_empty_lvl); 166 167 exit: 168 if(vox.data) MEM_RM(dev->allocator, vox.data); 169 if(out_oct) *out_oct = oct; 170 return res; 171 error: 172 if(oct) { 173 SVX(tree_ref_put(oct)); 174 oct = NULL; 175 } 176 goto exit; 177 } 178