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