Merge branch 'master' into blender2.8
[blender.git] / source / blender / blenkernel / intern / bvhutils.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) Blender Foundation.
19  * All rights reserved.
20  *
21  * The Original Code is: all of this file.
22  *
23  * Contributor(s): Andr Pinto.
24  *
25  * ***** END GPL LICENSE BLOCK *****
26  */
27
28 /** \file blender/blenkernel/intern/bvhutils.c
29  *  \ingroup bke
30  */
31
32 #include <stdio.h>
33 #include <string.h>
34 #include <math.h>
35 #include <assert.h>
36
37 #include "DNA_mesh_types.h"
38 #include "DNA_meshdata_types.h"
39
40 #include "BLI_utildefines.h"
41 #include "BLI_linklist.h"
42 #include "BLI_math.h"
43 #include "BLI_threads.h"
44
45 #include "BKE_bvhutils.h"
46 #include "BKE_editmesh.h"
47 #include "BKE_mesh.h"
48 #include "BKE_mesh_runtime.h"
49
50 #include "MEM_guardedalloc.h"
51
52 static ThreadRWMutex cache_rwlock = BLI_RWLOCK_INITIALIZER;
53
54 /* -------------------------------------------------------------------- */
55 /** \name Local Callbacks
56  * \{ */
57
58 /* Math stuff for ray casting on mesh faces and for nearest surface */
59
60 float bvhtree_ray_tri_intersection(
61         const BVHTreeRay *ray, const float UNUSED(m_dist),
62         const float v0[3], const float v1[3], const float v2[3])
63 {
64         float dist;
65
66 #ifdef USE_KDOPBVH_WATERTIGHT
67         if (isect_ray_tri_watertight_v3(ray->origin, ray->isect_precalc, v0, v1, v2, &dist, NULL))
68 #else
69         if (isect_ray_tri_epsilon_v3(ray->origin, ray->direction, v0, v1, v2, &dist, NULL, FLT_EPSILON))
70 #endif
71         {
72                 return dist;
73         }
74
75         return FLT_MAX;
76 }
77
78 float bvhtree_sphereray_tri_intersection(
79         const BVHTreeRay *ray, float radius, const float m_dist,
80         const float v0[3], const float v1[3], const float v2[3])
81 {
82
83         float idist;
84         float p1[3];
85         float hit_point[3];
86
87         madd_v3_v3v3fl(p1, ray->origin, ray->direction, m_dist);
88         if (isect_sweeping_sphere_tri_v3(ray->origin, p1, radius, v0, v1, v2, &idist, hit_point)) {
89                 return idist * m_dist;
90         }
91
92         return FLT_MAX;
93 }
94
95 /*
96  * BVH from meshes callbacks
97  */
98
99 /* Callback to bvh tree nearest point. The tree must have been built using bvhtree_from_mesh_faces.
100  * userdata must be a BVHMeshCallbackUserdata built from the same mesh as the tree. */
101 static void mesh_faces_nearest_point(void *userdata, int index, const float co[3], BVHTreeNearest *nearest)
102 {
103         const BVHTreeFromMesh *data = (BVHTreeFromMesh *) userdata;
104         const MVert *vert = data->vert;
105         const MFace *face = data->face + index;
106
107         const float *t0, *t1, *t2, *t3;
108         t0 = vert[face->v1].co;
109         t1 = vert[face->v2].co;
110         t2 = vert[face->v3].co;
111         t3 = face->v4 ? vert[face->v4].co : NULL;
112
113
114         do {
115                 float nearest_tmp[3], dist_sq;
116
117                 closest_on_tri_to_point_v3(nearest_tmp, co, t0, t1, t2);
118                 dist_sq = len_squared_v3v3(co, nearest_tmp);
119
120                 if (dist_sq < nearest->dist_sq) {
121                         nearest->index = index;
122                         nearest->dist_sq = dist_sq;
123                         copy_v3_v3(nearest->co, nearest_tmp);
124                         normal_tri_v3(nearest->no, t0, t1, t2);
125                 }
126
127                 t1 = t2;
128                 t2 = t3;
129                 t3 = NULL;
130
131         } while (t2);
132 }
133 /* copy of function above */
134 static void mesh_looptri_nearest_point(void *userdata, int index, const float co[3], BVHTreeNearest *nearest)
135 {
136         const BVHTreeFromMesh *data = (BVHTreeFromMesh *) userdata;
137         const MVert *vert = data->vert;
138         const MLoopTri *lt = &data->looptri[index];
139         const float *vtri_co[3] = {
140             vert[data->loop[lt->tri[0]].v].co,
141             vert[data->loop[lt->tri[1]].v].co,
142             vert[data->loop[lt->tri[2]].v].co,
143         };
144         float nearest_tmp[3], dist_sq;
145
146         closest_on_tri_to_point_v3(nearest_tmp, co, UNPACK3(vtri_co));
147         dist_sq = len_squared_v3v3(co, nearest_tmp);
148
149         if (dist_sq < nearest->dist_sq) {
150                 nearest->index = index;
151                 nearest->dist_sq = dist_sq;
152                 copy_v3_v3(nearest->co, nearest_tmp);
153                 normal_tri_v3(nearest->no, UNPACK3(vtri_co));
154         }
155 }
156 /* copy of function above (warning, should de-duplicate with editmesh_bvh.c) */
157 static void editmesh_looptri_nearest_point(void *userdata, int index, const float co[3], BVHTreeNearest *nearest)
158 {
159         const BVHTreeFromEditMesh *data = userdata;
160         BMEditMesh *em = data->em;
161         const BMLoop **ltri = (const BMLoop **)em->looptris[index];
162
163         const float *t0, *t1, *t2;
164         t0 = ltri[0]->v->co;
165         t1 = ltri[1]->v->co;
166         t2 = ltri[2]->v->co;
167
168         {
169                 float nearest_tmp[3], dist_sq;
170
171                 closest_on_tri_to_point_v3(nearest_tmp, co, t0, t1, t2);
172                 dist_sq = len_squared_v3v3(co, nearest_tmp);
173
174                 if (dist_sq < nearest->dist_sq) {
175                         nearest->index = index;
176                         nearest->dist_sq = dist_sq;
177                         copy_v3_v3(nearest->co, nearest_tmp);
178                         normal_tri_v3(nearest->no, t0, t1, t2);
179                 }
180         }
181 }
182
183 /* Callback to bvh tree raycast. The tree must have been built using bvhtree_from_mesh_faces.
184  * userdata must be a BVHMeshCallbackUserdata built from the same mesh as the tree. */
185 static void mesh_faces_spherecast(void *userdata, int index, const BVHTreeRay *ray, BVHTreeRayHit *hit)
186 {
187         const BVHTreeFromMesh *data = (BVHTreeFromMesh *) userdata;
188         const MVert *vert = data->vert;
189         const MFace *face = &data->face[index];
190
191         const float *t0, *t1, *t2, *t3;
192         t0 = vert[face->v1].co;
193         t1 = vert[face->v2].co;
194         t2 = vert[face->v3].co;
195         t3 = face->v4 ? vert[face->v4].co : NULL;
196
197
198         do {
199                 float dist;
200                 if (ray->radius == 0.0f)
201                         dist = bvhtree_ray_tri_intersection(ray, hit->dist, t0, t1, t2);
202                 else
203                         dist = bvhtree_sphereray_tri_intersection(ray, ray->radius, hit->dist, t0, t1, t2);
204
205                 if (dist >= 0 && dist < hit->dist) {
206                         hit->index = index;
207                         hit->dist = dist;
208                         madd_v3_v3v3fl(hit->co, ray->origin, ray->direction, dist);
209
210                         normal_tri_v3(hit->no, t0, t1, t2);
211                 }
212
213                 t1 = t2;
214                 t2 = t3;
215                 t3 = NULL;
216
217         } while (t2);
218 }
219 /* copy of function above */
220 static void mesh_looptri_spherecast(void *userdata, int index, const BVHTreeRay *ray, BVHTreeRayHit *hit)
221 {
222         const BVHTreeFromMesh *data = (BVHTreeFromMesh *) userdata;
223         const MVert *vert = data->vert;
224         const MLoopTri *lt = &data->looptri[index];
225         const float *vtri_co[3] = {
226             vert[data->loop[lt->tri[0]].v].co,
227             vert[data->loop[lt->tri[1]].v].co,
228             vert[data->loop[lt->tri[2]].v].co,
229         };
230         float dist;
231
232         if (ray->radius == 0.0f)
233                 dist = bvhtree_ray_tri_intersection(ray, hit->dist, UNPACK3(vtri_co));
234         else
235                 dist = bvhtree_sphereray_tri_intersection(ray, ray->radius, hit->dist, UNPACK3(vtri_co));
236
237         if (dist >= 0 && dist < hit->dist) {
238                 hit->index = index;
239                 hit->dist = dist;
240                 madd_v3_v3v3fl(hit->co, ray->origin, ray->direction, dist);
241
242                 normal_tri_v3(hit->no, UNPACK3(vtri_co));
243         }
244 }
245 /* copy of function above (warning, should de-duplicate with editmesh_bvh.c) */
246 static void editmesh_looptri_spherecast(void *userdata, int index, const BVHTreeRay *ray, BVHTreeRayHit *hit)
247 {
248         const BVHTreeFromEditMesh *data = (BVHTreeFromEditMesh *)userdata;
249         BMEditMesh *em = data->em;
250         const BMLoop **ltri = (const BMLoop **)em->looptris[index];
251
252         const float *t0, *t1, *t2;
253         t0 = ltri[0]->v->co;
254         t1 = ltri[1]->v->co;
255         t2 = ltri[2]->v->co;
256
257
258         {
259                 float dist;
260                 if (ray->radius == 0.0f)
261                         dist = bvhtree_ray_tri_intersection(ray, hit->dist, t0, t1, t2);
262                 else
263                         dist = bvhtree_sphereray_tri_intersection(ray, ray->radius, hit->dist, t0, t1, t2);
264
265                 if (dist >= 0 && dist < hit->dist) {
266                         hit->index = index;
267                         hit->dist = dist;
268                         madd_v3_v3v3fl(hit->co, ray->origin, ray->direction, dist);
269
270                         normal_tri_v3(hit->no, t0, t1, t2);
271                 }
272         }
273 }
274
275 /* Callback to bvh tree nearest point. The tree must have been built using bvhtree_from_mesh_edges.
276  * userdata must be a BVHMeshCallbackUserdata built from the same mesh as the tree. */
277 static void mesh_edges_nearest_point(void *userdata, int index, const float co[3], BVHTreeNearest *nearest)
278 {
279         const BVHTreeFromMesh *data = (BVHTreeFromMesh *) userdata;
280         const MVert *vert = data->vert;
281         const MEdge *edge = data->edge + index;
282         float nearest_tmp[3], dist_sq;
283
284         const float *t0, *t1;
285         t0 = vert[edge->v1].co;
286         t1 = vert[edge->v2].co;
287
288         closest_to_line_segment_v3(nearest_tmp, co, t0, t1);
289         dist_sq = len_squared_v3v3(nearest_tmp, co);
290
291         if (dist_sq < nearest->dist_sq) {
292                 nearest->index = index;
293                 nearest->dist_sq = dist_sq;
294                 copy_v3_v3(nearest->co, nearest_tmp);
295                 sub_v3_v3v3(nearest->no, t0, t1);
296                 normalize_v3(nearest->no);
297         }
298 }
299
300 /* Helper, does all the point-spherecast work actually. */
301 static void mesh_verts_spherecast_do(
302         int index, const float v[3], const BVHTreeRay *ray, BVHTreeRayHit *hit)
303 {
304         float dist;
305         const float *r1;
306         float r2[3], i1[3];
307         r1 = ray->origin;
308         add_v3_v3v3(r2, r1, ray->direction);
309
310         closest_to_line_segment_v3(i1, v, r1, r2);
311
312         /* No hit if closest point is 'behind' the origin of the ray, or too far away from it. */
313         if ((dot_v3v3v3(r1, i1, r2) >= 0.0f) && ((dist = len_v3v3(r1, i1)) < hit->dist)) {
314                 hit->index = index;
315                 hit->dist = dist;
316                 copy_v3_v3(hit->co, i1);
317         }
318 }
319
320 static void editmesh_verts_spherecast(void *userdata, int index, const BVHTreeRay *ray, BVHTreeRayHit *hit)
321 {
322         const BVHTreeFromEditMesh *data = userdata;
323         BMVert *eve = BM_vert_at_index(data->em->bm, index);
324
325         mesh_verts_spherecast_do(index, eve->co, ray, hit);
326 }
327
328 /* Callback to bvh tree raycast. The tree must have been built using bvhtree_from_mesh_verts.
329  * userdata must be a BVHMeshCallbackUserdata built from the same mesh as the tree. */
330 static void mesh_verts_spherecast(void *userdata, int index, const BVHTreeRay *ray, BVHTreeRayHit *hit)
331 {
332         const BVHTreeFromMesh *data = (BVHTreeFromMesh *)userdata;
333         const float *v = data->vert[index].co;
334
335         mesh_verts_spherecast_do(index, v, ray, hit);
336 }
337
338 /* Callback to bvh tree raycast. The tree must have been built using bvhtree_from_mesh_edges.
339  * userdata must be a BVHMeshCallbackUserdata built from the same mesh as the tree. */
340 static void mesh_edges_spherecast(void *userdata, int index, const BVHTreeRay *ray, BVHTreeRayHit *hit)
341 {
342         const BVHTreeFromMesh *data = (BVHTreeFromMesh *)userdata;
343         const MVert *vert = data->vert;
344         const MEdge *edge = &data->edge[index];
345
346         const float radius_sq = SQUARE(ray->radius);
347         float dist;
348         const float *v1, *v2, *r1;
349         float r2[3], i1[3], i2[3];
350         v1 = vert[edge->v1].co;
351         v2 = vert[edge->v2].co;
352
353         /* In case we get a zero-length edge, handle it as a point! */
354         if (equals_v3v3(v1, v2)) {
355                 mesh_verts_spherecast_do(index, v1, ray, hit);
356                 return;
357         }
358
359         r1 = ray->origin;
360         add_v3_v3v3(r2, r1, ray->direction);
361
362         if (isect_line_line_v3(v1, v2, r1, r2, i1, i2)) {
363                 /* No hit if intersection point is 'behind' the origin of the ray, or too far away from it. */
364                 if ((dot_v3v3v3(r1, i2, r2) >= 0.0f) && ((dist = len_v3v3(r1, i2)) < hit->dist)) {
365                         const float e_fac = line_point_factor_v3(i1, v1, v2);
366                         if (e_fac < 0.0f) {
367                                 copy_v3_v3(i1, v1);
368                         }
369                         else if (e_fac > 1.0f) {
370                                 copy_v3_v3(i1, v2);
371                         }
372                         /* Ensure ray is really close enough from edge! */
373                         if (len_squared_v3v3(i1, i2) <= radius_sq) {
374                                 hit->index = index;
375                                 hit->dist = dist;
376                                 copy_v3_v3(hit->co, i2);
377                         }
378                 }
379         }
380 }
381
382 /** \} */
383
384 /*
385  * BVH builders
386  */
387
388
389 /* -------------------------------------------------------------------- */
390
391 /** \name Vertex Builder
392  * \{ */
393
394 static BVHTree *bvhtree_from_editmesh_verts_create_tree(
395         float epsilon, int tree_type, int axis,
396         BMEditMesh *em, const int verts_num,
397         const BLI_bitmap *verts_mask, int verts_num_active)
398 {
399         BM_mesh_elem_table_ensure(em->bm, BM_VERT);
400         if (verts_mask) {
401                 BLI_assert(IN_RANGE_INCL(verts_num_active, 0, verts_num));
402         }
403         else {
404                 verts_num_active = verts_num;
405         }
406
407         BVHTree *tree = BLI_bvhtree_new(verts_num_active, epsilon, tree_type, axis);
408
409         if (tree) {
410                 for (int i = 0; i < verts_num; i++) {
411                         if (verts_mask && !BLI_BITMAP_TEST_BOOL(verts_mask, i)) {
412                                 continue;
413                         }
414                         BMVert *eve = BM_vert_at_index(em->bm, i);
415                         BLI_bvhtree_insert(tree, i, eve->co, 1);
416                 }
417                 BLI_assert(BLI_bvhtree_get_len(tree) == verts_num_active);
418                 BLI_bvhtree_balance(tree);
419         }
420
421         return tree;
422 }
423
424 static BVHTree *bvhtree_from_mesh_verts_create_tree(
425         float epsilon, int tree_type, int axis,
426         const MVert *vert, const int verts_num,
427         const BLI_bitmap *verts_mask, int verts_num_active)
428 {
429         BVHTree *tree = NULL;
430
431         if (verts_mask) {
432                 BLI_assert(IN_RANGE_INCL(verts_num_active, 0, verts_num));
433         }
434         else {
435                 verts_num_active = verts_num;
436         }
437
438         if (verts_num_active) {
439                 tree = BLI_bvhtree_new(verts_num_active, epsilon, tree_type, axis);
440
441                 if (tree) {
442                         for (int i = 0; i < verts_num; i++) {
443                                 if (verts_mask && !BLI_BITMAP_TEST_BOOL(verts_mask, i)) {
444                                         continue;
445                                 }
446                                 BLI_bvhtree_insert(tree, i, vert[i].co, 1);
447                         }
448                         BLI_assert(BLI_bvhtree_get_len(tree) == verts_num_active);
449                         BLI_bvhtree_balance(tree);
450                 }
451         }
452
453         return tree;
454 }
455
456 static void bvhtree_from_mesh_verts_setup_data(
457         BVHTreeFromMesh *data, BVHTree *tree, const bool is_cached,
458         const MVert *vert, const bool vert_allocated)
459 {
460         memset(data, 0, sizeof(*data));
461
462         data->tree = tree;
463         data->cached = is_cached;
464
465         /* a NULL nearest callback works fine
466          * remember the min distance to point is the same as the min distance to BV of point */
467         data->nearest_callback = NULL;
468         data->raycast_callback = mesh_verts_spherecast;
469
470         data->vert = vert;
471         data->vert_allocated = vert_allocated;
472 }
473
474 /* Builds a bvh tree where nodes are the vertices of the given em */
475 BVHTree *bvhtree_from_editmesh_verts_ex(
476         BVHTreeFromEditMesh *data, BMEditMesh *em,
477         const BLI_bitmap *verts_mask, int verts_num_active,
478         float epsilon, int tree_type, int axis)
479 {
480         BVHTree *tree = bvhtree_from_editmesh_verts_create_tree(
481                 epsilon, tree_type, axis,
482                 em, em->bm->totvert, verts_mask, verts_num_active);
483
484         if (tree) {
485                 memset(data, 0, sizeof(*data));
486                 data->tree = tree;
487                 data->em = em;
488                 data->nearest_callback = NULL;
489                 data->raycast_callback = editmesh_verts_spherecast;
490         }
491
492         return tree;
493 }
494
495 BVHTree *bvhtree_from_editmesh_verts(
496         BVHTreeFromEditMesh *data, BMEditMesh *em,
497         float epsilon, int tree_type, int axis, BVHCache **bvh_cache)
498 {
499         if (bvh_cache) {
500                 BLI_rw_mutex_lock(&cache_rwlock, THREAD_LOCK_READ);
501                 data->cached = bvhcache_find(*bvh_cache, BVHTREE_FROM_EM_VERTS, &data->tree);
502                 BLI_rw_mutex_unlock(&cache_rwlock);
503
504                 if (data->cached == false) {
505                         BLI_rw_mutex_lock(&cache_rwlock, THREAD_LOCK_WRITE);
506                         data->cached = bvhcache_find(
507                                 *bvh_cache, BVHTREE_FROM_EM_VERTS, &data->tree);
508                         if (data->cached == false) {
509                                 data->tree = bvhtree_from_editmesh_verts_ex(
510                                         data, em,
511                                         NULL, -1,
512                                         epsilon, tree_type, axis);
513
514                                 /* Save on cache for later use */
515                                 /* printf("BVHTree built and saved on cache\n"); */
516                                 bvhcache_insert(
517                                         bvh_cache, data->tree, BVHTREE_FROM_EM_VERTS);
518                         }
519                         BLI_rw_mutex_unlock(&cache_rwlock);
520                 }
521         }
522         else {
523                 data->tree = bvhtree_from_editmesh_verts_ex(
524                         data, em,
525                         NULL, -1,
526                         epsilon, tree_type, axis);
527         }
528
529         return data->tree;
530 }
531
532 /**
533  * Builds a bvh tree where nodes are the given vertices (note: does not copy given mverts!).
534  * \param vert_allocated if true, vert freeing will be done when freeing data.
535  * \param verts_mask if not null, true elements give which vert to add to BVH tree.
536  * \param verts_num_active if >= 0, number of active verts to add to BVH tree (else will be computed from mask).
537  */
538 BVHTree *bvhtree_from_mesh_verts_ex(
539         BVHTreeFromMesh *data, const MVert *vert, const int verts_num, const bool vert_allocated,
540         const BLI_bitmap *verts_mask, int verts_num_active,
541         float epsilon, int tree_type, int axis)
542 {
543         BVHTree *tree = bvhtree_from_mesh_verts_create_tree(
544                 epsilon, tree_type, axis, vert, verts_num, verts_mask, verts_num_active);
545
546         /* Setup BVHTreeFromMesh */
547         bvhtree_from_mesh_verts_setup_data(
548                 data, tree, false, vert, vert_allocated);
549
550         return tree;
551 }
552
553 /** \} */
554
555
556 /* -------------------------------------------------------------------- */
557
558 /** \name Edge Builder
559  * \{ */
560
561 static BVHTree *bvhtree_from_editmesh_edges_create_tree(
562         float epsilon, int tree_type, int axis,
563         BMEditMesh *em, const int edges_num,
564         const BLI_bitmap *edges_mask, int edges_num_active)
565 {
566         BM_mesh_elem_table_ensure(em->bm, BM_EDGE);
567         if (edges_mask) {
568                 BLI_assert(IN_RANGE_INCL(edges_num_active, 0, edges_num));
569         }
570         else {
571                 edges_num_active = edges_num;
572         }
573
574         BVHTree *tree = BLI_bvhtree_new(edges_num_active, epsilon, tree_type, axis);
575
576         if (tree) {
577                 int i;
578                 BMIter iter;
579                 BMEdge *eed;
580                 BM_ITER_MESH_INDEX (eed, &iter, em->bm, BM_EDGES_OF_MESH, i) {
581                         if (edges_mask && !BLI_BITMAP_TEST_BOOL(edges_mask, i)) {
582                                 continue;
583                         }
584                         float co[2][3];
585                         copy_v3_v3(co[0], eed->v1->co);
586                         copy_v3_v3(co[1], eed->v2->co);
587
588                         BLI_bvhtree_insert(tree, i, co[0], 2);
589                 }
590                 BLI_assert(BLI_bvhtree_get_len(tree) == edges_num_active);
591                 BLI_bvhtree_balance(tree);
592         }
593
594         return tree;
595 }
596
597 static BVHTree *bvhtree_from_mesh_edges_create_tree(
598         const MVert *vert, const MEdge *edge, const int edge_num,
599         const BLI_bitmap *edges_mask, int edges_num_active,
600         float epsilon, int tree_type, int axis)
601 {
602         BVHTree *tree = NULL;
603
604         if (edges_mask) {
605                 BLI_assert(IN_RANGE_INCL(edges_num_active, 0, edge_num));
606         }
607         else {
608                 edges_num_active = edge_num;
609         }
610
611         if (edges_num_active) {
612                 /* Create a bvh-tree of the given target */
613                 tree = BLI_bvhtree_new(edges_num_active, epsilon, tree_type, axis);
614                 if (tree) {
615                         for (int i = 0; i < edge_num; i++) {
616                                 if (edges_mask && !BLI_BITMAP_TEST_BOOL(edges_mask, i)) {
617                                         continue;
618                                 }
619                                 float co[2][3];
620                                 copy_v3_v3(co[0], vert[edge[i].v1].co);
621                                 copy_v3_v3(co[1], vert[edge[i].v2].co);
622
623                                 BLI_bvhtree_insert(tree, i, co[0], 2);
624                         }
625                         BLI_bvhtree_balance(tree);
626                 }
627         }
628
629         return tree;
630 }
631
632 static void bvhtree_from_mesh_edges_setup_data(
633         BVHTreeFromMesh *data, BVHTree *tree,
634         const bool is_cached,
635         const MVert *vert, const bool vert_allocated,
636         const MEdge *edge, const bool edge_allocated)
637 {
638         memset(data, 0, sizeof(*data));
639
640         data->tree = tree;
641
642         data->cached = is_cached;
643
644         data->nearest_callback = mesh_edges_nearest_point;
645         data->raycast_callback = mesh_edges_spherecast;
646
647         data->vert = vert;
648         data->vert_allocated = vert_allocated;
649         data->edge = edge;
650         data->edge_allocated = edge_allocated;
651 }
652
653 /* Builds a bvh tree where nodes are the edges of the given em */
654 BVHTree *bvhtree_from_editmesh_edges_ex(
655         BVHTreeFromEditMesh *data, BMEditMesh *em,
656         const BLI_bitmap *edges_mask, int edges_num_active,
657         float epsilon, int tree_type, int axis)
658 {
659         int edge_num = em->bm->totedge;
660
661         BVHTree *tree = bvhtree_from_editmesh_edges_create_tree(
662                 epsilon, tree_type, axis,
663                 em, edge_num, edges_mask, edges_num_active);
664
665         if (tree) {
666                 memset(data, 0, sizeof(*data));
667                 data->tree = tree;
668                 data->em = em;
669                 data->nearest_callback = NULL;  /* TODO */
670                 data->raycast_callback = NULL;  /* TODO */
671         }
672
673         return tree;
674 }
675
676 BVHTree *bvhtree_from_editmesh_edges(
677         BVHTreeFromEditMesh *data, BMEditMesh *em,
678         float epsilon, int tree_type, int axis, BVHCache **bvh_cache)
679 {
680         if (bvh_cache) {
681                 BLI_rw_mutex_lock(&cache_rwlock, THREAD_LOCK_READ);
682                 data->cached = bvhcache_find(*bvh_cache, BVHTREE_FROM_EM_EDGES, &data->tree);
683                 BLI_rw_mutex_unlock(&cache_rwlock);
684
685                 if (data->cached == false) {
686                         BLI_rw_mutex_lock(&cache_rwlock, THREAD_LOCK_WRITE);
687                         data->cached = bvhcache_find(
688                                 *bvh_cache, BVHTREE_FROM_EM_EDGES, &data->tree);
689                         if (data->cached == false) {
690                                 data->tree = bvhtree_from_editmesh_edges_ex(
691                                         data, em,
692                                         NULL, -1,
693                                         epsilon, tree_type, axis);
694
695                                 /* Save on cache for later use */
696                                 /* printf("BVHTree built and saved on cache\n"); */
697                                 bvhcache_insert(
698                                         bvh_cache, data->tree, BVHTREE_FROM_EM_EDGES);
699                         }
700                         BLI_rw_mutex_unlock(&cache_rwlock);
701                 }
702         }
703         else {
704                 data->tree = bvhtree_from_editmesh_edges_ex(
705                         data, em,
706                         NULL, -1,
707                         epsilon, tree_type, axis);
708         }
709
710         return data->tree;
711 }
712
713 /**
714  * Builds a bvh tree where nodes are the given edges .
715  * \param vert/edge_allocated if true, elem freeing will be done when freeing data.
716  * \param edges_mask if not null, true elements give which vert to add to BVH tree.
717  * \param edges_num_active if >= 0, number of active edges to add to BVH tree (else will be computed from mask).
718  */
719 BVHTree *bvhtree_from_mesh_edges_ex(
720         BVHTreeFromMesh *data,
721         const MVert *vert, const bool vert_allocated,
722         const MEdge *edge, const int edges_num, const bool edge_allocated,
723         const BLI_bitmap *edges_mask, int edges_num_active,
724         float epsilon, int tree_type, int axis)
725 {
726         BVHTree *tree = bvhtree_from_mesh_edges_create_tree(
727                 vert, edge, edges_num, edges_mask, edges_num_active,
728                 epsilon, tree_type, axis);
729
730         /* Setup BVHTreeFromMesh */
731         bvhtree_from_mesh_edges_setup_data(
732                 data, tree, false, vert, vert_allocated, edge, edge_allocated);
733
734         return tree;
735 }
736
737 /** \} */
738
739
740 /* -------------------------------------------------------------------- */
741
742 /** \name Tessellated Face Builder
743  * \{ */
744
745 static BVHTree *bvhtree_from_mesh_faces_create_tree(
746         float epsilon, int tree_type, int axis,
747         const MVert *vert, const MFace *face, const int faces_num,
748         const BLI_bitmap *faces_mask, int faces_num_active)
749 {
750         BVHTree *tree = NULL;
751         int i;
752
753         if (faces_num) {
754                 if (faces_mask) {
755                         BLI_assert(IN_RANGE_INCL(faces_num_active, 0, faces_num));
756                 }
757                 else {
758                         faces_num_active = faces_num;
759                 }
760
761                 /* Create a bvh-tree of the given target */
762                 /* printf("%s: building BVH, total=%d\n", __func__, numFaces); */
763                 tree = BLI_bvhtree_new(faces_num_active, epsilon, tree_type, axis);
764                 if (tree) {
765                         if (vert && face) {
766                                 for (i = 0; i < faces_num; i++) {
767                                         float co[4][3];
768                                         if (faces_mask && !BLI_BITMAP_TEST_BOOL(faces_mask, i)) {
769                                                 continue;
770                                         }
771
772                                         copy_v3_v3(co[0], vert[face[i].v1].co);
773                                         copy_v3_v3(co[1], vert[face[i].v2].co);
774                                         copy_v3_v3(co[2], vert[face[i].v3].co);
775                                         if (face[i].v4)
776                                                 copy_v3_v3(co[3], vert[face[i].v4].co);
777
778                                         BLI_bvhtree_insert(tree, i, co[0], face[i].v4 ? 4 : 3);
779                                 }
780                         }
781                         BLI_assert(BLI_bvhtree_get_len(tree) == faces_num_active);
782                         BLI_bvhtree_balance(tree);
783                 }
784         }
785
786         return tree;
787 }
788
789 static void bvhtree_from_mesh_faces_setup_data(
790         BVHTreeFromMesh *data, BVHTree *tree, const bool is_cached,
791         const MVert *vert, const bool vert_allocated,
792         const MFace *face, const bool face_allocated)
793 {
794         memset(data, 0, sizeof(*data));
795
796         data->tree = tree;
797         data->cached = is_cached;
798
799         data->nearest_callback = mesh_faces_nearest_point;
800         data->raycast_callback = mesh_faces_spherecast;
801
802         data->vert = vert;
803         data->vert_allocated = vert_allocated;
804         data->face = face;
805         data->face_allocated = face_allocated;
806 }
807
808 /**
809  * Builds a bvh tree where nodes are the given tessellated faces (note: does not copy given mfaces!).
810  * \param vert_allocated: if true, vert freeing will be done when freeing data.
811  * \param face_allocated: if true, face freeing will be done when freeing data.
812  * \param faces_mask: if not null, true elements give which faces to add to BVH tree.
813  * \param faces_num_active: if >= 0, number of active faces to add to BVH tree (else will be computed from mask).
814  */
815 BVHTree *bvhtree_from_mesh_faces_ex(
816         BVHTreeFromMesh *data, const MVert *vert, const bool vert_allocated,
817         const MFace *face, const int numFaces, const bool face_allocated,
818         const BLI_bitmap *faces_mask, int faces_num_active,
819         float epsilon, int tree_type, int axis)
820 {
821         BVHTree *tree = bvhtree_from_mesh_faces_create_tree(
822                 epsilon, tree_type, axis,
823                 vert, face, numFaces,
824                 faces_mask, faces_num_active);
825
826         /* Setup BVHTreeFromMesh */
827         bvhtree_from_mesh_faces_setup_data(
828                 data, tree, false, vert, vert_allocated, face, face_allocated);
829
830         return tree;
831 }
832
833 /** \} */
834
835
836 /* -------------------------------------------------------------------- */
837
838 /** \name LoopTri Face Builder
839  * \{ */
840
841 static BVHTree *bvhtree_from_editmesh_looptri_create_tree(
842         float epsilon, int tree_type, int axis,
843         BMEditMesh *em, const int looptri_num,
844         const BLI_bitmap *looptri_mask, int looptri_num_active)
845 {
846         BVHTree *tree = NULL;
847         int i;
848
849         if (looptri_num) {
850                 if (looptri_mask) {
851                         BLI_assert(IN_RANGE_INCL(looptri_num_active, 0, looptri_num));
852                 }
853                 else {
854                         looptri_num_active = looptri_num;
855                 }
856
857                 /* Create a bvh-tree of the given target */
858                 /* printf("%s: building BVH, total=%d\n", __func__, numFaces); */
859                 tree = BLI_bvhtree_new(looptri_num_active, epsilon, tree_type, axis);
860                 if (tree) {
861                         if (em) {
862                                 const struct BMLoop *(*looptris)[3] = (void *)em->looptris;
863
864                                 /* Insert BMesh-tessellation triangles into the bvh tree, unless they are hidden
865                                  * and/or selected. Even if the faces themselves are not selected for the snapped
866                                  * transform, having a vertex selected means the face (and thus it's tessellated
867                                  * triangles) will be moving and will not be a good snap targets. */
868                                 for (i = 0; i < looptri_num; i++) {
869                                         const BMLoop **ltri = looptris[i];
870                                         bool insert = looptri_mask ? BLI_BITMAP_TEST_BOOL(looptri_mask, i) : true;
871
872                                         if (insert) {
873                                                 /* No reason found to block hit-testing the triangle for snap, so insert it now.*/
874                                                 float co[3][3];
875                                                 copy_v3_v3(co[0], ltri[0]->v->co);
876                                                 copy_v3_v3(co[1], ltri[1]->v->co);
877                                                 copy_v3_v3(co[2], ltri[2]->v->co);
878
879                                                 BLI_bvhtree_insert(tree, i, co[0], 3);
880                                         }
881                                 }
882                         }
883                         BLI_assert(BLI_bvhtree_get_len(tree) == looptri_num_active);
884                         BLI_bvhtree_balance(tree);
885                 }
886         }
887
888         return tree;
889 }
890
891 static BVHTree *bvhtree_from_mesh_looptri_create_tree(
892         float epsilon, int tree_type, int axis,
893         const MVert *vert, const MLoop *mloop, const MLoopTri *looptri, const int looptri_num,
894         const BLI_bitmap *looptri_mask, int looptri_num_active)
895 {
896         BVHTree *tree = NULL;
897
898         if (looptri_mask) {
899                 BLI_assert(IN_RANGE_INCL(looptri_num_active, 0, looptri_num));
900         }
901         else {
902                 looptri_num_active = looptri_num;
903         }
904
905         if (looptri_num_active) {
906                 /* Create a bvh-tree of the given target */
907                 /* printf("%s: building BVH, total=%d\n", __func__, numFaces); */
908                 tree = BLI_bvhtree_new(looptri_num_active, epsilon, tree_type, axis);
909                 if (tree) {
910                         if (vert && looptri) {
911                                 for (int i = 0; i < looptri_num; i++) {
912                                         float co[3][3];
913                                         if (looptri_mask && !BLI_BITMAP_TEST_BOOL(looptri_mask, i)) {
914                                                 continue;
915                                         }
916
917                                         copy_v3_v3(co[0], vert[mloop[looptri[i].tri[0]].v].co);
918                                         copy_v3_v3(co[1], vert[mloop[looptri[i].tri[1]].v].co);
919                                         copy_v3_v3(co[2], vert[mloop[looptri[i].tri[2]].v].co);
920
921                                         BLI_bvhtree_insert(tree, i, co[0], 3);
922                                 }
923                         }
924                         BLI_assert(BLI_bvhtree_get_len(tree) == looptri_num_active);
925                         BLI_bvhtree_balance(tree);
926                 }
927         }
928
929         return tree;
930 }
931
932 static void bvhtree_from_mesh_looptri_setup_data(
933         BVHTreeFromMesh *data, BVHTree *tree, const bool is_cached,
934         const MVert *vert, const bool vert_allocated,
935         const MLoop *mloop, const bool loop_allocated,
936         const MLoopTri *looptri, const bool looptri_allocated)
937 {
938         memset(data, 0, sizeof(*data));
939
940         data->tree = tree;
941         data->cached = is_cached;
942
943         data->nearest_callback = mesh_looptri_nearest_point;
944         data->raycast_callback = mesh_looptri_spherecast;
945
946         data->vert = vert;
947         data->vert_allocated = vert_allocated;
948         data->loop = mloop;
949         data->loop_allocated = loop_allocated;
950         data->looptri = looptri;
951         data->looptri_allocated = looptri_allocated;
952 }
953
954 /**
955  * Builds a bvh tree where nodes are the looptri faces of the given bm
956  */
957 BVHTree *bvhtree_from_editmesh_looptri_ex(
958         BVHTreeFromEditMesh *data, BMEditMesh *em,
959         const BLI_bitmap *looptri_mask, int looptri_num_active,
960         float epsilon, int tree_type, int axis, BVHCache **bvhCache)
961 {
962         /* BMESH specific check that we have tessfaces,
963          * we _could_ tessellate here but rather not - campbell */
964
965         BVHTree *tree = NULL;
966         if (bvhCache) {
967                 BLI_rw_mutex_lock(&cache_rwlock, THREAD_LOCK_READ);
968                 bool in_cache = bvhcache_find(*bvhCache, BVHTREE_FROM_EM_LOOPTRI, &tree);
969                 BLI_rw_mutex_unlock(&cache_rwlock);
970                 if (in_cache == false) {
971                         BLI_rw_mutex_lock(&cache_rwlock, THREAD_LOCK_WRITE);
972                         in_cache = bvhcache_find(*bvhCache, BVHTREE_FROM_EM_LOOPTRI, &tree);
973                         if (in_cache == false) {
974                                 tree = bvhtree_from_editmesh_looptri_create_tree(
975                                         epsilon, tree_type, axis,
976                                         em, em->tottri, looptri_mask, looptri_num_active);
977
978                                 /* Save on cache for later use */
979                                 /* printf("BVHTree built and saved on cache\n"); */
980                                 bvhcache_insert(bvhCache, tree, BVHTREE_FROM_EM_LOOPTRI);
981
982                         }
983                         BLI_rw_mutex_unlock(&cache_rwlock);
984                 }
985         }
986         else {
987                 tree = bvhtree_from_editmesh_looptri_create_tree(
988                         epsilon, tree_type, axis,
989                         em, em->tottri, looptri_mask, looptri_num_active);
990         }
991
992         if (tree) {
993                 data->tree = tree;
994                 data->nearest_callback = editmesh_looptri_nearest_point;
995                 data->raycast_callback = editmesh_looptri_spherecast;
996                 data->em = em;
997                 data->cached = bvhCache != NULL;
998         }
999         return tree;
1000 }
1001
1002 BVHTree *bvhtree_from_editmesh_looptri(
1003         BVHTreeFromEditMesh *data, BMEditMesh *em,
1004         float epsilon, int tree_type, int axis, BVHCache **bvhCache)
1005 {
1006         return bvhtree_from_editmesh_looptri_ex(
1007                 data, em, NULL, -1,
1008                 epsilon, tree_type, axis, bvhCache);
1009 }
1010
1011 /**
1012  * Builds a bvh tree where nodes are the looptri faces of the given dm
1013  *
1014  * \note for editmesh this is currently a duplicate of bvhtree_from_mesh_faces_ex
1015  */
1016 BVHTree *bvhtree_from_mesh_looptri_ex(
1017         BVHTreeFromMesh *data,
1018         const struct MVert *vert, const bool vert_allocated,
1019         const struct MLoop *mloop, const bool loop_allocated,
1020         const struct MLoopTri *looptri, const int looptri_num, const bool looptri_allocated,
1021         const BLI_bitmap *looptri_mask, int looptri_num_active,
1022         float epsilon, int tree_type, int axis)
1023 {
1024         BVHTree *tree = bvhtree_from_mesh_looptri_create_tree(
1025                 epsilon, tree_type, axis,
1026                 vert, mloop, looptri, looptri_num,
1027                 looptri_mask, looptri_num_active);
1028
1029         /* Setup BVHTreeFromMesh */
1030         bvhtree_from_mesh_looptri_setup_data(
1031                 data, tree, false,
1032                 vert, vert_allocated,
1033                 mloop, loop_allocated,
1034                 looptri, looptri_allocated);
1035
1036         return tree;
1037 }
1038
1039 static BLI_bitmap *loose_verts_map_get(
1040         const MEdge *medge, int edges_num,
1041         const MVert *UNUSED(mvert), int verts_num,
1042         int *r_loose_vert_num)
1043 {
1044         BLI_bitmap *loose_verts_mask = BLI_BITMAP_NEW(verts_num, __func__);
1045         BLI_BITMAP_SET_ALL(loose_verts_mask, true, verts_num);
1046
1047         const MEdge *e = medge;
1048         int num_linked_verts = 0;
1049         for (;edges_num--; e++) {
1050                 if (BLI_BITMAP_TEST(loose_verts_mask, e->v1)) {
1051                         BLI_BITMAP_DISABLE(loose_verts_mask, e->v1);
1052                         num_linked_verts++;
1053                 }
1054                 if (BLI_BITMAP_TEST(loose_verts_mask, e->v2)) {
1055                         BLI_BITMAP_DISABLE(loose_verts_mask, e->v2);
1056                         num_linked_verts++;
1057                 }
1058         }
1059
1060         *r_loose_vert_num = verts_num - num_linked_verts;
1061
1062         return loose_verts_mask;
1063 }
1064
1065 static BLI_bitmap *loose_edges_map_get(
1066         const MEdge *medge, const int edges_len,
1067         int *r_loose_edge_len)
1068 {
1069         BLI_bitmap *loose_edges_mask = BLI_BITMAP_NEW(edges_len, __func__);
1070
1071         int loose_edges_len = 0;
1072         const MEdge *e = medge;
1073         for (int i = 0; i < edges_len; i++, e++) {
1074                 if (e->flag & ME_LOOSEEDGE) {
1075                         BLI_BITMAP_ENABLE(loose_edges_mask, i);
1076                         loose_edges_len++;
1077                 }
1078                 else {
1079                         BLI_BITMAP_DISABLE(loose_edges_mask, i);
1080                 }
1081         }
1082
1083         *r_loose_edge_len = loose_edges_len;
1084
1085         return loose_edges_mask;
1086 }
1087
1088 /**
1089  * Builds or queries a bvhcache for the cache bvhtree of the request type.
1090  */
1091 BVHTree *BKE_bvhtree_from_mesh_get(
1092         struct BVHTreeFromMesh *data, struct Mesh *mesh,
1093         const int type, const int tree_type)
1094 {
1095         struct BVHTreeFromMesh data_cp = {0};
1096
1097         BLI_rw_mutex_lock(&cache_rwlock, THREAD_LOCK_READ);
1098         data_cp.cached = bvhcache_find(mesh->runtime.bvh_cache, type, &data_cp.tree);
1099         BLI_rw_mutex_unlock(&cache_rwlock);
1100
1101         if (data_cp.cached && data_cp.tree == NULL) {
1102                 memset(data, 0, sizeof(*data));
1103                 return data_cp.tree;
1104         }
1105
1106         switch (type) {
1107                 case BVHTREE_FROM_VERTS:
1108                 case BVHTREE_FROM_LOOSEVERTS:
1109                         data_cp.raycast_callback = mesh_verts_spherecast;
1110
1111                         data_cp.vert = mesh->mvert;
1112
1113                         if (data_cp.cached == false) {
1114                                 BLI_rw_mutex_lock(&cache_rwlock, THREAD_LOCK_WRITE);
1115                                 data_cp.cached = bvhcache_find(
1116                                         mesh->runtime.bvh_cache, type, &data_cp.tree);
1117
1118                                 if (data_cp.cached == false) {
1119                                         BLI_bitmap *loose_verts_mask = NULL;
1120                                         int loose_vert_len = -1;
1121                                         int verts_len = mesh->totvert;
1122
1123                                         if (type == BVHTREE_FROM_LOOSEVERTS) {
1124                                                 loose_verts_mask = loose_verts_map_get(
1125                                                         mesh->medge, mesh->totedge, data_cp.vert,
1126                                                         verts_len, &loose_vert_len);
1127                                         }
1128
1129                                         data_cp.tree = bvhtree_from_mesh_verts_create_tree(
1130                                                 0.0, tree_type, 6, data_cp.vert, verts_len,
1131                                                 loose_verts_mask, loose_vert_len);
1132
1133                                         if (loose_verts_mask != NULL) {
1134                                                 MEM_freeN(loose_verts_mask);
1135                                         }
1136
1137                                         /* Save on cache for later use */
1138                                         /* printf("BVHTree built and saved on cache\n"); */
1139                                         bvhcache_insert(&mesh->runtime.bvh_cache, data_cp.tree, type);
1140                                 }
1141                                 BLI_rw_mutex_unlock(&cache_rwlock);
1142                         }
1143                         break;
1144
1145                 case BVHTREE_FROM_EDGES:
1146                 case BVHTREE_FROM_LOOSEEDGES:
1147                         data_cp.nearest_callback = mesh_edges_nearest_point;
1148                         data_cp.raycast_callback = mesh_edges_spherecast;
1149
1150                         data_cp.vert = mesh->mvert;
1151                         data_cp.edge = mesh->medge;
1152
1153                         if (data_cp.cached == false) {
1154                                 BLI_rw_mutex_lock(&cache_rwlock, THREAD_LOCK_WRITE);
1155                                 data_cp.cached = bvhcache_find(mesh->runtime.bvh_cache, type, &data_cp.tree);
1156                                 if (data_cp.cached == false) {
1157                                         BLI_bitmap *loose_edges_mask = NULL;
1158                                         int loose_edges_len = -1;
1159                                         int edges_len = mesh->totedge;
1160
1161                                         if (type == BVHTREE_FROM_LOOSEEDGES) {
1162                                                 loose_edges_mask = loose_edges_map_get(
1163                                                         data_cp.edge, edges_len, &loose_edges_len);
1164                                         }
1165
1166                                         data_cp.tree = bvhtree_from_mesh_edges_create_tree(
1167                                                 data_cp.vert, data_cp.edge, edges_len,
1168                                                 loose_edges_mask, loose_edges_len, 0.0, tree_type, 6);
1169
1170                                         if (loose_edges_mask != NULL) {
1171                                                 MEM_freeN(loose_edges_mask);
1172                                         }
1173
1174                                         /* Save on cache for later use */
1175                                         /* printf("BVHTree built and saved on cache\n"); */
1176                                         bvhcache_insert(&mesh->runtime.bvh_cache, data_cp.tree, type);
1177                                 }
1178                                 BLI_rw_mutex_unlock(&cache_rwlock);
1179                         }
1180                         break;
1181
1182                 case BVHTREE_FROM_FACES:
1183                         data_cp.nearest_callback = mesh_faces_nearest_point;
1184                         data_cp.raycast_callback = mesh_faces_spherecast;
1185
1186                         data_cp.vert = mesh->mvert;
1187                         data_cp.face = mesh->mface;
1188
1189                         if (data_cp.cached == false) {
1190                                 BLI_rw_mutex_lock(&cache_rwlock, THREAD_LOCK_WRITE);
1191                                 data_cp.cached = bvhcache_find(
1192                                         mesh->runtime.bvh_cache, BVHTREE_FROM_FACES, &data_cp.tree);
1193                                 if (data_cp.cached == false) {
1194                                         int num_faces = mesh->totface;
1195                                         BLI_assert(!(num_faces == 0 && mesh->totpoly != 0));
1196
1197                                         data_cp.tree = bvhtree_from_mesh_faces_create_tree(
1198                                                 0.0, tree_type, 6, data_cp.vert,
1199                                                 data_cp.face, num_faces, NULL, -1);
1200
1201                                         /* Save on cache for later use */
1202                                         /* printf("BVHTree built and saved on cache\n"); */
1203                                         bvhcache_insert(
1204                                                 &mesh->runtime.bvh_cache, data_cp.tree, BVHTREE_FROM_FACES);
1205                                 }
1206                                 BLI_rw_mutex_unlock(&cache_rwlock);
1207                         }
1208                         break;
1209
1210                 case BVHTREE_FROM_LOOPTRI:
1211                         data_cp.nearest_callback = mesh_looptri_nearest_point;
1212                         data_cp.raycast_callback = mesh_looptri_spherecast;
1213
1214                         data_cp.vert = mesh->mvert;
1215                         data_cp.loop = mesh->mloop;
1216
1217                         /* TODO: store looptris somewhere? */
1218                         data_cp.looptri = BKE_mesh_runtime_looptri_ensure(mesh);
1219
1220                         if (data_cp.cached == false) {
1221                                 BLI_rw_mutex_lock(&cache_rwlock, THREAD_LOCK_WRITE);
1222                                 data_cp.cached = bvhcache_find(
1223                                         mesh->runtime.bvh_cache, BVHTREE_FROM_LOOPTRI, &data_cp.tree);
1224                                 if (data_cp.cached == false) {
1225                                         int looptri_num = BKE_mesh_runtime_looptri_len(mesh);
1226                                         /* this assert checks we have looptris,
1227                                          * if not caller should use DM_ensure_looptri() */
1228                                         BLI_assert(!(looptri_num == 0 && mesh->totpoly != 0));
1229
1230                                         data_cp.tree = bvhtree_from_mesh_looptri_create_tree(
1231                                                 0.0, tree_type, 6,
1232                                                 data_cp.vert, data_cp.loop,
1233                                                 data_cp.looptri, looptri_num, NULL, -1);
1234
1235                                         /* Save on cache for later use */
1236                                         /* printf("BVHTree built and saved on cache\n"); */
1237                                         bvhcache_insert(
1238                                                 &mesh->runtime.bvh_cache, data_cp.tree, BVHTREE_FROM_LOOPTRI);
1239                                 }
1240                                 BLI_rw_mutex_unlock(&cache_rwlock);
1241                         }
1242                         break;
1243                 case BVHTREE_FROM_EM_VERTS:
1244                 case BVHTREE_FROM_EM_EDGES:
1245                 case BVHTREE_FROM_EM_LOOPTRI:
1246                         BLI_assert(false);
1247                         break;
1248         }
1249
1250         if (data_cp.tree != NULL) {
1251 #ifdef DEBUG
1252                 if (BLI_bvhtree_get_tree_type(data_cp.tree) != tree_type) {
1253                         printf("tree_type %d obtained instead of %d\n",
1254                                BLI_bvhtree_get_tree_type(data_cp.tree), tree_type);
1255                 }
1256 #endif
1257                 data_cp.cached = true;
1258                 memcpy(data, &data_cp, sizeof(*data));
1259         }
1260         else {
1261                 free_bvhtree_from_mesh(&data_cp);
1262                 memset(data, 0, sizeof(*data));
1263         }
1264
1265         return data_cp.tree;
1266 }
1267
1268 /** \} */
1269
1270
1271 /* Frees data allocated by a call to bvhtree_from_editmesh_*. */
1272 void free_bvhtree_from_editmesh(struct BVHTreeFromEditMesh *data)
1273 {
1274         if (data->tree) {
1275                 if (!data->cached) {
1276                         BLI_bvhtree_free(data->tree);
1277                 }
1278                 memset(data, 0, sizeof(*data));
1279         }
1280 }
1281
1282 /* Frees data allocated by a call to bvhtree_from_mesh_*. */
1283 void free_bvhtree_from_mesh(struct BVHTreeFromMesh *data)
1284 {
1285         if (data->tree && !data->cached) {
1286                 BLI_bvhtree_free(data->tree);
1287         }
1288
1289         if (data->vert_allocated) {
1290                 MEM_freeN((void *)data->vert);
1291         }
1292         if (data->edge_allocated) {
1293                 MEM_freeN((void *)data->edge);
1294         }
1295         if (data->face_allocated) {
1296                 MEM_freeN((void *)data->face);
1297         }
1298         if (data->loop_allocated) {
1299                 MEM_freeN((void *)data->loop);
1300         }
1301         if (data->looptri_allocated) {
1302                 MEM_freeN((void *)data->looptri);
1303         }
1304
1305         memset(data, 0, sizeof(*data));
1306 }
1307
1308
1309 /* -------------------------------------------------------------------- */
1310
1311 /** \name BVHCache
1312  * \{ */
1313
1314 typedef struct BVHCacheItem {
1315         int type;
1316         BVHTree *tree;
1317
1318 } BVHCacheItem;
1319
1320 /**
1321  * Queries a bvhcache for the cache bvhtree of the request type
1322  */
1323 bool bvhcache_find(const BVHCache *cache, int type, BVHTree **r_tree)
1324 {
1325         while (cache) {
1326                 const BVHCacheItem *item = cache->link;
1327                 if (item->type == type) {
1328                         *r_tree = item->tree;
1329                         return true;
1330                 }
1331                 cache = cache->next;
1332         }
1333         return false;
1334 }
1335
1336 bool bvhcache_has_tree(const BVHCache *cache, const BVHTree *tree)
1337 {
1338         while (cache) {
1339                 const BVHCacheItem *item = cache->link;
1340                 if (item->tree == tree) {
1341                         return true;
1342                 }
1343                 cache = cache->next;
1344         }
1345         return false;
1346 }
1347
1348 /**
1349  * Inserts a BVHTree of the given type under the cache
1350  * After that the caller no longer needs to worry when to free the BVHTree
1351  * as that will be done when the cache is freed.
1352  *
1353  * A call to this assumes that there was no previous cached tree of the given type
1354  */
1355 void bvhcache_insert(BVHCache **cache_p, BVHTree *tree, int type)
1356 {
1357         BVHCacheItem *item = NULL;
1358
1359         assert(bvhcache_find(*cache_p, type, &(BVHTree *){0}) == false);
1360
1361         item = MEM_mallocN(sizeof(BVHCacheItem), "BVHCacheItem");
1362
1363         item->type = type;
1364         item->tree = tree;
1365
1366         BLI_linklist_prepend(cache_p, item);
1367 }
1368
1369 /**
1370  * frees a bvhcache
1371  */
1372 static void bvhcacheitem_free(void *_item)
1373 {
1374         BVHCacheItem *item = (BVHCacheItem *)_item;
1375
1376         BLI_bvhtree_free(item->tree);
1377         MEM_freeN(item);
1378 }
1379
1380
1381 void bvhcache_free(BVHCache **cache_p)
1382 {
1383         BLI_linklist_free(*cache_p, (LinkNodeFreeFP)bvhcacheitem_free);
1384         *cache_p = NULL;
1385 }
1386
1387 /** \} */