ClangFormat: apply to source, most of intern
[blender.git] / source / blender / blenkernel / intern / editmesh_bvh.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  * The Original Code is Copyright (C) 2010 by Blender Foundation.
17  * All rights reserved.
18  */
19
20 /** \file
21  * \ingroup bke
22  */
23
24 #include "MEM_guardedalloc.h"
25
26 #include "BLI_math.h"
27 #include "BLI_kdopbvh.h"
28
29 #include "BKE_editmesh.h"
30
31 #include "BKE_editmesh_bvh.h" /* own include */
32
33 struct BMBVHTree {
34   BVHTree *tree;
35
36   BMLoop *(*looptris)[3];
37   int looptris_tot;
38
39   BMesh *bm;
40
41   const float (*cos_cage)[3];
42   bool cos_cage_free;
43
44   int flag;
45 };
46
47 BMBVHTree *BKE_bmbvh_new_from_editmesh(BMEditMesh *em,
48                                        int flag,
49                                        const float (*cos_cage)[3],
50                                        const bool cos_cage_free)
51 {
52   return BKE_bmbvh_new(em->bm, em->looptris, em->tottri, flag, cos_cage, cos_cage_free);
53 }
54
55 BMBVHTree *BKE_bmbvh_new_ex(BMesh *bm,
56                             BMLoop *(*looptris)[3],
57                             int looptris_tot,
58                             int flag,
59                             const float (*cos_cage)[3],
60                             const bool cos_cage_free,
61                             bool (*test_fn)(BMFace *, void *user_data),
62                             void *user_data)
63 {
64   /* could become argument */
65   const float epsilon = FLT_EPSILON * 2.0f;
66
67   BMBVHTree *bmtree = MEM_callocN(sizeof(*bmtree), "BMBVHTree");
68   float cos[3][3];
69   int i;
70   int tottri;
71
72   /* avoid testing every tri */
73   BMFace *f_test, *f_test_prev;
74   bool test_fn_ret;
75
76   /* BKE_editmesh_tessface_calc() must be called already */
77   BLI_assert(looptris_tot != 0 || bm->totface == 0);
78
79   if (cos_cage) {
80     BM_mesh_elem_index_ensure(bm, BM_VERT);
81   }
82
83   bmtree->looptris = looptris;
84   bmtree->looptris_tot = looptris_tot;
85   bmtree->bm = bm;
86   bmtree->cos_cage = cos_cage;
87   bmtree->cos_cage_free = cos_cage_free;
88   bmtree->flag = flag;
89
90   if (test_fn) {
91     /* callback must do... */
92     BLI_assert(!(flag & (BMBVH_RESPECT_SELECT | BMBVH_RESPECT_HIDDEN)));
93
94     f_test_prev = NULL;
95     test_fn_ret = false;
96
97     tottri = 0;
98     for (i = 0; i < looptris_tot; i++) {
99       f_test = looptris[i][0]->f;
100       if (f_test != f_test_prev) {
101         test_fn_ret = test_fn(f_test, user_data);
102         f_test_prev = f_test;
103       }
104
105       if (test_fn_ret) {
106         tottri++;
107       }
108     }
109   }
110   else {
111     tottri = looptris_tot;
112   }
113
114   bmtree->tree = BLI_bvhtree_new(tottri, epsilon, 8, 8);
115
116   f_test_prev = NULL;
117   test_fn_ret = false;
118
119   for (i = 0; i < looptris_tot; i++) {
120     if (test_fn) {
121       /* note, the arrays wont align now! take care */
122       f_test = looptris[i][0]->f;
123       if (f_test != f_test_prev) {
124         test_fn_ret = test_fn(f_test, user_data);
125         f_test_prev = f_test;
126       }
127
128       if (!test_fn_ret) {
129         continue;
130       }
131     }
132
133     if (cos_cage) {
134       copy_v3_v3(cos[0], cos_cage[BM_elem_index_get(looptris[i][0]->v)]);
135       copy_v3_v3(cos[1], cos_cage[BM_elem_index_get(looptris[i][1]->v)]);
136       copy_v3_v3(cos[2], cos_cage[BM_elem_index_get(looptris[i][2]->v)]);
137     }
138     else {
139       copy_v3_v3(cos[0], looptris[i][0]->v->co);
140       copy_v3_v3(cos[1], looptris[i][1]->v->co);
141       copy_v3_v3(cos[2], looptris[i][2]->v->co);
142     }
143
144     BLI_bvhtree_insert(bmtree->tree, i, (float *)cos, 3);
145   }
146
147   BLI_bvhtree_balance(bmtree->tree);
148
149   return bmtree;
150 }
151
152 static bool bm_face_is_select(BMFace *f, void *UNUSED(user_data))
153 {
154   return (BM_elem_flag_test(f, BM_ELEM_SELECT) != 0);
155 }
156
157 static bool bm_face_is_not_hidden(BMFace *f, void *UNUSED(user_data))
158 {
159   return (BM_elem_flag_test(f, BM_ELEM_HIDDEN) == 0);
160 }
161
162 BMBVHTree *BKE_bmbvh_new(BMesh *bm,
163                          BMLoop *(*looptris)[3],
164                          int looptris_tot,
165                          int flag,
166                          const float (*cos_cage)[3],
167                          const bool cos_cage_free)
168 {
169   bool (*test_fn)(BMFace *, void *user_data);
170
171   if (flag & BMBVH_RESPECT_SELECT) {
172     test_fn = bm_face_is_select;
173   }
174   else if (flag & BMBVH_RESPECT_HIDDEN) {
175     test_fn = bm_face_is_not_hidden;
176   }
177   else {
178     test_fn = NULL;
179   }
180
181   flag &= ~(BMBVH_RESPECT_SELECT | BMBVH_RESPECT_HIDDEN);
182
183   return BKE_bmbvh_new_ex(
184       bm, looptris, looptris_tot, flag, cos_cage, cos_cage_free, test_fn, NULL);
185 }
186
187 void BKE_bmbvh_free(BMBVHTree *bmtree)
188 {
189   BLI_bvhtree_free(bmtree->tree);
190
191   if (bmtree->cos_cage && bmtree->cos_cage_free) {
192     MEM_freeN((void *)bmtree->cos_cage);
193   }
194
195   MEM_freeN(bmtree);
196 }
197
198 BVHTree *BKE_bmbvh_tree_get(BMBVHTree *bmtree)
199 {
200   return bmtree->tree;
201 }
202
203 /* -------------------------------------------------------------------- */
204 /* Utility BMesh cast/intersect functions */
205
206 /**
207  * Return the coords from a triangle.
208  */
209 static void bmbvh_tri_from_face(const float *cos[3],
210                                 const BMLoop **ltri,
211                                 const float (*cos_cage)[3])
212 {
213   if (cos_cage == NULL) {
214     cos[0] = ltri[0]->v->co;
215     cos[1] = ltri[1]->v->co;
216     cos[2] = ltri[2]->v->co;
217   }
218   else {
219     cos[0] = cos_cage[BM_elem_index_get(ltri[0]->v)];
220     cos[1] = cos_cage[BM_elem_index_get(ltri[1]->v)];
221     cos[2] = cos_cage[BM_elem_index_get(ltri[2]->v)];
222   }
223 }
224
225 /* taken from bvhutils.c */
226
227 /* -------------------------------------------------------------------- */
228 /* BKE_bmbvh_ray_cast */
229
230 struct RayCastUserData {
231   /* from the bmtree */
232   const BMLoop *(*looptris)[3];
233   const float (*cos_cage)[3];
234
235   /* from the hit */
236   float uv[2];
237 };
238
239 static BMFace *bmbvh_ray_cast_handle_hit(BMBVHTree *bmtree,
240                                          struct RayCastUserData *bmcb_data,
241                                          const BVHTreeRayHit *hit,
242                                          float *r_dist,
243                                          float r_hitout[3],
244                                          float r_cagehit[3])
245 {
246   if (r_hitout) {
247     if (bmtree->flag & BMBVH_RETURN_ORIG) {
248       BMLoop **ltri = bmtree->looptris[hit->index];
249       interp_v3_v3v3v3_uv(r_hitout, ltri[0]->v->co, ltri[1]->v->co, ltri[2]->v->co, bmcb_data->uv);
250     }
251     else {
252       copy_v3_v3(r_hitout, hit->co);
253     }
254
255     if (r_cagehit) {
256       copy_v3_v3(r_cagehit, hit->co);
257     }
258   }
259
260   if (r_dist) {
261     *r_dist = hit->dist;
262   }
263
264   return bmtree->looptris[hit->index][0]->f;
265 }
266
267 static void bmbvh_ray_cast_cb(void *userdata, int index, const BVHTreeRay *ray, BVHTreeRayHit *hit)
268 {
269   struct RayCastUserData *bmcb_data = userdata;
270   const BMLoop **ltri = bmcb_data->looptris[index];
271   float dist, uv[2];
272   const float *tri_cos[3];
273   bool isect;
274
275   bmbvh_tri_from_face(tri_cos, ltri, bmcb_data->cos_cage);
276
277   isect =
278       (ray->radius > 0.0f ?
279            isect_ray_tri_epsilon_v3(ray->origin,
280                                     ray->direction,
281                                     tri_cos[0],
282                                     tri_cos[1],
283                                     tri_cos[2],
284                                     &dist,
285                                     uv,
286                                     ray->radius) :
287 #ifdef USE_KDOPBVH_WATERTIGHT
288            isect_ray_tri_watertight_v3(
289                ray->origin, ray->isect_precalc, tri_cos[0], tri_cos[1], tri_cos[2], &dist, uv));
290 #else
291            isect_ray_tri_v3(
292                ray->origin, ray->direction, tri_cos[0], tri_cos[1], tri_cos[2], &dist, uv));
293 #endif
294
295   if (isect && dist < hit->dist) {
296     hit->dist = dist;
297     hit->index = index;
298
299     copy_v3_v3(hit->no, ltri[0]->f->no);
300
301     madd_v3_v3v3fl(hit->co, ray->origin, ray->direction, dist);
302
303     copy_v2_v2(bmcb_data->uv, uv);
304   }
305 }
306
307 BMFace *BKE_bmbvh_ray_cast(BMBVHTree *bmtree,
308                            const float co[3],
309                            const float dir[3],
310                            const float radius,
311                            float *r_dist,
312                            float r_hitout[3],
313                            float r_cagehit[3])
314 {
315   BVHTreeRayHit hit;
316   struct RayCastUserData bmcb_data;
317   const float dist = r_dist ? *r_dist : FLT_MAX;
318
319   if (bmtree->cos_cage)
320     BLI_assert(!(bmtree->bm->elem_index_dirty & BM_VERT));
321
322   hit.dist = dist;
323   hit.index = -1;
324
325   /* ok to leave 'uv' uninitialized */
326   bmcb_data.looptris = (const BMLoop *(*)[3])bmtree->looptris;
327   bmcb_data.cos_cage = (const float(*)[3])bmtree->cos_cage;
328
329   BLI_bvhtree_ray_cast(bmtree->tree, co, dir, radius, &hit, bmbvh_ray_cast_cb, &bmcb_data);
330
331   if (hit.index != -1 && hit.dist != dist) {
332     return bmbvh_ray_cast_handle_hit(bmtree, &bmcb_data, &hit, r_dist, r_hitout, r_cagehit);
333   }
334
335   return NULL;
336 }
337
338 /* -------------------------------------------------------------------- */
339 /* bmbvh_ray_cast_cb_filter */
340
341 /* Same as BKE_bmbvh_ray_cast but takes a callback to filter out faces.
342  */
343
344 struct RayCastUserData_Filter {
345   struct RayCastUserData bmcb_data;
346
347   BMBVHTree_FaceFilter filter_cb;
348   void *filter_userdata;
349 };
350
351 static void bmbvh_ray_cast_cb_filter(void *userdata,
352                                      int index,
353                                      const BVHTreeRay *ray,
354                                      BVHTreeRayHit *hit)
355 {
356   struct RayCastUserData_Filter *bmcb_data_filter = userdata;
357   struct RayCastUserData *bmcb_data = &bmcb_data_filter->bmcb_data;
358   const BMLoop **ltri = bmcb_data->looptris[index];
359   if (bmcb_data_filter->filter_cb(ltri[0]->f, bmcb_data_filter->filter_userdata)) {
360     bmbvh_ray_cast_cb(bmcb_data, index, ray, hit);
361   }
362 }
363
364 BMFace *BKE_bmbvh_ray_cast_filter(BMBVHTree *bmtree,
365                                   const float co[3],
366                                   const float dir[3],
367                                   const float radius,
368                                   float *r_dist,
369                                   float r_hitout[3],
370                                   float r_cagehit[3],
371                                   BMBVHTree_FaceFilter filter_cb,
372                                   void *filter_userdata)
373 {
374   BVHTreeRayHit hit;
375   struct RayCastUserData_Filter bmcb_data_filter;
376   struct RayCastUserData *bmcb_data = &bmcb_data_filter.bmcb_data;
377
378   const float dist = r_dist ? *r_dist : FLT_MAX;
379
380   bmcb_data_filter.filter_cb = filter_cb;
381   bmcb_data_filter.filter_userdata = filter_userdata;
382
383   if (bmtree->cos_cage)
384     BLI_assert(!(bmtree->bm->elem_index_dirty & BM_VERT));
385
386   hit.dist = dist;
387   hit.index = -1;
388
389   /* ok to leave 'uv' uninitialized */
390   bmcb_data->looptris = (const BMLoop *(*)[3])bmtree->looptris;
391   bmcb_data->cos_cage = (const float(*)[3])bmtree->cos_cage;
392
393   BLI_bvhtree_ray_cast(
394       bmtree->tree, co, dir, radius, &hit, bmbvh_ray_cast_cb_filter, &bmcb_data_filter);
395   if (hit.index != -1 && hit.dist != dist) {
396     return bmbvh_ray_cast_handle_hit(bmtree, bmcb_data, &hit, r_dist, r_hitout, r_cagehit);
397   }
398
399   return NULL;
400 }
401
402 /* -------------------------------------------------------------------- */
403 /* BKE_bmbvh_find_vert_closest */
404
405 struct VertSearchUserData {
406   /* from the bmtree */
407   const BMLoop *(*looptris)[3];
408   const float (*cos_cage)[3];
409
410   /* from the hit */
411   float dist_max_sq;
412   int index_tri;
413 };
414
415 static void bmbvh_find_vert_closest_cb(void *userdata,
416                                        int index,
417                                        const float co[3],
418                                        BVHTreeNearest *hit)
419 {
420   struct VertSearchUserData *bmcb_data = userdata;
421   const BMLoop **ltri = bmcb_data->looptris[index];
422   const float dist_max_sq = bmcb_data->dist_max_sq;
423   int i;
424
425   const float *tri_cos[3];
426
427   bmbvh_tri_from_face(tri_cos, ltri, bmcb_data->cos_cage);
428
429   for (i = 0; i < 3; i++) {
430     const float dist_sq = len_squared_v3v3(co, tri_cos[i]);
431     if (dist_sq < hit->dist_sq && dist_sq < dist_max_sq) {
432       copy_v3_v3(hit->co, tri_cos[i]);
433       /* XXX, normal ignores cage */
434       copy_v3_v3(hit->no, ltri[i]->v->no);
435       hit->dist_sq = dist_sq;
436       hit->index = index;
437       bmcb_data->index_tri = i;
438     }
439   }
440 }
441
442 BMVert *BKE_bmbvh_find_vert_closest(BMBVHTree *bmtree, const float co[3], const float dist_max)
443 {
444   BVHTreeNearest hit;
445   struct VertSearchUserData bmcb_data;
446   const float dist_max_sq = dist_max * dist_max;
447
448   if (bmtree->cos_cage)
449     BLI_assert(!(bmtree->bm->elem_index_dirty & BM_VERT));
450
451   hit.dist_sq = dist_max_sq;
452   hit.index = -1;
453
454   bmcb_data.looptris = (const BMLoop *(*)[3])bmtree->looptris;
455   bmcb_data.cos_cage = (const float(*)[3])bmtree->cos_cage;
456   bmcb_data.dist_max_sq = dist_max_sq;
457
458   BLI_bvhtree_find_nearest(bmtree->tree, co, &hit, bmbvh_find_vert_closest_cb, &bmcb_data);
459   if (hit.index != -1) {
460     BMLoop **ltri = bmtree->looptris[hit.index];
461     return ltri[bmcb_data.index_tri]->v;
462   }
463
464   return NULL;
465 }
466
467 struct FaceSearchUserData {
468   /* from the bmtree */
469   const BMLoop *(*looptris)[3];
470   const float (*cos_cage)[3];
471
472   /* from the hit */
473   float dist_max_sq;
474 };
475
476 static void bmbvh_find_face_closest_cb(void *userdata,
477                                        int index,
478                                        const float co[3],
479                                        BVHTreeNearest *hit)
480 {
481   struct FaceSearchUserData *bmcb_data = userdata;
482   const BMLoop **ltri = bmcb_data->looptris[index];
483   const float dist_max_sq = bmcb_data->dist_max_sq;
484
485   const float *tri_cos[3];
486
487   bmbvh_tri_from_face(tri_cos, ltri, bmcb_data->cos_cage);
488
489   float co_close[3];
490   closest_on_tri_to_point_v3(co_close, co, UNPACK3(tri_cos));
491   const float dist_sq = len_squared_v3v3(co, co_close);
492   if (dist_sq < hit->dist_sq && dist_sq < dist_max_sq) {
493     /* XXX, normal ignores cage */
494     copy_v3_v3(hit->no, ltri[0]->f->no);
495     hit->dist_sq = dist_sq;
496     hit->index = index;
497   }
498 }
499
500 struct BMFace *BKE_bmbvh_find_face_closest(BMBVHTree *bmtree,
501                                            const float co[3],
502                                            const float dist_max)
503 {
504   BVHTreeNearest hit;
505   struct FaceSearchUserData bmcb_data;
506   const float dist_max_sq = dist_max * dist_max;
507
508   if (bmtree->cos_cage)
509     BLI_assert(!(bmtree->bm->elem_index_dirty & BM_VERT));
510
511   hit.dist_sq = dist_max_sq;
512   hit.index = -1;
513
514   bmcb_data.looptris = (const BMLoop *(*)[3])bmtree->looptris;
515   bmcb_data.cos_cage = (const float(*)[3])bmtree->cos_cage;
516   bmcb_data.dist_max_sq = dist_max_sq;
517
518   BLI_bvhtree_find_nearest(bmtree->tree, co, &hit, bmbvh_find_face_closest_cb, &bmcb_data);
519   if (hit.index != -1) {
520     BMLoop **ltri = bmtree->looptris[hit.index];
521     return ltri[0]->f;
522   }
523
524   return NULL;
525 }
526
527 /* -------------------------------------------------------------------- */
528 /* BKE_bmbvh_overlap */
529
530 struct BMBVHTree_OverlapData {
531   const BMBVHTree *tree_pair[2];
532   float epsilon;
533 };
534
535 static bool bmbvh_overlap_cb(void *userdata, int index_a, int index_b, int UNUSED(thread))
536 {
537   struct BMBVHTree_OverlapData *data = userdata;
538   const BMBVHTree *bmtree_a = data->tree_pair[0];
539   const BMBVHTree *bmtree_b = data->tree_pair[1];
540
541   BMLoop **tri_a = bmtree_a->looptris[index_a];
542   BMLoop **tri_b = bmtree_b->looptris[index_b];
543   const float *tri_a_co[3] = {tri_a[0]->v->co, tri_a[1]->v->co, tri_a[2]->v->co};
544   const float *tri_b_co[3] = {tri_b[0]->v->co, tri_b[1]->v->co, tri_b[2]->v->co};
545   float ix_pair[2][3];
546   int verts_shared = 0;
547
548   if (bmtree_a->looptris == bmtree_b->looptris) {
549     if (UNLIKELY(tri_a[0]->f == tri_b[0]->f)) {
550       return false;
551     }
552
553     verts_shared = (ELEM(tri_a_co[0], UNPACK3(tri_b_co)) + ELEM(tri_a_co[1], UNPACK3(tri_b_co)) +
554                     ELEM(tri_a_co[2], UNPACK3(tri_b_co)));
555
556     /* if 2 points are shared, bail out */
557     if (verts_shared >= 2) {
558       return false;
559     }
560   }
561
562   return (isect_tri_tri_epsilon_v3(
563               UNPACK3(tri_a_co), UNPACK3(tri_b_co), ix_pair[0], ix_pair[1], data->epsilon) &&
564           /* if we share a vertex, check the intersection isn't a 'point' */
565           ((verts_shared == 0) || (len_squared_v3v3(ix_pair[0], ix_pair[1]) > data->epsilon)));
566 }
567
568 /**
569  * Overlap indices reference the looptri's
570  */
571 BVHTreeOverlap *BKE_bmbvh_overlap(const BMBVHTree *bmtree_a,
572                                   const BMBVHTree *bmtree_b,
573                                   unsigned int *r_overlap_tot)
574 {
575   struct BMBVHTree_OverlapData data;
576
577   data.tree_pair[0] = bmtree_a;
578   data.tree_pair[1] = bmtree_b;
579   data.epsilon = max_ff(BLI_bvhtree_get_epsilon(bmtree_a->tree),
580                         BLI_bvhtree_get_epsilon(bmtree_b->tree));
581
582   return BLI_bvhtree_overlap(
583       bmtree_a->tree, bmtree_b->tree, r_overlap_tot, bmbvh_overlap_cb, &data);
584 }