test_s2d_closest_point.c (17132B)
1 /* Copyright (C) 2016-2021, 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 #define _POSIX_C_SOURCE 200112L /* nextafterf, exp2f, fabsf */ 17 18 #include "s2d.h" 19 #include "test_s2d_utils.h" 20 21 #include <rsys/float2.h> 22 23 #define ON_VERTEX_EPSILON 1.e-4f 24 #define POSITION_EPSILON 1.e-3f 25 26 struct closest_pt { 27 float pos[2]; 28 float normal[2]; 29 float dst; 30 unsigned iprim; 31 unsigned igeom; 32 }; 33 34 #define CLOSEST_PT_NULL__ { \ 35 {0,0}, \ 36 {0,0}, \ 37 FLT_MAX, \ 38 S2D_INVALID_ID, \ 39 S2D_INVALID_ID, \ 40 } 41 42 static const struct closest_pt CLOSEST_PT_NULL = CLOSEST_PT_NULL__; 43 44 /******************************************************************************* 45 * Helper functions 46 ******************************************************************************/ 47 /* Function that computes the point onto the segment that ensures the minimum 48 * distance between the submitted `pos' and the segment. Use this routine to 49 * cross check the result of the s2d_scene_view_closest_point function */ 50 static float* 51 closest_point_segment 52 (const float p[2], /* Input pos */ 53 const float a[2], /* 1st segment vertex */ 54 const float b[2], /* 2nd segment vertex */ 55 float pt[2]) /* Closest point */ 56 { 57 float v[2]; 58 float E[2]; 59 float dst; 60 float len; 61 62 f2_sub(v, p, a); 63 f2_sub(E, b, a); 64 len = f2_len(E); 65 dst = f2_dot(v, E) / len; 66 67 if(dst <= 0) return f2_set(pt, a); 68 if(dst >= len) return f2_set(pt, b); 69 70 f2_normalize(E, E); 71 f2_mulf(pt, E, dst); 72 f2_add(pt, pt, a); 73 return pt; 74 } 75 76 static void 77 closest_point_line_segments 78 (const float pos[2], 79 const float* verts, 80 const unsigned* ids, 81 const unsigned nsegs, 82 const unsigned geom_id, 83 const unsigned prim_to_filter, 84 struct closest_pt* pt) 85 { 86 unsigned iseg; 87 CHK(pos && verts && ids && pt); 88 89 *pt = CLOSEST_PT_NULL; 90 pt->igeom = geom_id; 91 pt->dst = FLT_MAX; 92 93 /* Find the closest point on the mesh */ 94 FOR_EACH(iseg, 0, nsegs) { 95 float v0[2]; 96 float v1[2]; 97 float closest_pt[2]; 98 float vec[2]; 99 float dst; 100 101 if(iseg == prim_to_filter) continue; 102 103 f2_set(v0, verts+ids[iseg*2+0]*2); 104 f2_set(v1, verts+ids[iseg*2+1]*2); 105 106 closest_point_segment(pos, v0, v1, closest_pt); 107 dst = f2_len(f2_sub(vec, closest_pt, pos)); 108 109 if(dst < pt->dst) { 110 f2_set(pt->pos, closest_pt); 111 pt->dst = dst; 112 pt->iprim = iseg; 113 pt->normal[0] = v1[1] - v0[1]; 114 pt->normal[1] = -v1[0] + v0[0]; 115 f2_normalize(pt->normal, pt->normal); 116 } 117 } 118 } 119 120 static INLINE int 121 hit_on_vertex(const struct s2d_hit* hit) 122 { 123 struct s2d_attrib v0, v1, pos; 124 float E[2]; 125 float hit_pos[2]; 126 float segment_len; 127 float hit_len0; 128 float hit_len1; 129 ASSERT(hit && !S2D_HIT_NONE(hit)); 130 131 /* Rertieve the segment vertices */ 132 S2D(segment_get_vertex_attrib(&hit->prim, 0, S2D_POSITION, &v0)); 133 S2D(segment_get_vertex_attrib(&hit->prim, 1, S2D_POSITION, &v1)); 134 135 /* Compute the length of the segment */ 136 segment_len = f2_len(f2_sub(E, v1.value, v0.value)); 137 138 /* Compute the hit position */ 139 CHK(s2d_primitive_get_attrib(&hit->prim, S2D_POSITION, hit->u, &pos) == RES_OK); 140 f2_set(hit_pos, pos.value); 141 142 /* Compute the length from hit position to segment vertices */ 143 hit_len0 = f2_len(f2_sub(E, v0.value, hit_pos)); 144 hit_len1 = f2_len(f2_sub(E, v1.value, hit_pos)); 145 146 if(hit_len0 / segment_len < ON_VERTEX_EPSILON 147 || hit_len1 / segment_len < ON_VERTEX_EPSILON) 148 return 1; 149 return 0; 150 } 151 152 static void 153 check_closest_point 154 (const struct s2d_hit* hit, 155 const struct closest_pt* pt) 156 { 157 struct s2d_attrib attr; 158 float N[2]; 159 160 CHK(hit && pt); 161 CHK(!S2D_HIT_NONE(hit)); 162 163 CHK(s2d_primitive_get_attrib 164 (&hit->prim, S2D_POSITION, hit->u, &attr) == RES_OK); 165 f2_normalize(N, hit->normal); 166 167 if(!hit_on_vertex(hit)) { 168 CHK(hit->prim.prim_id == pt->iprim); 169 } 170 171 if(hit->prim.prim_id == pt->iprim 172 && hit->prim.geom_id == pt->igeom) { 173 /* Due to numerical inaccuracies and/or the arbitrary order in which 174 * primitives are treated, 2 points on different primitive can have the 175 * same distance from the query position while their respective 176 * coordinates are not equal wrt POSITION_EPSILON. To avoid wrong 177 * assertion, we thus check the position returned by Star-2D against the 178 * manually computed position only if these positions lies on the same 179 * primitive */ 180 CHK(f2_eq_eps(pt->pos, attr.value, POSITION_EPSILON)); 181 CHK(f2_eq_eps(pt->normal, N, 1.e-4f)); 182 } 183 CHK(eq_epsf(hit->distance, pt->dst, 1.e-3f)); 184 } 185 186 /******************************************************************************* 187 * Square test 188 ******************************************************************************/ 189 struct square_filter_context { 190 float query_pos[2]; 191 float query_radius; 192 unsigned prim_to_filter; 193 }; 194 195 static int 196 square_filter 197 (const struct s2d_hit* hit, 198 const float org[2], 199 const float dir[2], 200 const float range[2], 201 void* query_data, 202 void* filter_data) 203 { 204 struct square_filter_context* ctx = query_data; 205 struct s2d_attrib attr; 206 float pos[3]; 207 float vec[3]; 208 209 CHK(hit && org && dir && range && !S2D_HIT_NONE(hit)); 210 CHK((intptr_t)filter_data == (intptr_t)0xDECAFBAD); 211 CHK(f2_normalize(vec, dir) != 0); 212 213 f2_add(pos, org, f2_mulf(pos, vec, hit->distance)); 214 CHK(s2d_primitive_get_attrib 215 (&hit->prim, S2D_POSITION, hit->u, &attr) == RES_OK); 216 CHK(f2_eq_eps(attr.value, pos, POSITION_EPSILON)); 217 218 if(!query_data) return 0; 219 220 CHK(f2_eq_eps(ctx->query_pos, org, POSITION_EPSILON)); 221 CHK(range[0] == 0); 222 CHK(range[1] == ctx->query_radius); 223 224 return ctx->prim_to_filter == hit->prim.prim_id; 225 } 226 227 static void 228 check_closest_point_square 229 (const float pos[2], 230 unsigned igeom, 231 unsigned prim_to_filter, 232 struct s2d_hit* hit) 233 { 234 struct closest_pt pt = CLOSEST_PT_NULL; 235 closest_point_line_segments 236 (pos, square_verts, square_ids, square_nsegs, igeom, prim_to_filter, &pt); 237 check_closest_point(hit, &pt); 238 } 239 240 static void 241 test_square(struct s2d_device* dev) 242 { 243 struct square_filter_context filter_ctx; 244 struct s2d_vertex_data vdata = S2D_VERTEX_DATA_NULL; 245 struct line_segments_desc desc; 246 struct s2d_hit hit = S2D_HIT_NULL; 247 struct s2d_scene* scn = NULL; 248 struct s2d_scene_view* view = NULL; 249 struct s2d_shape* shape = NULL; 250 void* ptr = (void*)((intptr_t)0xDECAFBAD); 251 float low[2], upp[2], mid[2]; 252 float pos[2]; 253 unsigned igeom; 254 size_t i; 255 256 /* Create the Star-2D scene */ 257 CHK(s2d_scene_create(dev, &scn) == RES_OK); 258 CHK(s2d_shape_create_line_segments(dev, &shape) == RES_OK); 259 CHK(s2d_shape_get_id(shape, &igeom) == RES_OK); 260 CHK(s2d_line_segments_set_hit_filter_function 261 (shape, square_filter, ptr) == RES_OK); 262 CHK(s2d_scene_attach_shape(scn, shape) == RES_OK); 263 264 vdata.usage = S2D_POSITION; 265 vdata.type = S2D_FLOAT2; 266 vdata.get = line_segments_get_position; 267 268 /* Setup the square */ 269 desc.vertices = square_verts; 270 desc.indices = square_ids; 271 CHK(s2d_line_segments_setup_indexed_vertices(shape, square_nsegs, 272 line_segments_get_ids, square_nverts, &vdata, 1, &desc) == RES_OK); 273 274 CHK(s2d_scene_view_create(scn, S2D_TRACE, &view) == RES_OK); 275 CHK(s2d_scene_view_get_aabb(view, low, upp) == RES_OK); 276 mid[0] = (low[0] + upp[0]) * 0.5f; 277 mid[1] = (low[1] + upp[1]) * 0.5f; 278 279 /* Check the closest point query on the square */ 280 FOR_EACH(i, 0, 10000) { 281 /* Randomly generate a point in a bounding box that is 2 times the size of 282 * the square AABB */ 283 pos[0] = mid[0] + (rand_canonic() * 2 - 1) * (upp[0] - low[0]); 284 pos[1] = mid[1] + (rand_canonic() * 2 - 1) * (upp[1] - low[1]); 285 CHK(s2d_scene_view_closest_point(view, pos, (float)INF, NULL, &hit) == RES_OK); 286 check_closest_point_square(pos, igeom, S2D_INVALID_ID, &hit); 287 } 288 289 /* Check closest point query filtering */ 290 filter_ctx.prim_to_filter = 2; 291 FOR_EACH(i, 0, 10000) { 292 /* Randomly generate a point in a bounding box that is 2 times the size of 293 * the square AABB */ 294 pos[0] = mid[0] + (rand_canonic() * 2 - 1) * (upp[0] - low[0]); 295 pos[1] = mid[1] + (rand_canonic() * 2 - 1) * (upp[1] - low[1]); 296 297 f2_set(filter_ctx.query_pos, pos); 298 filter_ctx.query_radius = (float)INF; 299 300 CHK(s2d_scene_view_closest_point 301 (view, pos, (float)INF, &filter_ctx, &hit) == RES_OK); 302 check_closest_point_square(pos, igeom, filter_ctx.prim_to_filter, &hit); 303 } 304 305 /* Clean up */ 306 CHK(s2d_shape_ref_put(shape) == RES_OK); 307 CHK(s2d_scene_ref_put(scn) == RES_OK); 308 CHK(s2d_scene_view_ref_put(view) == RES_OK); 309 } 310 311 /******************************************************************************* 312 * Single segment test 313 ******************************************************************************/ 314 static void 315 test_single_segment(struct s2d_device* dev) 316 { 317 struct s2d_vertex_data vdata = S2D_VERTEX_DATA_NULL; 318 struct line_segments_desc desc; 319 struct s2d_hit hit = S2D_HIT_NULL; 320 struct s2d_scene* scn = NULL; 321 struct s2d_scene_view* view = NULL; 322 struct s2d_shape* shape = NULL; 323 struct s2d_attrib attr; 324 float vertices[4]; 325 float v0[2], v1[2]; 326 float low[2], upp[2], mid[2]; 327 float pos[2]; 328 float closest_pos[2]; 329 size_t a, i, j; 330 unsigned indices[2] = {0, 1}; 331 332 f2(vertices+0, -0.5f, -0.3f); 333 f2(vertices+2, -0.4f, 0.2f); 334 335 CHK(s2d_scene_create(dev, &scn) == RES_OK); 336 CHK(s2d_shape_create_line_segments(dev, &shape) == RES_OK); 337 CHK(s2d_scene_attach_shape(scn, shape) == RES_OK); 338 339 vdata.usage = S2D_POSITION; 340 vdata.type = S2D_FLOAT2; 341 vdata.get = line_segments_get_position; 342 desc.vertices = vertices; 343 desc.indices = indices; 344 CHK(s2d_line_segments_setup_indexed_vertices 345 (shape, 1, line_segments_get_ids, 2, &vdata, 1, &desc) == RES_OK); 346 347 CHK(s2d_scene_view_create(scn, S2D_TRACE, &view) == RES_OK); 348 349 line_segments_get_position(0, v0, &desc); 350 line_segments_get_position(1, v1, &desc); 351 352 /* Compute the segment AABB */ 353 low[0] = MMIN(v0[0], v1[0]); 354 low[1] = MMIN(v0[1], v1[1]); 355 upp[0] = MMAX(v0[0], v1[0]); 356 upp[1] = MMAX(v0[1], v1[1]); 357 mid[0] = (low[0] + upp[0]) * 0.5f; 358 mid[1] = (low[1] + upp[1]) * 0.5f; 359 360 FOR_EACH(i, 0, 10000) { 361 /* Randomly generate a point in a bounding box that is 10 times the size of 362 * the segment AABB */ 363 pos[0] = mid[0] + (rand_canonic() * 2 - 1) * (upp[0] - low[0]) * 5.f; 364 pos[1] = mid[1] + (rand_canonic() * 2 - 1) * (upp[1] - low[1]) * 5.f; 365 366 CHK(s2d_scene_view_closest_point(view, pos, (float)INF, NULL, &hit) == RES_OK); 367 CHK(!S2D_HIT_NONE(&hit)); 368 CHK(s2d_primitive_get_attrib(&hit.prim, S2D_POSITION, hit.u, &attr) == RES_OK); 369 370 /* Cross check the closest point query result */ 371 closest_point_segment(pos, v0, v1, closest_pos); 372 CHK(f2_eq_eps(closest_pos, attr.value, 1.e-4f)); 373 } 374 375 FOR_EACH(i, 0, 10000) { 376 float radius; 377 378 /* Randomly generate a point in a bounding box that is 10 times the size of 379 * the segment AABB */ 380 pos[0] = mid[0] + (rand_canonic() * 2 - 1) * (upp[0] - low[0]) * 5.f; 381 pos[1] = mid[1] + (rand_canonic() * 2 - 1) * (upp[1] - low[1]) * 5.f; 382 383 CHK(s2d_scene_view_closest_point(view, pos, (float)INF, NULL, &hit) 384 == RES_OK); 385 CHK(!S2D_HIT_NONE(&hit)); 386 387 /* Check that the radius is an exclusive upper bound */ 388 radius = hit.distance; 389 CHK(s2d_scene_view_closest_point(view, pos, radius, NULL, &hit) == RES_OK); 390 CHK(S2D_HIT_NONE(&hit)); 391 radius = nextafterf(radius, FLT_MAX); 392 CHK(s2d_scene_view_closest_point(view, pos, radius, NULL, &hit) == RES_OK); 393 CHK(!S2D_HIT_NONE(&hit)); 394 CHK(hit.distance == nextafterf(radius, 0.f)); 395 } 396 397 CHK(s2d_scene_view_ref_put(view) == RES_OK); 398 CHK(s2d_shape_ref_put(shape) == RES_OK); 399 CHK(s2d_scene_ref_put(scn) == RES_OK); 400 401 /* Check accuracy on a configuration whose analytic distance is known */ 402 FOR_EACH(a, 0, 19) { 403 const float amplitude = exp2f((float)a); 404 const float eps = 1e-6f * amplitude; 405 FOR_EACH(i, 0, 1000) { 406 float A[2], B[2], AB[2], N[2], hit_N[2]; 407 int n; 408 409 /* Randomly generate a segment AB */ 410 FOR_EACH(n, 0, 2) 411 A[n] = (rand_canonic() - 0.5f) * amplitude; 412 do { 413 FOR_EACH(n, 0, 2) B[n] = (rand_canonic() - 0.5f) * amplitude; 414 } while (f2_eq_eps(A, B, eps)); 415 416 f2_sub(AB, B, A); 417 f2(N, AB[1], -AB[0]); /* Left hand convention */ 418 f2_normalize(N, N); 419 420 f2_set(vertices + 0, A); 421 f2_set(vertices + 2, B); 422 423 CHK(s2d_scene_create(dev, &scn) == RES_OK); 424 CHK(s2d_shape_create_line_segments(dev, &shape) == RES_OK); 425 CHK(s2d_scene_attach_shape(scn, shape) == RES_OK); 426 427 vdata.usage = S2D_POSITION; 428 vdata.type = S2D_FLOAT2; 429 vdata.get = line_segments_get_position; 430 desc.vertices = vertices; 431 desc.indices = indices; 432 CHK(s2d_line_segments_setup_indexed_vertices 433 (shape, 1, line_segments_get_ids, 2, &vdata, 1, &desc) == RES_OK); 434 435 CHK(s2d_scene_view_create(scn, S2D_TRACE, &view) == RES_OK); 436 437 FOR_EACH(j, 0, 1000) { 438 float proj[2]; /* Projection of pos on the line */ 439 float u, h, tmp[2]; 440 441 /* Randomly generate a pos not on the segment 442 * with know position wrt the problem: pos = A + u.AB + k.N */ 443 u = 3 * rand_canonic() - 1; 444 h = (2 * rand_canonic() - 1) * amplitude; 445 f2_add(proj, A, f2_mulf(proj, AB, u)); 446 f2_add(pos, proj, f2_mulf(pos, N, h)); 447 448 /* Compute closest point */ 449 CHK(s2d_scene_view_closest_point(view, pos, (float)INF, NULL, &hit) 450 == RES_OK); 451 CHK(!S2D_HIT_NONE(&hit)); 452 CHK(s2d_primitive_get_attrib(&hit.prim, S2D_POSITION, hit.u, &attr) 453 == RES_OK); 454 455 /* Check result */ 456 if(u <= 0) { 457 const float d = f2_len(f2_sub(tmp, pos, A)); 458 CHK(f2_eq_eps(attr.value, A, eps)); 459 CHK(eq_epsf(hit.distance, d, eps)); 460 } 461 else if(u >= 1) { 462 const float d = f2_len(f2_sub(tmp, pos, B)); 463 CHK(f2_eq_eps(attr.value, B, eps)); 464 CHK(eq_epsf(hit.distance, d, eps)); 465 } else { 466 CHK(f2_eq_eps(attr.value, proj, eps)); 467 CHK(eq_epsf(hit.distance, fabsf(h), eps)); 468 } 469 f2_normalize(hit_N, hit.normal); 470 CHK(f2_eq_eps(N, hit_N, FLT_EPSILON)); 471 } 472 473 CHK(s2d_scene_view_ref_put(view) == RES_OK); 474 CHK(s2d_shape_ref_put(shape) == RES_OK); 475 CHK(s2d_scene_ref_put(scn) == RES_OK); 476 } 477 } 478 } 479 480 /******************************************************************************* 481 * Miscellaneous test 482 ******************************************************************************/ 483 static void 484 test_api(struct s2d_device* dev) 485 { 486 struct s2d_hit hit = S2D_HIT_NULL; 487 struct s2d_scene* scn = NULL; 488 struct s2d_scene_view* view = NULL; 489 float pos[3] = {0}; 490 491 CHK(s2d_scene_create(dev, &scn) == RES_OK); 492 CHK(s2d_scene_view_create(scn, S2D_TRACE, &view) == RES_OK); 493 494 CHK(s2d_scene_view_closest_point(NULL, pos, 1.f, NULL, &hit) == RES_BAD_ARG); 495 CHK(s2d_scene_view_closest_point(view, NULL, 1.f, NULL, &hit) == RES_BAD_ARG); 496 CHK(s2d_scene_view_closest_point(view, pos, 0.f, NULL, &hit) == RES_BAD_ARG); 497 CHK(s2d_scene_view_closest_point(view, pos, 1.f, NULL, NULL) == RES_BAD_ARG); 498 CHK(s2d_scene_view_closest_point(view, pos, 1.f, NULL, &hit) == RES_OK); 499 CHK(S2D_HIT_NONE(&hit)); 500 501 CHK(s2d_scene_view_ref_put(view) == RES_OK); 502 CHK(s2d_scene_view_create(scn, S2D_SAMPLE, &view) == RES_OK); 503 CHK(s2d_scene_view_closest_point(view, pos, 1.f, NULL, &hit) == RES_BAD_OP); 504 505 CHK(s2d_scene_view_ref_put(view) == RES_OK); 506 CHK(s2d_scene_ref_put(scn) == RES_OK); 507 } 508 509 /******************************************************************************* 510 * Main function 511 ******************************************************************************/ 512 int 513 main(int argc, char** argv) 514 { 515 struct mem_allocator allocator; 516 struct s2d_device* dev = NULL; 517 (void)argc, (void)argv; 518 519 mem_init_proxy_allocator(&allocator, &mem_default_allocator); 520 CHK(s2d_device_create(NULL, &allocator, 1, &dev) == RES_OK); 521 522 test_api(dev); 523 test_single_segment(dev); 524 test_square(dev); 525 526 CHK(s2d_device_ref_put(dev) == RES_OK); 527 528 check_memory_allocator(&allocator); 529 mem_shutdown_proxy_allocator(&allocator); 530 CHK(mem_allocated_size() == 0); 531 return 0; 532 }