Polyfill2d: use kd-tree

Simple search for intersections became slow for larger concave ngons (100+)
Tested to work with ngons up to 75k sides, performance is approx ~6x faster then scanfill.

This is a 2D version of BLI_kdtree with modifications:
- nodes can be removed
- an index -> node map is stored (especially for tessellation)

1 files changed, 412 insertions, 19 deletions

diff --git a/source/blender/blenlib/intern/polyfill2d.c b/source/blender/blenlib/intern/polyfill2d.c
index 1c0b936a881..30d1ab42b02 100644
--- a/source/blender/blenlib/intern/polyfill2d.c
+++ b/source/blender/blenlib/intern/polyfill2d.c
@@ -57,6 +57,10 @@
 #define USE_CLIP_SWEEP
 // #define USE_CONVEX_SKIP_TEST
 
+#ifdef USE_CONVEX_SKIP
+#  define USE_KDTREE
+#endif
+
 // #define DEBUG_TIME
 #ifdef DEBUG_TIME
 #  include "PIL_time_utildefines.h"
@@ -64,6 +68,52 @@
 
 
 typedef signed char eSign;
+
+#ifdef USE_KDTREE
+/**
+ * This is a single purpose KDTree based on BLI_kdtree with some modifications
+ * to better suit polyfill2d.
+ *
+ *
+ * - #KDTreeNode2D is kept small (only 16 bytes),
+ *   by not storing coords in the nodes and using index values rather then pointers
+ *   to reference neg/pos values.
+ *
+ * - #kdtree2d_isect_tri is the only function currently used.
+ *   This simply intersects a triangle with the kdtree points.
+ *
+ * - the KDTree is only built & used when the polygon is concave.
+ */
+
+typedef bool axis_t;
+
+/* use for sorting */
+typedef struct KDTreeNode2D_head {
+	unsigned int neg, pos;
+	unsigned int index;
+} KDTreeNode2D_head;
+
+typedef struct KDTreeNode2D {
+	unsigned int neg, pos;
+	unsigned int index;
+	axis_t axis;  /* range is only (0-1) */
+	unsigned short flag;
+	unsigned int parent;
+} KDTreeNode2D;
+
+struct KDTree2D {
+	KDTreeNode2D *nodes;
+	const float (*coords)[2];
+	unsigned int root;
+	unsigned int totnode;
+	unsigned int *nodes_map;  /* index -> node lookup */
+};
+
+struct KDRange2D {
+	float min, max;
+};
+#endif  /* USE_KDTREE */
+
 enum {
 	CONCAVE = -1,
 	TANGENTIAL = 0,
@@ -82,6 +132,10 @@ typedef struct PolyFill {
 	/* A polygon with n vertices has a triangulation of n-2 triangles. */
 	unsigned int (*tris)[3];
 	unsigned int   tris_tot;
+
+#ifdef USE_KDTREE
+	struct KDTree2D kdtree;
+#endif
 } PolyFill;
 
 
@@ -140,6 +194,271 @@ static eSign span_tri_v2_sign(const float v1[2], const float v2[2], const float
 	return signum_i(area_tri_signed_v2_alt_2x(v3, v2, v1));
 }
 
+
+#ifdef USE_KDTREE
+#define KDNODE_UNSET ((unsigned int)-1)
+
+enum {
+	KDNODE_FLAG_REMOVED = (1 << 0),
+};
+
+static void kdtree2d_new(
+        struct KDTree2D *tree,
+        unsigned int tot,
+        const float (*coords)[2])
+{
+	/* set by caller */
+	// tree->nodes = nodes;
+	tree->coords = coords;
+	tree->root = KDNODE_UNSET;
+	tree->totnode = tot;
+}
+
+/**
+ * no need for kdtree2d_insert, since we know the coords array.
+ */
+static void kdtree2d_init(
+        struct KDTree2D *tree,
+        const unsigned int coords_tot,
+        const PolyIndex *indices)
+{
+	KDTreeNode2D *node;
+	unsigned int i;
+
+	for (i = 0, node = tree->nodes; i < coords_tot; i++) {
+		if (indices[i].sign != CONVEX) {
+			node->neg = node->pos = KDNODE_UNSET;
+			node->index = indices[i].index;
+			node->axis = 0;
+			node->flag = 0;
+			node++;
+		}
+	}
+
+	BLI_assert(tree->totnode == (node - tree->nodes));
+}
+
+static unsigned int kdtree2d_balance_recursive(
+        KDTreeNode2D *nodes, unsigned int totnode, axis_t axis,
+        const float (*coords)[2], const unsigned int ofs)
+{
+	KDTreeNode2D *node;
+	unsigned int neg, pos, median, i, j;
+
+	if (totnode <= 0) {
+		return KDNODE_UNSET;
+	}
+	else if (totnode == 1) {
+		return 0 + ofs;
+	}
+
+	/* quicksort style sorting around median */
+	neg = 0;
+	pos = totnode - 1;
+	median = totnode / 2;
+
+	while (pos > neg) {
+		const float co = coords[nodes[pos].index][axis];
+		i = neg - 1;
+		j = pos;
+
+		while (1) {
+			while (coords[nodes[++i].index][axis] < co) ;
+			while (coords[nodes[--j].index][axis] > co && j > neg) ;
+
+			if (i >= j) {
+				break;
+			}
+			SWAP(KDTreeNode2D_head, *(KDTreeNode2D_head *)&nodes[i], *(KDTreeNode2D_head *)&nodes[j]);
+		}
+
+		SWAP(KDTreeNode2D_head, *(KDTreeNode2D_head *)&nodes[i], *(KDTreeNode2D_head *)&nodes[pos]);
+		if (i >= median) {
+			pos = i - 1;
+		}
+		if (i <= median) {
+			neg = i + 1;
+		}
+	}
+
+	/* set node and sort subnodes */
+	node = &nodes[median];
+	node->axis = axis;
+	axis = !axis;
+	node->neg = kdtree2d_balance_recursive(nodes, median, axis, coords, ofs);
+	node->pos = kdtree2d_balance_recursive(&nodes[median + 1], (totnode - (median + 1)), axis, coords, (median + 1) + ofs);
+
+	return median + ofs;
+}
+
+static void kdtree2d_balance(
+        struct KDTree2D *tree)
+{
+	tree->root = kdtree2d_balance_recursive(tree->nodes, tree->totnode, 0, tree->coords, 0);
+}
+
+
+static void kdtree2d_init_mapping(
+        struct KDTree2D *tree)
+{
+	unsigned int i;
+	KDTreeNode2D *node;
+
+	for (i = 0, node = tree->nodes; i < tree->totnode; i++, node++) {
+		if (node->neg != KDNODE_UNSET) {
+			tree->nodes[node->neg].parent = i;
+		}
+		if (node->pos != KDNODE_UNSET) {
+			tree->nodes[node->pos].parent = i;
+		}
+
+		/* build map */
+		BLI_assert(tree->nodes_map[node->index] == KDNODE_UNSET);
+		tree->nodes_map[node->index] = i;
+	}
+
+	tree->nodes[tree->root].parent = KDNODE_UNSET;
+}
+
+static void kdtree2d_node_remove(
+        struct KDTree2D *tree,
+        unsigned int index)
+{
+	unsigned int node_index = tree->nodes_map[index];
+	KDTreeNode2D *node;
+
+	if (node_index == KDNODE_UNSET) {
+		return;
+	}
+	else {
+		tree->nodes_map[index] = KDNODE_UNSET;
+	}
+
+	node = &tree->nodes[node_index];
+	tree->totnode -= 1;
+
+	BLI_assert((node->flag & KDNODE_FLAG_REMOVED) == 0);
+	node->flag |= KDNODE_FLAG_REMOVED;
+
+	while ((node->neg == KDNODE_UNSET) &&
+	       (node->pos == KDNODE_UNSET) &&
+	       (node->parent != KDNODE_UNSET))
+	{
+		KDTreeNode2D *node_parent = &tree->nodes[node->parent];
+
+		BLI_assert((unsigned int)(node - tree->nodes) == node_index);
+		if (node_parent->neg == node_index) {
+			node_parent->neg = KDNODE_UNSET;
+		}
+		else {
+			BLI_assert(node_parent->pos == node_index);
+			node_parent->pos = KDNODE_UNSET;
+		}
+
+		if (node_parent->flag & KDNODE_FLAG_REMOVED) {
+			node_index = node->parent;
+			node = node_parent;
+		}
+		else {
+			break;
+		}
+	}
+}
+
+static bool kdtree2d_isect_tri_recursive(
+        const struct KDTree2D *tree,
+        const unsigned int tri_index[3],
+        const float       *tri_coords[3],
+        const float        tri_center[2],
+        const struct KDRange2D bounds[2],
+        const KDTreeNode2D *node)
+{
+	const float *co = tree->coords[node->index];
+
+	/* bounds then triangle intersect */
+	if ((node->flag & KDNODE_FLAG_REMOVED) == 0) {
+		/* bounding box test first */
+		if ((co[0] > bounds[0].min) &&
+		    (co[0] < bounds[0].max) &&
+		    (co[1] > bounds[1].min) &&
+		    (co[1] < bounds[1].max))
+		{
+			if ((span_tri_v2_sign(tri_coords[0], tri_coords[1], co) != CONCAVE) &&
+			    (span_tri_v2_sign(tri_coords[1], tri_coords[2], co) != CONCAVE) &&
+			    (span_tri_v2_sign(tri_coords[2], tri_coords[0], co) != CONCAVE))
+			{
+				if (!ELEM3(node->index, tri_index[0], tri_index[1], tri_index[2])) {
+					return true;
+				}
+			}
+		}
+	}
+
+#define KDTREE2D_ISECT_TRI_RECURSE_NEG \
+	(((node->neg != KDNODE_UNSET) && (co[node->axis] > bounds[node->axis].min)) &&   \
+	  (kdtree2d_isect_tri_recursive(tree, tri_index, tri_coords, tri_center, bounds, \
+	                                &tree->nodes[node->neg])))
+#define KDTREE2D_ISECT_TRI_RECURSE_POS \
+	(((node->pos != KDNODE_UNSET) && (co[node->axis] < bounds[node->axis].max)) &&   \
+	  (kdtree2d_isect_tri_recursive(tree, tri_index, tri_coords, tri_center, bounds, \
+	                                &tree->nodes[node->pos])))
+
+	if (tri_center[node->axis] > co[node->axis]) {
+		if (KDTREE2D_ISECT_TRI_RECURSE_POS) {
+			return true;
+		}
+		if (KDTREE2D_ISECT_TRI_RECURSE_NEG) {
+			return true;
+		}
+	}
+	else {
+		if (KDTREE2D_ISECT_TRI_RECURSE_NEG) {
+			return true;
+		}
+		if (KDTREE2D_ISECT_TRI_RECURSE_POS) {
+			return true;
+		}
+	}
+
+#undef KDTREE2D_ISECT_TRI_RECURSE_NEG
+#undef KDTREE2D_ISECT_TRI_RECURSE_POS
+
+	BLI_assert(node->index != KDNODE_UNSET);
+
+	return false;
+}
+
+static bool kdtree2d_isect_tri(
+        struct KDTree2D *tree,
+        const unsigned int ind[3])
+{
+	const float *vs[3];
+	unsigned int i;
+	struct KDRange2D bounds[2] = {
+	    {FLT_MAX, -FLT_MAX},
+	    {FLT_MAX, -FLT_MAX},
+	};
+	float tri_center[2] = {0.0f, 0.0f};
+
+	for (i = 0; i < 3; i++) {
+		vs[i] = tree->coords[ind[i]];
+
+		add_v2_v2(tri_center, vs[i]);
+
+		CLAMP_MAX(bounds[0].min, vs[i][0]);
+		CLAMP_MIN(bounds[0].max, vs[i][0]);
+		CLAMP_MAX(bounds[1].min, vs[i][1]);
+		CLAMP_MIN(bounds[1].max, vs[i][1]);
+	}
+
+	mul_v2_fl(tri_center, 1.0f / 3.0f);
+
+	return kdtree2d_isect_tri_recursive(tree, ind, vs, tri_center, bounds, &tree->nodes[tree->root]);
+}
+
+#endif  /* USE_KDTREE */
+
+
 static unsigned int *pf_tri_add(PolyFill *pf)
 {
 	return pf->tris[pf->tris_tot++];
@@ -147,6 +466,13 @@ static unsigned int *pf_tri_add(PolyFill *pf)
 
 static void pf_coord_remove(PolyFill *pf, PolyIndex *pi)
 {
+#ifdef USE_KDTREE
+	/* avoid double lookups, since convex coords are ignored when testing intersections */
+	if (pf->kdtree.totnode) {
+		kdtree2d_node_remove(&pf->kdtree, pi->index);
+	}
+#endif
+
 	pi->next->prev = pi->prev;
 	pi->prev->next = pi->next;
 
@@ -221,6 +547,9 @@ static void pf_triangulate(PolyFill *pf)
 #ifdef USE_CONVEX_SKIP
 			if (pi_prev->sign == CONVEX) {
 				pf->coords_tot_concave -= 1;
+#ifdef USE_KDTREE
+				kdtree2d_node_remove(&pf->kdtree, pi_prev->index);
+#endif
 			}
 #endif
 		}
@@ -229,6 +558,9 @@ static void pf_triangulate(PolyFill *pf)
 #ifdef USE_CONVEX_SKIP
 			if (pi_next->sign == CONVEX) {
 				pf->coords_tot_concave -= 1;
+#ifdef USE_KDTREE
+				kdtree2d_node_remove(&pf->kdtree, pi_next->index);
+#endif
 			}
 #endif
 		}
@@ -333,9 +665,11 @@ static bool pf_ear_tip_check(PolyFill *pf, PolyIndex *pi_ear_tip)
 	PolyIndex *pi_curr;
 	const float (*coords)[2] = pf->coords;
 
+#ifndef USE_KDTREE
 	const float *v1, *v2, *v3;
+#endif
 
-#ifdef USE_CONVEX_SKIP
+#if defined(USE_CONVEX_SKIP) && !defined(USE_KDTREE)
 	unsigned int coords_tot_concave_checked = 0;
 #endif
 
@@ -348,7 +682,7 @@ static bool pf_ear_tip_check(PolyFill *pf, PolyIndex *pi_ear_tip)
 		unsigned int coords_tot_concave_test = 0;
 		unsigned int i = pf->coords_tot;
 		while (i--) {
-			if (pf->indices[i].sign != CONVEX) {
+			if (coords_sign[indices[i]] != CONVEX) {
 				coords_tot_concave_test += 1;
 			}
 		}
@@ -366,6 +700,21 @@ static bool pf_ear_tip_check(PolyFill *pf, PolyIndex *pi_ear_tip)
 		return false;
 	}
 
+#ifdef USE_KDTREE
+	{
+		const unsigned int ind[3] = {
+		    pi_ear_tip->index,
+		    pi_ear_tip->next->index,
+		    pi_ear_tip->prev->index};
+
+		if (kdtree2d_isect_tri(&pf->kdtree, ind)) {
+			return false;
+		}
+	}
+	(void)pi_curr;
+	(void)coords;
+#else
+
 	v1 = coords[pi_ear_tip->prev->index];
 	v2 = coords[pi_ear_tip->index];
 	v3 = coords[pi_ear_tip->next->index];
@@ -399,6 +748,8 @@ static bool pf_ear_tip_check(PolyFill *pf, PolyIndex *pi_ear_tip)
 #endif
 		}
 	}
