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