Merged changes in the trunk up to revision 46309.
[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         {       
378                 float nearest_tmp[3], dist;
379                 int vertex, edge;
380                 
381                 dist = nearest_point_in_tri_surface(t0, t1, t2, co, &vertex, &edge, nearest_tmp);
382                 if (dist < nearest->dist) {
383                         nearest->index = index;
384                         nearest->dist = dist;
385                         copy_v3_v3(nearest->co, nearest_tmp);
386                         normal_tri_v3(nearest->no, t0, t1, t2);
387                 }
388
389                 t1 = t2;
390                 t2 = t3;
391                 t3 = NULL;
392
393         } while (t2);
394 }
395
396 // Callback to bvh tree raycast. The tree must bust have been built using bvhtree_from_mesh_faces.
397 // userdata must be a BVHMeshCallbackUserdata built from the same mesh as the tree.
398 static void mesh_faces_spherecast(void *userdata, int index, const BVHTreeRay *ray, BVHTreeRayHit *hit)
399 {
400         const BVHTreeFromMesh *data = (BVHTreeFromMesh*) userdata;
401         MVert *vert     = data->vert;
402         MFace *face = data->face + index;
403
404         float *t0, *t1, *t2, *t3;
405         t0 = vert[face->v1].co;
406         t1 = vert[face->v2].co;
407         t2 = vert[face->v3].co;
408         t3 = face->v4 ? vert[face->v4].co : NULL;
409
410         
411         do
412         {       
413                 float dist;
414                 if (data->sphere_radius == 0.0f)
415                         dist = bvhtree_ray_tri_intersection(ray, hit->dist, t0, t1, t2);
416                 else
417                         dist = sphereray_tri_intersection(ray, data->sphere_radius, hit->dist, t0, t1, t2);
418
419                 if (dist >= 0 && dist < hit->dist) {
420                         hit->index = index;
421                         hit->dist = dist;
422                         madd_v3_v3v3fl(hit->co, ray->origin, ray->direction, dist);
423
424                         normal_tri_v3(hit->no, t0, t1, t2);
425                 }
426
427                 t1 = t2;
428                 t2 = t3;
429                 t3 = NULL;
430
431         } while (t2);
432 }
433
434 // Callback to bvh tree nearest point. The tree must bust have been built using bvhtree_from_mesh_edges.
435 // userdata must be a BVHMeshCallbackUserdata built from the same mesh as the tree.
436 static void mesh_edges_nearest_point(void *userdata, int index, const float co[3], BVHTreeNearest *nearest)
437 {
438         const BVHTreeFromMesh *data = (BVHTreeFromMesh*) userdata;
439         MVert *vert     = data->vert;
440         MEdge *edge = data->edge + index;
441         float nearest_tmp[3], dist;
442
443         float *t0, *t1;
444         t0 = vert[ edge->v1 ].co;
445         t1 = vert[ edge->v2 ].co;
446
447         closest_to_line_segment_v3(nearest_tmp, co, t0, t1);
448         dist = len_squared_v3v3(nearest_tmp, co);
449         
450         if (dist < nearest->dist) {
451                 nearest->index = index;
452                 nearest->dist = dist;
453                 copy_v3_v3(nearest->co, nearest_tmp);
454                 sub_v3_v3v3(nearest->no, t0, t1);
455                 normalize_v3(nearest->no);
456         }
457 }
458
459 /*
460  * BVH builders
461  */
462 // Builds a bvh tree.. where nodes are the vertexs of the given mesh
463 BVHTree* bvhtree_from_mesh_verts(BVHTreeFromMesh *data, DerivedMesh *mesh, float epsilon, int tree_type, int axis)
464 {
465         BVHTree *tree = bvhcache_find(&mesh->bvhCache, BVHTREE_FROM_VERTICES);
466
467         //Not in cache
468         if (tree == NULL) {
469                 int i;
470                 int numVerts= mesh->getNumVerts(mesh);
471                 MVert *vert     = mesh->getVertDataArray(mesh, CD_MVERT);
472
473                 if (vert != NULL) {
474                         tree = BLI_bvhtree_new(numVerts, epsilon, tree_type, axis);
475
476                         if (tree != NULL) {
477                                 for (i = 0; i < numVerts; i++) {
478                                         BLI_bvhtree_insert(tree, i, vert[i].co, 1);
479                                 }
480
481                                 BLI_bvhtree_balance(tree);
482
483                                 //Save on cache for later use
484 //                              printf("BVHTree built and saved on cache\n");
485                                 bvhcache_insert(&mesh->bvhCache, tree, BVHTREE_FROM_VERTICES);
486                         }
487                 }
488         }
489         else {
490 //              printf("BVHTree is already build, using cached tree\n");
491         }
492
493
494         //Setup BVHTreeFromMesh
495         memset(data, 0, sizeof(*data));
496         data->tree = tree;
497
498         if (data->tree) {
499                 data->cached = TRUE;
500
501                 //a NULL nearest callback works fine
502                 //remeber the min distance to point is the same as the min distance to BV of point
503                 data->nearest_callback = NULL;
504                 data->raycast_callback = NULL;
505
506                 data->mesh = mesh;
507                 data->vert = mesh->getVertDataArray(mesh, CD_MVERT);
508                 data->face = mesh->getTessFaceDataArray(mesh, CD_MFACE);
509
510                 data->sphere_radius = epsilon;
511         }
512
513         return data->tree;
514 }
515
516 // Builds a bvh tree.. where nodes are the faces of the given mesh.
517 BVHTree* bvhtree_from_mesh_faces(BVHTreeFromMesh *data, DerivedMesh *mesh, float epsilon, int tree_type, int axis)
518 {
519         BVHTree *tree = bvhcache_find(&mesh->bvhCache, BVHTREE_FROM_FACES);
520
521         //Not in cache
522         if (tree == NULL) {
523                 int i;
524                 int numFaces= mesh->getNumTessFaces(mesh);
525
526                 /* BMESH specific check that we have tessfaces,
527                  * we _could_ tessellate here but rather not - campbell
528                  *
529                  * this assert checks we have tessfaces,
530                  * if not caller should use DM_ensure_tessface() */
531                 BLI_assert(!(numFaces == 0 && mesh->getNumPolys(mesh) != 0));
532
533                 if (numFaces != 0) {
534                         /* Create a bvh-tree of the given target */
535                         tree = BLI_bvhtree_new(numFaces, epsilon, tree_type, axis);
536                         if (tree != NULL) {
537                                 BMEditMesh *em= data->em_evil;
538                                 if (em) {
539                                         /* data->em_evil is only set for snapping, and only for the mesh of the object
540                                          * which is currently open in edit mode. When set, the bvhtree should not contain
541                                          * faces that will interfere with snapping (e.g. faces that are hidden/selected
542                                          * or faces that have selected verts).*/
543
544                                         /* XXX, for snap only, em & dm are assumed to be aligned, since dm is the em's cage */
545
546                                         /* Insert BMesh-tessellation triangles into the bvh tree, unless they are hidden
547                                          * and/or selected. Even if the faces themselves are not selected for the snapped
548                                          * transform, having a vertex selected means the face (and thus it's tessellated
549                                          * triangles) will be moving and will not be a good snap targets.*/
550                                         for (i = 0; i < em->tottri; i++) {
551                                                 BMLoop **tri = em->looptris[i];
552                                                 BMFace *f;
553                                                 BMVert *v;
554                                                 BMIter iter;
555                                                 int insert;
556
557                                                 /* Each loop of the triangle points back to the BMFace it was tessellated from.
558                                                  * All three should point to the same face, so just use the face from the first
559                                                  * loop.*/
560                                                 f = tri[0]->f;
561
562                                                 /* If the looptris is ordered such that all triangles tessellated from a single
563                                                  * faces are consecutive elements in the array, then we could speed up the tests
564                                                  * below by using the insert value from the previous iteration.*/
565
566                                                 /*Start with the assumption the triangle should be included for snapping.*/
567                                                 insert = 1;
568
569                                                 if (BM_elem_flag_test(f, BM_ELEM_SELECT) || BM_elem_flag_test(f, BM_ELEM_HIDDEN)) {
570                                                         /* Don't insert triangles tessellated from faces that are hidden
571                                                          * or selected*/
572                                                         insert = 0;
573                                                 }
574                                                 else {
575                                                         BM_ITER_ELEM (v, &iter, f, BM_VERTS_OF_FACE) {
576                                                                 if (BM_elem_flag_test(v, BM_ELEM_SELECT)) {
577                                                                         /* Don't insert triangles tessellated from faces that have
578                                                                          * any selected verts.*/
579                                                                         insert = 0;
580                                                                 }
581                                                         }
582                                                 }
583
584                                                 if (insert) {
585                                                         /* No reason found to block hit-testing the triangle for snap,
586                                                          * so insert it now.*/
587                                                         float co[4][3];
588                                                         copy_v3_v3(co[0], tri[0]->v->co);
589                                                         copy_v3_v3(co[1], tri[1]->v->co);
590                                                         copy_v3_v3(co[2], tri[2]->v->co);
591                                         
592                                                         BLI_bvhtree_insert(tree, i, co[0], 3);
593                                                 }
594                                         }
595                                 }
596                                 else {
597                                         MVert *vert     = mesh->getVertDataArray(mesh, CD_MVERT);
598                                         MFace *face = mesh->getTessFaceDataArray(mesh, CD_MFACE);
599
600                                         if (vert != NULL && face != NULL) {
601                                                 for (i = 0; i < numFaces; i++) {
602                                                         float co[4][3];
603                                                         copy_v3_v3(co[0], vert[ face[i].v1 ].co);
604                                                         copy_v3_v3(co[1], vert[ face[i].v2 ].co);
605                                                         copy_v3_v3(co[2], vert[ face[i].v3 ].co);
606                                                         if (face[i].v4)
607                                                                 copy_v3_v3(co[3], vert[ face[i].v4 ].co);
608                                         
609                                                         BLI_bvhtree_insert(tree, i, co[0], face[i].v4 ? 4 : 3);
610                                                 }
611                                         }
612                                 }
613                                 BLI_bvhtree_balance(tree);
614
615                                 //Save on cache for later use
616 //                              printf("BVHTree built and saved on cache\n");
617                                 bvhcache_insert(&mesh->bvhCache, tree, BVHTREE_FROM_FACES);
618                         }
619                 }
620         }
621         else {
622 //              printf("BVHTree is already build, using cached tree\n");
623         }
624
625
626         //Setup BVHTreeFromMesh
627         memset(data, 0, sizeof(*data));
628         data->tree = tree;
629
630         if (data->tree) {
631                 data->cached = TRUE;
632
633                 data->nearest_callback = mesh_faces_nearest_point;
634                 data->raycast_callback = mesh_faces_spherecast;
635
636                 data->mesh = mesh;
637                 data->vert = mesh->getVertDataArray(mesh, CD_MVERT);
638                 data->face = mesh->getTessFaceDataArray(mesh, CD_MFACE);
639
640                 data->sphere_radius = epsilon;
641         }
642         return data->tree;
643
644 }
645
646 // Builds a bvh tree.. where nodes are the faces of the given mesh.
647 BVHTree* bvhtree_from_mesh_edges(BVHTreeFromMesh *data, DerivedMesh *mesh, float epsilon, int tree_type, int axis)
648 {
649         BVHTree *tree = bvhcache_find(&mesh->bvhCache, BVHTREE_FROM_EDGES);
650
651         //Not in cache
652         if (tree == NULL) {
653                 int i;
654                 int numEdges= mesh->getNumEdges(mesh);
655                 MVert *vert     = mesh->getVertDataArray(mesh, CD_MVERT);
656                 MEdge *edge = mesh->getEdgeDataArray(mesh, CD_MEDGE);
657
658                 if (vert != NULL && edge != NULL) {
659                         /* Create a bvh-tree of the given target */
660                         tree = BLI_bvhtree_new(numEdges, epsilon, tree_type, axis);
661                         if (tree != NULL) {
662                                 for (i = 0; i < numEdges; i++) {
663                                         float co[4][3];
664                                         copy_v3_v3(co[0], vert[ edge[i].v1 ].co);
665                                         copy_v3_v3(co[1], vert[ edge[i].v2 ].co);
666                         
667                                         BLI_bvhtree_insert(tree, i, co[0], 2);
668                                 }
669                                 BLI_bvhtree_balance(tree);
670
671                                 //Save on cache for later use
672 //                              printf("BVHTree built and saved on cache\n");
673                                 bvhcache_insert(&mesh->bvhCache, tree, BVHTREE_FROM_EDGES);
674                         }
675                 }
676         }
677         else {
678 //              printf("BVHTree is already build, using cached tree\n");
679         }
680
681
682         //Setup BVHTreeFromMesh
683         memset(data, 0, sizeof(*data));
684         data->tree = tree;
685
686         if (data->tree) {
687                 data->cached = TRUE;
688
689                 data->nearest_callback = mesh_edges_nearest_point;
690                 data->raycast_callback = NULL;
691
692                 data->mesh = mesh;
693                 data->vert = mesh->getVertDataArray(mesh, CD_MVERT);
694                 data->edge = mesh->getEdgeDataArray(mesh, CD_MEDGE);
695
696                 data->sphere_radius = epsilon;
697         }
698         return data->tree;
699
700 }
701
702 // Frees data allocated by a call to bvhtree_from_mesh_*.
703 void free_bvhtree_from_mesh(struct BVHTreeFromMesh *data)
704 {
705         if (data->tree) {
706                 if (!data->cached)
707                         BLI_bvhtree_free(data->tree);
708
709                 memset(data, 0, sizeof(*data));
710         }
711 }
712
713
714 /* BVHCache */
715 typedef struct BVHCacheItem
716 {
717         int type;
718         BVHTree *tree;
719
720 } BVHCacheItem;
721
722 static void bvhcacheitem_set_if_match(void *_cached, void *_search)
723 {
724         BVHCacheItem * cached = (BVHCacheItem *)_cached;
725         BVHCacheItem * search = (BVHCacheItem *)_search;
726
727         if (search->type == cached->type) {
728                 search->tree = cached->tree;            
729         }
730
731
732 BVHTree *bvhcache_find(BVHCache *cache, int type)
733 {
734         BVHCacheItem item;
735         item.type = type;
736         item.tree = NULL;
737
738         BLI_linklist_apply(*cache, bvhcacheitem_set_if_match, &item);
739         return item.tree;
740 }
741
742 void bvhcache_insert(BVHCache *cache, BVHTree *tree, int type)
743 {
744         BVHCacheItem *item = NULL;
745
746         assert(tree != NULL);
747         assert(bvhcache_find(cache, type) == NULL);
748
749         item = MEM_mallocN(sizeof(BVHCacheItem), "BVHCacheItem");
750         assert(item != NULL);
751
752         item->type = type;
753         item->tree = tree;
754
755         BLI_linklist_prepend(cache, item);
756 }
757
758
759 void bvhcache_init(BVHCache *cache)
760 {
761         *cache = NULL;
762 }
763
764 static void bvhcacheitem_free(void *_item)
765 {
766         BVHCacheItem *item = (BVHCacheItem *)_item;
767
768         BLI_bvhtree_free(item->tree);
769         MEM_freeN(item);
770 }
771
772
773 void bvhcache_free(BVHCache *cache)
774 {
775         BLI_linklist_free(*cache, (LinkNodeFreeFP)bvhcacheitem_free);
776         *cache = NULL;
777 }
778
779