+#endif  /* USE_KDTREE */
+
 	return true;
 }
 
@@ -420,30 +771,28 @@ static void pf_ear_tip_cut(PolyFill *pf, PolyIndex *pi_ear_tip)
  * \return triples of triangle indices in clockwise order.
  *         Note the returned array is reused for later calls to the same method.
  */
-static void polyfill_calc_ex(
+static void polyfill_prepare(
+        PolyFill *pf,
         const float (*coords)[2],
         const unsigned int coords_tot,
         int coords_sign,
         unsigned int (*r_tris)[3],
-
         PolyIndex *r_indices)
 {
-	PolyFill pf;
-
 	/* localize */
 	PolyIndex *indices = r_indices;
 
 	unsigned int i;
 
 	/* assign all polyfill members here */
-	pf.indices = r_indices;
-	pf.coords = coords;
-	pf.coords_tot = coords_tot;
+	pf->indices = r_indices;
+	pf->coords = coords;
+	pf->coords_tot = coords_tot;
 #ifdef USE_CONVEX_SKIP
-	pf.coords_tot_concave = 0;
+	pf->coords_tot_concave = 0;
 #endif
-	pf.tris = r_tris;
-	pf.tris_tot = 0;
+	pf->tris = r_tris;
+	pf->tris_tot = 0;
 
 	if (coords_sign == 0) {
 		coords_sign = (cross_poly_v2(coords, coords_tot) >= 0.0f) ? 1 : -1;
@@ -481,15 +830,31 @@ static void polyfill_calc_ex(
 
 	for (i = 0; i < coords_tot; i++) {
 		PolyIndex *pi = &indices[i];
-		pf_coord_sign_calc(&pf, pi);
+		pf_coord_sign_calc(pf, pi);
 #ifdef USE_CONVEX_SKIP
 		if (pi->sign != CONVEX) {
-			pf.coords_tot_concave += 1;
+			pf->coords_tot_concave += 1;
 		}
 #endif
 	}
+}
 
-	pf_triangulate(&pf);
+static void polyfill_calc(
+        PolyFill *pf)
+{
+#ifdef USE_KDTREE
+#ifdef USE_CONVEX_SKIP
+	if (pf->coords_tot_concave)
+#endif
+	{
+		kdtree2d_new(&pf->kdtree, pf->coords_tot_concave, pf->coords);
+		kdtree2d_init(&pf->kdtree, pf->coords_tot, pf->indices);
+		kdtree2d_balance(&pf->kdtree);
+		kdtree2d_init_mapping(&pf->kdtree);
+	}
+#endif
+
+	pf_triangulate(pf);
 }
 
 void BLI_polyfill_calc_arena(
@@ -500,19 +865,33 @@ void BLI_polyfill_calc_arena(
 
         struct MemArena *arena)
 {
+	PolyFill pf;
 	PolyIndex *indices = BLI_memarena_alloc(arena, sizeof(*indices) * coords_tot);
 
 #ifdef DEBUG_TIME
 	TIMEIT_START(polyfill2d);
 #endif
 
-	polyfill_calc_ex(
+	polyfill_prepare(
+	        &pf,
 	        coords, coords_tot, coords_sign,
 	        r_tris,
 	        /* cache */
-
 	        indices);
 
+#ifdef USE_KDTREE
+	if (pf.coords_tot_concave) {
+		pf.kdtree.nodes = BLI_memarena_alloc(arena, sizeof(*pf.kdtree.nodes) * pf.coords_tot_concave);
+		pf.kdtree.nodes_map = memset(BLI_memarena_alloc(arena, sizeof(*pf.kdtree.nodes_map) * coords_tot),
+		                             0xff, sizeof(*pf.kdtree.nodes_map) * coords_tot);
+	}
+	else {
+		pf.kdtree.totnode = 0;
+	}
+#endif
+
+	polyfill_calc(&pf);
+
 	/* indices are no longer needed,
 	 * caller can clear arena */
 
@@ -527,19 +906,33 @@ void BLI_polyfill_calc(
         const int coords_sign,
         unsigned int (*r_tris)[3])
 {
+	PolyFill pf;
 	PolyIndex *indices = BLI_array_alloca(indices, coords_tot);
 
 #ifdef DEBUG_TIME
 	TIMEIT_START(polyfill2d);
 #endif
 
-	polyfill_calc_ex(
+	polyfill_prepare(
+	        &pf,
 	        coords, coords_tot, coords_sign,
 	        r_tris,
 	        /* cache */
-
 	        indices);
 
+#ifdef USE_KDTREE
+	if (pf.coords_tot_concave) {
+		pf.kdtree.nodes = BLI_array_alloca(pf.kdtree.nodes, pf.coords_tot_concave);
+		pf.kdtree.nodes_map = memset(BLI_array_alloca(pf.kdtree.nodes_map, coords_tot),
+		                             0xff, sizeof(*pf.kdtree.nodes_map) * coords_tot);
+	}
+	else {
+		pf.kdtree.totnode = 0;
+	}
+#endif
+
+	polyfill_calc(&pf);
+
 #ifdef DEBUG_TIME
 	TIMEIT_END(polyfill2d);
 #endif