svx_tree.c (12109B)
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_device.h" 18 #include "svx_tree.h" 19 #include "svx_tree_generic_func.h" 20 21 #include <rsys/double3.h> 22 #include <rsys/mem_allocator.h> 23 24 /* Generate the generic binary functions */ 25 #define TREE_DIMENSION 1 26 #include "svx_tree_generic_func.h" 27 28 /* Generate the generic octree functions */ 29 #define TREE_DIMENSION 3 30 #include "svx_tree_generic_func.h" 31 32 /******************************************************************************* 33 * Helper function 34 ******************************************************************************/ 35 static res_T 36 check_octree(struct svx_tree* tree) 37 { 38 ASSERT(tree && tree->type == SVX_OCTREE); 39 40 if(tree->frame[0] != SVX_AXIS_X 41 && tree->frame[1] != SVX_AXIS_Y 42 && tree->frame[2] != SVX_AXIS_Z) 43 return RES_BAD_ARG; 44 45 if(!IS_POW2(tree->definition)) 46 return RES_BAD_ARG; 47 48 if(tree->lower[0] >= tree->upper[0] 49 || tree->lower[1] >= tree->upper[1] 50 || tree->lower[2] >= tree->upper[2]) 51 return RES_BAD_ARG; 52 53 if(tree->tree_low[0] >= tree->tree_upp[0] 54 || tree->tree_low[1] >= tree->tree_upp[1] 55 || tree->tree_low[2] >= tree->tree_upp[2]) 56 return RES_BAD_ARG; 57 58 if(tree->tree_low[0] != tree->lower[0] 59 || tree->tree_low[1] != tree->lower[1] 60 || tree->tree_low[2] != tree->lower[2]) 61 return RES_BAD_ARG; 62 63 if(tree->tree_upp[0] < tree->upper[0] 64 || tree->tree_upp[1] < tree->upper[1] 65 || tree->tree_upp[2] < tree->upper[2]) 66 return RES_BAD_ARG; 67 68 if(tree->tree_size[0] != tree->tree_upp[0] - tree->tree_low[0] 69 || tree->tree_size[1] != tree->tree_upp[1] - tree->tree_low[1] 70 || tree->tree_size[2] != tree->tree_upp[2] - tree->tree_low[2]) 71 return RES_BAD_ARG; 72 73 #ifndef NDEBUG 74 { 75 size_t nleaves = 0; 76 res_T res = buffer_check_tree(&tree->buffer, tree->root, 3, &nleaves); 77 if(res != RES_OK) return res; 78 79 if(nleaves != tree->nleaves) 80 return RES_BAD_ARG; 81 } 82 #endif 83 84 return RES_OK; 85 } 86 87 static res_T 88 check_bintree(struct svx_tree* tree) 89 { 90 enum svx_axis axis = SVX_AXIS_NONE__; 91 ASSERT(tree && tree->type == SVX_BINTREE); 92 93 if(tree->frame[0] == SVX_AXIS_NONE__ 94 || tree->frame[1] != SVX_AXIS_NONE__ 95 || tree->frame[2] != SVX_AXIS_NONE__) 96 return RES_BAD_ARG; 97 98 if(!IS_POW2(tree->definition)) 99 return RES_BAD_ARG; 100 101 axis = tree->frame[0]; 102 if(tree->lower[axis] >= tree->upper[axis]) 103 return RES_BAD_ARG; 104 105 if(tree->tree_low[axis] >= tree->tree_upp[axis]) 106 return RES_BAD_ARG; 107 108 if(tree->tree_low[axis] != tree->lower[axis]) 109 return RES_BAD_ARG; 110 111 if(tree->tree_upp[axis] < tree->upper[axis]) 112 return RES_BAD_ARG; 113 114 if(tree->tree_size[axis] != tree->tree_upp[axis] - tree->tree_low[axis]) 115 return RES_BAD_ARG; 116 117 #ifndef NDEBUG 118 { 119 size_t nleaves = 0; 120 res_T res = buffer_check_tree(&tree->buffer, tree->root, 1, &nleaves); 121 if(res != RES_OK) return res; 122 123 if(nleaves != tree->nleaves) 124 return RES_BAD_ARG; 125 } 126 #endif 127 128 return RES_OK; 129 } 130 131 static void 132 tree_clear(struct svx_tree* tree) 133 { 134 ASSERT(tree); 135 tree->definition = 0; 136 d3_splat(tree->lower, DBL_MAX); 137 d3_splat(tree->upper,-DBL_MAX); 138 d3_splat(tree->tree_low, DBL_MAX); 139 d3_splat(tree->tree_upp,-DBL_MAX); 140 d3_splat(tree->tree_size, -1); 141 tree->root = BUFFER_INDEX_NULL; 142 buffer_clear(&tree->buffer); 143 memset(tree->root_attr, 0, sizeof(tree->root_attr)); 144 tree->nleaves = 0; 145 tree->depth = 0; 146 tree->frame[0] = SVX_AXIS_NONE__; 147 tree->frame[1] = SVX_AXIS_NONE__; 148 tree->frame[2] = SVX_AXIS_NONE__; 149 tree->type = -1; 150 } 151 152 static void 153 tree_release(ref_T* ref) 154 { 155 struct svx_tree* tree; 156 struct svx_device* dev; 157 ASSERT(ref); 158 tree = CONTAINER_OF(ref, struct svx_tree, ref); 159 buffer_release(&tree->buffer); 160 dev = tree->dev; 161 MEM_RM(dev->allocator, tree); 162 SVX(device_ref_put(dev)); 163 } 164 165 /******************************************************************************* 166 * Exported functions 167 ******************************************************************************/ 168 res_T 169 svx_tree_create_from_stream 170 (struct svx_device* dev, 171 FILE* stream, 172 struct svx_tree** out_tree) 173 { 174 struct svx_tree* tree = NULL; 175 res_T res = RES_BAD_ARG; 176 177 if(!stream || !out_tree) { 178 res = RES_BAD_ARG; 179 goto error; 180 } 181 182 res = tree_create 183 (dev, -1/* Unknown tree type */, 1/* Dummy voxel size */, &tree); 184 if(res != RES_OK) goto error; 185 186 /* Setup the tree data from the stream */ 187 res = tree_read(tree, stream); 188 if(res != RES_OK) goto error; 189 190 switch(tree->type) { 191 case SVX_OCTREE: res = check_octree(tree); break; 192 case SVX_BINTREE: res = check_bintree(tree); break; 193 default: FATAL("Unreachable code.\n"); break; 194 } 195 if(res != RES_OK) goto error; 196 197 exit: 198 if(out_tree) *out_tree = tree; 199 return res; 200 error: 201 if(tree) { SVX(tree_ref_put(tree)); tree = NULL; } 202 goto exit; 203 } 204 205 res_T 206 svx_tree_ref_get(struct svx_tree* tree) 207 { 208 if(!tree) return RES_BAD_ARG; 209 ref_get(&tree->ref); 210 return RES_OK; 211 } 212 213 res_T 214 svx_tree_ref_put(struct svx_tree* tree) 215 { 216 if(!tree) return RES_BAD_ARG; 217 ref_put(&tree->ref, tree_release); 218 return RES_OK; 219 } 220 221 res_T 222 svx_tree_get_desc 223 (const struct svx_tree* tree, struct svx_tree_desc* desc) 224 { 225 if(!tree || !desc) return RES_BAD_ARG; 226 d3_set(desc->lower, tree->lower); 227 d3_set(desc->upper, tree->upper); 228 desc->nleaves = tree->nleaves; 229 desc->nvoxels = buffer_absolute_attr_index 230 (&tree->buffer, tree->buffer.attr_head) + 1; /* Root node */ 231 desc->depth = tree->depth; 232 desc->type = tree->type; 233 desc->frame[0] = tree->frame[0]; 234 desc->frame[1] = tree->frame[1]; 235 desc->frame[2] = tree->frame[2]; 236 return RES_OK; 237 } 238 239 res_T 240 svx_tree_for_each_leaf(struct svx_tree* tree, svx_leaf_function_T func, void* ctx) 241 { 242 res_T res = RES_OK; 243 if(!tree) return RES_BAD_ARG; 244 switch(tree->type) { 245 case SVX_BINTREE: res = bintree_for_each_leaf(tree, func, ctx); break; 246 case SVX_OCTREE: res = octree_for_each_leaf(tree, func, ctx); break; 247 default: FATAL("Unreachable code.\n"); break; 248 } 249 return res; 250 } 251 252 res_T 253 svx_tree_trace_ray 254 (struct svx_tree* tree, 255 const double org[3], 256 const double dir[3], 257 const double range[2], 258 const svx_hit_challenge_T challenge, /* NULL <=> Traversed up to the leaves */ 259 const svx_hit_filter_T filter, /* NULL <=> Stop RT at the 1st hit voxel */ 260 void* context, /* Data sent to the filter functor */ 261 struct svx_hit* hit) 262 { 263 res_T res = RES_OK; 264 if(!tree) return RES_BAD_ARG; 265 switch(tree->type) { 266 case SVX_BINTREE: 267 res = bintree_trace_ray 268 (tree, org, dir, range, challenge, filter, context, hit); 269 break; 270 case SVX_OCTREE: 271 res = octree_trace_ray 272 (tree, org, dir, range, challenge, filter, context, hit); 273 break; 274 default: FATAL("Unreachable code.\n"); break; 275 } 276 return res; 277 } 278 279 res_T 280 svx_tree_at 281 (struct svx_tree* tree, 282 const double pos[3], 283 svx_at_filter_T filter, 284 void* context, 285 struct svx_voxel* vox) 286 { 287 res_T res = RES_OK; 288 if(!tree) return RES_BAD_ARG; 289 switch(tree->type){ 290 case SVX_BINTREE: res = bintree_at(tree, pos, filter, context, vox); break; 291 case SVX_OCTREE: res = octree_at(tree, pos, filter, context, vox); break; 292 default: FATAL("Unreachable code.\n"); break; 293 } 294 return res; 295 } 296 297 res_T 298 svx_tree_write(const struct svx_tree* tree, FILE* stream) 299 { 300 res_T res = RES_OK; 301 302 if(!tree || !stream) { 303 res = RES_BAD_ARG; 304 goto error; 305 } 306 307 #define WRITE(Var, N) { \ 308 if(fwrite((Var), sizeof(*(Var)), (N), stream) != (N)) { \ 309 res = RES_IO_ERR; \ 310 goto error; \ 311 } \ 312 } (void)0 313 WRITE(&SVX_TREE_VERSION, 1); 314 WRITE(&tree->definition, 1); 315 WRITE(tree->lower, 3); 316 WRITE(tree->upper, 3); 317 WRITE(tree->tree_low, 3); 318 WRITE(tree->tree_upp, 3); 319 WRITE(tree->tree_size, 3); 320 WRITE(&tree->nleaves, 1); 321 WRITE(&tree->depth, 1); 322 WRITE(tree->frame, 3); 323 WRITE(&tree->type, 1); 324 WRITE(&tree->root, 1); 325 WRITE(tree->root_attr, SVX_MAX_SIZEOF_VOXEL); 326 #undef WRITE 327 328 res = buffer_write(&tree->buffer, stream); 329 if(res != RES_OK) goto error; 330 331 exit: 332 return res; 333 error: 334 goto exit; 335 } 336 337 /******************************************************************************* 338 * Local functions 339 ******************************************************************************/ 340 res_T 341 tree_create 342 (struct svx_device* dev, 343 const enum svx_tree_type type, 344 const size_t vxsz, 345 struct svx_tree** out_tree) 346 { 347 struct svx_tree* tree = NULL; 348 res_T res = RES_OK; 349 350 if(!dev || !out_tree) { 351 res = RES_BAD_ARG; 352 goto error; 353 } 354 355 tree = MEM_ALLOC_ALIGNED(dev->allocator, sizeof(*tree), 16); 356 if(!tree) { 357 res = RES_MEM_ERR; 358 goto error; 359 } 360 memset(tree, 0, sizeof(*tree)); 361 ref_init(&tree->ref); 362 SVX(device_ref_get(dev)); 363 buffer_init(dev->allocator, vxsz, &tree->buffer); 364 tree->dev = dev; 365 d3_splat(tree->lower, DBL_MAX); 366 d3_splat(tree->upper,-DBL_MAX); 367 d3_splat(tree->tree_low, DBL_MAX); 368 d3_splat(tree->tree_upp,-DBL_MAX); 369 d3_splat(tree->tree_size, -1); 370 tree->type = type; 371 tree->frame[0] = SVX_AXIS_NONE__; 372 tree->frame[1] = SVX_AXIS_NONE__; 373 tree->frame[2] = SVX_AXIS_NONE__; 374 exit: 375 if(out_tree) *out_tree = tree; 376 return res; 377 error: 378 if(tree) { 379 SVX(tree_ref_put(tree)); 380 tree = NULL; 381 } 382 goto exit; 383 } 384 385 res_T 386 tree_read(struct svx_tree* tree, FILE* stream) 387 { 388 int version = 0; 389 res_T res = RES_OK; 390 ASSERT(tree && stream); 391 392 tree_clear(tree); 393 394 #define READ(Var, N) { \ 395 if(fread((Var), sizeof(*(Var)), (N), stream) != (N)) { \ 396 if(feof(stream)) { \ 397 res = RES_BAD_ARG; \ 398 } else if(ferror(stream)) { \ 399 res = RES_IO_ERR; \ 400 } else { \ 401 res = RES_UNKNOWN_ERR; \ 402 } \ 403 goto error; \ 404 } \ 405 } (void)0 406 407 /* Currently only one version of the tree data structure could be serialized. 408 * The version management is thus as simple as rejecting any tree data 409 * structure whose version is not the current version. */ 410 READ(&version, 1); 411 if(version != SVX_TREE_VERSION) { 412 log_err(tree->dev, 413 "%s: unexpected tree version %d. Expecting a tree in version %d.\n", 414 FUNC_NAME, version, SVX_TREE_VERSION); 415 res = RES_BAD_ARG; 416 goto error; 417 } 418 419 READ(&tree->definition, 1); 420 READ(tree->lower, 3); 421 READ(tree->upper, 3); 422 READ(tree->tree_low, 3); 423 READ(tree->tree_upp, 3); 424 READ(tree->tree_size, 3); 425 READ(&tree->nleaves, 1); 426 READ(&tree->depth, 1); 427 READ(tree->frame, 3); 428 READ(&tree->type, 1); 429 READ(&tree->root, 1); 430 READ(tree->root_attr, SVX_MAX_SIZEOF_VOXEL); 431 #undef READ 432 433 res = buffer_read(&tree->buffer, stream); 434 if(res != RES_OK) goto error; 435 436 exit: 437 return res; 438 error: 439 tree_clear(tree); 440 goto exit; 441 } 442