suvm_volume_at.c (6336B)
1 /* Copyright (C) 2020-2023 |Méso|Star> (contact@meso-star.com) 2 * 3 * This program is free software: you can redistribute it and/or modify 4 * it under the terms of the GNU General Public License as published by 5 * the Free Software Foundation, either version 3 of the License, or 6 * (at your option) any later version. 7 * 8 * This program is distributed in the hope that it will be useful, 9 * but WITHOUT ANY WARRANTY; without even the implied warranty of 10 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 11 * GNU General Public License for more details. 12 * 13 * You should have received a copy of the GNU General Public License 14 * along with this program. If not, see <http://www.gnu.org/licenses/>. */ 15 16 #include "suvm.h" 17 #include "suvm_device.h" 18 #include "suvm_volume.h" 19 20 /******************************************************************************* 21 * Helper functions 22 ******************************************************************************/ 23 static FINLINE int 24 pos_in_aabb 25 (const float pos[3], 26 const float low[3], 27 const float upp[3]) 28 { 29 return low[0] <= pos[0] && upp[0] >= pos[0] 30 && low[1] <= pos[1] && upp[1] >= pos[1] 31 && low[2] <= pos[2] && upp[2] >= pos[2]; 32 } 33 34 /* Return 1 if pos lies is included into the leaf or 0 otherwise */ 35 static int 36 intersect_leaf 37 (const struct suvm_volume* vol, 38 const struct node_leaf* leaf, 39 const float pos[3], 40 float bcoords[4]) /* Barycentric coordinates of pos into the leaf */ 41 { 42 const uint32_t* tetra; 43 const float *v0, *v1, *v2, *v3; 44 float n0[3], n1[3], n2[3], n3[3]; 45 float p[3]; 46 float d0, d1, d2, d3; 47 float h1, h2, h3; 48 size_t itetra; 49 ASSERT(vol && leaf && pos && bcoords); 50 ASSERT(leaf->prim_id < volume_get_primitives_count(vol)); 51 52 itetra = leaf->prim_id; 53 tetra = darray_u32_cdata_get(&vol->indices) + itetra*4/*#vertices per tetra*/; 54 55 /* Fetch the first tetrahedron vertex */ 56 v0 = darray_float_cdata_get(&vol->positions) + tetra[0]*3; 57 58 /* Compute the distance from pos to the bottom facet */ 59 volume_primitive_get_facet_normal(vol, itetra, 0, n0); 60 d0 = f3_dot(n0, f3_sub(p, pos, v0)); 61 if(d0 < 0) return 0; 62 63 /* Fetch the top tetrahedron vertex */ 64 v3 = darray_float_cdata_get(&vol->positions) + tetra[3]*3; 65 f3_sub(p, pos, v3); 66 67 /* Compute the distance from pos to the 'side' facets */ 68 volume_primitive_get_facet_normal(vol, itetra, 1, n1); 69 d1 = f3_dot(n1, p); 70 if(d1 < 0) return 0; 71 volume_primitive_get_facet_normal(vol, itetra, 2, n2); 72 d2 = f3_dot(n2, p); 73 if(d2 < 0) return 0; 74 volume_primitive_get_facet_normal(vol, itetra, 3, n3); 75 d3 = f3_dot(n3, p); 76 if(d3 < 0) return 0; 77 78 /* Fetch the two remaining vertices (i.e. the second and third ones) */ 79 v1 = darray_float_cdata_get(&vol->positions) + tetra[1]*3; 80 v2 = darray_float_cdata_get(&vol->positions) + tetra[2]*3; 81 82 /* Distance of tetrahedron corners to their opposite face */ 83 h1 = f3_dot(n1, f3_sub(p, v2, v1)); 84 h2 = f3_dot(n2, f3_sub(p, v0, v2)); 85 h3 = f3_dot(n3, f3_sub(p, v1, v3)); 86 87 /* Compute the barycentric coordinates, i.e. ratio of distances */ 88 bcoords[0] = d2 / h2; 89 bcoords[1] = d3 / h3; 90 bcoords[2] = d1 / h1; 91 92 #if 1 93 /* Do not use d0 and h0 to calculate the last barycentric coordinate but 94 * calculate it from the 3 others. This gets around the numerical imprecision 95 * and ensures that the sum of the bcoords is indeed 1 */ 96 bcoords[3] = 1.f - bcoords[0] - bcoords[1] - bcoords[2]; 97 #else 98 { 99 const float h0 = f3_dot(n0, f3_sub(p, v3, v0)); 100 bcoords[3] = d0 / h0; 101 } 102 #endif 103 104 return 1; 105 } 106 107 /******************************************************************************* 108 * Exported function 109 ******************************************************************************/ 110 res_T 111 suvm_volume_at 112 (struct suvm_volume* vol, 113 const double pos_in[3], 114 struct suvm_primitive* prim, 115 double barycentric_coords[4]) 116 { 117 struct darray_node stack; 118 struct node* node = NULL; 119 struct node_leaf* leaf = NULL; 120 struct node_inner* inner = NULL; 121 float pos[3]; 122 size_t iprim = SIZE_MAX; /* Index of the hit primitive */ 123 int stack_is_init = 0; 124 res_T res = RES_OK; 125 126 if(!vol || !pos_in || !prim || !barycentric_coords) { 127 res = RES_BAD_ARG; 128 goto error; 129 } 130 *prim = SUVM_PRIMITIVE_NULL; 131 pos[0] = (float)pos_in[0]; 132 pos[1] = (float)pos_in[1]; 133 pos[2] = (float)pos_in[2]; 134 darray_node_init(vol->dev->allocator, &stack); 135 stack_is_init = 1; 136 137 node = vol->bvh_root; 138 ASSERT(node); 139 140 /* Is the position included in the AABB of the volume */ 141 if(!pos_in_aabb(pos, vol->low, vol->upp)) { 142 goto exit; 143 } 144 145 res = darray_node_reserve(&stack, 32); 146 if(res != RES_OK) goto error; 147 148 for(;;) { 149 size_t istack; 150 151 if(node->type == NODE_LEAF) { 152 float bcoords[4]; 153 leaf = CONTAINER_OF(node, struct node_leaf, node); 154 if(intersect_leaf(vol, leaf, pos, bcoords)) { 155 iprim = leaf->prim_id; 156 barycentric_coords[0] = (double)bcoords[0]; 157 barycentric_coords[1] = (double)bcoords[1]; 158 barycentric_coords[2] = (double)bcoords[2]; 159 barycentric_coords[3] = (double)bcoords[3]; 160 break; 161 } 162 163 } else { 164 int traverse_child[2] = {0,0}; 165 inner = CONTAINER_OF(node, struct node_inner, node); 166 167 /* Check pos against the children's AABBs */ 168 traverse_child[0] = pos_in_aabb(pos, inner->low[0], inner->upp[0]); 169 traverse_child[1] = pos_in_aabb(pos, inner->low[1], inner->upp[1]); 170 171 if(traverse_child[0] && traverse_child[1]) { /* Traverse both children */ 172 res = darray_node_push_back(&stack, &inner->children[1]); 173 if(UNLIKELY(res != RES_OK)) goto error; 174 node = inner->children[0]; 175 continue; 176 177 } else if(traverse_child[0]) { /* Traverse only child 0 */ 178 node = inner->children[0]; 179 continue; 180 181 } else if(traverse_child[1]) { /* Traverse only child 1 */ 182 node = inner->children[1]; 183 continue; 184 } 185 } 186 187 istack = darray_node_size_get(&stack); 188 if(!istack) break; /* No more stack entry: stop traversal */ 189 190 /* Pop the stack */ 191 node = darray_node_data_get(&stack)[istack-1]; 192 darray_node_pop_back(&stack); 193 } 194 195 /* pos lies into a tetrahedron */ 196 if(iprim != SIZE_MAX) { 197 volume_primitive_setup(vol, iprim, prim); 198 } 199 200 exit: 201 if(stack_is_init) darray_node_release(&stack); 202 return res; 203 error: 204 goto exit; 205 } 206