doxygen: add newline after \file
[blender.git] / source / blender / bmesh / tools / bmesh_path.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  * Find a path between 2 elements.
21  */
22
23 #include "MEM_guardedalloc.h"
24
25 #include "BLI_math.h"
26 #include "BLI_linklist.h"
27 #include "BLI_heap_simple.h"
28
29 #include "bmesh.h"
30 #include "bmesh_path.h"  /* own include */
31
32
33 /* -------------------------------------------------------------------- */
34 /* Generic Helpers */
35
36 /**
37  * Use skip options when we want to start measuring from a boundary.
38  */
39 static float step_cost_3_v3_ex(
40         const float v1[3], const float v2[3], const float v3[3],
41         bool skip_12, bool skip_23)
42 {
43         float d1[3], d2[3];
44
45         /* The cost is based on the simple sum of the length of the two edgees... */
46         sub_v3_v3v3(d1, v2, v1);
47         sub_v3_v3v3(d2, v3, v2);
48         const float cost_12 = normalize_v3(d1);
49         const float cost_23 = normalize_v3(d2);
50         const float cost = ((skip_12 ? 0.0f : cost_12) +
51                             (skip_23 ? 0.0f : cost_23));
52
53         /* but is biased to give higher values to sharp turns, so that it will take
54          * paths with fewer "turns" when selecting between equal-weighted paths between
55          * the two edges */
56         return cost * (1.0f + 0.5f * (2.0f - sqrtf(fabsf(dot_v3v3(d1, d2)))));
57 }
58
59 static float step_cost_3_v3(
60         const float v1[3], const float v2[3], const float v3[3])
61 {
62         return step_cost_3_v3_ex(v1, v2, v3, false, false);
63 }
64
65
66 /* -------------------------------------------------------------------- */
67 /* BM_mesh_calc_path_vert */
68
69 static void verttag_add_adjacent(
70         HeapSimple *heap, BMVert *v_a, BMVert **verts_prev, float *cost,
71         const struct BMCalcPathParams *params)
72 {
73         const int v_a_index = BM_elem_index_get(v_a);
74
75         {
76                 BMIter eiter;
77                 BMEdge *e;
78                 /* loop over faces of face, but do so by first looping over loops */
79                 BM_ITER_ELEM (e, &eiter, v_a, BM_EDGES_OF_VERT) {
80                         BMVert *v_b = BM_edge_other_vert(e, v_a);
81                         if (!BM_elem_flag_test(v_b, BM_ELEM_TAG)) {
82                                 /* we know 'v_b' is not visited, check it out! */
83                                 const int v_b_index = BM_elem_index_get(v_b);
84                                 const float cost_cut = params->use_topology_distance ?
85                                         1.0f : len_v3v3(v_a->co, v_b->co);
86                                 const float cost_new = cost[v_a_index] + cost_cut;
87
88                                 if (cost[v_b_index] > cost_new) {
89                                         cost[v_b_index] = cost_new;
90                                         verts_prev[v_b_index] = v_a;
91                                         BLI_heapsimple_insert(heap, cost_new, v_b);
92                                 }
93                         }
94                 }
95         }
96
97         if (params->use_step_face) {
98                 BMIter liter;
99                 BMLoop *l;
100                 /* loop over faces of face, but do so by first looping over loops */
101                 BM_ITER_ELEM (l, &liter, v_a, BM_LOOPS_OF_VERT) {
102                         if (l->f->len > 3) {
103                                 /* skip loops on adjacent edges */
104                                 BMLoop *l_iter = l->next->next;
105                                 do {
106                                         BMVert *v_b = l_iter->v;
107                                         if (!BM_elem_flag_test(v_b, BM_ELEM_TAG)) {
108                                                 /* we know 'v_b' is not visited, check it out! */
109                                                 const int v_b_index = BM_elem_index_get(v_b);
110                                                 const float cost_cut = params->use_topology_distance ?
111                                                         1.0f : len_v3v3(v_a->co, v_b->co);
112                                                 const float cost_new = cost[v_a_index] + cost_cut;
113
114                                                 if (cost[v_b_index] > cost_new) {
115                                                         cost[v_b_index] = cost_new;
116                                                         verts_prev[v_b_index] = v_a;
117                                                         BLI_heapsimple_insert(heap, cost_new, v_b);
118                                                 }
119                                         }
120                                 } while ((l_iter = l_iter->next) != l->prev);
121                         }
122                 }
123         }
124 }
125
126 LinkNode *BM_mesh_calc_path_vert(
127         BMesh *bm, BMVert *v_src, BMVert *v_dst, const struct BMCalcPathParams *params,
128         bool (*filter_fn)(BMVert *, void *user_data), void *user_data)
129 {
130         LinkNode *path = NULL;
131         /* BM_ELEM_TAG flag is used to store visited edges */
132         BMVert *v;
133         BMIter viter;
134         HeapSimple *heap;
135         float *cost;
136         BMVert **verts_prev;
137         int i, totvert;
138
139         /* note, would pass BM_EDGE except we are looping over all faces anyway */
140         // BM_mesh_elem_index_ensure(bm, BM_VERT /* | BM_EDGE */); // NOT NEEDED FOR FACETAG
141
142         BM_ITER_MESH_INDEX (v, &viter, bm, BM_VERTS_OF_MESH, i) {
143                 BM_elem_flag_set(v, BM_ELEM_TAG, !filter_fn(v, user_data));
144                 BM_elem_index_set(v, i); /* set_inline */
145         }
146         bm->elem_index_dirty &= ~BM_VERT;
147
148         /* alloc */
149         totvert = bm->totvert;
150         verts_prev = MEM_callocN(sizeof(*verts_prev) * totvert, __func__);
151         cost = MEM_mallocN(sizeof(*cost) * totvert, __func__);
152
153         copy_vn_fl(cost, totvert, 1e20f);
154
155         /*
156          * Arrays are now filled as follows:
157          *
158          * As the search continues, verts_prev[n] will be the previous verts on the shortest
159          * path found so far to face n. BM_ELEM_TAG is used to tag elements we have visited,
160          * cost[n] will contain the length of the shortest
161          * path to face n found so far, Finally, heap is a priority heap which is built on the
162          * the same data as the cost array, but inverted: it is a worklist of faces prioritized
163          * by the shortest path found so far to the face.
164          */
165
166         /* regular dijkstra shortest path, but over faces instead of vertices */
167         heap = BLI_heapsimple_new();
168         BLI_heapsimple_insert(heap, 0.0f, v_src);
169         cost[BM_elem_index_get(v_src)] = 0.0f;
170
171         while (!BLI_heapsimple_is_empty(heap)) {
172                 v = BLI_heapsimple_pop_min(heap);
173
174                 if (v == v_dst)
175                         break;
176
177                 if (!BM_elem_flag_test(v, BM_ELEM_TAG)) {
178                         BM_elem_flag_enable(v, BM_ELEM_TAG);
179                         verttag_add_adjacent(heap, v, verts_prev, cost, params);
180                 }
181         }
182
183         if (v == v_dst) {
184                 do {
185                         BLI_linklist_prepend(&path, v);
186                 } while ((v = verts_prev[BM_elem_index_get(v)]));
187         }
188
189         MEM_freeN(verts_prev);
190         MEM_freeN(cost);
191         BLI_heapsimple_free(heap, NULL);
192
193         return path;
194 }
195
196 /* -------------------------------------------------------------------- */
197 /* BM_mesh_calc_path_edge */
198
199 static float edgetag_cut_cost_vert(BMEdge *e_a, BMEdge *e_b, BMVert *v)
200 {
201         BMVert *v1 = BM_edge_other_vert(e_a, v);
202         BMVert *v2 = BM_edge_other_vert(e_b, v);
203         return step_cost_3_v3(v1->co, v->co, v2->co);
204 }
205
206 static float edgetag_cut_cost_face(BMEdge *e_a, BMEdge *e_b, BMFace *f)
207 {
208         float e_a_cent[3], e_b_cent[3], f_cent[3];
209
210         mid_v3_v3v3(e_a_cent, e_a->v1->co, e_a->v1->co);
211         mid_v3_v3v3(e_b_cent, e_b->v1->co, e_b->v1->co);
212
213         BM_face_calc_center_median_weighted(f, f_cent);
214
215         return step_cost_3_v3(e_a_cent, e_b_cent, f_cent);
216 }
217
218 static void edgetag_add_adjacent(
219         HeapSimple *heap, BMEdge *e_a, BMEdge **edges_prev, float *cost,
220         const struct BMCalcPathParams *params)
221 {
222         const int e_a_index = BM_elem_index_get(e_a);
223
224         /* unlike vert/face, stepping faces disables scanning connected edges
225          * and only steps over faces (selecting a ring of edges instead of a loop) */
226         if (params->use_step_face == false) {
227                 BMIter viter;
228                 BMVert *v;
229
230                 BMIter eiter;
231                 BMEdge *e_b;
232
233                 BM_ITER_ELEM (v, &viter, e_a, BM_VERTS_OF_EDGE) {
234
235                         /* don't walk over previous vertex */
236                         if ((edges_prev[e_a_index]) &&
237                             (BM_vert_in_edge(edges_prev[e_a_index], v)))
238                         {
239                                 continue;
240                         }
241
242                         BM_ITER_ELEM (e_b, &eiter, v, BM_EDGES_OF_VERT) {
243                                 if (!BM_elem_flag_test(e_b, BM_ELEM_TAG)) {
244                                         /* we know 'e_b' is not visited, check it out! */
245                                         const int e_b_index = BM_elem_index_get(e_b);
246                                         const float cost_cut = params->use_topology_distance ?
247                                                 1.0f : edgetag_cut_cost_vert(e_a, e_b, v);
248                                         const float cost_new = cost[e_a_index] + cost_cut;
249
250                                         if (cost[e_b_index] > cost_new) {
251                                                 cost[e_b_index] = cost_new;
252                                                 edges_prev[e_b_index] = e_a;
253                                                 BLI_heapsimple_insert(heap, cost_new, e_b);
254                                         }
255                                 }
256                         }
257                 }
258         }
259         else {
260                 BMLoop *l_first, *l_iter;
261
262                 l_iter = l_first = e_a->l;
263                 do {
264                         BMLoop *l_cycle_iter, *l_cycle_end;
265
266                         l_cycle_iter = l_iter->next;
267                         l_cycle_end = l_iter;
268
269                         /* good, but we need to allow this otherwise paths may fail to connect at all */
270 #if 0
271                         if (l_iter->f->len > 3) {
272                                 l_cycle_iter = l_cycle_iter->next;
273                                 l_cycle_end = l_cycle_end->prev;
274                         }
275 #endif
276
277                         do {
278                                 BMEdge *e_b = l_cycle_iter->e;
279                                 if (!BM_elem_flag_test(e_b, BM_ELEM_TAG)) {
280                                         /* we know 'e_b' is not visited, check it out! */
281                                         const int e_b_index = BM_elem_index_get(e_b);
282                                         const float cost_cut = params->use_topology_distance ?
283                                                 1.0f : edgetag_cut_cost_face(e_a, e_b, l_iter->f);
284                                         const float cost_new = cost[e_a_index] + cost_cut;
285
286                                         if (cost[e_b_index] > cost_new) {
287                                                 cost[e_b_index] = cost_new;
288                                                 edges_prev[e_b_index] = e_a;
289                                                 BLI_heapsimple_insert(heap, cost_new, e_b);
290                                         }
291                                 }
292                         } while ((l_cycle_iter = l_cycle_iter->next) != l_cycle_end);
293                 } while ((l_iter = l_iter->radial_next) != l_first);
294         }
295 }
296
297
298 LinkNode *BM_mesh_calc_path_edge(
299         BMesh *bm, BMEdge *e_src, BMEdge *e_dst, const struct BMCalcPathParams *params,
300         bool (*filter_fn)(BMEdge *, void *user_data), void *user_data)
301 {
302         LinkNode *path = NULL;
303         /* BM_ELEM_TAG flag is used to store visited edges */
304         BMEdge *e;
305         BMIter eiter;
306         HeapSimple *heap;
307         float *cost;
308         BMEdge **edges_prev;
309         int i, totedge;
310
311         /* note, would pass BM_EDGE except we are looping over all edges anyway */
312         BM_mesh_elem_index_ensure(bm, BM_VERT /* | BM_EDGE */);
313
314         BM_ITER_MESH_INDEX (e, &eiter, bm, BM_EDGES_OF_MESH, i) {
315                 BM_elem_flag_set(e, BM_ELEM_TAG, !filter_fn(e, user_data));
316                 BM_elem_index_set(e, i); /* set_inline */
317         }
318         bm->elem_index_dirty &= ~BM_EDGE;
319
320         /* alloc */
321         totedge = bm->totedge;
322         edges_prev = MEM_callocN(sizeof(*edges_prev) * totedge, "SeamPathPrevious");
323         cost = MEM_mallocN(sizeof(*cost) * totedge, "SeamPathCost");
324
325         copy_vn_fl(cost, totedge, 1e20f);
326
327         /*
328          * Arrays are now filled as follows:
329          *
330          * As the search continues, prevedge[n] will be the previous edge on the shortest
331          * path found so far to edge n. BM_ELEM_TAG is used to tag elements we have visited,
332          * cost[n] will contain the length of the shortest
333          * path to edge n found so far, Finally, heap is a priority heap which is built on the
334          * the same data as the cost array, but inverted: it is a worklist of edges prioritized
335          * by the shortest path found so far to the edge.
336          */
337
338         /* regular dijkstra shortest path, but over edges instead of vertices */
339         heap = BLI_heapsimple_new();
340         BLI_heapsimple_insert(heap, 0.0f, e_src);
341         cost[BM_elem_index_get(e_src)] = 0.0f;
342
343         while (!BLI_heapsimple_is_empty(heap)) {
344                 e = BLI_heapsimple_pop_min(heap);
345
346                 if (e == e_dst)
347                         break;
348
349                 if (!BM_elem_flag_test(e, BM_ELEM_TAG)) {
350                         BM_elem_flag_enable(e, BM_ELEM_TAG);
351                         edgetag_add_adjacent(heap, e, edges_prev, cost, params);
352                 }
353         }
354
355         if (e == e_dst) {
356                 do {
357                         BLI_linklist_prepend(&path, e);
358                 } while ((e = edges_prev[BM_elem_index_get(e)]));
359         }
360
361         MEM_freeN(edges_prev);
362         MEM_freeN(cost);
363         BLI_heapsimple_free(heap, NULL);
364
365         return path;
366 }
367
368
369 /* -------------------------------------------------------------------- */
370 /* BM_mesh_calc_path_face */
371
372 static float facetag_cut_cost_edge(BMFace *f_a, BMFace *f_b, BMEdge *e, const void * const f_endpoints[2])
373 {
374         float f_a_cent[3];
375         float f_b_cent[3];
376         float e_cent[3];
377
378         BM_face_calc_center_median_weighted(f_a, f_a_cent);
379         BM_face_calc_center_median_weighted(f_b, f_b_cent);
380 #if 0
381         mid_v3_v3v3(e_cent, e->v1->co, e->v2->co);
382 #else
383         /* for triangle fans it gives better results to pick a point on the edge */
384         {
385                 float ix_e[3], ix_f[3], f;
386                 isect_line_line_v3(e->v1->co, e->v2->co, f_a_cent, f_b_cent, ix_e, ix_f);
387                 f = line_point_factor_v3(ix_e, e->v1->co, e->v2->co);
388                 if (f < 0.0f) {
389                         copy_v3_v3(e_cent, e->v1->co);
390                 }
391                 else if (f > 1.0f) {
392                         copy_v3_v3(e_cent, e->v2->co);
393                 }
394                 else {
395                         copy_v3_v3(e_cent, ix_e);
396                 }
397         }
398 #endif
399
400         return step_cost_3_v3_ex(
401                 f_a_cent, e_cent, f_b_cent,
402                 (f_a == f_endpoints[0]), (f_b == f_endpoints[1]));
403 }
404
405 static float facetag_cut_cost_vert(BMFace *f_a, BMFace *f_b, BMVert *v, const void * const f_endpoints[2])
406 {
407         float f_a_cent[3];
408         float f_b_cent[3];
409
410         BM_face_calc_center_median_weighted(f_a, f_a_cent);
411         BM_face_calc_center_median_weighted(f_b, f_b_cent);
412
413         return step_cost_3_v3_ex(
414                 f_a_cent, v->co, f_b_cent,
415                 (f_a == f_endpoints[0]), (f_b == f_endpoints[1]));
416 }
417
418 static void facetag_add_adjacent(
419         HeapSimple *heap, BMFace *f_a, BMFace **faces_prev, float *cost,
420         const void * const f_endpoints[2], const struct BMCalcPathParams *params)
421 {
422         const int f_a_index = BM_elem_index_get(f_a);
423
424         /* loop over faces of face, but do so by first looping over loops */
425         {
426                 BMIter liter;
427                 BMLoop *l_a;
428
429                 BM_ITER_ELEM (l_a, &liter, f_a, BM_LOOPS_OF_FACE) {
430                         BMLoop *l_first, *l_iter;
431
432                         l_iter = l_first = l_a;
433                         do {
434                                 BMFace *f_b = l_iter->f;
435                                 if (!BM_elem_flag_test(f_b, BM_ELEM_TAG)) {
436                                         /* we know 'f_b' is not visited, check it out! */
437                                         const int f_b_index = BM_elem_index_get(f_b);
438                                         const float cost_cut = params->use_topology_distance ?
439                                                 1.0f : facetag_cut_cost_edge(f_a, f_b, l_iter->e, f_endpoints);
440                                         const float cost_new = cost[f_a_index] + cost_cut;
441
442                                         if (cost[f_b_index] > cost_new) {
443                                                 cost[f_b_index] = cost_new;
444                                                 faces_prev[f_b_index] = f_a;
445                                                 BLI_heapsimple_insert(heap, cost_new, f_b);
446                                         }
447                                 }
448                         } while ((l_iter = l_iter->radial_next) != l_first);
449                 }
450         }
451
452         if (params->use_step_face) {
453                 BMIter liter;
454                 BMLoop *l_a;
455
456                 BM_ITER_ELEM (l_a, &liter, f_a, BM_LOOPS_OF_FACE) {
457                         BMIter litersub;
458                         BMLoop *l_b;
459                         BM_ITER_ELEM (l_b, &litersub, l_a->v, BM_LOOPS_OF_VERT) {
460                                 if ((l_a != l_b) && !BM_loop_share_edge_check(l_a, l_b)) {
461                                         BMFace *f_b = l_b->f;
462                                         if (!BM_elem_flag_test(f_b, BM_ELEM_TAG)) {
463                                                 /* we know 'f_b' is not visited, check it out! */
464                                                 const int f_b_index = BM_elem_index_get(f_b);
465                                                 const float cost_cut = params->use_topology_distance ?
466                                                         1.0f : facetag_cut_cost_vert(f_a, f_b, l_a->v, f_endpoints);
467                                                 const float cost_new = cost[f_a_index] + cost_cut;
468
469                                                 if (cost[f_b_index] > cost_new) {
470                                                         cost[f_b_index] = cost_new;
471                                                         faces_prev[f_b_index] = f_a;
472                                                         BLI_heapsimple_insert(heap, cost_new, f_b);
473                                                 }
474                                         }
475                                 }
476                         }
477                 }
478         }
479 }
480
481 LinkNode *BM_mesh_calc_path_face(
482         BMesh *bm, BMFace *f_src, BMFace *f_dst, const struct BMCalcPathParams *params,
483         bool (*filter_fn)(BMFace *, void *user_data), void *user_data)
484 {
485         LinkNode *path = NULL;
486         /* BM_ELEM_TAG flag is used to store visited edges */
487         BMFace *f;
488         BMIter fiter;
489         HeapSimple *heap;
490         float *cost;
491         BMFace **faces_prev;
492         int i, totface;
493
494         /* Start measuring face path at the face edges, ignoring their centers. */
495         const void * const f_endpoints[2] = {f_src, f_dst};
496
497         /* note, would pass BM_EDGE except we are looping over all faces anyway */
498         // BM_mesh_elem_index_ensure(bm, BM_VERT /* | BM_EDGE */); // NOT NEEDED FOR FACETAG
499
500         BM_ITER_MESH_INDEX (f, &fiter, bm, BM_FACES_OF_MESH, i) {
501                 BM_elem_flag_set(f, BM_ELEM_TAG, !filter_fn(f, user_data));
502                 BM_elem_index_set(f, i); /* set_inline */
503         }
504         bm->elem_index_dirty &= ~BM_FACE;
505
506         /* alloc */
507         totface = bm->totface;
508         faces_prev = MEM_callocN(sizeof(*faces_prev) * totface, __func__);
509         cost = MEM_mallocN(sizeof(*cost) * totface, __func__);
510
511         copy_vn_fl(cost, totface, 1e20f);
512
513         /*
514          * Arrays are now filled as follows:
515          *
516          * As the search continues, faces_prev[n] will be the previous face on the shortest
517          * path found so far to face n. BM_ELEM_TAG is used to tag elements we have visited,
518          * cost[n] will contain the length of the shortest
519          * path to face n found so far, Finally, heap is a priority heap which is built on the
520          * the same data as the cost array, but inverted: it is a worklist of faces prioritized
521          * by the shortest path found so far to the face.
522          */
523
524         /* regular dijkstra shortest path, but over faces instead of vertices */
525         heap = BLI_heapsimple_new();
526         BLI_heapsimple_insert(heap, 0.0f, f_src);
527         cost[BM_elem_index_get(f_src)] = 0.0f;
528
529         while (!BLI_heapsimple_is_empty(heap)) {
530                 f = BLI_heapsimple_pop_min(heap);
531
532                 if (f == f_dst)
533                         break;
534
535                 if (!BM_elem_flag_test(f, BM_ELEM_TAG)) {
536                         BM_elem_flag_enable(f, BM_ELEM_TAG);
537                         facetag_add_adjacent(heap, f, faces_prev, cost, f_endpoints, params);
538                 }
539         }
540
541         if (f == f_dst) {
542                 do {
543                         BLI_linklist_prepend(&path, f);
544                 } while ((f = faces_prev[BM_elem_index_get(f)]));
545         }
546
547         MEM_freeN(faces_prev);
548         MEM_freeN(cost);
549         BLI_heapsimple_free(heap, NULL);
550
551         return path;
552 }