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