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