style cleanup: blenkernel
[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
42 #include "BKE_DerivedMesh.h"
43 #include "BKE_tessmesh.h"
44
45 #include "BLI_math.h"
46 #include "MEM_guardedalloc.h"
47
48 /* Math stuff for ray casting on mesh faces and for nearest surface */
49
50 float bvhtree_ray_tri_intersection(const BVHTreeRay *ray, const float UNUSED(m_dist), const float v0[3], const float v1[3], const float v2[3])
51 {
52         float dist;
53
54         if (isect_ray_tri_epsilon_v3(ray->origin, ray->direction, v0, v1, v2, &dist, NULL, FLT_EPSILON))
55                 return dist;
56
57         return FLT_MAX;
58 }
59
60 static float sphereray_tri_intersection(const BVHTreeRay *ray, float radius, const float m_dist, const float v0[3], const float v1[3], const float v2[3])
61 {
62         
63         float idist;
64         float p1[3];
65         float plane_normal[3], hit_point[3];
66
67         normal_tri_v3(plane_normal, v0, v1, v2);
68
69         madd_v3_v3v3fl(p1, ray->origin, ray->direction, m_dist);
70         if (isect_sweeping_sphere_tri_v3(ray->origin, p1, radius, v0, v1, v2, &idist, hit_point)) {
71                 return idist * m_dist;
72         }
73
74         return FLT_MAX;
75 }
76
77
78 /*
79  * Function adapted from David Eberly's distance tools (LGPL)
80  * http://www.geometrictools.com/LibFoundation/Distance/Distance.html
81  */
82 float nearest_point_in_tri_surface(const float v0[3], const float v1[3], const float v2[3], const float p[3], int *v, int *e, float nearest[3])
83 {
84         float diff[3];
85         float e0[3];
86         float e1[3];
87         float A00;
88         float A01;
89         float A11;
90         float B0;
91         float B1;
92         float C;
93         float Det;
94         float S;
95         float T;
96         float sqrDist;
97         int lv = -1, le = -1;
98         
99         sub_v3_v3v3(diff, v0, p);
100         sub_v3_v3v3(e0, v1, v0);
101         sub_v3_v3v3(e1, v2, v0);
102         
103         A00 = dot_v3v3(e0, e0);
104         A01 = dot_v3v3(e0, e1);
105         A11 = dot_v3v3(e1, e1);
106         B0 = dot_v3v3(diff, e0);
107         B1 = dot_v3v3(diff, e1);
108         C = dot_v3v3(diff, diff);
109         Det = fabs(A00 * A11 - A01 * A01);
110         S = A01 * B1 - A11 * B0;
111         T = A01 * B0 - A00 * B1;
112
113         if (S + T <= Det) {
114                 if (S < 0.0f) {
115                         if (T < 0.0f) { /* Region 4 */
116                                 if (B0 < 0.0f) {
117                                         T = 0.0f;
118                                         if (-B0 >= A00) {
119                                                 S = 1.0f;
120                                                 sqrDist = A00 + 2.0f * B0 + C;
121                                                 lv = 1;
122                                         }
123                                         else {
124                                                 if (fabsf(A00) > FLT_EPSILON)
125                                                         S = -B0 / A00;
126                                                 else
127                                                         S = 0.0f;
128                                                 sqrDist = B0 * S + C;
129                                                 le = 0;
130                                         }
131                                 }
132                                 else {
133                                         S = 0.0f;
134                                         if (B1 >= 0.0f) {
135                                                 T = 0.0f;
136                                                 sqrDist = C;
137                                                 lv = 0;
138                                         }
139                                         else if (-B1 >= A11) {
140                                                 T = 1.0f;
141                                                 sqrDist = A11 + 2.0f * B1 + C;
142                                                 lv = 2;
143                                         }
144                                         else {
145                                                 if (fabsf(A11) > FLT_EPSILON)
146                                                         T = -B1 / A11;
147                                                 else
148                                                         T = 0.0f;
149                                                 sqrDist = B1 * T + C;
150                                                 le = 1;
151                                         }
152                                 }
153                         }
154                         else { /* Region 3 */
155                                 S = 0.0f;
156                                 if (B1 >= 0.0f) {
157                                         T = 0.0f;
158                                         sqrDist = C;
159                                         lv = 0;
160                                 }
161                                 else if (-B1 >= A11) {
162                                         T = 1.0f;
163                                         sqrDist = A11 + 2.0f * B1 + C;
164                                         lv = 2;
165                                 }
166                                 else {
167                                         if (fabsf(A11) > FLT_EPSILON)
168                                                 T = -B1 / A11;
169                                         else
170                                                 T = 0.0;
171                                         sqrDist = B1 * T + C;
172                                         le = 1;
173                                 }
174                         }
175                 }
176                 else if (T < 0.0f) { /* Region 5 */
177                         T = 0.0f;
178                         if (B0 >= 0.0f) {
179                                 S = 0.0f;
180                                 sqrDist = C;
181                                 lv = 0;
182                         }
183                         else if (-B0 >= A00) {
184                                 S = 1.0f;
185                                 sqrDist = A00 + 2.0f * B0 + C;
186                                 lv = 1;
187                         }
188                         else {
189                                 if (fabsf(A00) > FLT_EPSILON)
190                                         S = -B0 / A00;
191                                 else
192                                         S = 0.0f;
193                                 sqrDist = B0 * S + C;
194                                 le = 0;
195                         }
196                 }
197                 else { /* Region 0 */
198                         /* Minimum at interior lv */
199                         float invDet;
200                         if (fabsf(Det) > FLT_EPSILON)
201                                 invDet = 1.0f / Det;
202                         else
203                                 invDet = 0.0f;
204                         S *= invDet;
205                         T *= invDet;
206                         sqrDist = S * (A00 * S + A01 * T + 2.0f * B0) +
207                                   T * (A01 * S + A11 * T + 2.0f * B1) + C;
208                 }
209         }
210         else {
211                 float tmp0, tmp1, numer, denom;
212
213                 if (S < 0.0f) { /* Region 2 */
214                         tmp0 = A01 + B0;
215                         tmp1 = A11 + B1;
216                         if (tmp1 > tmp0) {
217                                 numer = tmp1 - tmp0;
218                                 denom = A00 - 2.0f * A01 + A11;
219                                 if (numer >= denom) {
220                                         S = 1.0f;
221                                         T = 0.0f;
222                                         sqrDist = A00 + 2.0f * B0 + C;
223                                         lv = 1;
224                                 }
225                                 else {
226                                         if (fabsf(denom) > FLT_EPSILON)
227                                                 S = numer / denom;
228                                         else
229                                                 S = 0.0f;
230                                         T = 1.0f - S;
231                                         sqrDist = S * (A00 * S + A01 * T + 2.0f * B0) +
232                                                   T * (A01 * S + A11 * T + 2.0f * B1) + C;
233                                         le = 2;
234                                 }
235                         }
236                         else {
237                                 S = 0.0f;
238                                 if (tmp1 <= 0.0f) {
239                                         T = 1.0f;
240                                         sqrDist = A11 + 2.0f * B1 + C;
241                                         lv = 2;
242                                 }
243                                 else if (B1 >= 0.0f) {
244                                         T = 0.0f;
245                                         sqrDist = C;
246                                         lv = 0;
247                                 }
248                                 else {
249                                         if (fabsf(A11) > FLT_EPSILON)
250                                                 T = -B1 / A11;
251                                         else
252                                                 T = 0.0f;
253                                         sqrDist = B1 * T + C;
254                                         le = 1;
255                                 }
256                         }
257                 }
258                 else if (T < 0.0f) { /* Region 6 */
259                         tmp0 = A01 + B1;
260                         tmp1 = A00 + B0;
261                         if (tmp1 > tmp0) {
262                                 numer = tmp1 - tmp0;
263                                 denom = A00 - 2.0f * A01 + A11;
264                                 if (numer >= denom) {
265                                         T = 1.0f;
266                                         S = 0.0f;
267                                         sqrDist = A11 + 2.0f * B1 + C;
268                                         lv = 2;
269                                 }
270                                 else {
271                                         if (fabsf(denom) > FLT_EPSILON)
272                                                 T = numer / denom;
273                                         else
274                                                 T = 0.0f;
275                                         S = 1.0f - T;
276                                         sqrDist = S * (A00 * S + A01 * T + 2.0f * B0) +
277                                                   T * (A01 * S + A11 * T + 2.0f * B1) + C;
278                                         le = 2;
279                                 }
280                         }
281                         else {
282                                 T = 0.0f;
283                                 if (tmp1 <= 0.0f) {
284                                         S = 1.0f;
285                                         sqrDist = A00 + 2.0f * B0 + C;
286                                         lv = 1;
287                                 }
288                                 else if (B0 >= 0.0f) {
289                                         S = 0.0f;
290                                         sqrDist = C;
291                                         lv = 0;
292                                 }
293                                 else {
294                                         if (fabsf(A00) > FLT_EPSILON)
295                                                 S = -B0 / A00;
296                                         else
297                                                 S = 0.0f;
298                                         sqrDist = B0 * S + C;
299                                         le = 0;
300                                 }
301                         }
302                 }
303                 else { /* Region 1 */
304                         numer = A11 + B1 - A01 - B0;
305                         if (numer <= 0.0f) {
306                                 S = 0.0f;
307                                 T = 1.0f;
308                                 sqrDist = A11 + 2.0f * B1 + C;
309                                 lv = 2;
310                         }
311                         else {
312                                 denom = A00 - 2.0f * A01 + A11;
313                                 if (numer >= denom) {
314                                         S = 1.0f;
315                                         T = 0.0f;
316                                         sqrDist = A00 + 2.0f * B0 + C;
317                                         lv = 1;
318                                 }
319                                 else {
320                                         if (fabsf(denom) > FLT_EPSILON)
321                                                 S = numer / denom;
322                                         else
323                                                 S = 0.0f;
324                                         T = 1.0f - S;
325                                         sqrDist = S * (A00 * S + A01 * T + 2.0f * B0) +
326                                                   T * (A01 * S + A11 * T + 2.0f * B1) + C;
327                                         le = 2;
328                                 }
329                         }
330                 }
331         }
332
333         // Account for numerical round-off error
334         if (sqrDist < FLT_EPSILON)
335                 sqrDist = 0.0f;
336         
337         {
338                 float w[3], x[3], y[3], z[3];
339                 copy_v3_v3(w, v0);
340                 copy_v3_v3(x, e0);
341                 mul_v3_fl(x, S);
342                 copy_v3_v3(y, e1);
343                 mul_v3_fl(y, T);
344                 add_v3_v3v3(z, w, x);
345                 add_v3_v3v3(z, z, y);
346                 //sub_v3_v3v3(d, p, z);
347                 copy_v3_v3(nearest, z);
348                 // d = p - ( v0 + S * e0 + T * e1 );
349         }
350         *v = lv;
351         *e = le;
352
353         return sqrDist;
354 }
355
356
357 /*
358  * BVH from meshs callbacks
359  */
360
361 // Callback to bvh tree nearest point. The tree must bust have been built using bvhtree_from_mesh_faces.
362 // userdata must be a BVHMeshCallbackUserdata built from the same mesh as the tree.
363 static void mesh_faces_nearest_point(void *userdata, int index, const float co[3], BVHTreeNearest *nearest)
364 {
365         const BVHTreeFromMesh *data = (BVHTreeFromMesh *) userdata;
366         MVert *vert = data->vert;
367         MFace *face = data->face + index;
368
369         float *t0, *t1, *t2, *t3;
370         t0 = vert[face->v1].co;
371         t1 = vert[face->v2].co;
372         t2 = vert[face->v3].co;
373         t3 = face->v4 ? vert[face->v4].co : NULL;
374
375         
376         do {
377                 float nearest_tmp[3], dist;
378                 int vertex, edge;
379                 
380                 dist = nearest_point_in_tri_surface(t0, t1, t2, co, &vertex, &edge, nearest_tmp);
381                 if (dist < nearest->dist) {
382                         nearest->index = index;
383                         nearest->dist = dist;
384                         copy_v3_v3(nearest->co, nearest_tmp);
385                         normal_tri_v3(nearest->no, t0, t1, t2);
386                 }
387
388                 t1 = t2;
389                 t2 = t3;
390                 t3 = NULL;
391
392         } while (t2);
393 }
394
395 // Callback to bvh tree raycast. The tree must bust have been built using bvhtree_from_mesh_faces.
396 // userdata must be a BVHMeshCallbackUserdata built from the same mesh as the tree.
397 static void mesh_faces_spherecast(void *userdata, int index, const BVHTreeRay *ray, BVHTreeRayHit *hit)
398 {
399         const BVHTreeFromMesh *data = (BVHTreeFromMesh *) userdata;
400         MVert *vert = data->vert;
401         MFace *face = data->face + index;
402
403         float *t0, *t1, *t2, *t3;
404         t0 = vert[face->v1].co;
405         t1 = vert[face->v2].co;
406         t2 = vert[face->v3].co;
407         t3 = face->v4 ? vert[face->v4].co : NULL;
408
409         
410         do {
411                 float dist;
412                 if (data->sphere_radius == 0.0f)
413                         dist = bvhtree_ray_tri_intersection(ray, hit->dist, t0, t1, t2);
414                 else
415                         dist = sphereray_tri_intersection(ray, data->sphere_radius, hit->dist, t0, t1, t2);
416
417                 if (dist >= 0 && dist < hit->dist) {
418                         hit->index = index;
419                         hit->dist = dist;
420                         madd_v3_v3v3fl(hit->co, ray->origin, ray->direction, dist);
421
422                         normal_tri_v3(hit->no, t0, t1, t2);
423                 }
424
425                 t1 = t2;
426                 t2 = t3;
427                 t3 = NULL;
428
429         } while (t2);
430 }
431
432 // Callback to bvh tree nearest point. The tree must bust have been built using bvhtree_from_mesh_edges.
433 // userdata must be a BVHMeshCallbackUserdata built from the same mesh as the tree.
434 static void mesh_edges_nearest_point(void *userdata, int index, const float co[3], BVHTreeNearest *nearest)
435 {
436         const BVHTreeFromMesh *data = (BVHTreeFromMesh *) userdata;
437         MVert *vert = data->vert;
438         MEdge *edge = data->edge + index;
439         float nearest_tmp[3], dist;
440
441         float *t0, *t1;
442         t0 = vert[edge->v1].co;
443         t1 = vert[edge->v2].co;
444
445         closest_to_line_segment_v3(nearest_tmp, co, t0, t1);
446         dist = len_squared_v3v3(nearest_tmp, co);
447         
448         if (dist < nearest->dist) {
449                 nearest->index = index;
450                 nearest->dist = dist;
451                 copy_v3_v3(nearest->co, nearest_tmp);
452                 sub_v3_v3v3(nearest->no, t0, t1);
453                 normalize_v3(nearest->no);
454         }
455 }
456
457 /*
458  * BVH builders
459  */
460 // Builds a bvh tree.. where nodes are the vertexs of the given mesh
461 BVHTree *bvhtree_from_mesh_verts(BVHTreeFromMesh *data, DerivedMesh *mesh, float epsilon, int tree_type, int axis)
462 {
463         BVHTree *tree = bvhcache_find(&mesh->bvhCache, BVHTREE_FROM_VERTICES);
464
465         //Not in cache
466         if (tree == NULL) {
467                 int i;
468                 int numVerts = mesh->getNumVerts(mesh);
469                 MVert *vert = mesh->getVertDataArray(mesh, CD_MVERT);
470
471                 if (vert != NULL) {
472                         tree = BLI_bvhtree_new(numVerts, epsilon, tree_type, axis);
473
474                         if (tree != NULL) {
475                                 for (i = 0; i < numVerts; i++) {
476                                         BLI_bvhtree_insert(tree, i, vert[i].co, 1);
477                                 }
478
479                                 BLI_bvhtree_balance(tree);
480
481                                 //Save on cache for later use
482 //                              printf("BVHTree built and saved on cache\n");
483                                 bvhcache_insert(&mesh->bvhCache, tree, BVHTREE_FROM_VERTICES);
484                         }
485                 }
486         }
487         else {
488 //              printf("BVHTree is already build, using cached tree\n");
489         }
490
491
492         //Setup BVHTreeFromMesh
493         memset(data, 0, sizeof(*data));
494         data->tree = tree;
495
496         if (data->tree) {
497                 data->cached = TRUE;
498
499                 //a NULL nearest callback works fine
500                 //remeber the min distance to point is the same as the min distance to BV of point
501                 data->nearest_callback = NULL;
502                 data->raycast_callback = NULL;
503
504                 data->mesh = mesh;
505                 data->vert = mesh->getVertDataArray(mesh, CD_MVERT);
506                 data->face = mesh->getTessFaceDataArray(mesh, CD_MFACE);
507
508                 data->sphere_radius = epsilon;
509         }
510
511         return data->tree;
512 }
513
514 // Builds a bvh tree.. where nodes are the faces of the given mesh.
515 BVHTree *bvhtree_from_mesh_faces(BVHTreeFromMesh *data, DerivedMesh *mesh, float epsilon, int tree_type, int axis)
516 {
517         BVHTree *tree = bvhcache_find(&mesh->bvhCache, BVHTREE_FROM_FACES);
518
519         //Not in cache
520         if (tree == NULL) {
521                 int i;
522                 int numFaces = mesh->getNumTessFaces(mesh);
523
524                 /* BMESH specific check that we have tessfaces,
525                  * we _could_ tessellate here but rather not - campbell
526                  *
527                  * this assert checks we have tessfaces,
528                  * if not caller should use DM_ensure_tessface() */
529                 BLI_assert(!(numFaces == 0 && mesh->getNumPolys(mesh) != 0));
530
531                 if (numFaces != 0) {
532                         /* Create a bvh-tree of the given target */
533                         tree = BLI_bvhtree_new(numFaces, epsilon, tree_type, axis);
534                         if (tree != NULL) {
535                                 BMEditMesh *em = data->em_evil;
536                                 if (em) {
537                                         /* data->em_evil is only set for snapping, and only for the mesh of the object
538                                          * which is currently open in edit mode. When set, the bvhtree should not contain
539                                          * faces that will interfere with snapping (e.g. faces that are hidden/selected
540                                          * or faces that have selected verts).*/
541
542                                         /* XXX, for snap only, em & dm are assumed to be aligned, since dm is the em's cage */
543
544                                         /* Insert BMesh-tessellation triangles into the bvh tree, unless they are hidden
545                                          * and/or selected. Even if the faces themselves are not selected for the snapped
546                                          * transform, having a vertex selected means the face (and thus it's tessellated
547                                          * triangles) will be moving and will not be a good snap targets.*/
548                                         for (i = 0; i < em->tottri; i++) {
549                                                 BMLoop **tri = em->looptris[i];
550                                                 BMFace *f;
551                                                 BMVert *v;
552                                                 BMIter iter;
553                                                 int insert;
554
555                                                 /* Each loop of the triangle points back to the BMFace it was tessellated from.
556                                                  * All three should point to the same face, so just use the face from the first
557                                                  * loop.*/
558                                                 f = tri[0]->f;
559
560                                                 /* If the looptris is ordered such that all triangles tessellated from a single
561                                                  * faces are consecutive elements in the array, then we could speed up the tests
562                                                  * below by using the insert value from the previous iteration.*/
563
564                                                 /*Start with the assumption the triangle should be included for snapping.*/
565                                                 insert = 1;
566
567                                                 if (BM_elem_flag_test(f, BM_ELEM_SELECT) || BM_elem_flag_test(f, BM_ELEM_HIDDEN)) {
568                                                         /* Don't insert triangles tessellated from faces that are hidden
569                                                          * or selected*/
570                                                         insert = 0;
571                                                 }
572                                                 else {
573                                                         BM_ITER_ELEM (v, &iter, f, BM_VERTS_OF_FACE) {
574                                                                 if (BM_elem_flag_test(v, BM_ELEM_SELECT)) {
575                                                                         /* Don't insert triangles tessellated from faces that have
576                                                                          * any selected verts.*/
577                                                                         insert = 0;
578                                                                 }
579                                                         }
580                                                 }
581
582                                                 if (insert) {
583                                                         /* No reason found to block hit-testing the triangle for snap,
584                                                          * so insert it now.*/
585                                                         float co[4][3];
586                                                         copy_v3_v3(co[0], tri[0]->v->co);
587                                                         copy_v3_v3(co[1], tri[1]->v->co);
588                                                         copy_v3_v3(co[2], tri[2]->v->co);
589                                         
590                                                         BLI_bvhtree_insert(tree, i, co[0], 3);
591                                                 }
592                                         }
593                                 }
594                                 else {
595                                         MVert *vert = mesh->getVertDataArray(mesh, CD_MVERT);
596                                         MFace *face = mesh->getTessFaceDataArray(mesh, CD_MFACE);
597
598                                         if (vert != NULL && face != NULL) {
599                                                 for (i = 0; i < numFaces; i++) {
600                                                         float co[4][3];
601                                                         copy_v3_v3(co[0], vert[face[i].v1].co);
602                                                         copy_v3_v3(co[1], vert[face[i].v2].co);
603                                                         copy_v3_v3(co[2], vert[face[i].v3].co);
604                                                         if (face[i].v4)
605                                                                 copy_v3_v3(co[3], vert[face[i].v4].co);
606                                         
607                                                         BLI_bvhtree_insert(tree, i, co[0], face[i].v4 ? 4 : 3);
608                                                 }
609                                         }
610                                 }
611                                 BLI_bvhtree_balance(tree);
612
613                                 //Save on cache for later use
614 //                              printf("BVHTree built and saved on cache\n");
615                                 bvhcache_insert(&mesh->bvhCache, tree, BVHTREE_FROM_FACES);
616                         }
617                 }
618         }
619         else {
620 //              printf("BVHTree is already build, using cached tree\n");
621         }
622
623
624         //Setup BVHTreeFromMesh
625         memset(data, 0, sizeof(*data));
626         data->tree = tree;
627
628         if (data->tree) {
629                 data->cached = TRUE;
630
631                 data->nearest_callback = mesh_faces_nearest_point;
632                 data->raycast_callback = mesh_faces_spherecast;
633
634                 data->mesh = mesh;
635                 data->vert = mesh->getVertDataArray(mesh, CD_MVERT);
636                 data->face = mesh->getTessFaceDataArray(mesh, CD_MFACE);
637
638                 data->sphere_radius = epsilon;
639         }
640         return data->tree;
641
642 }
643
644 // Builds a bvh tree.. where nodes are the faces of the given mesh.
645 BVHTree *bvhtree_from_mesh_edges(BVHTreeFromMesh *data, DerivedMesh *mesh, float epsilon, int tree_type, int axis)
646 {
647         BVHTree *tree = bvhcache_find(&mesh->bvhCache, BVHTREE_FROM_EDGES);
648
649         //Not in cache
650         if (tree == NULL) {
651                 int i;
652                 int numEdges = mesh->getNumEdges(mesh);
653                 MVert *vert = mesh->getVertDataArray(mesh, CD_MVERT);
654                 MEdge *edge = mesh->getEdgeDataArray(mesh, CD_MEDGE);
655
656                 if (vert != NULL && edge != NULL) {
657                         /* Create a bvh-tree of the given target */
658                         tree = BLI_bvhtree_new(numEdges, epsilon, tree_type, axis);
659                         if (tree != NULL) {
660                                 for (i = 0; i < numEdges; i++) {
661                                         float co[4][3];
662                                         copy_v3_v3(co[0], vert[edge[i].v1].co);
663                                         copy_v3_v3(co[1], vert[edge[i].v2].co);
664                         
665                                         BLI_bvhtree_insert(tree, i, co[0], 2);
666                                 }
667                                 BLI_bvhtree_balance(tree);
668
669                                 //Save on cache for later use
670 //                              printf("BVHTree built and saved on cache\n");
671                                 bvhcache_insert(&mesh->bvhCache, tree, BVHTREE_FROM_EDGES);
672                         }
673                 }
674         }
675         else {
676 //              printf("BVHTree is already build, using cached tree\n");
677         }
678
679
680         //Setup BVHTreeFromMesh
681         memset(data, 0, sizeof(*data));
682         data->tree = tree;
683
684         if (data->tree) {
685                 data->cached = TRUE;
686
687                 data->nearest_callback = mesh_edges_nearest_point;
688                 data->raycast_callback = NULL;
689
690                 data->mesh = mesh;
691                 data->vert = mesh->getVertDataArray(mesh, CD_MVERT);
692                 data->edge = mesh->getEdgeDataArray(mesh, CD_MEDGE);
693
694                 data->sphere_radius = epsilon;
695         }
696         return data->tree;
697
698 }
699
700 // Frees data allocated by a call to bvhtree_from_mesh_*.
701 void free_bvhtree_from_mesh(struct BVHTreeFromMesh *data)
702 {
703         if (data->tree) {
704                 if (!data->cached)
705                         BLI_bvhtree_free(data->tree);
706
707                 memset(data, 0, sizeof(*data));
708         }
709 }
710
711
712 /* BVHCache */
713 typedef struct BVHCacheItem {
714         int type;
715         BVHTree *tree;
716
717 } BVHCacheItem;
718
719 static void bvhcacheitem_set_if_match(void *_cached, void *_search)
720 {
721         BVHCacheItem *cached = (BVHCacheItem *)_cached;
722         BVHCacheItem *search = (BVHCacheItem *)_search;
723
724         if (search->type == cached->type) {
725                 search->tree = cached->tree;            
726         }
727
728
729 BVHTree *bvhcache_find(BVHCache *cache, int type)
730 {
731         BVHCacheItem item;
732         item.type = type;
733         item.tree = NULL;
734
735         BLI_linklist_apply(*cache, bvhcacheitem_set_if_match, &item);
736         return item.tree;
737 }
738
739 void bvhcache_insert(BVHCache *cache, BVHTree *tree, int type)
740 {
741         BVHCacheItem *item = NULL;
742
743         assert(tree != NULL);
744         assert(bvhcache_find(cache, type) == NULL);
745
746         item = MEM_mallocN(sizeof(BVHCacheItem), "BVHCacheItem");
747         assert(item != NULL);
748
749         item->type = type;
750         item->tree = tree;
751
752         BLI_linklist_prepend(cache, item);
753 }
754
755
756 void bvhcache_init(BVHCache *cache)
757 {
758         *cache = NULL;
759 }
760
761 static void bvhcacheitem_free(void *_item)
762 {
763         BVHCacheItem *item = (BVHCacheItem *)_item;
764
765         BLI_bvhtree_free(item->tree);
766         MEM_freeN(item);
767 }
768
769
770 void bvhcache_free(BVHCache *cache)
771 {
772         BLI_linklist_free(*cache, (LinkNodeFreeFP)bvhcacheitem_free);
773         *cache = NULL;
774 }
775
776