svx_bintree_trace_ray.c (10718B)
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 /******************************************************************************* 23 * Helper function 24 ******************************************************************************/ 25 static FINLINE void 26 setup_hit 27 (struct svx_tree* btree, 28 const struct buffer_index iattr, /* Index toward the voxel attributes */ 29 const double distance_min, /* Dst from ray org to the voxel entry point */ 30 const double distance_max, /* Dst from ray_org to the voxel exit point */ 31 const double lower, /* Lower bound of the current voxel in [0, 1] space */ 32 const double scale_exp2, /* Size of the voxel in [0, 1] space */ 33 const size_t depth, /* Depth of the voxel in the octree hierarchy */ 34 const int is_leaf, /* Define if the voxel is a leaf or not */ 35 const int flip, /* Define if the btree was reverted or not */ 36 struct svx_hit* hit) 37 { 38 enum svx_axis axis; 39 double low; 40 double upp; 41 ASSERT(btree && hit); 42 ASSERT(distance_min >= 0 && distance_min <= distance_max); 43 44 hit->distance[0] = distance_min; 45 hit->distance[1] = distance_max; 46 hit->voxel.data = buffer_get_attr(&btree->buffer, iattr); 47 hit->voxel.depth = depth, 48 hit->voxel.id = buffer_absolute_attr_index(&btree->buffer, iattr); 49 hit->voxel.is_leaf = is_leaf; 50 51 /* Transform the voxel aabb in the [0, 1]^3 normalized space and flip if 52 * necessary */ 53 low = flip ? 1 - lower - scale_exp2 : lower; 54 upp = low + scale_exp2; 55 56 /* Transform the voxel AABB in world space */ 57 axis = btree->frame[0]; 58 d3_splat(hit->voxel.lower, -INF); 59 d3_splat(hit->voxel.upper, INF); 60 hit->voxel.lower[axis] = low * btree->tree_size[axis] + btree->tree_low[axis]; 61 hit->voxel.upper[axis] = upp * btree->tree_size[axis] + btree->tree_low[axis]; 62 } 63 64 /******************************************************************************* 65 * Exported function 66 ******************************************************************************/ 67 res_T 68 bintree_trace_ray 69 (struct svx_tree* btree, 70 const double ray_org[3], 71 const double ray_dir[3], 72 const double ray_range[2], 73 const svx_hit_challenge_T challenge, 74 const svx_hit_filter_T filter, 75 void* context, 76 struct svx_hit* hit) 77 { 78 #define STACK_LENGTH 23 /* #mantissa bits */ 79 struct stack_entry { 80 float scale_exp2; 81 uint32_t depth; 82 struct buffer_index inode; 83 }; 84 ALIGN(64) struct stack_entry stack[STACK_LENGTH]; 85 STATIC_ASSERT(sizeof(struct stack_entry) == 16, Unexpected_sizeof_stack_entry); 86 87 struct buffer_index inode; 88 size_t istack; /* Top of the stack */ 89 size_t iaxis; /* Id in [0, 2] of the axis along which the tree is defined */ 90 double rdir[3]; /* Ray direction adjusted wrt numerical problems */ 91 double pos_min, pos_max; /* Min/Max pos along the ray in [0,1] +dir 1D space */ 92 double org; /* Ray origin in the [0,1] +dir 1D space */ 93 double dir; /* Ray direction in the [0,1] +dir 1D space */ 94 double ts; /* 1/(Ray direction) in the [0,1] +dir 1D space */ 95 double rcp_btreesz; /* Reciprocal of the binary tree size */ 96 double low; /* Lower bound of the traversed node */ 97 float scale_exp2; /* Current size of a node in the [0,1] +dir 1D space */ 98 uint32_t depth; /* Depth of the traversed nodes */ 99 int ichild; /* Traversed child */ 100 int flip = 0; /* Is the ray flipped? */ 101 int null_dir = 0; /* Is the 1D dir is roughly null? */ 102 103 if(!btree || !ray_org || !ray_dir || !ray_range || btree->type != SVX_BINTREE 104 || !d3_is_normalized(ray_dir) || !hit) { 105 return RES_BAD_ARG; 106 } 107 108 *hit = SVX_HIT_NULL; 109 if(ray_range[0] >= ray_range[1]) { /* Disabled ray */ 110 return RES_OK; 111 } 112 113 iaxis = btree->frame[0]; 114 rcp_btreesz = 1.0 / (double)btree->tree_size[iaxis]; 115 ASSERT(rcp_btreesz > 0); 116 117 d3_set(rdir, ray_dir); 118 119 /* Define whether the direction of the 1D ray is approximately aligned with the 120 * infinite dimension. If it is, make sure it's really aligned so that the 121 * caller can't have a ray that escapes the voxel from which the ray starts. 122 * Indeed, even with an approximately zero direction along the 1D axis, the ray 123 * can still cross the boundary of a voxel if the caller decides to make it 124 * travel long distances. */ 125 null_dir = eq_eps(ray_dir[iaxis], 0, 1.e-6); 126 if(null_dir) { 127 rdir[iaxis] = 0; 128 d3_normalize(rdir, rdir); 129 } 130 131 /* Transform the ray origin in [0, 1] space */ 132 org = (ray_org[iaxis] - btree->tree_low[iaxis]) * rcp_btreesz; 133 /* Transform the direction in the normalized bintree space */ 134 dir = (rdir[iaxis] * rcp_btreesz); 135 136 /* The ray starts outside the binary tree and point outward the bin tree: it 137 * cannot intersect the binary tree */ 138 if((org > 1 && dir >= 0) 139 || (org < 0 && dir <= 0)) 140 return RES_OK; 141 142 /* Mirror rays with negative direction */ 143 if(dir < 0) { 144 flip = 1; 145 org = 1 - org; 146 dir = -dir; 147 } 148 149 /* Let a ray r defined as O + tD with O the ray origin and D the direction; 150 * and X an axis aligned plane. r intersects X at a distance t computed as 151 * below : 152 * t = (X-O) / D 153 * Note that one can transform r in r' with a transform M without any impact 154 * on the t parameter: 155 * M.X = M.O * t(M.D) 156 * t = (M.X-M.O)/(M.D) = (M.X-M.O)*ts; with ts = 1/(M.D) */ 157 ts = null_dir ? INF : 1.f / dir; 158 159 /* Compute the range in [0, 1] of the ray/binary tree intersection */ 160 pos_min = MMAX(dir * ray_range[0] + org, 0); 161 pos_max = MMIN(dir * ray_range[1] + org, 1); 162 163 /* Challenge the root node */ 164 if(challenge) { 165 struct svx_hit hit_root; 166 const double t_min = null_dir ? ray_range[0] : (pos_min - org) * ts; 167 const double t_max = null_dir ? ray_range[1] : (pos_max - org) * ts; 168 struct buffer_index iattr_dummy = buffer_get_child_attr_index 169 (&btree->buffer, btree->root, 0/*arbitrarly child index*/); 170 171 /* Use the regular setup_hit procedure by providing a dummy attribute 172 * index, and then overwrite the voxel data with the root one */ 173 setup_hit(btree, iattr_dummy, t_min, t_max, 0.f/*low*/, 1.f/*scale_exp2*/, 174 0/*depth*/, 0/*is_leaf*/, flip, &hit_root); 175 hit_root.voxel.data = btree->root_attr; 176 177 if(challenge(&hit_root, ray_org, rdir, ray_range, context)) { 178 if(!filter /* By default, i.e. with no filter, stop the traversal */ 179 || !filter(&hit_root, ray_org, rdir, ray_range, context)) { 180 *hit = hit_root; 181 return RES_OK; /* Do not traverse the binary tree */ 182 } 183 } 184 } 185 186 /* Define the first traversed child and set its lower bound */ 187 if(pos_min <= 0.5) { 188 /* Note that we use less than or *equal* in the previous test to be 189 * consistent with the svx_tree_at function: a position onto a boundary is 190 * owned by the node before the boundary */ 191 ichild = 0; 192 low = 0.0; 193 } else { 194 ichild = 1; 195 low = 0.5; 196 } 197 198 /* Init the traversal */ 199 depth = 1; 200 istack = 0; 201 scale_exp2 = 0.5f; 202 inode = btree->root; 203 204 /* Here we go */ 205 while(pos_min < pos_max) { 206 const struct buffer_xnode* node = buffer_get_node(&btree->buffer, inode); 207 const int ichild_adjusted = ichild ^ flip; 208 const int ichild_flag = BIT(ichild_adjusted); 209 210 if(node->is_valid & ichild_flag) { 211 const int is_leaf = (node->is_leaf & ichild_flag) != 0; 212 int go_deeper = 1; 213 214 if(is_leaf || challenge) { 215 struct svx_hit hit_tmp; 216 const double upp = MMIN(low + scale_exp2, pos_max) ; 217 const double t_min = null_dir ? ray_range[0] : (pos_min - org) * ts; 218 const double t_max = null_dir ? ray_range[1] : (upp - org) * ts; 219 const struct buffer_index iattr = buffer_get_child_attr_index 220 (&btree->buffer, inode, ichild_adjusted); 221 ASSERT(t_min <= t_max); 222 223 setup_hit(btree, iattr, t_min, t_max, low, scale_exp2, depth, is_leaf, 224 flip, &hit_tmp); 225 226 if(is_leaf 227 || challenge(&hit_tmp, ray_org, rdir, ray_range, context)) { 228 go_deeper = 0; 229 /* Stop the traversal if no filter is defined or if the filter 230 * function returns 0 */ 231 if(!filter /* By default, i.e. with no filter, stop the traversal */ 232 || !filter(&hit_tmp, ray_org, rdir, ray_range, context)) { 233 *hit = hit_tmp; 234 break; 235 } 236 237 /* Stop traversal if no more voxel can be traversed */ 238 if(null_dir) break; 239 } 240 } 241 242 if(go_deeper) { 243 const float scale_exp2_child = scale_exp2 * 0.5f; 244 const double child_mid = low + scale_exp2_child; 245 246 /* Push parent node if necessary */ 247 if(ichild == 0 && !null_dir) { 248 stack[istack].inode = inode; 249 stack[istack].scale_exp2 = scale_exp2; 250 stack[istack].depth = depth; 251 ++istack; 252 } 253 254 /* Get the node index of the traversed child */ 255 inode = buffer_get_child_node_index 256 (&btree->buffer, inode, (int)ichild_adjusted); 257 258 ichild = pos_min > child_mid; 259 if(ichild == 1) low = child_mid; 260 scale_exp2 = scale_exp2_child; 261 ++depth; 262 continue; 263 } 264 } 265 266 /* Purse traversal */ 267 ASSERT(pos_min >= low && pos_min <= low + scale_exp2); 268 low += scale_exp2; /* Lower bound of the next node to traverse */ 269 pos_min = low; /* Snap the pos_min to the next node lower bound */ 270 ++ichild; 271 272 if(ichild > 1) { /* No more child to traverse in this node => Pop node */ 273 if(istack == 0) break; /* No more node to traverse */ 274 275 /* Pop node */ 276 --istack; 277 inode = stack[istack].inode; 278 scale_exp2 = stack[istack].scale_exp2; 279 depth = stack[istack].depth; 280 281 /* A node is pushed on the stack if one of its child was not traversed. 282 * Furthermore, this intersector ensures that the ray dir is alwas 283 * positive along the axis of the binary tree. As a consequence, the 284 * child 0 is always traversed first and the remaining child to traverse 285 * of a pushed node is thus always the child 1 */ 286 ichild = 1; 287 } 288 } 289 return RES_OK; 290 }