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