svx_octree_trace_ray.c (16660B)
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_tree.h" 19 20 #include <rsys/double3.h> 21 22 struct ray { 23 /* Ray parameters in the normalized octree space [1, 2]^3 whose ray direction 24 * is assumed to be always negative. octant_mask defines which dimension was 25 * reverted to ensure the negative ray direction: the bit 1, 2 and 3 are set if 26 * the Z, Y and X dimension was reverted, respectively. */ 27 double org[3]; 28 double range[2]; 29 double ts[3]; /* 1 / -abs(dir) */ 30 uint32_t octant_mask; 31 32 /* Ray origin and direction in world space. Note that the ray range is 33 * independent of the octree space. Let a ray `r' in world space defined as 34 * `r = O + tD'; one can transform the ray in another space by the Matrix M 35 * as `r' = M.O + t*(M.D)' without any impact on the t parameter and thus on 36 * its range. Only the norm of ray direction might be updated. */ 37 double orgws[3]; 38 double dirws[3]; 39 }; 40 41 /******************************************************************************* 42 * Helper functions 43 ******************************************************************************/ 44 static FINLINE uint32_t 45 ftoui(const float f) 46 { 47 union { uint32_t ui; float f; } ucast; 48 ucast.f = f; 49 return ucast.ui; 50 } 51 52 static FINLINE float 53 uitof(const uint32_t ui) 54 { 55 union { uint32_t ui; float f; } ucast; 56 ucast.ui = ui; 57 return ucast.f; 58 } 59 60 static FINLINE void 61 setup_hit 62 (struct svx_tree* oct, 63 const struct buffer_index iattr, /* Index toward the voxel attributes */ 64 const double distance_min, /* Dst from ray org to the voxel entry point */ 65 const double distance_max, /* Dst from ray_org to the voxel exit point */ 66 const float corner[3], /* Corner of the current voxel in [1, 2]^3 space */ 67 const double scale_exp2, /* Size of the voxel in [1, 2]^3 space */ 68 const size_t depth, /* Depth of the voxel in the octree hierarchy */ 69 const int is_leaf, /* Define if the voxel is a leaf or not */ 70 const uint32_t octant_mask, /* bitmask of reverted dimensions */ 71 struct svx_hit* hit) 72 { 73 double low[3], upp[3]; 74 ASSERT(oct && corner && hit); 75 ASSERT(distance_min >= 0 && distance_min <= distance_max); 76 77 hit->distance[0] = distance_min; 78 hit->distance[1] = distance_max; 79 hit->voxel.data = buffer_get_attr(&oct->buffer, iattr); 80 hit->voxel.depth = depth, 81 hit->voxel.id = buffer_absolute_attr_index(&oct->buffer, iattr); 82 hit->voxel.is_leaf = is_leaf; 83 84 /* Transform the voxel aabb in the [0, 1]^3 normalized space and flip it wrt 85 * to the octant mask of the ray */ 86 low[0] = corner[0] - 1; 87 low[1] = corner[1] - 1; 88 low[2] = corner[2] - 1; 89 if(octant_mask & 4) low[0] = 1 - low[0] - scale_exp2; 90 if(octant_mask & 2) low[1] = 1 - low[1] - scale_exp2; 91 if(octant_mask & 1) low[2] = 1 - low[2] - scale_exp2; 92 upp[0] = low[0] + scale_exp2; 93 upp[1] = low[1] + scale_exp2; 94 upp[2] = low[2] + scale_exp2; 95 96 /* Transform the voxel AABB in world space */ 97 hit->voxel.lower[0] = low[0] * oct->tree_size[0] + oct->tree_low[0]; 98 hit->voxel.lower[1] = low[1] * oct->tree_size[1] + oct->tree_low[1]; 99 hit->voxel.lower[2] = low[2] * oct->tree_size[2] + oct->tree_low[2]; 100 hit->voxel.upper[0] = upp[0] * oct->tree_size[0] + oct->tree_low[0]; 101 hit->voxel.upper[1] = upp[1] * oct->tree_size[1] + oct->tree_low[1]; 102 hit->voxel.upper[2] = upp[2] * oct->tree_size[2] + oct->tree_low[2]; 103 } 104 105 /* Return RES_BAD_OP if the ray cannot intersect the scene */ 106 static res_T 107 setup_ray 108 (const struct svx_tree* oct, 109 const double org[3], 110 const double dir[3], 111 const double range[2], 112 struct ray* ray) 113 { 114 double rcp_ocsz[3]; /* Reciprocal size of the World space octree AABB */ 115 double dir_adjusted[3]; 116 ASSERT(oct); 117 118 if(range[0] >= range[1]) return RES_BAD_OP; /* Disabled ray */ 119 120 /* Ray paralelle to an axis and that does not intersect the scene AABB */ 121 if((!dir[0] && (org[0] < oct->lower[0] || org[0] > oct->upper[0])) 122 || (!dir[1] && (org[1] < oct->lower[1] || org[1] > oct->upper[1])) 123 || (!dir[2] && (org[2] < oct->lower[2] || org[2] > oct->upper[2]))) { 124 return RES_BAD_OP; 125 } 126 127 /* Compute reciprocal size of the world space octree AABB */ 128 rcp_ocsz[0] = 1.0 / oct->tree_size[0]; 129 rcp_ocsz[1] = 1.0 / oct->tree_size[1]; 130 rcp_ocsz[2] = 1.0 / oct->tree_size[2]; 131 132 /* Transform the ray origin in the [1, 2] space */ 133 ray->org[0] = (org[0] - oct->tree_low[0]) * rcp_ocsz[0] + 1.0; 134 ray->org[1] = (org[1] - oct->tree_low[1]) * rcp_ocsz[1] + 1.0; 135 ray->org[2] = (org[2] - oct->tree_low[2]) * rcp_ocsz[2] + 1.0; 136 137 /* Setup the ray range */ 138 ray->range[0] = range[0]; 139 ray->range[1] = range[1]; 140 141 /* Transform the direction in the normalized octree space */ 142 dir_adjusted[0] = dir[0] * rcp_ocsz[0]; 143 dir_adjusted[1] = dir[1] * rcp_ocsz[1]; 144 dir_adjusted[2] = dir[2] * rcp_ocsz[2]; 145 146 /* Let a ray defined as org + t*dir and X the coordinate of an axis aligned 147 * plane. The ray intersects X at t = (X - org)/dir = (X - org) * ts; with ts 148 * = 1/dir. Note that one assume that dir is always negative. */ 149 ray->ts[0] = 1.0 / -fabs(dir_adjusted[0]); 150 ray->ts[1] = 1.0 / -fabs(dir_adjusted[1]); 151 ray->ts[2] = 1.0 / -fabs(dir_adjusted[2]); 152 153 /* Mirror rays with position directions */ 154 ray->octant_mask = 0; 155 if(dir[0] > 0) { ray->octant_mask ^= 4; ray->org[0] = 3.0 - ray->org[0]; } 156 if(dir[1] > 0) { ray->octant_mask ^= 2; ray->org[1] = 3.0 - ray->org[1]; } 157 if(dir[2] > 0) { ray->octant_mask ^= 1; ray->org[2] = 3.0 - ray->org[2]; } 158 159 /* Save the world space ray origin */ 160 ray->orgws[0] = org[0]; 161 ray->orgws[1] = org[1]; 162 ray->orgws[2] = org[2]; 163 164 /* Save the world space ray direction */ 165 ray->dirws[0] = dir[0]; 166 ray->dirws[1] = dir[1]; 167 ray->dirws[2] = dir[2]; 168 169 return RES_OK; 170 } 171 172 static res_T 173 trace_ray 174 (struct svx_tree* oct, 175 const struct ray* ray, 176 const svx_hit_challenge_T challenge, 177 const svx_hit_filter_T filter, 178 void* context, 179 struct svx_hit* hit) 180 { 181 #define SCALE_MAX 23 /* #mantisse bits */ 182 struct stack_entry { 183 struct buffer_index inode; 184 double t_max; 185 } stack[SCALE_MAX + 1/*Dummy entry use to avoid invalid read*/]; 186 struct buffer_index inode; 187 double t_min, t_max; 188 float corner[3]; 189 float scale_exp2; 190 uint32_t scale_max; 191 uint32_t scale; /* stack index */ 192 uint32_t ichild; 193 ASSERT(oct && ray && hit && oct->type == SVX_OCTREE); 194 195 *hit = SVX_HIT_NULL; /* Initialise the hit to "no intersection" */ 196 197 /* Compute the min/max ray intersection with the octree AABB in normalized 198 * space. Note that in this space, the octree AABB is in [1, 2]^3 */ 199 t_min = (2 - ray->org[0]) * ray->ts[0]; 200 t_min = MMAX((2 - ray->org[1]) * ray->ts[1], t_min); 201 t_min = MMAX((2 - ray->org[2]) * ray->ts[2], t_min); 202 t_max = (1 - ray->org[0]) * ray->ts[0]; 203 t_max = MMIN((1 - ray->org[1]) * ray->ts[1], t_max); 204 t_max = MMIN((1 - ray->org[2]) * ray->ts[2], t_max); 205 t_min = MMAX(ray->range[0], t_min); 206 t_max = MMIN(ray->range[1], t_max); 207 if(t_min >= t_max) return RES_OK; /* No intersection */ 208 209 /* Challenge the root */ 210 if(challenge) { 211 struct svx_hit hit_root; 212 struct buffer_index iattr_dummy = buffer_get_child_attr_index 213 (&oct->buffer, oct->root, 0/*arbitrarly child index*/); 214 215 /* Lower left corner of the root node in the [1, 2]^3 space */ 216 corner[0] = 1.f; 217 corner[1] = 1.f; 218 corner[2] = 1.f; 219 220 /* Use the regular setup_hit procedure by providing a dummy attribute 221 * index, and then overwrite the voxel data with the root one */ 222 setup_hit(oct, iattr_dummy, t_min, t_max, corner, 1.f/*scale_exp2*/, 223 0/*depth*/, 0/*is_leaf*/, ray->octant_mask, &hit_root); 224 hit_root.voxel.data = oct->root_attr; 225 226 if(challenge(&hit_root, ray->orgws, ray->dirws, ray->range, context)) { 227 if(!filter /* By default, i.e. with no filter, stop the traversal */ 228 || !filter(&hit_root, ray->orgws, ray->dirws, ray->range, context)) { 229 *hit = hit_root; 230 return RES_OK; /* Do not traverse the octree */ 231 } 232 } 233 } 234 235 /* Traversal initialisation */ 236 inode = oct->root; 237 scale_exp2 = 0.5f; 238 scale = SCALE_MAX - 1; 239 240 /* Define the first child id and the position of its lower left corner in the 241 * normalized octree space, i.e. in [1, 2]^3 */ 242 ichild = 0; 243 corner[0] = 1.f; 244 corner[1] = 1.f; 245 corner[2] = 1.f; 246 if((1.5 - ray->org[0])*ray->ts[0] > t_min) { ichild ^= 4; corner[0] = 1.5f; } 247 if((1.5 - ray->org[1])*ray->ts[1] > t_min) { ichild ^= 2; corner[1] = 1.5f; } 248 if((1.5 - ray->org[2])*ray->ts[2] > t_min) { ichild ^= 1; corner[2] = 1.5f; } 249 250 /* Octree traversal */ 251 scale_max = scale + 1; 252 while(scale < scale_max && t_min < t_max) { 253 const struct buffer_xnode* node = buffer_get_node(&oct->buffer, inode); 254 double t_corner[3]; 255 double t_max_corner; 256 double t_max_child; 257 uint32_t ichild_adjusted = ichild ^ ray->octant_mask; 258 uint32_t ichild_flag = (uint32_t)BIT(ichild_adjusted); 259 uint32_t istep; 260 261 /* Compute the exit point of the ray in the current child node */ 262 t_corner[0] = (corner[0] - ray->org[0])*ray->ts[0]; 263 t_corner[1] = (corner[1] - ray->org[1])*ray->ts[1]; 264 t_corner[2] = (corner[2] - ray->org[2])*ray->ts[2]; 265 t_max_corner = MMIN(MMIN(t_corner[0], t_corner[1]), t_corner[2]); 266 267 /* Traverse the current child */ 268 if((node->is_valid & ichild_flag) 269 && t_min <= (t_max_child = MMIN(t_max, t_max_corner))) { 270 const int is_leaf = (node->is_leaf & ichild_flag) != 0; 271 int go_deeper = 1; 272 273 /* If the current voxel is a leaf or if a challenge function is set, 274 * check the current hit */ 275 if(is_leaf || challenge) { 276 struct svx_hit hit_tmp; 277 const size_t depth = SCALE_MAX - scale; 278 const struct buffer_index iattr = buffer_get_child_attr_index 279 (&oct->buffer, inode, (int)ichild_adjusted); 280 281 setup_hit(oct, iattr, t_min, t_max_child, corner, scale_exp2, depth, 282 is_leaf, ray->octant_mask, &hit_tmp); 283 284 if(is_leaf 285 || challenge(&hit_tmp, ray->orgws, ray->dirws, ray->range, context)) { 286 go_deeper = 0; 287 /* Stop the traversal if no filter is defined or if the filter 288 * function returns 0 */ 289 if(!filter /* By default, i.e. with no filter, stop the traversal */ 290 || !filter(&hit_tmp, ray->orgws, ray->dirws, ray->range, context)) { 291 *hit = hit_tmp; 292 break; 293 } 294 } 295 } 296 297 if(go_deeper) { 298 double t_max_parent; 299 double t_center[3]; 300 float scale_exp2_child; 301 302 t_max_parent = t_max; 303 t_max = t_max_child; 304 305 scale_exp2_child = scale_exp2 * 0.5f; 306 307 /* center = corner - scale_exp2_child => 308 * t_center = ts*(corner + scale_exp2_child - org) 309 * t_center = t_corner + ts*scale_exp2_child 310 * Anyway we perforrm the whole computation to avoid numerical issues */ 311 t_center[0] = (corner[0] - ray->org[0] + scale_exp2_child) * ray->ts[0]; 312 t_center[1] = (corner[1] - ray->org[1] + scale_exp2_child) * ray->ts[1]; 313 t_center[2] = (corner[2] - ray->org[2] + scale_exp2_child) * ray->ts[2]; 314 315 /* Push the parent node */ 316 stack[scale].t_max = t_max_parent; 317 stack[scale].inode = inode; 318 319 /* Get the node index of the traversed child */ 320 inode = buffer_get_child_node_index 321 (&oct->buffer, inode, (int)ichild_adjusted); 322 323 /* Define the id and the lower left corner of the first grand child */ 324 ichild = 0; 325 if(t_center[0] > t_min) { ichild ^= 4; corner[0] += scale_exp2_child; } 326 if(t_center[1] > t_min) { ichild ^= 2; corner[1] += scale_exp2_child; } 327 if(t_center[2] > t_min) { ichild ^= 1; corner[2] += scale_exp2_child; } 328 329 --scale; 330 scale_exp2 = scale_exp2_child; 331 continue; 332 } 333 } 334 335 /* Define the id and the lower left corner of the next child */ 336 istep = 0; 337 if(t_corner[0] <= t_max_corner) { istep ^= 4; corner[0] -= scale_exp2; } 338 if(t_corner[1] <= t_max_corner) { istep ^= 2; corner[1] -= scale_exp2; } 339 if(t_corner[2] <= t_max_corner) { istep ^= 1; corner[2] -= scale_exp2; } 340 ichild ^= istep; 341 342 t_min = t_max_corner; /* Adjust the ray range */ 343 344 if((ichild & istep) != 0) { /* The ray exits the child. Pop the stack. */ 345 uint32_t diff_bits = 0; 346 uint32_t shift[3]; 347 348 /* The IEEE-754 encoding of a single precision floating point number `f' 349 * whose binary representation is `F' is defined as: 350 * f = (-1)^S * 2^(E-127) 351 * * (1 + For(i in [0..22]) { M & BIT(22-i) ? 2^i : 0 }) 352 * with S = F / 2^31; E = F / 2^23 and M = F & (2^23 - 1). 353 * 354 * We transformed the SVO in a normalized translated space of [1, 2]^3. 355 * As a consequence, the coordinates of the lower left `corner' of a node 356 * have always a null exponent (i.e. E = 127). In addition, note that for 357 * each dimension, the i^th bit of the mantissa M is set if the corner is 358 * greater or equal to the median split of the node at the (23-i)^th 359 * level. 360 * 361 * For instance, considering the mantissa M=0x480000 of a X coordinate of 362 * a node lower left corner. According to its binary encoding, i.e. 363 * `100 1000 0000 0000 0000 0000', one can assume that: 364 * - X >= 1 + 2^-1 365 * - X < 1 + 2^-1 + 2^-2 366 * - X < 1 + 2^-1 + 2^-3 367 * - X >= 1 + 2^-1 + 2^-4 368 * 369 * Note that we ensure that the traversal along each dimension is from 2 370 * to 1, i.e. the ray direction are always negative. To define if the 371 * median split of a node is traversed by a ray, it is thus sufficient to 372 * track the update of its corresponding bit from 1 to 0. 373 * 374 * In the following code we use this property to find the highest level 375 * into the node hierarchy whose median split was traversed by the ray. 376 * This is particularly usefull since, thanks to this information, one 377 * can pop the traversal stack directly up to this level. This remove the 378 * recursive popping and thus drastically reduced the computation cost of 379 * the 'stack pop' procedure. */ 380 if(istep & 4) diff_bits |= ftoui(corner[0]) ^ ftoui(corner[0]+scale_exp2); 381 if(istep & 2) diff_bits |= ftoui(corner[1]) ^ ftoui(corner[1]+scale_exp2); 382 if(istep & 1) diff_bits |= ftoui(corner[2]) ^ ftoui(corner[2]+scale_exp2); 383 scale = (ftoui((float)diff_bits) >> 23) - 127; 384 scale_exp2 = uitof(((scale - SCALE_MAX) + 127) << 23); 385 386 inode = stack[scale].inode; 387 t_max = stack[scale].t_max; 388 389 /* Compute the lower corner of the popped node */ 390 shift[0] = ftoui(corner[0]) >> scale; 391 shift[1] = ftoui(corner[1]) >> scale; 392 shift[2] = ftoui(corner[2]) >> scale; 393 corner[0] = uitof(shift[0] << scale); 394 corner[1] = uitof(shift[1] << scale); 395 corner[2] = uitof(shift[2] << scale); 396 397 /* Define the index of the popped node */ 398 ichild = ((shift[0] & 1) << 2) | ((shift[1] & 1) << 1) | (shift[2] & 1); 399 } 400 } 401 402 return RES_OK; 403 } 404 405 /******************************************************************************* 406 * Local function 407 ******************************************************************************/ 408 res_T 409 octree_trace_ray 410 (struct svx_tree* oct, 411 const double org[3], 412 const double dir[3], 413 const double range[2], 414 const svx_hit_challenge_T challenge, 415 const svx_hit_filter_T filter, 416 void* context, 417 struct svx_hit* hit) 418 { 419 struct ray ray; 420 res_T res = RES_OK; 421 422 if(!oct || !org || !dir || !range || !hit || oct->type != SVX_OCTREE 423 || !d3_is_normalized(dir)) { 424 res = RES_BAD_ARG; 425 goto error; 426 } 427 428 *hit = SVX_HIT_NULL; 429 430 res = setup_ray(oct, org, dir, range, &ray); 431 if(res == RES_BAD_OP) { /* The ray cannot intersect the scene. */ 432 res = RES_OK; 433 goto exit; 434 } 435 436 res = trace_ray(oct, &ray, challenge, filter, context, hit); 437 if(res != RES_OK) goto error; 438 439 exit: 440 return res; 441 error: 442 goto exit; 443 } 444 445