s3d_scene_view_closest_point.c (14360B)
1 /* Copyright (C) 2015-2023, 2026 |Méso|Star> (contact@meso-star.com) 2 * 3 * This file is part of Star-3D. 4 * 5 * Star-3D is free software: you can redistribute it and/or modify 6 * it under the terms of the GNU General Public License as published by 7 * the Free Software Foundation, either version 3 of the License, or 8 * (at your option) any later version. 9 * 10 * Star-3D is distributed in the hope that it will be useful, 11 * but WITHOUT ANY WARRANTY; without even the implied warranty of 12 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 13 * GNU General Public License for more details. 14 * 15 * You should have received a copy of the GNU General Public License 16 * along with Star-3D. If not, see <http://www.gnu.org/licenses/>. */ 17 18 #include "s3d.h" 19 #include "s3d_device_c.h" 20 #include "s3d_instance.h" 21 #include "s3d_geometry.h" 22 #include "s3d_mesh.h" 23 #include "s3d_scene_view_c.h" 24 #include "s3d_sphere.h" 25 26 #include <rsys/float2.h> 27 #include <rsys/float3.h> 28 #include <rsys/double2.h> 29 #include <rsys/double3.h> 30 #include <rsys/float33.h> 31 32 struct point_query_context { 33 struct RTCPointQueryContext rtc; 34 struct s3d_scene_view* scnview; 35 float radius; /* Submitted radius */ 36 void* data; /* Per point query defined data */ 37 }; 38 39 /******************************************************************************* 40 * Helper functions 41 ******************************************************************************/ 42 static INLINE double* 43 closest_point_triangle 44 (const double p[3], /* Point */ 45 const double a[3], /* 1st triangle vertex */ 46 const double b[3], /* 2nd triangle vertex */ 47 const double c[3], /* 3rd triangle vertex */ 48 double closest_pt[3], /* Closest position */ 49 double uv[2]) /* UV of the closest position */ 50 { 51 double ab[3], ac[3], ap[3], bp[3], cp[3]; 52 double d1, d2, d3, d4, d5, d6; 53 double va, vb, vc; 54 double rcp_triangle_area; 55 double v, w; 56 ASSERT(p && a && b && c && closest_pt && uv); 57 58 d3_sub(ab, b, a); 59 d3_sub(ac, c, a); 60 61 /* Check if the closest point is the triangle vertex 'a' */ 62 d3_sub(ap, p, a); 63 d1 = d3_dot(ab, ap); 64 d2 = d3_dot(ac, ap); 65 if(d1 <= 0 && d2 <= 0) { 66 uv[0] = 1; 67 uv[1] = 0; 68 return d3_set(closest_pt, a); 69 } 70 71 /* Check if the closest point is the triangle vertex 'b' */ 72 d3_sub(bp, p, b); 73 d3 = d3_dot(ab, bp); 74 d4 = d3_dot(ac, bp); 75 if(d3 >= 0 && d4 <= d3) { 76 uv[0] = 0; 77 uv[1] = 1; 78 return d3_set(closest_pt, b); 79 } 80 81 /* Check if the closest point is the triangle vertex 'c' */ 82 d3_sub(cp, p, c); 83 d5 = d3_dot(ab, cp); 84 d6 = d3_dot(ac, cp); 85 if(d6 >= 0 && d5 <= d6) { 86 uv[0] = 0; 87 uv[1] = 0; 88 return d3_set(closest_pt, c); 89 } 90 91 /* Check if the closest point is on the triangle edge 'ab' */ 92 vc = d1*d4 - d3*d2; 93 if(vc <= 0 && d1 >= 0 && d3 <= 0) { 94 const double s = d1 / (d1 - d3); 95 closest_pt[0] = a[0] + s*ab[0]; 96 closest_pt[1] = a[1] + s*ab[1]; 97 closest_pt[2] = a[2] + s*ab[2]; 98 uv[0] = 1 - s; 99 uv[1] = s; 100 return closest_pt; 101 } 102 103 /* Check if the closest point is on the triangle edge 'ac' */ 104 vb = d5*d2 - d1*d6; 105 if(vb <= 0 && d2 >= 0 && d6 <= 0) { 106 const double s = d2 / (d2 - d6); 107 closest_pt[0] = a[0] + s*ac[0]; 108 closest_pt[1] = a[1] + s*ac[1]; 109 closest_pt[2] = a[2] + s*ac[2]; 110 uv[0] = 1 - s; 111 uv[1] = 0; 112 return closest_pt; 113 } 114 115 /* Check if the closest point is on the triangle edge 'bc' */ 116 va = d3*d6 - d5*d4; 117 if(va <= 0 && (d4 - d3) >= 0 && (d5 - d6) >= 0) { 118 const double s = (d4 - d3) / ((d4 - d3) + (d5 - d6)); 119 closest_pt[0] = b[0] + s*(c[0] - b[0]); 120 closest_pt[1] = b[1] + s*(c[1] - b[1]); 121 closest_pt[2] = b[2] + s*(c[2] - b[2]); 122 uv[0] = 0; 123 uv[1] = 1 - s; 124 return closest_pt; 125 } 126 127 /* The closest point lies in the triangle: compute its barycentric 128 * coordinates */ 129 rcp_triangle_area = 1 / (va + vb + vc); 130 v = vb * rcp_triangle_area; 131 w = vc * rcp_triangle_area; 132 133 /* Save the uv barycentric coordinates */ 134 uv[0] = 1 - v - w; 135 uv[1] = v; 136 ASSERT(eq_eps(uv[0] + uv[1] + w, 1, 1.e-4)); 137 138 if(uv[0] < 0) { /* Handle precision issues */ 139 if(uv[1] > w) uv[1] += uv[0]; 140 uv[0] = 0; 141 } 142 143 /* Use the barycentric coordinates to compute the world space position of the 144 * closest point onto the triangle */ 145 closest_pt[0] = a[0] + v*ab[0] + w*ac[0]; 146 closest_pt[1] = a[1] + v*ab[1] + w*ac[1]; 147 closest_pt[2] = a[2] + v*ab[2] + w*ac[2]; 148 return closest_pt; 149 } 150 151 static bool 152 closest_point_mesh 153 (struct RTCPointQueryFunctionArguments* args, 154 struct geometry* geom, 155 struct geometry* inst, /* Can be NULL */ 156 const float radius, 157 void* query_data) 158 { 159 struct s3d_hit hit = S3D_HIT_NULL; 160 struct s3d_hit* out_hit = NULL; 161 struct hit_filter* filter = NULL; 162 const uint32_t* ids = NULL; 163 double closest_point[3]; 164 double query_pos_ws[3]; /* World space query position */ 165 double query_pos_ls[3]; /* Local space query position */ 166 double v0[3], v1[3], v2[3]; 167 float E0[3], E1[3], Ng[3]; 168 double vec[3]; 169 double uv[2]; 170 float dst; 171 float pos[3], dir[3], range[2]; 172 int flip_surface = 0; 173 ASSERT(args && geom && geom->type == GEOM_MESH && radius >= 0); 174 ASSERT(args->primID < mesh_get_ntris(geom->data.mesh)); 175 176 /* Fetch triangle indices */ 177 ids = mesh_get_ids(geom->data.mesh) + args->primID*3/*#indices per triangle*/; 178 179 /* Fetch triangle vertices */ 180 ASSERT(geom->data.mesh->attribs_type[S3D_POSITION] == S3D_FLOAT3); 181 d3_set_f3(v0, mesh_get_pos(geom->data.mesh) + ids[0]*3/*#coords per vertex*/); 182 d3_set_f3(v1, mesh_get_pos(geom->data.mesh) + ids[1]*3/*#coords per vertex*/); 183 d3_set_f3(v2, mesh_get_pos(geom->data.mesh) + ids[2]*3/*#coords per vertex*/); 184 185 /* Local copy of the query position */ 186 query_pos_ws[0] = args->query->x; 187 query_pos_ws[1] = args->query->y; 188 query_pos_ws[2] = args->query->z; 189 190 if(!args->context->instStackSize) { /* The mesh is instantiated */ 191 query_pos_ls[0] = query_pos_ws[0]; 192 query_pos_ls[1] = query_pos_ws[1]; 193 query_pos_ls[2] = query_pos_ws[2]; 194 } else { 195 const float* world2inst; 196 double a[3], b[3], c[3], tmp[3]; 197 ASSERT(args->context->instStackSize == 1); 198 ASSERT(inst && inst->type == GEOM_INSTANCE); 199 200 world2inst = args->context->world2inst[0]; 201 202 /* Transform the query position in instance space */ 203 d3_muld(a, d3_set_f3(tmp, world2inst+0), query_pos_ws[0]); 204 d3_muld(b, d3_set_f3(tmp, world2inst+4), query_pos_ws[1]); 205 d3_muld(c, d3_set_f3(tmp, world2inst+8), query_pos_ws[2]); 206 query_pos_ls[0] = a[0] + b[0] + c[0] + world2inst[12]; 207 query_pos_ls[1] = a[1] + b[1] + c[1] + world2inst[13]; 208 query_pos_ls[2] = a[2] + b[2] + c[2] + world2inst[14]; 209 210 flip_surface = inst->flip_surface; 211 } 212 213 /* Compute the closest point onto the triangle from the submitted point */ 214 closest_point_triangle(query_pos_ls, v0, v1, v2, closest_point, uv); 215 216 /* Compute the distance */ 217 d3_sub(vec, closest_point, query_pos_ls); 218 dst = (float)d3_len(vec); 219 220 /* Transform the distance in world space */ 221 if(args->context->instStackSize != 0) { 222 ASSERT(args->similarityScale > 0); 223 dst /= args->similarityScale; 224 } 225 226 if(dst >= args->query->radius) return 0; 227 228 /* Compute the triangle normal in world space (left hand convention). Keep 229 * it in float to avoid double-cast accuracy loss wrt user computed result */ 230 f3_sub(E0, mesh_get_pos(geom->data.mesh) + ids[1] * 3, 231 mesh_get_pos(geom->data.mesh) + ids[0] * 3); 232 f3_sub(E1, mesh_get_pos(geom->data.mesh) + ids[2] * 3, 233 mesh_get_pos(geom->data.mesh) + ids[0] * 3); 234 f3_cross(Ng, E1, E0); 235 236 /* Flip the geometric normal wrt the flip surface flag */ 237 flip_surface ^= geom->flip_surface; 238 if(flip_surface) f3_minus(Ng, Ng); 239 240 /* Setup the hit */ 241 hit.prim.shape__ = geom; 242 hit.prim.inst__ = inst; 243 hit.distance = dst; 244 hit.uv[0] = (float)uv[0]; 245 hit.uv[1] = (float)uv[1]; 246 hit.normal[0] = Ng[0]; 247 hit.normal[1] = Ng[1]; 248 hit.normal[2] = Ng[2]; 249 hit.prim.prim_id = args->primID; 250 hit.prim.geom_id = geom->name; 251 hit.prim.inst_id = inst ? inst->name : S3D_INVALID_ID; 252 hit.prim.scene_prim_id = 253 hit.prim.prim_id 254 + geom->scene_prim_id_offset 255 + (inst ? inst->scene_prim_id_offset : 0); 256 257 range[0] = 0; 258 range[1] = radius; 259 260 /* `dir' is the direction along which the closest point was found. We thus 261 * submit it to the filter function as the direction corresponding to the 262 * computed hit */ 263 f3_set_d3(dir, vec); 264 f3_set_d3(pos, query_pos_ws); 265 266 filter = &geom->data.mesh->filter; 267 if(filter->func 268 && filter->func(&hit, pos, dir, range, query_data, filter->data)) { 269 return 0; /* This point is filtered. Discard it! */ 270 } 271 272 /* Update output data */ 273 out_hit = args->userPtr; 274 *out_hit = hit; 275 276 /* Shrink the query radius */ 277 args->query->radius = dst; 278 279 return 1; /* Notify that the query radius was updated */ 280 } 281 282 static bool 283 closest_point_sphere 284 (struct RTCPointQueryFunctionArguments* args, 285 struct geometry* geom, 286 struct geometry* inst, 287 const float radius, /* User defined radius */ 288 void* query_data) 289 { 290 struct s3d_hit hit = S3D_HIT_NULL; 291 struct s3d_hit* out_hit = NULL; 292 struct hit_filter* filter = NULL; 293 float query_pos[3]; 294 float sphere_pos[3]; 295 float Ng[3]; 296 float dir[3], range[2]; 297 float uv[2]; 298 float dst; 299 float len; 300 int flip_surface = 0; 301 ASSERT(args && geom && geom->type == GEOM_SPHERE && args->primID == 0); 302 ASSERT(radius >= 0); 303 304 /* Local copy of the query position */ 305 query_pos[0] = args->query->x; 306 query_pos[1] = args->query->y; 307 query_pos[2] = args->query->z; 308 309 f3_set(sphere_pos, geom->data.sphere->pos); 310 if(args->context->instStackSize) { /* The sphere is instantiated */ 311 const float* transform; 312 transform = inst->data.instance->transform; 313 ASSERT(args->context->instStackSize == 1); 314 ASSERT(inst && inst->type == GEOM_INSTANCE); 315 ASSERT(f3_eq_eps(transform+0, args->context->inst2world[0]+0, 1.e-6f)); 316 ASSERT(f3_eq_eps(transform+3, args->context->inst2world[0]+4, 1.e-6f)); 317 ASSERT(f3_eq_eps(transform+6, args->context->inst2world[0]+8, 1.e-6f)); 318 ASSERT(f3_eq_eps(transform+9, args->context->inst2world[0]+12,1.e-6f)); 319 320 /* Transform the sphere position in world space */ 321 f33_mulf3(sphere_pos, transform, sphere_pos); 322 f3_add(sphere_pos, transform+9, sphere_pos); 323 324 flip_surface = inst->flip_surface; 325 } 326 327 /* Compute the distance from the query pos to the sphere center */ 328 f3_sub(Ng, query_pos, sphere_pos); 329 len = f3_len(Ng); 330 331 /* Evaluate the distance from the query pos to the sphere surface */ 332 dst = fabsf(len - geom->data.sphere->radius); 333 334 /* The closest point onto the sphere is outside the query radius */ 335 if(dst >= args->query->radius) 336 return 0; 337 338 /* Normalize the hit normal */ 339 if(len > 0) { 340 f3_divf(Ng, Ng, len); 341 } else { 342 /* The query position is equal to the sphere center. Arbitrarily choose the 343 * point along the +X axis as the closest point */ 344 Ng[0] = 1.f; 345 Ng[1] = 0.f; 346 Ng[2] = 0.f; 347 } 348 349 /* Compute the uv of the found point */ 350 sphere_normal_to_uv(Ng, uv); 351 352 /* Flip the geometric normal wrt the flip surface flag */ 353 flip_surface ^= geom->flip_surface; 354 if(flip_surface) f3_minus(Ng, Ng); 355 356 /* Setup the hit */ 357 hit.prim.shape__ = geom; 358 hit.prim.inst__ = inst; 359 hit.distance = dst; 360 hit.uv[0] = uv[0]; 361 hit.uv[1] = uv[1]; 362 hit.normal[0] = Ng[0]; 363 hit.normal[1] = Ng[1]; 364 hit.normal[2] = Ng[2]; 365 hit.prim.prim_id = args->primID; 366 hit.prim.geom_id = geom->name; 367 hit.prim.inst_id = inst ? inst->name : S3D_INVALID_ID; 368 hit.prim.scene_prim_id = 369 hit.prim.prim_id 370 + geom->scene_prim_id_offset 371 + (inst ? inst->scene_prim_id_offset : 0); 372 373 range[0] = 0; 374 range[1] = radius; 375 376 /* Use the reversed geometric normal as the hit direction since it is along 377 * this vector that the closest point was effectively computed */ 378 f3_minus(dir, Ng); 379 filter = &geom->data.sphere->filter; 380 if(filter->func 381 && filter->func(&hit, query_pos, dir, range, query_data, filter->data)) { 382 return 0; 383 } 384 385 /* Update output data */ 386 out_hit = args->userPtr; 387 *out_hit = hit; 388 389 /* Shrink the query radius */ 390 args->query->radius = dst; 391 392 return 1; /* Notify that the query radius was updated */ 393 } 394 395 static bool 396 closest_point(struct RTCPointQueryFunctionArguments* args) 397 { 398 struct point_query_context* ctx = NULL; 399 struct geometry* geom = NULL; 400 struct geometry* inst = NULL; 401 bool query_radius_is_upd = false; 402 ASSERT(args); 403 404 ctx = CONTAINER_OF(args->context, struct point_query_context, rtc); 405 if(args->context->instStackSize == 0) { 406 geom = scene_view_geometry_from_embree_id 407 (ctx->scnview, args->geomID); 408 } else { 409 ASSERT(args->context->instStackSize == 1); 410 inst = scene_view_geometry_from_embree_id 411 (ctx->scnview, args->context->instID[0]); 412 geom = scene_view_geometry_from_embree_id 413 (inst->data.instance->scnview, args->geomID); 414 } 415 416 switch(geom->type) { 417 case GEOM_MESH: 418 query_radius_is_upd = closest_point_mesh 419 (args, geom, inst, ctx->radius, ctx->data); 420 break; 421 case GEOM_SPHERE: 422 query_radius_is_upd = closest_point_sphere 423 (args, geom, inst, ctx->radius, ctx->data); 424 break; 425 default: FATAL("Unreachable code\n"); break; 426 } 427 return query_radius_is_upd; 428 } 429 430 /******************************************************************************* 431 * Exported functions 432 ******************************************************************************/ 433 res_T 434 s3d_scene_view_closest_point 435 (struct s3d_scene_view* scnview, 436 const float pos[3], 437 const float radius, 438 void* query_data, 439 struct s3d_hit* hit) 440 { 441 struct RTCPointQuery query; 442 struct point_query_context query_ctx; 443 444 if(!scnview || !pos || radius <= 0 || !hit) 445 return RES_BAD_ARG; 446 if((scnview->mask & S3D_TRACE) == 0) { 447 log_error(scnview->scn->dev, 448 "%s: the S3D_TRACE flag is not active onto the submitted scene view.\n", 449 FUNC_NAME); 450 return RES_BAD_OP; 451 } 452 453 *hit = S3D_HIT_NULL; 454 455 /* Initialise the point query */ 456 query.x = pos[0]; 457 query.y = pos[1]; 458 query.z = pos[2]; 459 query.radius = radius; 460 query.time = FLT_MAX; /* Invalid fields */ 461 462 /* Initialise the point query context */ 463 rtcInitPointQueryContext(&query_ctx.rtc); 464 query_ctx.scnview = scnview; 465 query_ctx.radius = radius; 466 query_ctx.data = query_data; 467 468 /* Here we go! */ 469 rtcPointQuery(scnview->rtc_scn, &query, &query_ctx.rtc, closest_point, hit); 470 471 return RES_OK; 472 } 473