Polyfill2d: use kd-tree
authorCampbell Barton <ideasman42@gmail.com>
Wed, 11 Jun 2014 00:17:22 +0000 (10:17 +1000)
committerCampbell Barton <ideasman42@gmail.com>
Fri, 13 Jun 2014 22:27:19 +0000 (08:27 +1000)
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)

source/blender/blenlib/intern/polyfill2d.c

index 1c0b936a881abddab0b502e00d1fc3a57a9d99e2..30d1ab42b02a6d7ee72da0ebf7a0d133e5c8aeb9 100644 (file)
 #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"
 
 
 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