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