d48635d21d98e0294e5bc6e72b311fca1f746a20
[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_meshdata_types.h"
38
39 #include "BLI_utildefines.h"
40 #include "BLI_linklist.h"
41 #include "BLI_math.h"
42 #include "BLI_threads.h"
43
44 #include "BKE_DerivedMesh.h"
45 #include "BKE_editmesh.h"
46
47 #include "MEM_guardedalloc.h"
48
49 static ThreadRWMutex cache_rwlock = BLI_RWLOCK_INITIALIZER;
50
51 /* -------------------------------------------------------------------- */
52 /** \name Local Callbacks
53  * \{ */
54
55 /* Math stuff for ray casting on mesh faces and for nearest surface */
56
57 float bvhtree_ray_tri_intersection(
58         const BVHTreeRay *ray, const float UNUSED(m_dist),
59         const float v0[3], const float v1[3], const float v2[3])
60 {
61         float dist;
62
63 #ifdef USE_KDOPBVH_WATERTIGHT
64         if (isect_ray_tri_watertight_v3(ray->origin, ray->isect_precalc, v0, v1, v2, &dist, NULL))
65 #else
66         if (isect_ray_tri_epsilon_v3(ray->origin, ray->direction, v0, v1, v2, &dist, NULL, FLT_EPSILON))
67 #endif
68         {
69                 return dist;
70         }
71
72         return FLT_MAX;
73 }
74
75 float bvhtree_sphereray_tri_intersection(
76         const BVHTreeRay *ray, float radius, const float m_dist,
77         const float v0[3], const float v1[3], const float v2[3])
78 {
79         
80         float idist;
81         float p1[3];
82         float hit_point[3];
83
84         madd_v3_v3v3fl(p1, ray->origin, ray->direction, m_dist);
85         if (isect_sweeping_sphere_tri_v3(ray->origin, p1, radius, v0, v1, v2, &idist, hit_point)) {
86                 return idist * m_dist;
87         }
88
89         return FLT_MAX;
90 }
91
92 /*
93  * BVH from meshes callbacks
94  */
95
96 /* Callback to bvh tree nearest point. The tree must have been built using bvhtree_from_mesh_faces.
97  * userdata must be a BVHMeshCallbackUserdata built from the same mesh as the tree. */
98 static void mesh_faces_nearest_point(void *userdata, int index, const float co[3], BVHTreeNearest *nearest)
99 {
100         const BVHTreeFromMesh *data = (BVHTreeFromMesh *) userdata;
101         const MVert *vert = data->vert;
102         const MFace *face = data->face + index;
103
104         const float *t0, *t1, *t2, *t3;
105         t0 = vert[face->v1].co;
106         t1 = vert[face->v2].co;
107         t2 = vert[face->v3].co;
108         t3 = face->v4 ? vert[face->v4].co : NULL;
109
110         
111         do {
112                 float nearest_tmp[3], dist_sq;
113
114                 closest_on_tri_to_point_v3(nearest_tmp, co, t0, t1, t2);
115                 dist_sq = len_squared_v3v3(co, nearest_tmp);
116
117                 if (dist_sq < nearest->dist_sq) {
118                         nearest->index = index;
119                         nearest->dist_sq = dist_sq;
120                         copy_v3_v3(nearest->co, nearest_tmp);
121                         normal_tri_v3(nearest->no, t0, t1, t2);
122                 }
123
124                 t1 = t2;
125                 t2 = t3;
126                 t3 = NULL;
127
128         } while (t2);
129 }
130 /* copy of function above */
131 static void mesh_looptri_nearest_point(void *userdata, int index, const float co[3], BVHTreeNearest *nearest)
132 {
133         const BVHTreeFromMesh *data = (BVHTreeFromMesh *) userdata;
134         const MVert *vert = data->vert;
135         const MLoopTri *lt = &data->looptri[index];
136         const float *vtri_co[3] = {
137             vert[data->loop[lt->tri[0]].v].co,
138             vert[data->loop[lt->tri[1]].v].co,
139             vert[data->loop[lt->tri[2]].v].co,
140         };
141         float nearest_tmp[3], dist_sq;
142
143         closest_on_tri_to_point_v3(nearest_tmp, co, UNPACK3(vtri_co));
144         dist_sq = len_squared_v3v3(co, nearest_tmp);
145
146         if (dist_sq < nearest->dist_sq) {
147                 nearest->index = index;
148                 nearest->dist_sq = dist_sq;
149                 copy_v3_v3(nearest->co, nearest_tmp);
150                 normal_tri_v3(nearest->no, UNPACK3(vtri_co));
151         }
152 }
153 /* copy of function above (warning, should de-duplicate with editmesh_bvh.c) */
154 static void editmesh_faces_nearest_point(void *userdata, int index, const float co[3], BVHTreeNearest *nearest)
155 {
156         const BVHTreeFromMesh *data = (BVHTreeFromMesh *) userdata;
157         BMEditMesh *em = data->em_evil;
158         const BMLoop **ltri = (const BMLoop **)em->looptris[index];
159
160         const float *t0, *t1, *t2;
161         t0 = ltri[0]->v->co;
162         t1 = ltri[1]->v->co;
163         t2 = ltri[2]->v->co;
164
165         {
166                 float nearest_tmp[3], dist_sq;
167
168                 closest_on_tri_to_point_v3(nearest_tmp, co, t0, t1, t2);
169                 dist_sq = len_squared_v3v3(co, nearest_tmp);
170
171                 if (dist_sq < nearest->dist_sq) {
172                         nearest->index = index;
173                         nearest->dist_sq = dist_sq;
174                         copy_v3_v3(nearest->co, nearest_tmp);
175                         normal_tri_v3(nearest->no, t0, t1, t2);
176                 }
177         }
178 }
179
180 /* Callback to bvh tree raycast. The tree must have been built using bvhtree_from_mesh_faces.
181  * userdata must be a BVHMeshCallbackUserdata built from the same mesh as the tree. */
182 static void mesh_faces_spherecast(void *userdata, int index, const BVHTreeRay *ray, BVHTreeRayHit *hit)
183 {
184         const BVHTreeFromMesh *data = (BVHTreeFromMesh *) userdata;
185         const MVert *vert = data->vert;
186         const MFace *face = &data->face[index];
187
188         const float *t0, *t1, *t2, *t3;
189         t0 = vert[face->v1].co;
190         t1 = vert[face->v2].co;
191         t2 = vert[face->v3].co;
192         t3 = face->v4 ? vert[face->v4].co : NULL;
193
194         
195         do {
196                 float dist;
197                 if (data->sphere_radius == 0.0f)
198                         dist = bvhtree_ray_tri_intersection(ray, hit->dist, t0, t1, t2);
199                 else
200                         dist = bvhtree_sphereray_tri_intersection(ray, data->sphere_radius, hit->dist, t0, t1, t2);
201
202                 if (dist >= 0 && dist < hit->dist) {
203                         hit->index = index;
204                         hit->dist = dist;
205                         madd_v3_v3v3fl(hit->co, ray->origin, ray->direction, dist);
206
207                         normal_tri_v3(hit->no, t0, t1, t2);
208                 }
209
210                 t1 = t2;
211                 t2 = t3;
212                 t3 = NULL;
213
214         } while (t2);
215 }
216 /* copy of function above */
217 static void mesh_looptri_spherecast(void *userdata, int index, const BVHTreeRay *ray, BVHTreeRayHit *hit)
218 {
219         const BVHTreeFromMesh *data = (BVHTreeFromMesh *) userdata;
220         const MVert *vert = data->vert;
221         const MLoopTri *lt = &data->looptri[index];
222         const float *vtri_co[3] = {
223             vert[data->loop[lt->tri[0]].v].co,
224             vert[data->loop[lt->tri[1]].v].co,
225             vert[data->loop[lt->tri[2]].v].co,
226         };
227         float dist;
228
229         if (data->sphere_radius == 0.0f)
230                 dist = bvhtree_ray_tri_intersection(ray, hit->dist, UNPACK3(vtri_co));
231         else
232                 dist = bvhtree_sphereray_tri_intersection(ray, data->sphere_radius, hit->dist, UNPACK3(vtri_co));
233
234         if (dist >= 0 && dist < hit->dist) {
235                 hit->index = index;
236                 hit->dist = dist;
237                 madd_v3_v3v3fl(hit->co, ray->origin, ray->direction, dist);
238
239                 normal_tri_v3(hit->no, UNPACK3(vtri_co));
240         }
241 }
242 /* copy of function above (warning, should de-duplicate with editmesh_bvh.c) */
243 static void editmesh_faces_spherecast(void *userdata, int index, const BVHTreeRay *ray, BVHTreeRayHit *hit)
244 {
245         const BVHTreeFromMesh *data = (BVHTreeFromMesh *) userdata;
246         BMEditMesh *em = data->em_evil;
247         const BMLoop **ltri = (const BMLoop **)em->looptris[index];
248
249         const float *t0, *t1, *t2;
250         t0 = ltri[0]->v->co;
251         t1 = ltri[1]->v->co;
252         t2 = ltri[2]->v->co;
253
254
255         {
256                 float dist;
257                 if (data->sphere_radius == 0.0f)
258                         dist = bvhtree_ray_tri_intersection(ray, hit->dist, t0, t1, t2);
259                 else
260                         dist = bvhtree_sphereray_tri_intersection(ray, data->sphere_radius, hit->dist, t0, t1, t2);
261
262                 if (dist >= 0 && dist < hit->dist) {
263                         hit->index = index;
264                         hit->dist = dist;
265                         madd_v3_v3v3fl(hit->co, ray->origin, ray->direction, dist);
266
267                         normal_tri_v3(hit->no, t0, t1, t2);
268                 }
269         }
270 }
271
272 /* Callback to bvh tree nearest point. The tree must have been built using bvhtree_from_mesh_edges.
273  * userdata must be a BVHMeshCallbackUserdata built from the same mesh as the tree. */
274 static void mesh_edges_nearest_point(void *userdata, int index, const float co[3], BVHTreeNearest *nearest)
275 {
276         const BVHTreeFromMesh *data = (BVHTreeFromMesh *) userdata;
277         const MVert *vert = data->vert;
278         const MEdge *edge = data->edge + index;
279         float nearest_tmp[3], dist_sq;
280
281         const float *t0, *t1;
282         t0 = vert[edge->v1].co;
283         t1 = vert[edge->v2].co;
284
285         closest_to_line_segment_v3(nearest_tmp, co, t0, t1);
286         dist_sq = len_squared_v3v3(nearest_tmp, co);
287         
288         if (dist_sq < nearest->dist_sq) {
289                 nearest->index = index;
290                 nearest->dist_sq = dist_sq;
291                 copy_v3_v3(nearest->co, nearest_tmp);
292                 sub_v3_v3v3(nearest->no, t0, t1);
293                 normalize_v3(nearest->no);
294         }
295 }
296
297 /* Helper, does all the point-spherecast work actually. */
298 static void mesh_verts_spherecast_do(
299         const BVHTreeFromMesh *UNUSED(data), int index, const float v[3], const BVHTreeRay *ray, BVHTreeRayHit *hit)
300 {
301         float dist;
302         const float *r1;
303         float r2[3], i1[3];
304         r1 = ray->origin;
305         add_v3_v3v3(r2, r1, ray->direction);
306
307         closest_to_line_segment_v3(i1, v, r1, r2);
308
309         /* No hit if closest point is 'behind' the origin of the ray, or too far away from it. */
310         if ((dot_v3v3v3(r1, i1, r2) >= 0.0f) && ((dist = len_v3v3(r1, i1)) < hit->dist)) {
311                 hit->index = index;
312                 hit->dist = dist;
313                 copy_v3_v3(hit->co, i1);
314         }
315 }
316
317 /* Callback to bvh tree raycast. The tree must have been built using bvhtree_from_mesh_verts.
318  * userdata must be a BVHMeshCallbackUserdata built from the same mesh as the tree. */
319 static void mesh_verts_spherecast(void *userdata, int index, const BVHTreeRay *ray, BVHTreeRayHit *hit)
320 {
321         const BVHTreeFromMesh *data = (BVHTreeFromMesh *)userdata;
322         const float *v = data->vert[index].co;
323
324         mesh_verts_spherecast_do(data, index, v, ray, hit);
325 }
326
327 /* Callback to bvh tree raycast. The tree must have been built using bvhtree_from_mesh_edges.
328  * userdata must be a BVHMeshCallbackUserdata built from the same mesh as the tree. */
329 static void mesh_edges_spherecast(void *userdata, int index, const BVHTreeRay *ray, BVHTreeRayHit *hit)
330 {
331         const BVHTreeFromMesh *data = (BVHTreeFromMesh *)userdata;
332         const MVert *vert = data->vert;
333         const MEdge *edge = &data->edge[index];
334
335         const float radius_sq = SQUARE(data->sphere_radius);
336         float dist;
337         const float *v1, *v2, *r1;
338         float r2[3], i1[3], i2[3];
339         v1 = vert[edge->v1].co;
340         v2 = vert[edge->v2].co;
341
342         /* In case we get a zero-length edge, handle it as a point! */
343         if (equals_v3v3(v1, v2)) {
344                 mesh_verts_spherecast_do(data, index, v1, ray, hit);
345                 return;
346         }
347
348         r1 = ray->origin;
349         add_v3_v3v3(r2, r1, ray->direction);
350
351         if (isect_line_line_v3(v1, v2, r1, r2, i1, i2)) {
352                 /* No hit if intersection point is 'behind' the origin of the ray, or too far away from it. */
353                 if ((dot_v3v3v3(r1, i2, r2) >= 0.0f) && ((dist = len_v3v3(r1, i2)) < hit->dist)) {
354                         const float e_fac = line_point_factor_v3(i1, v1, v2);
355                         if (e_fac < 0.0f) {
356                                 copy_v3_v3(i1, v1);
357                         }
358                         else if (e_fac > 1.0f) {
359                                 copy_v3_v3(i1, v2);
360                         }
361                         /* Ensure ray is really close enough from edge! */
362                         if (len_squared_v3v3(i1, i2) <= radius_sq) {
363                                 hit->index = index;
364                                 hit->dist = dist;
365                                 copy_v3_v3(hit->co, i2);
366                         }
367                 }
368         }
369 }
370
371 /** \} */
372
373 /*
374  * BVH builders
375  */
376
377
378 /* -------------------------------------------------------------------- */
379
380 /** \name Vertex Builder
381  * \{ */
382
383 static BVHTree *bvhtree_from_mesh_verts_create_tree(
384         float epsilon, int tree_type, int axis,
385         MVert *vert, const int numVerts,
386         BLI_bitmap *mask, int numVerts_active)
387 {
388         BVHTree *tree = NULL;
389         int i;
390
391         if (vert) {
392                 if (mask && numVerts_active < 0) {
393                         numVerts_active = 0;
394                         for (i = 0; i < numVerts; i++) {
395                                 if (BLI_BITMAP_TEST_BOOL(mask, i)) {
396                                         numVerts_active++;
397                                 }
398                         }
399                 }
400                 else if (!mask) {
401                         numVerts_active = numVerts;
402                 }
403
404                 tree = BLI_bvhtree_new(numVerts_active, epsilon, tree_type, axis);
405
406                 if (tree) {
407                         for (i = 0; i < numVerts; i++) {
408                                 if (mask && !BLI_BITMAP_TEST_BOOL(mask, i)) {
409                                         continue;
410                                 }
411                                 BLI_bvhtree_insert(tree, i, vert[i].co, 1);
412                         }
413
414                         BLI_bvhtree_balance(tree);
415                 }
416         }
417
418         return tree;
419 }
420
421 static void bvhtree_from_mesh_verts_setup_data(
422         BVHTreeFromMesh *data, BVHTree *tree, const bool is_cached, float epsilon,
423         MVert *vert, const bool vert_allocated)
424 {
425         memset(data, 0, sizeof(*data));
426
427         if (tree) {
428                 data->tree = tree;
429                 data->cached = is_cached;
430
431                 /* a NULL nearest callback works fine
432                  * remember the min distance to point is the same as the min distance to BV of point */
433                 data->nearest_callback = NULL;
434                 data->raycast_callback = mesh_verts_spherecast;
435
436                 data->vert = vert;
437                 data->vert_allocated = vert_allocated;
438                 //data->face = DM_get_tessface_array(dm, &data->face_allocated);  /* XXX WHY???? */
439
440                 data->sphere_radius = epsilon;
441         }
442         else {
443                 if (vert_allocated) {
444                         MEM_freeN(vert);
445                 }
446         }
447 }
448
449 /* Builds a bvh tree where nodes are the vertices of the given dm */
450 BVHTree *bvhtree_from_mesh_verts(BVHTreeFromMesh *data, DerivedMesh *dm, float epsilon, int tree_type, int axis)
451 {
452         BVHTree *tree;
453         MVert *vert;
454         bool vert_allocated;
455
456         BLI_rw_mutex_lock(&cache_rwlock, THREAD_LOCK_READ);
457         tree = bvhcache_find(&dm->bvhCache, BVHTREE_FROM_VERTS);
458         BLI_rw_mutex_unlock(&cache_rwlock);
459
460         vert = DM_get_vert_array(dm, &vert_allocated);
461
462         /* Not in cache */
463         if (tree == NULL) {
464                 BLI_rw_mutex_lock(&cache_rwlock, THREAD_LOCK_WRITE);
465                 tree = bvhcache_find(&dm->bvhCache, BVHTREE_FROM_VERTS);
466                 if (tree == NULL) {
467                         tree = bvhtree_from_mesh_verts_create_tree(epsilon, tree_type, axis, vert, dm->getNumVerts(dm), NULL, -1);
468                         if (tree) {
469                                 /* Save on cache for later use */
470                                 /* printf("BVHTree built and saved on cache\n"); */
471                                 bvhcache_insert(&dm->bvhCache, tree, BVHTREE_FROM_VERTS);
472                         }
473                 }
474                 BLI_rw_mutex_unlock(&cache_rwlock);
475         }
476         else {
477                 /* printf("BVHTree is already build, using cached tree\n"); */
478         }
479
480         /* Setup BVHTreeFromMesh */
481         bvhtree_from_mesh_verts_setup_data(data, tree, true, epsilon, vert, vert_allocated);
482
483         return data->tree;
484 }
485
486 /**
487  * Builds a bvh tree where nodes are the given vertices (note: does not copy given mverts!).
488  * \param vert_allocated if true, vert freeing will be done when freeing data.
489  * \param mask if not null, true elements give which vert to add to BVH tree.
490  * \param numVerts_active if >= 0, number of active verts to add to BVH tree (else will be computed from mask).
491  */
492 BVHTree *bvhtree_from_mesh_verts_ex(
493         BVHTreeFromMesh *data, MVert *vert, const int numVerts, const bool vert_allocated,
494         BLI_bitmap *mask, int numVerts_active,
495         float epsilon, int tree_type, int axis)
496 {
497         BVHTree *tree = bvhtree_from_mesh_verts_create_tree(epsilon, tree_type, axis, vert, numVerts, mask, numVerts_active);
498
499         /* Setup BVHTreeFromMesh */
500         bvhtree_from_mesh_verts_setup_data(data, tree, false, epsilon, vert, vert_allocated);
501
502         return data->tree;
503 }
504
505 /** \} */
506
507
508 /* -------------------------------------------------------------------- */
509
510 /** \name Edge Builder
511  * \{ */
512
513 /* Builds a bvh tree where nodes are the edges of the given dm */
514 BVHTree *bvhtree_from_mesh_edges(BVHTreeFromMesh *data, DerivedMesh *dm, float epsilon, int tree_type, int axis)
515 {
516         BVHTree *tree;
517         MVert *vert;
518         MEdge *edge;
519         bool vert_allocated, edge_allocated;
520
521         BLI_rw_mutex_lock(&cache_rwlock, THREAD_LOCK_READ);
522         tree = bvhcache_find(&dm->bvhCache, BVHTREE_FROM_EDGES);
523         BLI_rw_mutex_unlock(&cache_rwlock);
524
525         vert = DM_get_vert_array(dm, &vert_allocated);
526         edge = DM_get_edge_array(dm, &edge_allocated);
527
528         /* Not in cache */
529         if (tree == NULL) {
530                 BLI_rw_mutex_lock(&cache_rwlock, THREAD_LOCK_WRITE);
531                 tree = bvhcache_find(&dm->bvhCache, BVHTREE_FROM_EDGES);
532                 if (tree == NULL) {
533                         int i;
534                         int numEdges = dm->getNumEdges(dm);
535
536                         if (vert != NULL && edge != NULL) {
537                                 /* Create a bvh-tree of the given target */
538                                 tree = BLI_bvhtree_new(numEdges, epsilon, tree_type, axis);
539                                 if (tree != NULL) {
540                                         for (i = 0; i < numEdges; i++) {
541                                                 float co[2][3];
542                                                 copy_v3_v3(co[0], vert[edge[i].v1].co);
543                                                 copy_v3_v3(co[1], vert[edge[i].v2].co);
544
545                                                 BLI_bvhtree_insert(tree, i, co[0], 2);
546                                         }
547                                         BLI_bvhtree_balance(tree);
548
549                                         /* Save on cache for later use */
550                                         /* printf("BVHTree built and saved on cache\n"); */
551                                         bvhcache_insert(&dm->bvhCache, tree, BVHTREE_FROM_EDGES);
552                                 }
553                         }
554                 }
555                 BLI_rw_mutex_unlock(&cache_rwlock);
556         }
557         else {
558                 /* printf("BVHTree is already build, using cached tree\n"); */
559         }
560
561
562         /* Setup BVHTreeFromMesh */
563         memset(data, 0, sizeof(*data));
564         data->tree = tree;
565
566         if (data->tree) {
567                 data->cached = true;
568
569                 data->nearest_callback = mesh_edges_nearest_point;
570                 data->raycast_callback = mesh_edges_spherecast;
571
572                 data->vert = vert;
573                 data->vert_allocated = vert_allocated;
574                 data->edge = edge;
575                 data->edge_allocated = edge_allocated;
576
577                 data->sphere_radius = epsilon;
578         }
579         else {
580                 if (vert_allocated) {
581                         MEM_freeN(vert);
582                 }
583                 if (edge_allocated) {
584                         MEM_freeN(edge);
585                 }
586         }
587         return data->tree;
588 }
589
590 /** \} */
591
592
593 /* -------------------------------------------------------------------- */
594
595 /** \name Tessellated Face Builder
596  * \{ */
597
598 static BVHTree *bvhtree_from_mesh_faces_create_tree(
599         float epsilon, int tree_type, int axis,
600         BMEditMesh *em, MVert *vert, MFace *face, const int numFaces,
601         BLI_bitmap *mask, int numFaces_active)
602 {
603         BVHTree *tree = NULL;
604         int i;
605
606         if (numFaces) {
607                 if (mask && numFaces_active < 0) {
608                         numFaces_active = 0;
609                         for (i = 0; i < numFaces; i++) {
610                                 if (BLI_BITMAP_TEST_BOOL(mask, i)) {
611                                         numFaces_active++;
612                                 }
613                         }
614                 }
615                 else if (!mask) {
616                         numFaces_active = numFaces;
617                 }
618
619                 /* Create a bvh-tree of the given target */
620                 /* printf("%s: building BVH, total=%d\n", __func__, numFaces); */
621                 tree = BLI_bvhtree_new(numFaces_active, epsilon, tree_type, axis);
622                 if (tree) {
623                         if (em) {
624                                 const struct BMLoop *(*looptris)[3] = (void *)em->looptris;
625
626                                 /* avoid double-up on face searches for quads-ngons */
627                                 bool insert_prev = false;
628                                 BMFace *f_prev = NULL;
629
630                                 /* data->em_evil is only set for snapping, and only for the mesh of the object
631                                  * which is currently open in edit mode. When set, the bvhtree should not contain
632                                  * faces that will interfere with snapping (e.g. faces that are hidden/selected
633                                  * or faces that have selected verts). */
634
635                                 /* Insert BMesh-tessellation triangles into the bvh tree, unless they are hidden
636                                  * and/or selected. Even if the faces themselves are not selected for the snapped
637                                  * transform, having a vertex selected means the face (and thus it's tessellated
638                                  * triangles) will be moving and will not be a good snap targets. */
639                                 for (i = 0; i < numFaces; i++) {
640                                         const BMLoop **ltri = looptris[i];
641                                         BMFace *f = ltri[0]->f;
642                                         bool insert = mask ? BLI_BITMAP_TEST_BOOL(mask, i) : true;
643
644                                         /* Start with the assumption the triangle should be included for snapping. */
645                                         if (f == f_prev) {
646                                                 insert = insert_prev;
647                                         }
648                                         else if (insert) {
649                                                 if (tree_type == BVHTREE_FROM_FACES_EDITMESH_ALL) {
650                                                         /* pass */
651                                                 }
652                                                 else if (BM_elem_flag_test(f, BM_ELEM_SELECT) || BM_elem_flag_test(f, BM_ELEM_HIDDEN)) {
653                                                         /* Don't insert triangles tessellated from faces that are hidden or selected */
654                                                         insert = false;
655                                                 }
656                                                 else {
657                                                         BMLoop *l_iter, *l_first;
658                                                         l_iter = l_first = BM_FACE_FIRST_LOOP(f);
659                                                         do {
660                                                                 if (BM_elem_flag_test(l_iter->v, BM_ELEM_SELECT)) {
661                                                                         /* Don't insert triangles tessellated from faces that have any selected verts */
662                                                                         insert = false;
663                                                                         break;
664                                                                 }
665                                                         } while ((l_iter = l_iter->next) != l_first);
666                                                 }
667
668                                                 /* skip if face doesn't change */
669                                                 f_prev = f;
670                                                 insert_prev = insert;
671                                         }
672
673                                         if (insert) {
674                                                 /* No reason found to block hit-testing the triangle for snap, so insert it now.*/
675                                                 float co[3][3];
676                                                 copy_v3_v3(co[0], ltri[0]->v->co);
677                                                 copy_v3_v3(co[1], ltri[1]->v->co);
678                                                 copy_v3_v3(co[2], ltri[2]->v->co);
679
680                                                 BLI_bvhtree_insert(tree, i, co[0], 3);
681                                         }
682                                 }
683                         }
684                         else {
685                                 if (vert && face) {
686                                         for (i = 0; i < numFaces; i++) {
687                                                 float co[4][3];
688                                                 if (mask && !BLI_BITMAP_TEST_BOOL(mask, i)) {
689                                                         continue;
690                                                 }
691
692                                                 copy_v3_v3(co[0], vert[face[i].v1].co);
693                                                 copy_v3_v3(co[1], vert[face[i].v2].co);
694                                                 copy_v3_v3(co[2], vert[face[i].v3].co);
695                                                 if (face[i].v4)
696                                                         copy_v3_v3(co[3], vert[face[i].v4].co);
697
698                                                 BLI_bvhtree_insert(tree, i, co[0], face[i].v4 ? 4 : 3);
699                                         }
700                                 }
701                         }
702                         BLI_bvhtree_balance(tree);
703                 }
704         }
705
706         return tree;
707 }
708
709 static void bvhtree_from_mesh_faces_setup_data(
710         BVHTreeFromMesh *data, BVHTree *tree, const bool is_cached,
711         float epsilon, BMEditMesh *em,
712         MVert *vert, const bool vert_allocated,
713         MFace *face, const bool face_allocated)
714 {
715         memset(data, 0, sizeof(*data));
716         data->em_evil = em;
717
718         if (tree) {
719                 data->tree = tree;
720                 data->cached = is_cached;
721
722                 if (em) {
723                         data->nearest_callback = editmesh_faces_nearest_point;
724                         data->raycast_callback = editmesh_faces_spherecast;
725                 }
726                 else {
727                         data->nearest_callback = mesh_faces_nearest_point;
728                         data->raycast_callback = mesh_faces_spherecast;
729
730                         data->vert = vert;
731                         data->vert_allocated = vert_allocated;
732                         data->face = face;
733                         data->face_allocated = face_allocated;
734                 }
735
736                 data->sphere_radius = epsilon;
737         }
738         else {
739                 if (vert_allocated) {
740                         MEM_freeN(vert);
741                 }
742                 if (face_allocated) {
743                         MEM_freeN(face);
744                 }
745         }
746 }
747
748 /* Builds a bvh tree where nodes are the tesselated faces of the given dm */
749 BVHTree *bvhtree_from_mesh_faces(BVHTreeFromMesh *data, DerivedMesh *dm, float epsilon, int tree_type, int axis)
750 {
751         BMEditMesh *em = data->em_evil;
752         const int bvhcache_type = em ?
753                 (data->em_evil_all ? BVHTREE_FROM_FACES_EDITMESH_ALL : BVHTREE_FROM_FACES_EDITMESH_SNAP) :
754                 BVHTREE_FROM_FACES;
755         BVHTree *tree;
756         MVert *vert = NULL;
757         MFace *face = NULL;
758         bool vert_allocated = false, face_allocated = false;
759
760         BLI_rw_mutex_lock(&cache_rwlock, THREAD_LOCK_READ);
761         tree = bvhcache_find(&dm->bvhCache, bvhcache_type);
762         BLI_rw_mutex_unlock(&cache_rwlock);
763
764         if (em == NULL) {
765                 vert = DM_get_vert_array(dm, &vert_allocated);
766                 face = DM_get_tessface_array(dm, &face_allocated);
767         }
768
769         /* Not in cache */
770         if (tree == NULL) {
771                 BLI_rw_mutex_lock(&cache_rwlock, THREAD_LOCK_WRITE);
772                 tree = bvhcache_find(&dm->bvhCache, bvhcache_type);
773                 if (tree == NULL) {
774                         int numFaces;
775
776                         /* BMESH specific check that we have tessfaces,
777                          * we _could_ tessellate here but rather not - campbell
778                          *
779                          * this assert checks we have tessfaces,
780                          * if not caller should use DM_ensure_tessface() */
781                         if (em) {
782                                 numFaces = em->tottri;
783                         }
784                         else {
785                                 numFaces = dm->getNumTessFaces(dm);
786                                 BLI_assert(!(numFaces == 0 && dm->getNumPolys(dm) != 0));
787                         }
788
789                         tree = bvhtree_from_mesh_faces_create_tree(epsilon, tree_type, axis, em, vert, face, numFaces, NULL, -1);
790                         if (tree) {
791                                 /* Save on cache for later use */
792                                 /* printf("BVHTree built and saved on cache\n"); */
793                                 bvhcache_insert(&dm->bvhCache, tree, bvhcache_type);
794                         }
795                 }
796                 BLI_rw_mutex_unlock(&cache_rwlock);
797         }
798         else {
799                 /* printf("BVHTree is already build, using cached tree\n"); */
800         }
801
802         /* Setup BVHTreeFromMesh */
803         bvhtree_from_mesh_faces_setup_data(data, tree, true, epsilon, em, vert, vert_allocated, face, face_allocated);
804
805         return data->tree;
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 mask if not null, true elements give which faces to add to BVH tree.
813  * \param numFaces_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, MVert *vert, const bool vert_allocated,
817         MFace *face, const int numFaces, const bool face_allocated,
818         BLI_bitmap *mask, int numFaces_active, float epsilon, int tree_type, int axis)
819 {
820         BVHTree *tree = bvhtree_from_mesh_faces_create_tree(
821                 epsilon, tree_type, axis, NULL, vert, face, numFaces,
822                 mask, numFaces_active);
823
824         /* Setup BVHTreeFromMesh */
825         bvhtree_from_mesh_faces_setup_data(data, tree, false, epsilon, NULL, vert, vert_allocated, face, face_allocated);
826
827         return data->tree;
828 }
829
830 /** \} */
831
832
833 /* -------------------------------------------------------------------- */
834
835 /** \name LoopTri Face Builder
836  * \{ */
837
838 static BVHTree *bvhtree_from_mesh_looptri_create_tree(
839         float epsilon, int tree_type, int axis,
840         BMEditMesh *em, const MVert *vert, const MLoop *mloop, const MLoopTri *looptri, const int looptri_num,
841         BLI_bitmap *mask, int looptri_num_active)
842 {
843         BVHTree *tree = NULL;
844         int i;
845
846         if (looptri_num) {
847                 if (mask && looptri_num_active < 0) {
848                         looptri_num_active = 0;
849                         for (i = 0; i < looptri_num; i++) {
850                                 if (BLI_BITMAP_TEST_BOOL(mask, i)) {
851                                         looptri_num_active++;
852                                 }
853                         }
854                 }
855                 else if (!mask) {
856                         looptri_num_active = looptri_num;
857                 }
858
859                 /* Create a bvh-tree of the given target */
860                 /* printf("%s: building BVH, total=%d\n", __func__, numFaces); */
861                 tree = BLI_bvhtree_new(looptri_num_active, epsilon, tree_type, axis);
862                 if (tree) {
863                         if (em) {
864                                 const struct BMLoop *(*looptris)[3] = (void *)em->looptris;
865
866                                 /* avoid double-up on face searches for quads-ngons */
867                                 bool insert_prev = false;
868                                 BMFace *f_prev = NULL;
869
870                                 /* data->em_evil is only set for snapping, and only for the mesh of the object
871                                  * which is currently open in edit mode. When set, the bvhtree should not contain
872                                  * faces that will interfere with snapping (e.g. faces that are hidden/selected
873                                  * or faces that have selected verts). */
874
875                                 /* Insert BMesh-tessellation triangles into the bvh tree, unless they are hidden
876                                  * and/or selected. Even if the faces themselves are not selected for the snapped
877                                  * transform, having a vertex selected means the face (and thus it's tessellated
878                                  * triangles) will be moving and will not be a good snap targets. */
879                                 for (i = 0; i < looptri_num; i++) {
880                                         const BMLoop **ltri = looptris[i];
881                                         BMFace *f = ltri[0]->f;
882                                         bool insert = mask ? BLI_BITMAP_TEST_BOOL(mask, i) : true;
883
884                                         /* Start with the assumption the triangle should be included for snapping. */
885                                         if (f == f_prev) {
886                                                 insert = insert_prev;
887                                         }
888                                         else if (insert) {
889                                                 if (tree_type == BVHTREE_FROM_FACES_EDITMESH_ALL) {
890                                                         /* pass */
891                                                 }
892                                                 else if (BM_elem_flag_test(f, BM_ELEM_SELECT) || BM_elem_flag_test(f, BM_ELEM_HIDDEN)) {
893                                                         /* Don't insert triangles tessellated from faces that are hidden or selected */
894                                                         insert = false;
895                                                 }
896                                                 else {
897                                                         BMLoop *l_iter, *l_first;
898                                                         l_iter = l_first = BM_FACE_FIRST_LOOP(f);
899                                                         do {
900                                                                 if (BM_elem_flag_test(l_iter->v, BM_ELEM_SELECT)) {
901                                                                         /* Don't insert triangles tessellated from faces that have any selected verts */
902                                                                         insert = false;
903                                                                         break;
904                                                                 }
905                                                         } while ((l_iter = l_iter->next) != l_first);
906                                                 }
907
908                                                 /* skip if face doesn't change */
909                                                 f_prev = f;
910                                                 insert_prev = insert;
911                                         }
912
913                                         if (insert) {
914                                                 /* No reason found to block hit-testing the triangle for snap, so insert it now.*/
915                                                 float co[3][3];
916                                                 copy_v3_v3(co[0], ltri[0]->v->co);
917                                                 copy_v3_v3(co[1], ltri[1]->v->co);
918                                                 copy_v3_v3(co[2], ltri[2]->v->co);
919
920                                                 BLI_bvhtree_insert(tree, i, co[0], 3);
921                                         }
922                                 }
923                         }
924                         else {
925                                 if (vert && looptri) {
926                                         for (i = 0; i < looptri_num; i++) {
927                                                 float co[3][3];
928                                                 if (mask && !BLI_BITMAP_TEST_BOOL(mask, i)) {
929                                                         continue;
930                                                 }
931
932                                                 copy_v3_v3(co[0], vert[mloop[looptri[i].tri[0]].v].co);
933                                                 copy_v3_v3(co[1], vert[mloop[looptri[i].tri[1]].v].co);
934                                                 copy_v3_v3(co[2], vert[mloop[looptri[i].tri[2]].v].co);
935
936                                                 BLI_bvhtree_insert(tree, i, co[0], 3);
937                                         }
938                                 }
939                         }
940                         BLI_bvhtree_balance(tree);
941                 }
942         }
943
944         return tree;
945 }
946
947 static void bvhtree_from_mesh_looptri_setup_data(
948         BVHTreeFromMesh *data, BVHTree *tree, const bool is_cached,
949         float epsilon, BMEditMesh *em,
950         const MVert *vert, const bool vert_allocated,
951         const MLoop *mloop, const bool loop_allocated,
952         const MLoopTri *looptri, const bool looptri_allocated)
953 {
954         memset(data, 0, sizeof(*data));
955         data->em_evil = em;
956
957         if (tree) {
958                 data->tree = tree;
959                 data->cached = is_cached;
960
961                 if (em) {
962                         data->nearest_callback = editmesh_faces_nearest_point;
963                         data->raycast_callback = editmesh_faces_spherecast;
964                 }
965                 else {
966                         data->nearest_callback = mesh_looptri_nearest_point;
967                         data->raycast_callback = mesh_looptri_spherecast;
968
969                         data->vert = vert;
970                         data->vert_allocated = vert_allocated;
971                         data->loop = mloop;
972                         data->loop_allocated = loop_allocated;
973                         data->looptri = looptri;
974                         data->looptri_allocated = looptri_allocated;
975                 }
976
977                 data->sphere_radius = epsilon;
978         }
979         else {
980                 if (vert_allocated) {
981                         MEM_freeN((void *)vert);
982                 }
983                 if (loop_allocated) {
984                         MEM_freeN((void *)mloop);
985                 }
986                 if (looptri_allocated) {
987                         MEM_freeN((void *)looptri);
988                 }
989         }
990 }
991
992 /**
993  * Builds a bvh tree where nodes are the looptri faces of the given dm
994  *
995  * \note for editmesh this is currently a duplicate of bvhtree_from_mesh_faces
996  */
997 BVHTree *bvhtree_from_mesh_looptri(BVHTreeFromMesh *data, DerivedMesh *dm, float epsilon, int tree_type, int axis)
998 {
999         BMEditMesh *em = data->em_evil;
1000         const int bvhcache_type = em ?
1001                 (data->em_evil_all ? BVHTREE_FROM_FACES_EDITMESH_ALL : BVHTREE_FROM_FACES_EDITMESH_SNAP) :
1002                 BVHTREE_FROM_LOOPTRI;
1003         BVHTree *tree;
1004         MVert *mvert = NULL;
1005         MLoop *mloop = NULL;
1006         const MLoopTri *looptri = NULL;
1007         bool vert_allocated = false;
1008         bool loop_allocated = false;
1009         bool looptri_allocated = false;
1010
1011         BLI_rw_mutex_lock(&cache_rwlock, THREAD_LOCK_READ);
1012         tree = bvhcache_find(&dm->bvhCache, bvhcache_type);
1013         BLI_rw_mutex_unlock(&cache_rwlock);
1014
1015         if (em == NULL) {
1016                 MPoly *mpoly;
1017                 bool poly_allocated = false;
1018
1019                 mvert = DM_get_vert_array(dm, &vert_allocated);
1020                 mpoly = DM_get_poly_array(dm, &poly_allocated);
1021
1022                 mloop = DM_get_loop_array(dm, &loop_allocated);
1023                 looptri = DM_get_looptri_array(
1024                         dm,
1025                         mvert,
1026                         mpoly, dm->getNumPolys(dm),
1027                         mloop, dm->getNumLoops(dm),
1028                         &looptri_allocated);
1029
1030                 if (poly_allocated) {
1031                         MEM_freeN(mpoly);
1032                 }
1033
1034         }
1035
1036         /* Not in cache */
1037         if (tree == NULL) {
1038                 BLI_rw_mutex_lock(&cache_rwlock, THREAD_LOCK_WRITE);
1039                 tree = bvhcache_find(&dm->bvhCache, bvhcache_type);
1040                 if (tree == NULL) {
1041                         int looptri_num;
1042
1043                         /* BMESH specific check that we have tessfaces,
1044                          * we _could_ tessellate here but rather not - campbell
1045                          *
1046                          * this assert checks we have tessfaces,
1047                          * if not caller should use DM_ensure_tessface() */
1048                         if (em) {
1049                                 looptri_num = em->tottri;
1050                         }
1051                         else {
1052                                 looptri_num = dm->getNumLoopTri(dm);
1053                                 BLI_assert(!(looptri_num == 0 && dm->getNumPolys(dm) != 0));
1054                         }
1055
1056                         tree = bvhtree_from_mesh_looptri_create_tree(
1057                                 epsilon, tree_type, axis, em,
1058                                 mvert, mloop, looptri, looptri_num, NULL, -1);
1059                         if (tree) {
1060                                 /* Save on cache for later use */
1061                                 /* printf("BVHTree built and saved on cache\n"); */
1062                                 bvhcache_insert(&dm->bvhCache, tree, bvhcache_type);
1063                         }
1064                 }
1065                 BLI_rw_mutex_unlock(&cache_rwlock);
1066         }
1067         else {
1068                 /* printf("BVHTree is already build, using cached tree\n"); */
1069         }
1070
1071         /* Setup BVHTreeFromMesh */
1072         bvhtree_from_mesh_looptri_setup_data(
1073                 data, tree, true, epsilon, em,
1074                 mvert, vert_allocated,
1075                 mloop, loop_allocated,
1076                 looptri, looptri_allocated);
1077
1078         return data->tree;
1079 }
1080
1081 BVHTree *bvhtree_from_mesh_looptri_ex(
1082         BVHTreeFromMesh *data,
1083         const struct MVert *vert, const bool vert_allocated,
1084         const struct MLoop *mloop, const bool loop_allocated,
1085         const struct MLoopTri *looptri, const int looptri_num, const bool looptri_allocated,
1086         BLI_bitmap *mask, int looptri_num_active,
1087         float epsilon, int tree_type, int axis)
1088 {
1089         BVHTree *tree = bvhtree_from_mesh_looptri_create_tree(
1090                 epsilon, tree_type, axis, NULL, vert, mloop, looptri, looptri_num,
1091                 mask, looptri_num_active);
1092
1093         /* Setup BVHTreeFromMesh */
1094         bvhtree_from_mesh_looptri_setup_data(
1095                 data, tree, false, epsilon, NULL,
1096                 vert, vert_allocated,
1097                 mloop, loop_allocated,
1098                 looptri, looptri_allocated);
1099
1100         return data->tree;
1101 }
1102
1103 /** \} */
1104
1105
1106 /* Frees data allocated by a call to bvhtree_from_mesh_*. */
1107 void free_bvhtree_from_mesh(struct BVHTreeFromMesh *data)
1108 {
1109         if (data->tree) {
1110                 if (!data->cached) {
1111                         BLI_bvhtree_free(data->tree);
1112                 }
1113
1114                 if (data->vert_allocated) {
1115                         MEM_freeN((void *)data->vert);
1116                 }
1117                 if (data->edge_allocated) {
1118                         MEM_freeN((void *)data->edge);
1119                 }
1120                 if (data->face_allocated) {
1121                         MEM_freeN((void *)data->face);
1122                 }
1123                 if (data->loop_allocated) {
1124                         MEM_freeN((void *)data->loop);
1125                 }
1126                 if (data->looptri_allocated) {
1127                         MEM_freeN((void *)data->looptri);
1128                 }
1129
1130                 memset(data, 0, sizeof(*data));
1131         }
1132 }
1133
1134
1135 /* -------------------------------------------------------------------- */
1136
1137 /** \name BVHCache
1138  * \{ */
1139
1140 typedef struct BVHCacheItem {
1141         int type;
1142         BVHTree *tree;
1143
1144 } BVHCacheItem;
1145
1146 static void bvhcacheitem_set_if_match(void *_cached, void *_search)
1147 {
1148         BVHCacheItem *cached = (BVHCacheItem *)_cached;
1149         BVHCacheItem *search = (BVHCacheItem *)_search;
1150
1151         if (search->type == cached->type) {
1152                 search->tree = cached->tree;
1153         }
1154
1155
1156 BVHTree *bvhcache_find(BVHCache *cache, int type)
1157 {
1158         BVHCacheItem item;
1159         item.type = type;
1160         item.tree = NULL;
1161
1162         BLI_linklist_apply(*cache, bvhcacheitem_set_if_match, &item);
1163         return item.tree;
1164 }
1165
1166 void bvhcache_insert(BVHCache *cache, BVHTree *tree, int type)
1167 {
1168         BVHCacheItem *item = NULL;
1169
1170         assert(tree != NULL);
1171         assert(bvhcache_find(cache, type) == NULL);
1172
1173         item = MEM_mallocN(sizeof(BVHCacheItem), "BVHCacheItem");
1174         assert(item != NULL);
1175
1176         item->type = type;
1177         item->tree = tree;
1178
1179         BLI_linklist_prepend(cache, item);
1180 }
1181
1182
1183 void bvhcache_init(BVHCache *cache)
1184 {
1185         *cache = NULL;
1186 }
1187
1188 static void bvhcacheitem_free(void *_item)
1189 {
1190         BVHCacheItem *item = (BVHCacheItem *)_item;
1191
1192         BLI_bvhtree_free(item->tree);
1193         MEM_freeN(item);
1194 }
1195
1196
1197 void bvhcache_free(BVHCache *cache)
1198 {
1199         BLI_linklist_free(*cache, (LinkNodeFreeFP)bvhcacheitem_free);
1200         *cache = NULL;
1201 }
1202
1203 /** \} */