commit 818e40e24d75578d1aeb2153aa5e804bb9018836
parent 8fcfd5fddd719b84f952f749a535a76f7c0f670e
Author: Vincent Forest <vincent.forest@meso-star.com>
Date: Tue, 23 Aug 2016 14:44:07 +0200
Begin the implementation of the cpr_mesh_clip function
Setup the Weiler & Atherton clipping algorithm. Build the clip &
candidate polygon node lists and link them at edge intersections.
Diffstat:
| M | src/cpr_mesh.c | | | 492 | +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++-- |
1 file changed, 485 insertions(+), 7 deletions(-)
diff --git a/src/cpr_mesh.c b/src/cpr_mesh.c
@@ -15,11 +15,52 @@
#include "cpr.h"
+#include <rsys/double2.h>
#include <rsys/dynamic_array_double.h>
#include <rsys/dynamic_array_size_t.h>
+#include <rsys/hash_table.h>
#include <rsys/mem_allocator.h>
#include <rsys/ref_count.h>
+struct edge {
+ size_t v0;
+ size_t v1;
+};
+
+static INLINE char
+edge_eq(const struct edge* a, const struct edge* b)
+{
+ return a->v0 == b->v0 && a->v1 == b->v1;
+}
+
+#define HTABLE_NAME edge
+#define HTABLE_DATA size_t /* Index of the firs node */
+#define HTABLE_KEY struct edge
+#define HTABLE_KEY_FUNCTOR_EQ edge_eq
+#include <rsys/hash_table.h>
+
+struct node {
+ size_t next;
+ size_t prev;
+ size_t link;
+ size_t id;
+ double t;
+};
+
+#define DARRAY_NAME node
+#define DARRAY_DATA struct node
+#include <rsys/dynamic_array.h>
+
+struct polygon {
+ struct darray_double coords;
+};
+
+struct hit {
+ double pos[2];
+ double t0; /* Parametric intersection on 1st edge */
+ double t1; /* Parametric intersection on 2nd edge */
+};
+
struct cpr_mesh {
struct darray_double coords;
struct darray_size_t indices;
@@ -31,6 +72,393 @@ struct cpr_mesh {
/*******************************************************************************
* Helper function
******************************************************************************/
+static INLINE void
+polygon_init(struct mem_allocator* allocator, struct polygon* poly)
+{
+ ASSERT(allocator && poly);
+ darray_double_init(allocator, &poly->coords);
+}
+
+static INLINE void
+polygon_release(struct polygon* poly)
+{
+ ASSERT(poly);
+ darray_double_release(&poly->coords);
+}
+
+static res_T
+polygon_setup(struct polygon* poly, const struct cpr_polygon* desc)
+{
+ size_t ivert;
+ res_T res = RES_OK;
+ ASSERT(poly && desc);
+
+ if(!desc->get_position && !desc->nvertices) {
+ res = RES_BAD_ARG;
+ goto error;
+ }
+
+ res = darray_double_resize(&poly->coords, desc->nvertices * 2/*#coords*/);
+ if(res != RES_OK) goto error;
+
+ FOR_EACH(ivert, 0, desc->nvertices) {
+ double* pos = darray_double_data_get(&poly->coords) + ivert*2;
+ desc->get_position(ivert, pos, desc->context);
+ }
+
+exit:
+ return res;
+error:
+ darray_double_clear(&poly->coords);
+ goto exit;
+}
+
+static res_T
+polygon_setup_nodes(struct polygon* poly, struct darray_node* nodes)
+{
+ size_t nverts;
+ size_t ivert;
+ res_T res = RES_OK;
+ ASSERT(poly && nodes);
+
+ darray_node_clear(nodes);
+ nverts = darray_double_size_get(&poly->coords)/2/*#coords*/;
+
+ res = darray_node_resize(nodes, nverts);
+ if(res != RES_OK) goto error;
+
+ FOR_EACH(ivert, 0, nverts) {
+ struct node* node = darray_node_data_get(nodes) + ivert;
+ node->next = (ivert == nverts - 1) ? 0 : ivert + 1;
+ node->prev = (ivert == 0) ? nverts - 1 : ivert - 1;
+ node->link = SIZE_MAX;
+ node->id = ivert;
+ node->t = -1.0;
+ }
+
+exit:
+ return res;
+error:
+ darray_node_clear(nodes);
+ goto exit;
+}
+
+static res_T
+mesh_setup_edge_nodes
+ (struct cpr_mesh* mesh,
+ struct darray_node* nodes,
+ struct htable_edge* edges)
+{
+ size_t itri, ntris;
+ res_T res = RES_OK;
+ ASSERT(mesh && nodes);
+
+ darray_node_clear(nodes);
+ htable_edge_clear(edges);
+
+ CPR(mesh_get_triangles_count(mesh, &ntris));
+
+ FOR_EACH(itri, 0, ntris) {
+ struct edge edge;
+ size_t iedge;
+ size_t ids[3];
+
+ CPR(mesh_get_indices(mesh, itri, ids));
+ FOR_EACH(iedge, 0, 3) {
+ struct node* node;
+ size_t inode;
+
+ edge.v0 = ids[iedge];
+ edge.v1 = ids[(iedge + 1)%3];
+ if(edge.v0 > edge.v1) SWAP(size_t, edge.v0, edge.v1);
+
+ if(htable_edge_find(edges, &edge)) continue;
+
+ inode = darray_node_size_get(nodes);
+
+ res = htable_edge_set(edges, &edge, &inode);
+ if(res != RES_OK) goto error;
+ res = darray_node_resize(nodes, inode + 2);
+ if(res != RES_OK) goto error;
+
+ node = darray_node_data_get(nodes) + inode;
+ node[0].next = inode + 1;
+ node[0].prev = SIZE_MAX;
+ node[0].link = SIZE_MAX;
+ node[0].id = edge.v0;
+ node[0].t = -1.0;
+
+ node[1].next = SIZE_MAX;
+ node[1].prev = inode;
+ node[1].link = SIZE_MAX;
+ node[1].id = edge.v1;
+ node[1].t = -1.0;
+ }
+ }
+
+exit:
+ return res;
+error:
+ darray_node_clear(nodes);
+ goto exit;
+}
+
+static int
+intersect_edges
+ (double edge0[2][2],
+ double edge1[2][2],
+ struct hit* hit)
+{
+ double org0[2], org1[2];
+ double dir0[2], dir1[2];
+ double tmp[2];
+ double a, b;
+ int i;
+
+ /* Express the edges as org<0|1> + t<0|1>*dir<0|1> */
+ d2_set(org0, edge0[0]);
+ d2_set(org1, edge1[0]);
+ d2_sub(dir0, edge0[1], edge0[0]);
+ d2_sub(dir1, edge1[1], edge1[0]);
+
+ /* Solve org0 + t0*dir0 = org1 + t1*dir1
+ * (org0 + t0*dir0) x dir1 = (org1 + t1*dir1) x dir1
+ * org0 x dir1 + t0*(dir0 x dir1) = org1 x dir1
+ * t0 = (org1 - org0) x dir1 / (dir0 x dir1) */
+ d2_sub(tmp, org1, org0);
+ b = d2_cross(dir0, dir1);
+ if(b == 0) return 0; /* Edges are colinear => i.e. no intersection */
+ a = d2_cross(tmp, dir1);
+ hit->t0 = a / b;
+
+ /* Compute the hit position */
+ d2_muld(hit->pos, dir0, hit->t0);
+ d2_add(hit->pos, org0, hit->pos);
+
+ /* Compute the paremtric hit coordinate on the edge1 */
+ i = dir1[0] != 0 ? 0 : 1;
+ hit->t1 = (hit->pos[i] - org1[i]) / dir1[i];
+
+#ifndef NDEBUG
+ d2_muld(tmp, dir1, hit->t1);
+ d2_add(tmp, org1, tmp);
+ ASSERT(d2_eq_eps(hit->pos, tmp, 1.e-6));
+#endif
+
+ /* Ensure that the hit lie in the edge boundaries */
+ return hit->t0 >= 0.0 && hit->t0 <= 1.0 && hit->t1 >= 0.0 && hit->t1 <= 1.0;
+}
+
+static res_T
+compute_hit_nodes
+ (struct darray_node* mesh_nodes,
+ struct darray_double* mesh_coords,
+ struct darray_node* poly_nodes,
+ struct darray_double* poly_coords)
+{
+ size_t nmesh_edges, npoly_edges;
+ size_t imesh_edge, ipoly_edge;
+ res_T res = RES_OK;
+ ASSERT(mesh_nodes && mesh_coords && poly_nodes && poly_coords);
+
+ nmesh_edges = darray_node_size_get(mesh_nodes)/2;
+ npoly_edges = darray_node_size_get(poly_nodes);
+
+ FOR_EACH(imesh_edge, 0, nmesh_edges) {
+ struct node* mesh_node = darray_node_data_get(mesh_nodes) + imesh_edge;
+ double mesh_edge[2][2];
+
+ /* Fetch the coordinates of the edge0 vertices */
+ d2_set(mesh_edge[0], darray_double_data_get(mesh_coords) + mesh_node[0].id*2);
+ d2_set(mesh_edge[1], darray_double_data_get(mesh_coords) + mesh_node[1].id*2);
+
+ FOR_EACH(ipoly_edge, 0, npoly_edges) {
+ const size_t ipoly_node0 = ipoly_edge;
+ const size_t ipoly_node1 = (ipoly_edge + 1) % npoly_edges;
+ struct node* poly_node0;
+ struct node* poly_node1;
+ struct node* node;
+ struct node* mesh_hit_node;
+ struct node* poly_hit_node;
+ struct hit hit;
+ size_t inode;
+ size_t imesh_hit_node, ipoly_hit_node;
+ size_t imesh_hit_pos, ipoly_hit_pos;
+ double poly_edge[2][2];
+
+ poly_node0 = darray_node_data_get(poly_nodes) + ipoly_node0;
+ poly_node1 = darray_node_data_get(poly_nodes) + ipoly_node1;
+
+ /* Fetch the coordinates of the edge1 vertices */
+ d2_set(poly_edge[0], darray_double_data_get(poly_coords) + poly_node0->id*2);
+ d2_set(poly_edge[1], darray_double_data_get(poly_coords) + poly_node1->id*2);
+
+ /* Find the intersection between the 2 edges */
+ if(!intersect_edges(mesh_edge, poly_edge, &hit))
+ continue;
+
+ /* Allocate the hit node in the mesh node list */
+ imesh_hit_node = darray_node_size_get(mesh_nodes);
+ res = darray_node_resize(mesh_nodes, imesh_hit_node + 1);
+ if(res != RES_OK) goto error;
+ mesh_hit_node = darray_node_data_get(mesh_nodes) + imesh_hit_node;
+
+ /* Allocate the hit node in the polygon node list */
+ ipoly_hit_node = darray_node_size_get(poly_nodes);
+ res = darray_node_resize(poly_nodes, ipoly_hit_node + 1);
+ if(res != RES_OK) goto error;
+ poly_hit_node = darray_node_data_get(poly_nodes) + ipoly_hit_node;
+
+ /* Register the hit position in the mesh vertex buffer */
+ imesh_hit_pos = darray_double_size_get(mesh_coords);
+ res = darray_double_resize(mesh_coords, imesh_hit_pos + 2);
+ if(res != RES_OK) goto error;
+ d2_set(darray_double_data_get(mesh_coords) + imesh_hit_pos, hit.pos);
+ imesh_hit_pos /= 2/*#coords per vertex*/;
+
+ /* Register the hit position in the vertex buffer 1 */
+ ipoly_hit_pos = darray_double_size_get(poly_coords);
+ res = darray_double_resize(poly_coords, ipoly_hit_pos + 2);
+ if(res != RES_OK) goto error;
+ d2_set(darray_double_data_get(poly_coords) + ipoly_hit_pos, hit.pos);
+ ipoly_hit_pos /= 2/*#coords per vertex*/;
+
+ /* Insert the hit node in the mesh node list */
+ mesh_node = darray_node_data_get(mesh_nodes) + imesh_edge*2;
+ node = darray_node_data_get(mesh_nodes) + mesh_node[0].next;
+ while(node->link != SIZE_MAX && node->t <= hit.t0) {
+ node = darray_node_data_get(mesh_nodes) + node->next;
+ }
+ inode = (size_t)(node - darray_node_data_get(mesh_nodes));
+ mesh_hit_node->next = inode;
+ mesh_hit_node->prev = node->prev;
+ mesh_hit_node->link = ipoly_hit_node; /* Hit id in the poly node list */
+ mesh_hit_node->id = imesh_hit_pos;
+ mesh_hit_node->t = hit.t0;
+ darray_node_data_get(mesh_nodes)[node->prev].next = imesh_hit_node;
+ node->prev = imesh_hit_node;
+
+ /* Insert the hit node in the poly node list 1 */
+ poly_node0 = darray_node_data_get(poly_nodes) + ipoly_node0;
+ node = darray_node_data_get(poly_nodes) + poly_node0->next;
+ while(node->link != SIZE_MAX && node->t <= hit.t1) {
+ node = darray_node_data_get(poly_nodes) + node->next;
+ }
+ inode = (size_t)(node - darray_node_data_get(poly_nodes));
+ poly_hit_node->next = inode;
+ poly_hit_node->prev = node->prev;
+ poly_hit_node->link = imesh_hit_node; /* Hit id in the mesh node list */
+ poly_hit_node->id = ipoly_hit_pos;
+ poly_hit_node->t = hit.t1;
+ darray_node_data_get(poly_nodes)[node->prev].next = ipoly_hit_node;
+ node->prev = ipoly_hit_node;
+ }
+ }
+
+exit:
+ return res;
+error:
+ goto exit;
+}
+
+static void
+vertices_setup_inside_polygon_flag
+ (const struct darray_double* coords,
+ const struct darray_node* poly_nodes,
+ const struct darray_double* poly_coords,
+ char* is_inside)
+{
+ size_t ivert, nverts;
+ size_t iedge, nedges;
+ ASSERT(coords && poly_nodes && poly_coords && is_inside);
+
+ nverts = darray_double_size_get(coords) / 2/*#coords per vertex*/;
+ nedges = darray_node_size_get(poly_nodes);
+
+ FOR_EACH(ivert, 0, nverts) {
+ const double* v = darray_double_cdata_get(coords) + ivert*2;
+ char inside = 1;
+ for(iedge = 0; inside && iedge < nedges; ++iedge) {
+ const struct node* node0 = darray_node_cdata_get(poly_nodes) + iedge;
+ const struct node* node1 = darray_node_cdata_get(poly_nodes) + node0->next;
+ const double* c0 = darray_double_cdata_get(poly_coords) + node0->id*2;
+ const double* c1 = darray_double_cdata_get(poly_coords) + node1->id*2;
+ double e0[2], e1[2];
+ d2_sub(e0, c0, v);
+ d2_sub(e1, c1, v);
+ inside = d2_cross(e1, e0) < 0;
+ }
+ is_inside[ivert] = inside;
+ }
+}
+
+static res_T
+mesh_setup_candidate_nodes
+ (const struct cpr_mesh* mesh,
+ const size_t itri,
+ struct htable_edge* edges,
+ const struct darray_node* mesh_nodes,
+ struct darray_node* poly_nodes,
+ struct darray_node* cand_nodes)
+{
+ const struct node* msh_nodes;
+ size_t ids[3];
+ size_t iedge;
+ size_t icand_node_last;
+ res_T res = RES_OK;
+
+ ASSERT(mesh_nodes && cand_nodes);
+ ASSERT(itri < darray_node_size_get(mesh_nodes) /
+ (3/*#edge per triangle*/*2/*#node per edge*/));
+
+ darray_node_clear(cand_nodes);
+ CPR(mesh_get_indices(mesh, itri, ids));
+
+ msh_nodes = darray_node_cdata_get(mesh_nodes);
+
+ FOR_EACH(iedge, 0, 3) {
+ struct edge edge;
+ size_t imesh_node;
+ int flip_edge;
+
+ edge.v0 = ids[iedge], edge.v1 = ids[(iedge+1)%3];
+ if((flip_edge = edge.v0 > edge.v1)) SWAP(size_t, edge.v0, edge.v1);
+
+ imesh_node = *htable_edge_find(edges, &edge) + (size_t)flip_edge;
+
+ do {
+ struct node* cand_node;
+ const size_t icand_node = darray_node_size_get(cand_nodes);
+
+ res = darray_node_resize(cand_nodes, icand_node + 1);
+ if(res != RES_OK) goto error;
+ cand_node = darray_node_data_get(cand_nodes) + icand_node;
+
+ cand_node->prev = icand_node - 1;
+ cand_node->next = icand_node + 1;
+ cand_node->link = msh_nodes[imesh_node].link;
+ cand_node->id = msh_nodes[imesh_node].id;
+ cand_node->t = msh_nodes[imesh_node].t;
+ if(cand_node->link) {
+ darray_node_data_get(poly_nodes)[cand_node->link].link =
+ icand_node;
+ }
+ } while(flip_edge
+ ? msh_nodes[msh_nodes[imesh_node].prev].prev != SIZE_MAX
+ : msh_nodes[msh_nodes[imesh_node].next].next != SIZE_MAX);
+
+ icand_node_last = darray_node_size_get(cand_nodes) - 1;
+ darray_node_data_get(cand_nodes)[0].prev = icand_node_last;
+ darray_node_data_get(cand_nodes)[icand_node_last].next = 0;
+ }
+
+exit:
+ return res;
+error:
+ darray_node_clear(cand_nodes);
+ goto exit;
+}
+
static void
mesh_release(ref_T* ref)
{
@@ -130,8 +558,8 @@ cpr_mesh_setup_indexed_vertices
ids = darray_size_t_data_get(&mesh->indices);
FOR_EACH(i, 0, ntris) {
get_indices(i, ids +i*3, data);
- if(ids[i*3+0] >= nverts
- || ids[i*3+1] >= nverts
+ if(ids[i*3+0] >= nverts
+ || ids[i*3+1] >= nverts
|| ids[i*3+2] >= nverts) {
res = RES_BAD_ARG;
goto error;
@@ -178,7 +606,7 @@ cpr_mesh_get_indices
ids[1] = darray_size_t_cdata_get(&mesh->indices)[itri*3 + 1];
ids[2] = darray_size_t_cdata_get(&mesh->indices)[itri*3 + 2];
return RES_OK;
-}
+}
res_T
cpr_mesh_get_position
@@ -196,9 +624,59 @@ cpr_mesh_get_position
res_T
cpr_mesh_clip(struct cpr_mesh* mesh, struct cpr_polygon* polygon)
{
- if(!mesh || !polygon) return RES_BAD_ARG;
- FATAL("Not implemented yet!\n");
- /* TODO */
- return RES_OK;
+ struct darray_node mesh_nodes;
+ struct darray_node poly_nodes;
+ struct darray_node cand_nodes; /* Candidate */
+ struct darray_char is_inside;
+ struct htable_edge edges;
+ struct polygon poly;
+ size_t itri, ntris;
+ res_T res = RES_OK;
+
+ darray_node_init(mesh->allocator, &mesh_nodes);
+ darray_node_init(mesh->allocator, &poly_nodes);
+ darray_node_init(mesh->allocator, &cand_nodes);
+ darray_char_init(mesh->allocator, &is_inside);
+ htable_edge_init(mesh->allocator, &edges);
+ polygon_init(mesh->allocator, &poly);
+
+ if(!mesh || !polygon) {
+ res = RES_BAD_ARG;
+ goto error;
+ }
+
+ res = polygon_setup(&poly, polygon);
+ if(res != RES_OK) goto error;
+ res = polygon_setup_nodes(&poly, &poly_nodes);
+ if(res != RES_OK) goto error;
+ vertices_setup_inside_polygon_flag(&mesh->coords, &poly_nodes,
+ &poly.coords, darray_char_data_get(&is_inside));
+
+ res = mesh_setup_edge_nodes(mesh, &mesh_nodes, &edges);
+ if(res != RES_OK) goto error;
+ res = compute_hit_nodes
+ (&mesh_nodes, &mesh->coords, &poly_nodes, &poly.coords);
+ if(res != RES_OK) goto error;
+
+ CPR(mesh_get_triangles_count(mesh, &ntris));
+ FOR_EACH(itri, 0, ntris) {
+ res = mesh_setup_candidate_nodes
+ (mesh, itri, &edges, &mesh_nodes, &poly_nodes, &cand_nodes);
+ if(res != RES_OK) goto error;
+
+ /*TODO clip(&cand_nodes, &poly_nodes);*/
+ }
+
+exit:
+ darray_node_release(&mesh_nodes);
+ darray_node_release(&poly_nodes);
+ darray_node_release(&cand_nodes);
+ darray_char_release(&is_inside);
+ htable_edge_release(&edges);
+ polygon_release(&poly);
+ return res;
+
+error:
+ goto exit;
}
