polygon.c (13395B)
1 /* Copyright (C) 2014-2017, 2021-2023 Vincent Forest (vaplv@free.fr) 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 "polygon.h" 17 18 #include <rsys/float3.h> 19 #include <rsys/dynamic_array_u32.h> 20 #include <rsys/hash_table.h> 21 #include <rsys/mem_allocator.h> 22 #include <rsys/ref_count.h> 23 24 #include <float.h> 25 26 /* Declare the htable_u32 data structure */ 27 #define HTABLE_NAME u32 28 #define HTABLE_KEY uint32_t 29 #define HTABLE_DATA char 30 #include <rsys/hash_table.h> 31 32 struct vertex_node { 33 uint32_t next, prev; /* Index toward the next/prev linked vertex */ 34 float pos[3]; /* World position of the vertex */ 35 }; 36 37 #define DARRAY_NAME vertex_node 38 #define DARRAY_DATA struct vertex_node 39 #include <rsys/dynamic_array.h> 40 41 struct polygon { 42 ref_T ref; 43 struct mem_allocator* allocator; 44 45 struct darray_vertex_node pool; /* Linked list of vertices */ 46 struct htable_u32 vertices_concave; /* Set of concave vertex nodes */ 47 struct darray_u32 triangle_ids;/* Vertex indices defining a triangular mesh */ 48 uint32_t vertices; /* Index toward the first vertex */ 49 uint32_t nvertices; /* Total vertices count. May be == to the size of pool */ 50 }; 51 52 /******************************************************************************* 53 * Helper functions 54 ******************************************************************************/ 55 /* Remove the last vertices if they are the same of the first one */ 56 static void 57 cleanup_polygon(struct polygon* poly) 58 { 59 struct vertex_node* first; 60 struct vertex_node* last; 61 struct vertex_node* nodes; 62 ASSERT(poly); 63 64 if(poly->nvertices <= 1) return; 65 66 nodes = darray_vertex_node_data_get(&poly->pool); 67 first = nodes + poly->vertices; 68 69 for(;;) { 70 last = nodes + first->prev; 71 if(!f3_eq_eps(first->pos, last->pos, 1.e-6f) || first == last) 72 break; 73 74 nodes[last->prev].next = last->next; 75 nodes[last->next].prev = last->prev; 76 --poly->nvertices; 77 } 78 } 79 80 81 static void 82 node_normal_compute 83 (const struct polygon* poly, 84 const uint32_t inode, 85 float normal[3]) 86 { 87 float e0[3], e1[3]; 88 const struct vertex_node* nodes; 89 ASSERT(poly && normal && inode < darray_vertex_node_size_get(&poly->pool)); 90 ASSERT(poly->nvertices >= 3); 91 nodes = darray_vertex_node_cdata_get(&poly->pool); 92 f3_sub(e0, nodes[nodes[inode].prev].pos, nodes[inode].pos); 93 f3_sub(e1, nodes[nodes[inode].next].pos, nodes[inode].pos); 94 f3_cross(normal, e0, e1); 95 } 96 97 static void /* Compute the normal of a convex vertex */ 98 convex_normal_compute(struct polygon* poly, float normal[3]) 99 { 100 float bbox_min[3] = { FLT_MAX, FLT_MAX, FLT_MAX }; 101 float bbox_max[3] = {-FLT_MAX,-FLT_MAX,-FLT_MAX }; 102 struct vertex_node* nodes; 103 uint32_t iconvex = 0; 104 uint32_t inode; 105 uint32_t ivert; 106 ASSERT(poly && normal && poly->nvertices >= 3); 107 108 nodes = darray_vertex_node_data_get(&poly->pool); 109 inode = poly->vertices; 110 111 /* Look for a convex vertex, i.e. a vertex lying on the polygon bbox */ 112 FOR_EACH(ivert, 0, poly->nvertices) { 113 struct vertex_node* vertex = nodes + inode; 114 int i; 115 FOR_EACH(i, 0, 3) { 116 if(bbox_min[i] > vertex->pos[i]) { 117 bbox_min[i] = vertex->pos[i]; 118 iconvex = inode; 119 } 120 if(bbox_max[i] < vertex->pos[i]) { 121 bbox_max[i] = vertex->pos[i]; 122 iconvex = inode; 123 } 124 } 125 inode = vertex->next; 126 } 127 ASSERT(inode == poly->vertices); 128 node_normal_compute(poly, iconvex, normal); 129 } 130 131 static char 132 node_is_an_ear 133 (const struct polygon* poly, 134 struct htable_u32* vertices_concave, 135 const uint32_t inode) 136 { 137 struct htable_u32_iterator it, it_end; 138 const struct vertex_node* nodes; 139 float E1[3], E2[3], P[3], normal[3]; 140 float len, det; 141 (void)len; 142 143 ASSERT(poly && vertices_concave); 144 ASSERT(inode < darray_vertex_node_size_get(&poly->pool)); 145 146 if(!htable_u32_size_get(vertices_concave)) /* Polygon is convex */ 147 return 1; 148 if(htable_u32_find(vertices_concave, &inode)) /* `Concave' vertex */ 149 return 0; 150 151 nodes = darray_vertex_node_cdata_get(&poly->pool); 152 153 /* Setup the "Moller trumbore" normal/{prev, curr, next} triangle test */ 154 f3_sub(E1, nodes[nodes[inode].prev].pos, nodes[inode].pos); 155 f3_sub(E2, nodes[nodes[inode].next].pos, nodes[inode].pos); 156 f3_cross(normal, E1, E2); 157 len = f3_normalize(normal, normal); 158 if(eq_epsf(len, 0.f, 1.e-6f)) return 1; 159 160 f3_cross(P, normal, E2); 161 det = f3_dot(P, E1); 162 163 /* Check that {prev, curr, next} doesn't contain a concave vertex */ 164 htable_u32_begin(vertices_concave, &it); 165 htable_u32_end(vertices_concave, &it_end); 166 while(!htable_u32_iterator_eq(&it, &it_end)) { 167 float T[3], u; 168 const uint32_t inode_concave = *htable_u32_iterator_key_get(&it); 169 htable_u32_iterator_next(&it); 170 if(inode_concave == nodes[inode].next 171 || inode_concave == nodes[inode].prev) 172 continue; 173 /* Project the concave vertex position onto the {prev, curr, next} triangle 174 * plane and test if it lies into it */ 175 f3_sub(T, nodes[inode_concave].pos, nodes[inode].pos); 176 u = f3_dot(T, P) / det; 177 if(u >= 0.f && u <= 1.f) { 178 float Q[3], v; 179 f3_cross(Q, T, E1); 180 v = f3_dot(Q, normal) / det; 181 if(v >= 0.f && v <= 1.f && (u + v) <= 1.f) 182 return 0; 183 } 184 } 185 return 1; 186 } 187 188 static void 189 release_polygon(ref_T* ref) 190 { 191 struct polygon* poly = CONTAINER_OF(ref, struct polygon, ref); 192 ASSERT(ref); 193 darray_vertex_node_release(&poly->pool); 194 htable_u32_release(&poly->vertices_concave); 195 darray_u32_release(&poly->triangle_ids); 196 MEM_RM(poly->allocator, poly); 197 } 198 199 /******************************************************************************* 200 * Exported functions 201 ******************************************************************************/ 202 res_T 203 polygon_create(struct mem_allocator* allocator, struct polygon** out_polygon) 204 { 205 struct polygon* poly = NULL; 206 struct mem_allocator* mem_allocator; 207 res_T res = RES_OK; 208 209 if(!out_polygon) { 210 res = RES_BAD_ARG; 211 goto error; 212 } 213 mem_allocator = allocator ? allocator : &mem_default_allocator; 214 poly = MEM_CALLOC(mem_allocator, 1, sizeof(struct polygon)); 215 if(!poly) { 216 res = RES_MEM_ERR; 217 goto error; 218 } 219 poly->allocator = mem_allocator; 220 ref_init(&poly->ref); 221 darray_vertex_node_init(poly->allocator, &poly->pool); 222 htable_u32_init(poly->allocator, &poly->vertices_concave); 223 darray_u32_init(poly->allocator, &poly->triangle_ids); 224 poly->vertices = UINT32_MAX; /* <=> No vertex */ 225 poly->nvertices = 0; 226 227 exit: 228 if(out_polygon) 229 *out_polygon = poly; 230 return res; 231 232 error: 233 if(poly) { 234 POLYGON(ref_put(poly)); 235 poly = NULL; 236 } 237 goto exit; 238 } 239 240 res_T 241 polygon_ref_get(struct polygon* polygon) 242 { 243 if(!polygon) return RES_BAD_ARG; 244 ref_get(&polygon->ref); 245 return RES_OK; 246 } 247 248 res_T 249 polygon_ref_put(struct polygon* polygon) 250 { 251 if(!polygon) return RES_BAD_ARG; 252 ref_put(&polygon->ref, release_polygon); 253 return RES_OK; 254 } 255 256 res_T 257 polygon_clear(struct polygon* poly) 258 { 259 if(!poly) return RES_BAD_ARG; 260 darray_vertex_node_clear(&poly->pool); 261 htable_u32_clear(&poly->vertices_concave); 262 poly->vertices = UINT32_MAX; 263 poly->nvertices = 0; 264 return RES_OK; 265 } 266 267 res_T 268 polygon_vertex_add(struct polygon* poly, const float pos[3]) 269 { 270 struct vertex_node* nodes; 271 struct vertex_node* node_free; 272 uint32_t inode_free; 273 res_T res = RES_OK; 274 275 if(!poly || !pos) { 276 res = RES_BAD_ARG; 277 goto error; 278 } 279 280 nodes = darray_vertex_node_data_get(&poly->pool); 281 282 /* Skip the vertex if it is roughly equal to the previous one */ 283 if(poly->nvertices && f3_eq_eps(nodes[poly->nvertices-1].pos, pos, 1.e-6f)) 284 goto exit; 285 286 if(poly->nvertices >= 2) { 287 /* Compute the area of the triangle built from the 2 last vertices and the 288 * new one. If its area is ~0 then the vertices are aligned and 289 * consequently the intermediary vertex can be removed */ 290 struct vertex_node* v0 = nodes + nodes[poly->vertices].prev; /* <=> tail */ 291 struct vertex_node* v1 = nodes + v0->prev; 292 float e0[3], e1[3], normal[3], area; 293 f3_sub(e0, v0->pos, pos); 294 f3_sub(e1, v1->pos, pos); 295 area = f3_len(f3_cross(normal, e0, e1)); 296 if(eq_eps(area, 0.f, 1.e-6f * 0.5f)) { 297 f3_set(v0->pos, pos); 298 goto exit; 299 } 300 } 301 302 /* Alloc a new vertex node */ 303 inode_free = (uint32_t)darray_vertex_node_size_get(&poly->pool); 304 res = darray_vertex_node_resize(&poly->pool, inode_free + 1); 305 if(res != RES_OK) 306 goto error; 307 nodes = darray_vertex_node_data_get(&poly->pool); 308 node_free = nodes + inode_free; 309 310 /* Init the new node */ 311 node_free->next = node_free->prev = inode_free; 312 f3_set(node_free->pos, pos); 313 314 /* Enqueu the new node into the linked list of the polygon vertices */ 315 if(poly->vertices == UINT32_MAX) { 316 poly->vertices = inode_free; 317 } else { 318 node_free->prev = nodes[poly->vertices].prev; 319 node_free->next = poly->vertices; 320 nodes[nodes[poly->vertices].prev].next = inode_free; 321 nodes[poly->vertices].prev = inode_free; 322 } 323 ++poly->nvertices; 324 325 exit: 326 return res; 327 error: 328 goto exit; 329 } 330 331 res_T 332 polygon_vertices_count_get(const struct polygon* poly, uint32_t* nvertices) 333 { 334 if(!poly || !nvertices) return RES_BAD_ARG; 335 *nvertices = poly->nvertices; 336 return RES_OK; 337 } 338 339 res_T 340 polygon_vertex_get 341 (const struct polygon* poly, const uint32_t ivertex, float pos[3]) 342 { 343 const struct vertex_node* node; 344 if(!poly || !pos || ivertex >= poly->nvertices) 345 return RES_BAD_ARG; 346 node = darray_vertex_node_cdata_get(&poly->pool) + ivertex; 347 f3_set(pos, node->pos); 348 return RES_OK; 349 } 350 351 res_T 352 polygon_triangulate 353 (struct polygon* poly, 354 const uint32_t** indices, 355 uint32_t* nindices) 356 { 357 struct vertex_node* nodes; 358 uint32_t ivert, inode; 359 float normal_convex[3], normal[3]; 360 res_T res = RES_OK; 361 362 if(!poly || !indices || !nindices) { 363 res = RES_BAD_ARG; 364 goto error; 365 } 366 367 cleanup_polygon(poly); 368 369 htable_u32_clear(&poly->vertices_concave); 370 darray_u32_clear(&poly->triangle_ids); 371 372 if(poly->nvertices < 3) /* Point or line */ 373 goto exit; 374 375 if(poly->nvertices == 3) { /* Already a triangle */ 376 FOR_EACH(ivert, 0, 3) { 377 res = darray_u32_push_back(&poly->triangle_ids, &ivert); 378 if(res != RES_OK) 379 goto error; 380 } 381 goto exit; 382 } 383 384 convex_normal_compute(poly, normal_convex); 385 386 nodes = darray_vertex_node_data_get(&poly->pool); 387 inode = poly->vertices; 388 389 FOR_EACH(inode, 0, poly->nvertices) { /* Find the list of concave vertices */ 390 node_normal_compute(poly, inode, normal); 391 if(f3_dot(normal_convex, normal) < 0.f) { 392 const char dummy = 1; 393 res = htable_u32_set(&poly->vertices_concave, &inode, &dummy); 394 if(res != RES_OK) 395 goto error; 396 } 397 } 398 399 /* Implementation of the ear cutting algorithm "The Graham Scan Triangulates 400 * Simple Polygons */ 401 inode = nodes[nodes[poly->vertices].next].next; 402 while(inode != poly->vertices) { 403 if(!node_is_an_ear(poly, &poly->vertices_concave, nodes[inode].prev)) { 404 inode = nodes[inode].next; 405 } else { /* `Prev' is an ear */ 406 const uint32_t iv0 = nodes[nodes[inode].prev].prev; 407 const uint32_t iv1 = nodes[inode].prev; 408 const uint32_t iv2 = inode; 409 uint32_t inode_prev; 410 411 /* Register the {prev.prev, prev, cur} triangle */ 412 if(RES_OK != (res = darray_u32_push_back(&poly->triangle_ids, &iv0)) 413 || RES_OK != (res = darray_u32_push_back(&poly->triangle_ids, &iv1)) 414 || RES_OK != (res = darray_u32_push_back(&poly->triangle_ids, &iv2))) 415 goto error; 416 417 /* Cut the ear by removing `prev' from the node list */ 418 inode_prev = nodes[inode].prev; 419 nodes[nodes[inode_prev].prev].next = nodes[inode_prev].next; 420 nodes[nodes[inode_prev].next].prev = nodes[inode_prev].prev; 421 if(poly->vertices == inode_prev) 422 poly->vertices = inode; 423 424 node_normal_compute(poly, inode, normal); 425 if(f3_dot(normal_convex, normal) > 0.f) /* inode is convex */ 426 htable_u32_erase(&poly->vertices_concave, &inode); 427 428 node_normal_compute(poly, nodes[inode].prev, normal); 429 if(f3_dot(normal_convex, normal) > 0.f) /* prev(inode) is convex */ 430 htable_u32_erase(&poly->vertices_concave, &nodes[inode].prev); 431 432 if(nodes[inode].prev == poly->vertices) 433 inode = nodes[inode].next; 434 } 435 } 436 437 exit: 438 if(poly) { 439 if(indices && nindices && poly) { 440 *nindices = (uint32_t)darray_u32_size_get(&poly->triangle_ids); 441 *indices = *nindices ? darray_u32_cdata_get(&poly->triangle_ids) : NULL; 442 } 443 if(poly->nvertices) { /* Restore the linked list */ 444 poly->vertices = 0; 445 nodes = darray_vertex_node_data_get(&poly->pool); 446 FOR_EACH(inode, 0, poly->nvertices) { 447 nodes[inode].prev = inode == 0 ? poly->nvertices - 1 : inode - 1; 448 nodes[inode].next = inode == poly->nvertices - 1 ? 0 : inode + 1; 449 } 450 } 451 } 452 return res; 453 error: 454 if(poly) 455 darray_u32_clear(&poly->triangle_ids); 456 goto exit; 457 } 458