Merge branch 'blender2.7'
[blender.git] / source / blender / bmesh / operators / bmo_connect_pair.c
1 /*
2  * This program is free software; you can redistribute it and/or
3  * modify it under the terms of the GNU General Public License
4  * as published by the Free Software Foundation; either version 2
5  * of the License, or (at your option) any later version.
6  *
7  * This program is distributed in the hope that it will be useful,
8  * but WITHOUT ANY WARRANTY; without even the implied warranty of
9  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
10  * GNU General Public License for more details.
11  *
12  * You should have received a copy of the GNU General Public License
13  * along with this program; if not, write to the Free Software Foundation,
14  * Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
15  */
16
17 /** \file \ingroup bmesh
18  *
19  * Connect vertex pair across multiple faces (splits faces).
20  */
21
22 #include "MEM_guardedalloc.h"
23
24 #include "BLI_math.h"
25 #include "BLI_utildefines.h"
26 #include "BLI_heap_simple.h"
27
28 #include "bmesh.h"
29
30 #include "intern/bmesh_operators_private.h" /* own include */
31
32 #include "BLI_mempool.h"
33
34 /**
35  * Method for connecting across many faces.
36  *
37  * - use the line between both verts and their normal average to construct a matrix.
38  * - using the matrix, we can find all intersecting verts/edges.
39  * - walk the connected data and find the shortest path.
40  *   - store a heap of paths which are being scanned (#PathContext.states).
41  *   - continuously search the shortest path in the heap.
42  *   - never step over the same element twice (tag elements as #ELE_TOUCHED).
43  *     this avoids going into an eternal loop if there are many possible branches (see T45582).
44  *   - when running into a branch, create a new #PathLinkState state and add to the heap.
45  *   - when the target is reached, finish - since none of the other paths can be shorter then the one just found.
46  * - if the connection can't be found - fail.
47  * - with the connection found, split all edges tagging verts (or tag verts that sit on the intersection).
48  * - run the standard connect operator.
49  */
50
51 #define CONNECT_EPS 0.0001f
52 #define VERT_OUT 1
53 #define VERT_EXCLUDE 2
54
55 /* typically hidden faces */
56 #define FACE_EXCLUDE 2
57
58 /* any element we've walked over (only do it once!) */
59 #define ELE_TOUCHED 4
60
61 #define FACE_WALK_TEST(f)  (CHECK_TYPE_INLINE(f, BMFace *), \
62         BMO_face_flag_test(pc->bm_bmoflag, f, FACE_EXCLUDE) == 0)
63 #define VERT_WALK_TEST(v)  (CHECK_TYPE_INLINE(v, BMVert *), \
64         BMO_vert_flag_test(pc->bm_bmoflag, v, VERT_EXCLUDE) == 0)
65
66 #if 0
67 #define ELE_TOUCH_TEST(e) ( \
68         CHECK_TYPE_ANY(e, BMVert *, BMEdge *, BMElem *, BMElemF *), \
69         BMO_elem_flag_test(pc->bm_bmoflag, (BMElemF *)e, ELE_TOUCHED) \
70 )
71 #endif
72 #define ELE_TOUCH_MARK(e) { \
73         CHECK_TYPE_ANY(e, BMVert *, BMEdge *, BMElem *, BMElemF *); \
74         BMO_elem_flag_enable(pc->bm_bmoflag, (BMElemF *)e, ELE_TOUCHED); \
75 } ((void)0)
76
77 #define ELE_TOUCH_TEST_VERT(v) BMO_vert_flag_test(pc->bm_bmoflag, v, ELE_TOUCHED)
78 // #define ELE_TOUCH_MARK_VERT(v) BMO_vert_flag_enable(pc->bm_bmoflag, (BMElemF *)v, ELE_TOUCHED)
79
80 #define ELE_TOUCH_TEST_EDGE(e) BMO_edge_flag_test(pc->bm_bmoflag, e, ELE_TOUCHED)
81 // #define ELE_TOUCH_MARK_EDGE(e) BMO_edge_flag_enable(pc->bm_bmoflag, (BMElemF *)e, ELE_TOUCHED)
82
83 // #define ELE_TOUCH_TEST_FACE(f) BMO_face_flag_test(pc->bm_bmoflag, f, ELE_TOUCHED)
84 // #define ELE_TOUCH_MARK_FACE(f) BMO_face_flag_enable(pc->bm_bmoflag, (BMElemF *)f, ELE_TOUCHED)
85
86 // #define DEBUG_PRINT
87
88 typedef struct PathContext {
89         HeapSimple *states;
90         float matrix[3][3];
91         float axis_sep;
92
93         /* only to access BMO flags */
94         BMesh *bm_bmoflag;
95
96         BMVert *v_a, *v_b;
97
98         BLI_mempool *link_pool;
99 } PathContext;
100
101 /**
102  * Single linked list where each item contains state and points to previous path item.
103  */
104 typedef struct PathLink {
105         struct PathLink *next;
106         BMElem *ele;       /* edge or vert */
107         BMElem *ele_from;  /* edge or face we came from (not 'next->ele') */
108 } PathLink;
109
110 typedef struct PathLinkState {
111         /* chain of links */
112         struct PathLink *link_last;
113
114         /* length along links */
115         float dist;
116         float co_prev[3];
117 } PathLinkState;
118
119 /**
120  * \name Min Dist Dir Util
121  *
122  * Simply getting the closest intersecting vert/edge is _not_ good enough. see T43792
123  * we need to get the closest in both directions since the absolute closest may be a dead-end.
124  *
125  * Logic is simple:
126  *
127  * - first intersection, store the direction.
128  * - successive intersections will update the first distance if its aligned with the first hit.
129  *   otherwise update the opposite distance.
130  * - caller stores best outcome in both directions.
131  *
132  * \{ */
133
134 typedef struct MinDistDir {
135         /* distance in both directions (FLT_MAX == uninitialized) */
136         float dist_min[2];
137         /* direction of the first intersection found */
138         float dir[3];
139 } MinDistDir;
140
141 #define MIN_DIST_DIR_INIT {{FLT_MAX, FLT_MAX}}
142
143 static int min_dist_dir_test(MinDistDir *mddir, const float dist_dir[3], const float dist_sq)
144 {
145
146         if (mddir->dist_min[0] == FLT_MAX) {
147                 return 0;
148         }
149         else {
150                 if (dot_v3v3(dist_dir, mddir->dir) > 0.0f) {
151                         if (dist_sq < mddir->dist_min[0]) {
152                                 return 0;
153                         }
154                 }
155                 else {
156                         if (dist_sq < mddir->dist_min[1]) {
157                                 return 1;
158                         }
159                 }
160         }
161
162         return -1;
163 }
164
165 static void min_dist_dir_update(MinDistDir *dist, const float dist_dir[3])
166 {
167         if (dist->dist_min[0] == FLT_MAX) {
168                 copy_v3_v3(dist->dir, dist_dir);
169         }
170 }
171
172 /** \} */
173
174
175 static int state_isect_co_pair(
176         const PathContext *pc,
177         const float co_a[3], const float co_b[3])
178 {
179         const float diff_a = dot_m3_v3_row_x((float (*)[3])pc->matrix, co_a) - pc->axis_sep;
180         const float diff_b = dot_m3_v3_row_x((float (*)[3])pc->matrix, co_b) - pc->axis_sep;
181
182         const int test_a = (fabsf(diff_a) < CONNECT_EPS) ? 0 : (diff_a < 0.0f) ? -1 : 1;
183         const int test_b = (fabsf(diff_b) < CONNECT_EPS) ? 0 : (diff_b < 0.0f) ? -1 : 1;
184
185         if ((test_a && test_b) && (test_a != test_b)) {
186                 return 1;  /* on either side */
187         }
188         else {
189                 return 0;
190         }
191 }
192
193 static int state_isect_co_exact(
194         const PathContext *pc,
195         const float co[3])
196 {
197         const float diff = dot_m3_v3_row_x((float (*)[3])pc->matrix, co) - pc->axis_sep;
198         return (fabsf(diff) <= CONNECT_EPS);
199 }
200
201 static float state_calc_co_pair_fac(
202         const PathContext *pc,
203         const float co_a[3], const float co_b[3])
204 {
205         float diff_a, diff_b, diff_tot;
206
207         diff_a = fabsf(dot_m3_v3_row_x((float (*)[3])pc->matrix, co_a) - pc->axis_sep);
208         diff_b = fabsf(dot_m3_v3_row_x((float (*)[3])pc->matrix, co_b) - pc->axis_sep);
209         diff_tot = (diff_a + diff_b);
210         return (diff_tot > FLT_EPSILON) ? (diff_a / diff_tot) : 0.5f;
211 }
212
213 static void state_calc_co_pair(
214         const PathContext *pc,
215         const float co_a[3], const float co_b[3],
216         float r_co[3])
217 {
218         const float fac = state_calc_co_pair_fac(pc, co_a, co_b);
219         interp_v3_v3v3(r_co, co_a, co_b, fac);
220 }
221
222 #ifndef NDEBUG
223 /**
224  * Ideally we wouldn't need this and for most cases we don't.
225  * But when a face has vertices that are on the boundary more than once this becomes tricky.
226  */
227 static bool state_link_find(const PathLinkState *state, BMElem *ele)
228 {
229         PathLink *link = state->link_last;
230         BLI_assert(ELEM(ele->head.htype, BM_VERT, BM_EDGE, BM_FACE));
231         if (link) {
232                 do {
233                         if (link->ele == ele) {
234                                 return true;
235                         }
236                 } while ((link = link->next));
237         }
238         return false;
239 }
240 #endif
241
242 static void state_link_add(
243         PathContext *pc, PathLinkState *state,
244         BMElem *ele, BMElem *ele_from)
245 {
246         PathLink *step_new = BLI_mempool_alloc(pc->link_pool);
247         BLI_assert(ele != ele_from);
248         BLI_assert(state_link_find(state, ele) == false);
249
250         /* never walk onto this again */
251         ELE_TOUCH_MARK(ele);
252
253 #ifdef DEBUG_PRINT
254         printf("%s: adding to state %p, %.4f - ", __func__, state, state->dist);
255         if (ele->head.htype == BM_VERT) {
256                 printf("vert %d, ", BM_elem_index_get(ele));
257         }
258         else if (ele->head.htype == BM_EDGE) {
259                 printf("edge %d, ", BM_elem_index_get(ele));
260         }
261         else {
262                 BLI_assert(0);
263         }
264
265         if (ele_from == NULL) {
266                 printf("from NULL\n");
267         }
268         else if (ele_from->head.htype == BM_EDGE) {
269                 printf("from edge %d\n", BM_elem_index_get(ele_from));
270         }
271         else if (ele_from->head.htype == BM_FACE) {
272                 printf("from face %d\n", BM_elem_index_get(ele_from));
273         }
274         else {
275                 BLI_assert(0);
276         }
277 #endif
278
279         /* track distance */
280         {
281                 float co[3];
282                 if (ele->head.htype == BM_VERT) {
283                         copy_v3_v3(co, ((BMVert *)ele)->co);
284                 }
285                 else if (ele->head.htype == BM_EDGE) {
286                         state_calc_co_pair(pc, ((BMEdge *)ele)->v1->co, ((BMEdge *)ele)->v2->co, co);
287                 }
288                 else {
289                         BLI_assert(0);
290                 }
291
292                 /* tally distance */
293                 if (ele_from) {
294                         state->dist += len_v3v3(state->co_prev, co);
295                 }
296                 copy_v3_v3(state->co_prev, co);
297         }
298
299         step_new->ele = ele;
300         step_new->ele_from = ele_from;
301         step_new->next = state->link_last;
302         state->link_last = step_new;
303 }
304
305 static PathLinkState *state_dupe_add(
306         PathLinkState *state, const PathLinkState *state_orig)
307 {
308         state = MEM_mallocN(sizeof(*state), __func__);
309         *state = *state_orig;
310         return state;
311 }
312
313 static PathLinkState *state_link_add_test(
314         PathContext *pc, PathLinkState *state, const PathLinkState *state_orig,
315         BMElem *ele, BMElem *ele_from)
316 {
317         const bool is_new = (state_orig->link_last != state->link_last);
318         if (is_new) {
319                 state = state_dupe_add(state, state_orig);
320         }
321
322         state_link_add(pc, state, ele, ele_from);
323
324         /* after adding a link so we use the updated 'state->dist' */
325         if (is_new) {
326                 BLI_heapsimple_insert(pc->states, state->dist, state);
327         }
328
329         return state;
330 }
331
332 /* walk around the face edges */
333 static PathLinkState *state_step__face_edges(
334         PathContext *pc,
335         PathLinkState *state, const PathLinkState *state_orig,
336         BMLoop *l_iter, BMLoop *l_last,
337         MinDistDir *mddir)
338 {
339
340         BMLoop *l_iter_best[2] = {NULL, NULL};
341         int i;
342
343         do {
344                 if (state_isect_co_pair(pc, l_iter->v->co, l_iter->next->v->co)) {
345                         float dist_test;
346                         float co_isect[3];
347                         float dist_dir[3];
348                         int index;
349
350                         state_calc_co_pair(pc, l_iter->v->co, l_iter->next->v->co, co_isect);
351
352                         sub_v3_v3v3(dist_dir, co_isect, state_orig->co_prev);
353                         dist_test = len_squared_v3(dist_dir);
354                         if ((index = min_dist_dir_test(mddir, dist_dir, dist_test)) != -1) {
355                                 BMElem *ele_next      = (BMElem *)l_iter->e;
356                                 BMElem *ele_next_from = (BMElem *)l_iter->f;
357
358                                 if (FACE_WALK_TEST((BMFace *)ele_next_from) &&
359                                     (ELE_TOUCH_TEST_EDGE((BMEdge *)ele_next) == false))
360                                 {
361                                         min_dist_dir_update(mddir, dist_dir);
362                                         mddir->dist_min[index] = dist_test;
363                                         l_iter_best[index] = l_iter;
364                                 }
365                         }
366                 }
367         } while ((l_iter = l_iter->next) != l_last);
368
369         for (i = 0; i < 2; i++) {
370                 if ((l_iter = l_iter_best[i])) {
371                         BMElem *ele_next      = (BMElem *)l_iter->e;
372                         BMElem *ele_next_from = (BMElem *)l_iter->f;
373                         state = state_link_add_test(pc, state, state_orig, ele_next, ele_next_from);
374                 }
375         }
376
377         return state;
378 }
379
380 /* walk around the face verts */
381 static PathLinkState *state_step__face_verts(
382         PathContext *pc,
383         PathLinkState *state, const PathLinkState *state_orig,
384         BMLoop *l_iter, BMLoop *l_last,
385         MinDistDir *mddir)
386 {
387         BMLoop *l_iter_best[2] = {NULL, NULL};
388         int i;
389
390         do {
391                 if (state_isect_co_exact(pc, l_iter->v->co)) {
392                         float dist_test;
393                         const float *co_isect = l_iter->v->co;
394                         float dist_dir[3];
395                         int index;
396
397                         sub_v3_v3v3(dist_dir, co_isect, state_orig->co_prev);
398                         dist_test = len_squared_v3(dist_dir);
399                         if ((index = min_dist_dir_test(mddir, dist_dir, dist_test)) != -1) {
400                                 BMElem *ele_next      = (BMElem *)l_iter->v;
401                                 BMElem *ele_next_from = (BMElem *)l_iter->f;
402
403                                 if (FACE_WALK_TEST((BMFace *)ele_next_from) &&
404                                     (ELE_TOUCH_TEST_VERT((BMVert *)ele_next) == false))
405                                 {
406                                         min_dist_dir_update(mddir, dist_dir);
407                                         mddir->dist_min[index] = dist_test;
408                                         l_iter_best[index] = l_iter;
409                                 }
410                         }
411                 }
412         } while ((l_iter = l_iter->next) != l_last);
413
414         for (i = 0; i < 2; i++) {
415                 if ((l_iter = l_iter_best[i])) {
416                         BMElem *ele_next      = (BMElem *)l_iter->v;
417                         BMElem *ele_next_from = (BMElem *)l_iter->f;
418                         state = state_link_add_test(pc, state, state_orig, ele_next, ele_next_from);
419                 }
420         }
421
422         return state;
423 }
424
425 static bool state_step(PathContext *pc, PathLinkState *state)
426 {
427         PathLinkState state_orig = *state;
428         BMElem *ele = state->link_last->ele;
429         const void *ele_from = state->link_last->ele_from;
430
431         if (ele->head.htype == BM_EDGE) {
432                 BMEdge *e = (BMEdge *)ele;
433
434                 BMIter liter;
435                 BMLoop *l_start;
436
437                 BM_ITER_ELEM (l_start, &liter, e, BM_LOOPS_OF_EDGE) {
438                         if ((l_start->f != ele_from) &&
439                             FACE_WALK_TEST(l_start->f))
440                         {
441                                 MinDistDir mddir = MIN_DIST_DIR_INIT;
442                                 /* very similar to block below */
443                                 state = state_step__face_edges(pc, state, &state_orig,
444                                                                l_start->next, l_start, &mddir);
445                                 state = state_step__face_verts(pc, state, &state_orig,
446                                                                l_start->next->next, l_start, &mddir);
447                         }
448                 }
449         }
450         else if (ele->head.htype == BM_VERT) {
451                 BMVert *v = (BMVert *)ele;
452
453                 /* vert loops */
454                 {
455                         BMIter liter;
456                         BMLoop *l_start;
457
458                         BM_ITER_ELEM (l_start, &liter, v, BM_LOOPS_OF_VERT) {
459                                 if ((l_start->f != ele_from) &&
460                                     FACE_WALK_TEST(l_start->f))
461                                 {
462                                         MinDistDir mddir = MIN_DIST_DIR_INIT;
463                                         /* very similar to block above */
464                                         state = state_step__face_edges(pc, state, &state_orig,
465                                                                        l_start->next, l_start->prev, &mddir);
466                                         if (l_start->f->len > 3) {
467                                                 /* adjacent verts are handled in state_step__vert_edges */
468                                                 state = state_step__face_verts(pc, state, &state_orig,
469                                                                                l_start->next->next, l_start->prev, &mddir);
470                                         }
471                                 }
472                         }
473                 }
474
475                 /* vert edges  */
476                 {
477                         BMIter eiter;
478                         BMEdge *e;
479                         BM_ITER_ELEM (e, &eiter, v, BM_EDGES_OF_VERT) {
480                                 BMVert *v_other = BM_edge_other_vert(e, v);
481                                 if (((BMElem *)e != ele_from) &&
482                                     VERT_WALK_TEST(v_other))
483                                 {
484                                         if (state_isect_co_exact(pc, v_other->co)) {
485                                                 BMElem *ele_next      = (BMElem *)v_other;
486                                                 BMElem *ele_next_from = (BMElem *)e;
487                                                 if (ELE_TOUCH_TEST_VERT((BMVert *)ele_next) == false) {
488                                                         state = state_link_add_test(pc, state, &state_orig, ele_next, ele_next_from);
489                                                 }
490                                         }
491                                 }
492                         }
493                 }
494         }
495         else {
496                 BLI_assert(0);
497         }
498         return (state_orig.link_last != state->link_last);
499 }
500
501 /**
502  * Get a orientation matrix from 2 vertices.
503  */
504 static void bm_vert_pair_to_matrix(BMVert *v_pair[2], float r_unit_mat[3][3])
505 {
506         const float eps = 1e-8f;
507
508         float basis_dir[3];
509         float basis_tmp[3];
510         float basis_nor[3];
511
512         sub_v3_v3v3(basis_dir, v_pair[0]->co, v_pair[1]->co);
513         normalize_v3(basis_dir);
514
515 #if 0
516         add_v3_v3v3(basis_nor, v_pair[0]->no, v_pair[1]->no);
517         cross_v3_v3v3(basis_tmp, basis_nor, basis_dir);
518         cross_v3_v3v3(basis_nor, basis_tmp, basis_dir);
519 #else
520         /* align both normals to the directions before combining */
521         {
522                 float basis_nor_a[3];
523                 float basis_nor_b[3];
524
525                 /* align normal to direction */
526                 project_plane_normalized_v3_v3v3(basis_nor_a, v_pair[0]->no, basis_dir);
527                 project_plane_normalized_v3_v3v3(basis_nor_b, v_pair[1]->no, basis_dir);
528
529                 /* don't normalize before combining so as normals approach the direction, they have less effect (T46784). */
530
531                 /* combine the normals */
532                 /* for flipped faces */
533                 if (dot_v3v3(basis_nor_a, basis_nor_b) < 0.0f) {
534                         negate_v3(basis_nor_b);
535                 }
536                 add_v3_v3v3(basis_nor, basis_nor_a, basis_nor_b);
537         }
538 #endif
539
540         /* get third axis */
541         normalize_v3(basis_nor);
542         cross_v3_v3v3(basis_tmp, basis_dir, basis_nor);
543
544
545         /* Try get the axis from surrounding faces, fallback to 'ortho_v3_v3' */
546         if (UNLIKELY(normalize_v3(basis_tmp) < eps)) {
547                 /* vertex normals are directly opposite */
548
549                 /* find the loop with the lowest angle */
550                 struct { float nor[3]; float angle_cos; } axis_pair[2];
551                 int i;
552
553                 for (i = 0; i < 2; i++) {
554                         BMIter liter;
555                         BMLoop *l;
556
557                         zero_v2(axis_pair[i].nor);
558                         axis_pair[i].angle_cos = -FLT_MAX;
559
560                         BM_ITER_ELEM (l, &liter, v_pair[i], BM_LOOPS_OF_VERT) {
561                                 float basis_dir_proj[3];
562                                 float angle_cos_test;
563
564                                 /* project basis dir onto the normal to find its closest angle */
565                                 project_plane_normalized_v3_v3v3(basis_dir_proj, basis_dir, l->f->no);
566
567                                 if (normalize_v3(basis_dir_proj) > eps) {
568                                         angle_cos_test = dot_v3v3(basis_dir_proj, basis_dir);
569
570                                         if (angle_cos_test > axis_pair[i].angle_cos) {
571                                                 axis_pair[i].angle_cos = angle_cos_test;
572                                                 copy_v3_v3(axis_pair[i].nor, basis_dir_proj);
573                                         }
574                                 }
575                         }
576                 }
577
578                 /* create a new 'basis_nor' from the best direction.
579                  * note: we could add the directions,
580                  * but this more often gives 45d rotated matrix, so just use the best one. */
581                 copy_v3_v3(basis_nor, axis_pair[axis_pair[0].angle_cos < axis_pair[1].angle_cos].nor);
582                 project_plane_normalized_v3_v3v3(basis_nor, basis_nor, basis_dir);
583
584                 cross_v3_v3v3(basis_tmp, basis_dir, basis_nor);
585
586                 /* last resort, pick _any_ ortho axis */
587                 if (UNLIKELY(normalize_v3(basis_tmp) < eps)) {
588                         ortho_v3_v3(basis_nor, basis_dir);
589                         normalize_v3(basis_nor);
590                         cross_v3_v3v3(basis_tmp, basis_dir, basis_nor);
591                         normalize_v3(basis_tmp);
592                 }
593         }
594
595         copy_v3_v3(r_unit_mat[0], basis_tmp);
596         copy_v3_v3(r_unit_mat[1], basis_dir);
597         copy_v3_v3(r_unit_mat[2], basis_nor);
598         if (invert_m3(r_unit_mat) == false) {
599                 unit_m3(r_unit_mat);
600         }
601 }
602
603 void bmo_connect_vert_pair_exec(BMesh *bm, BMOperator *op)
604 {
605         BMOpSlot *op_verts_slot = BMO_slot_get(op->slots_in, "verts");
606
607         PathContext pc;
608         PathLinkState state_best = {NULL};
609
610         if (op_verts_slot->len != 2) {
611                 /* fail! */
612                 return;
613         }
614
615         pc.bm_bmoflag = bm;
616         pc.v_a = ((BMVert **)op_verts_slot->data.p)[0];
617         pc.v_b = ((BMVert **)op_verts_slot->data.p)[1];
618
619         /* fail! */
620         if (!(pc.v_a && pc.v_b)) {
621                 return;
622         }
623
624 #ifdef DEBUG_PRINT
625         printf("%s: v_a: %d\n", __func__, BM_elem_index_get(pc.v_a));
626         printf("%s: v_b: %d\n", __func__, BM_elem_index_get(pc.v_b));
627 #endif
628
629         /* tag so we won't touch ever (typically hidden faces) */
630         BMO_slot_buffer_flag_enable(bm, op->slots_in, "faces_exclude", BM_FACE, FACE_EXCLUDE);
631         BMO_slot_buffer_flag_enable(bm, op->slots_in, "verts_exclude", BM_VERT, VERT_EXCLUDE);
632
633         /* setup context */
634         {
635                 pc.states = BLI_heapsimple_new();
636                 pc.link_pool = BLI_mempool_create(sizeof(PathLink), 0, 512, BLI_MEMPOOL_NOP);
637         }
638
639         /* calculate matrix */
640         {
641                 bm_vert_pair_to_matrix(&pc.v_a, pc.matrix);
642                 pc.axis_sep = dot_m3_v3_row_x(pc.matrix, pc.v_a->co);
643         }
644
645         /* add first vertex */
646         {
647                 PathLinkState *state;
648                 state = MEM_callocN(sizeof(*state), __func__);
649                 state_link_add(&pc, state, (BMElem *)pc.v_a, NULL);
650                 BLI_heapsimple_insert(pc.states, state->dist, state);
651         }
652
653
654         while (!BLI_heapsimple_is_empty(pc.states)) {
655
656 #ifdef DEBUG_PRINT
657                 printf("\n%s: stepping %u\n", __func__, BLI_heapsimple_len(pc.states));
658 #endif
659
660                 while (!BLI_heapsimple_is_empty(pc.states)) {
661                         PathLinkState *state = BLI_heapsimple_pop_min(pc.states);
662
663                         /* either we insert this into 'pc.states' or its freed */
664                         bool continue_search;
665
666                         if (state->link_last->ele == (BMElem *)pc.v_b) {
667                                 /* pass, wait until all are found */
668 #ifdef DEBUG_PRINT
669                                 printf("%s: state %p loop found %.4f\n", __func__, state, state->dist);
670 #endif
671                                 state_best = *state;
672
673                                 /* we're done, exit all loops */
674                                 BLI_heapsimple_clear(pc.states, MEM_freeN);
675                                 continue_search = false;
676                         }
677                         else if (state_step(&pc, state)) {
678                                 continue_search = true;
679                         }
680                         else {
681                                 /* didn't reach the end, remove it,
682                                  * links are shared between states so just free the link_pool at the end */
683
684 #ifdef DEBUG_PRINT
685                                 printf("%s: state %p removed\n", __func__, state);
686 #endif
687                                 continue_search = false;
688                         }
689
690                         if (continue_search) {
691                                 BLI_heapsimple_insert(pc.states, state->dist, state);
692                         }
693                         else {
694                                 MEM_freeN(state);
695                         }
696                 }
697         }
698
699         if (state_best.link_last) {
700                 PathLink *link;
701
702                 /* find the best state */
703                 link = state_best.link_last;
704                 do {
705                         if (link->ele->head.htype == BM_EDGE) {
706                                 BMEdge *e = (BMEdge *)link->ele;
707                                 BMVert *v_new;
708                                 float e_fac = state_calc_co_pair_fac(&pc, e->v1->co, e->v2->co);
709                                 v_new = BM_edge_split(bm, e, e->v1, NULL, e_fac);
710                                 BMO_vert_flag_enable(bm, v_new, VERT_OUT);
711                         }
712                         else if (link->ele->head.htype == BM_VERT) {
713                                 BMVert *v = (BMVert *)link->ele;
714                                 BMO_vert_flag_enable(bm, v, VERT_OUT);
715                         }
716                         else {
717                                 BLI_assert(0);
718                         }
719                 } while ((link = link->next));
720         }
721
722         BMO_vert_flag_enable(bm, pc.v_a, VERT_OUT);
723         BMO_vert_flag_enable(bm, pc.v_b, VERT_OUT);
724
725         BLI_mempool_destroy(pc.link_pool);
726
727         BLI_heapsimple_free(pc.states, MEM_freeN);
728
729 #if 1
730         if (state_best.link_last) {
731                 BMOperator op_sub;
732                 BMO_op_initf(bm, &op_sub, 0,
733                              "connect_verts verts=%fv faces_exclude=%s check_degenerate=%b",
734                              VERT_OUT, op, "faces_exclude", true);
735                 BMO_op_exec(bm, &op_sub);
736                 BMO_slot_copy(&op_sub, slots_out, "edges.out",
737                               op,      slots_out, "edges.out");
738                 BMO_op_finish(bm, &op_sub);
739         }
740 #endif
741 }