svn merge ^/trunk/blender -r43819:43830
[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 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])
52 {
53         float dist;
54
55         if(isect_ray_tri_epsilon_v3(ray->origin, ray->direction, v0, v1, v2, &dist, NULL, FLT_EPSILON))
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[3], const float v1[3], const float v2[3])
62 {
63         
64         float idist;
65         float p1[3];
66         float plane_normal[3], hit_point[3];
67
68         normal_tri_v3(plane_normal, v0, v1, v2);
69
70         madd_v3_v3v3fl(p1, ray->origin, ray->direction, m_dist);
71         if(isect_sweeping_sphere_tri_v3(ray->origin, p1, radius, v0, v1, 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 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])
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         sub_v3_v3v3(diff, v0, p);
102         sub_v3_v3v3(e0, v1, v0);
103         sub_v3_v3v3(e1, v2, v0);
104         
105         A00 = dot_v3v3(e0, e0);
106         A01 = dot_v3v3(e0, e1 );
107         A11 = dot_v3v3(e1, e1 );
108         B0 = dot_v3v3(diff, e0 );
109         B1 = dot_v3v3(diff, e1 );
110         C = dot_v3v3(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 = 1.0f;
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                 copy_v3_v3(w, v0);
383                 copy_v3_v3(x, e0);
384                 mul_v3_fl(x, S);
385                 copy_v3_v3(y, e1);
386                 mul_v3_fl(y, T);
387                 add_v3_v3v3(z, w, x);
388                 add_v3_v3v3(z, z, y);
389                 //sub_v3_v3v3(d, p, z);
390                 copy_v3_v3(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[3], 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                         copy_v3_v3(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 = bvhtree_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                         madd_v3_v3v3fl(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[3], 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         closest_to_line_segment_v3(nearest_tmp, co, t0, t1);
493         dist = len_squared_v3v3(nearest_tmp, co);
494         
495         if(dist < nearest->dist)
496         {
497                 nearest->index = index;
498                 nearest->dist = dist;
499                 copy_v3_v3(nearest->co, nearest_tmp);
500                 sub_v3_v3v3(nearest->no, t0, t1);
501                 normalize_v3(nearest->no);
502         }
503 }
504
505 /*
506  * BVH builders
507  */
508 // Builds a bvh tree.. where nodes are the vertexs of the given mesh
509 BVHTree* bvhtree_from_mesh_verts(BVHTreeFromMesh *data, DerivedMesh *mesh, float epsilon, int tree_type, int axis)
510 {
511         BVHTree *tree = bvhcache_find(&mesh->bvhCache, BVHTREE_FROM_VERTICES);
512
513         //Not in cache
514         if(tree == NULL)
515         {
516                 int i;
517                 int numVerts= mesh->getNumVerts(mesh);
518                 MVert *vert     = mesh->getVertDataArray(mesh, CD_MVERT);
519
520                 if(vert != NULL)
521                 {
522                         tree = BLI_bvhtree_new(numVerts, epsilon, tree_type, axis);
523
524                         if(tree != NULL)
525                         {
526                                 for(i = 0; i < numVerts; i++)
527                                         BLI_bvhtree_insert(tree, i, vert[i].co, 1);
528
529                                 BLI_bvhtree_balance(tree);
530
531                                 //Save on cache for later use
532 //                              printf("BVHTree built and saved on cache\n");
533                                 bvhcache_insert(&mesh->bvhCache, tree, BVHTREE_FROM_VERTICES);
534                         }
535                 }
536         }
537         else
538         {
539 //              printf("BVHTree is already build, using cached tree\n");
540         }
541
542
543         //Setup BVHTreeFromMesh
544         memset(data, 0, sizeof(*data));
545         data->tree = tree;
546
547         if(data->tree)
548         {
549                 data->cached = TRUE;
550
551                 //a NULL nearest callback works fine
552                 //remeber the min distance to point is the same as the min distance to BV of point
553                 data->nearest_callback = NULL;
554                 data->raycast_callback = NULL;
555
556                 data->mesh = mesh;
557                 data->vert = mesh->getVertDataArray(mesh, CD_MVERT);
558                 data->face = mesh->getTessFaceDataArray(mesh, CD_MFACE);
559
560                 data->sphere_radius = epsilon;
561         }
562
563         return data->tree;
564 }
565
566 // Builds a bvh tree.. where nodes are the faces of the given mesh.
567 BVHTree* bvhtree_from_mesh_faces(BVHTreeFromMesh *data, DerivedMesh *mesh, float epsilon, int tree_type, int axis)
568 {
569         BVHTree *tree = bvhcache_find(&mesh->bvhCache, BVHTREE_FROM_FACES);
570
571         //Not in cache
572         if(tree == NULL)
573         {
574                 int i;
575                 int numFaces= mesh->getNumTessFaces(mesh);
576
577                 /* BMESH spesific check that we have tessfaces,
578                  * we _could_ tesselate here but rather not - campbell
579                  *
580                  * this assert checks we have tessfaces,
581                  * if not caller should use DM_ensure_tessface() */
582                 BLI_assert(!(numFaces == 0 && mesh->getNumPolys(mesh) != 0));
583
584                 if(numFaces != 0)
585                 {
586                         /* Create a bvh-tree of the given target */
587                         tree = BLI_bvhtree_new(numFaces, epsilon, tree_type, axis);
588                         if(tree != NULL)
589                         {
590                                 BMEditMesh *em= data->em_evil;
591                                 if(em) {
592                                         /*data->em_evil is only set for snapping, and only for the mesh of the object
593                                           which is currently open in edit mode. When set, the bvhtree should not contain
594                                           faces that will interfere with snapping (e.g. faces that are hidden/selected
595                                           or faces that have selected verts).*/
596
597                                         /* XXX, for snap only, em & dm are assumed to be aligned, since dm is the em's cage */
598
599                                         /*Insert BMesh-tesselation triangles into the bvh tree, unless they are hidden
600                                           and/or selected. Even if the faces themselves are not selected for the snapped
601                                           transform, having a vertex selected means the face (and thus it's tesselated
602                                           triangles) will be moving and will not be a good snap targets.*/
603                                         for (i = 0; i < em->tottri; i++) {
604                                                 BMLoop **tri = em->looptris[i];
605                                                 BMFace *f;
606                                                 BMVert *v;
607                                                 BMIter iter;
608                                                 int insert;
609
610                                                 /*Each loop of the triangle points back to the BMFace it was tesselated from.
611                                                   All three should point to the same face, so just use the face from the first
612                                                   loop.*/
613                                                 f = tri[0]->f;
614
615                                                 /*If the looptris is ordered such that all triangles tesselated from a single
616                                                   faces are consecutive elements in the array, then we could speed up the tests
617                                                   below by using the insert value from the previous iteration.*/
618
619                                                 /*Start with the assumption the triangle should be included for snapping.*/
620                                                 insert = 1;
621
622                                                 if (BM_TestHFlag(f, BM_SELECT) || BM_TestHFlag(f, BM_HIDDEN)) {
623                                                         /*Don't insert triangles tesselated from faces that are hidden
624                                                           or selected*/
625                                                         insert = 0;
626                                                 }
627                                                 else {
628                                                         BM_ITER(v, &iter, em->bm, BM_VERTS_OF_FACE, f) {
629                                                                 if (BM_TestHFlag(v, BM_SELECT)) {
630                                                                         /*Don't insert triangles tesselated from faces that have
631                                                                           any selected verts.*/
632                                                                         insert = 0;
633                                                                 }
634                                                         }
635                                                 }
636
637                                                 if (insert)
638                                                 {
639                                                         /*No reason found to block hit-testing the triangle for snap,
640                                                           so insert it now.*/
641                                                         float co[4][3];
642                                                         copy_v3_v3(co[0], tri[0]->v->co);
643                                                         copy_v3_v3(co[1], tri[1]->v->co);
644                                                         copy_v3_v3(co[2], tri[2]->v->co);
645                                         
646                                                         BLI_bvhtree_insert(tree, i, co[0], 3);
647                                                 }
648                                         }
649                                 }
650                                 else {
651                                         MVert *vert     = mesh->getVertDataArray(mesh, CD_MVERT);
652                                         MFace *face = mesh->getTessFaceDataArray(mesh, CD_MFACE);
653
654                                         if (vert != NULL && face != NULL) {
655                                                 for(i = 0; i < numFaces; i++) {
656                                                         float co[4][3];
657                                                         copy_v3_v3(co[0], vert[ face[i].v1 ].co);
658                                                         copy_v3_v3(co[1], vert[ face[i].v2 ].co);
659                                                         copy_v3_v3(co[2], vert[ face[i].v3 ].co);
660                                                         if(face[i].v4)
661                                                                 copy_v3_v3(co[3], vert[ face[i].v4 ].co);
662                                         
663                                                         BLI_bvhtree_insert(tree, i, co[0], face[i].v4 ? 4 : 3);
664                                                 }
665                                         }
666                                 }
667                                 BLI_bvhtree_balance(tree);
668
669                                 //Save on cache for later use
670 //                              printf("BVHTree built and saved on cache\n");
671                                 bvhcache_insert(&mesh->bvhCache, tree, BVHTREE_FROM_FACES);
672                         }
673                 }
674         }
675         else
676         {
677 //              printf("BVHTree is already build, using cached tree\n");
678         }
679
680
681         //Setup BVHTreeFromMesh
682         memset(data, 0, sizeof(*data));
683         data->tree = tree;
684
685         if(data->tree)
686         {
687                 data->cached = TRUE;
688
689                 data->nearest_callback = mesh_faces_nearest_point;
690                 data->raycast_callback = mesh_faces_spherecast;
691
692                 data->mesh = mesh;
693                 data->vert = mesh->getVertDataArray(mesh, CD_MVERT);
694                 data->face = mesh->getTessFaceDataArray(mesh, CD_MFACE);
695
696                 data->sphere_radius = epsilon;
697         }
698         return data->tree;
699
700 }
701
702 // Builds a bvh tree.. where nodes are the faces of the given mesh.
703 BVHTree* bvhtree_from_mesh_edges(BVHTreeFromMesh *data, DerivedMesh *mesh, float epsilon, int tree_type, int axis)
704 {
705         BVHTree *tree = bvhcache_find(&mesh->bvhCache, BVHTREE_FROM_EDGES);
706
707         //Not in cache
708         if(tree == NULL)
709         {
710                 int i;
711                 int numEdges= mesh->getNumEdges(mesh);
712                 MVert *vert     = mesh->getVertDataArray(mesh, CD_MVERT);
713                 MEdge *edge = mesh->getEdgeDataArray(mesh, CD_MEDGE);
714
715                 if(vert != NULL && edge != NULL)
716                 {
717                         /* Create a bvh-tree of the given target */
718                         tree = BLI_bvhtree_new(numEdges, epsilon, tree_type, axis);
719                         if(tree != NULL)
720                         {
721                                 for(i = 0; i < numEdges; i++)
722                                 {
723                                         float co[4][3];
724                                         copy_v3_v3(co[0], vert[ edge[i].v1 ].co);
725                                         copy_v3_v3(co[1], vert[ edge[i].v2 ].co);
726                         
727                                         BLI_bvhtree_insert(tree, i, co[0], 2);
728                                 }
729                                 BLI_bvhtree_balance(tree);
730
731                                 //Save on cache for later use
732 //                              printf("BVHTree built and saved on cache\n");
733                                 bvhcache_insert(&mesh->bvhCache, tree, BVHTREE_FROM_EDGES);
734                         }
735                 }
736         }
737         else
738         {
739 //              printf("BVHTree is already build, using cached tree\n");
740         }
741
742
743         //Setup BVHTreeFromMesh
744         memset(data, 0, sizeof(*data));
745         data->tree = tree;
746
747         if(data->tree)
748         {
749                 data->cached = TRUE;
750
751                 data->nearest_callback = mesh_edges_nearest_point;
752                 data->raycast_callback = NULL;
753
754                 data->mesh = mesh;
755                 data->vert = mesh->getVertDataArray(mesh, CD_MVERT);
756                 data->edge = mesh->getEdgeDataArray(mesh, CD_MEDGE);
757
758                 data->sphere_radius = epsilon;
759         }
760         return data->tree;
761
762 }
763
764 // Frees data allocated by a call to bvhtree_from_mesh_*.
765 void free_bvhtree_from_mesh(struct BVHTreeFromMesh *data)
766 {
767         if(data->tree)
768         {
769                 if(!data->cached)
770                         BLI_bvhtree_free(data->tree);
771
772                 memset( data, 0, sizeof(*data) );
773         }
774 }
775
776
777 /* BVHCache */
778 typedef struct BVHCacheItem
779 {
780         int type;
781         BVHTree *tree;
782
783 } BVHCacheItem;
784
785 static void bvhcacheitem_set_if_match(void *_cached, void *_search)
786 {
787         BVHCacheItem * cached = (BVHCacheItem *)_cached;
788         BVHCacheItem * search = (BVHCacheItem *)_search;
789
790         if(search->type == cached->type)
791         {
792                 search->tree = cached->tree;            
793         }
794
795
796 BVHTree *bvhcache_find(BVHCache *cache, int type)
797 {
798         BVHCacheItem item;
799         item.type = type;
800         item.tree = NULL;
801
802         BLI_linklist_apply(*cache, bvhcacheitem_set_if_match, &item);
803         return item.tree;
804 }
805
806 void bvhcache_insert(BVHCache *cache, BVHTree *tree, int type)
807 {
808         BVHCacheItem *item = NULL;
809
810         assert( tree != NULL );
811         assert( bvhcache_find(cache, type) == NULL );
812
813         item = MEM_mallocN(sizeof(BVHCacheItem), "BVHCacheItem");
814         assert( item != NULL );
815
816         item->type = type;
817         item->tree = tree;
818
819         BLI_linklist_prepend( cache, item );
820 }
821
822
823 void bvhcache_init(BVHCache *cache)
824 {
825         *cache = NULL;
826 }
827
828 static void bvhcacheitem_free(void *_item)
829 {
830         BVHCacheItem *item = (BVHCacheItem *)_item;
831
832         BLI_bvhtree_free(item->tree);
833         MEM_freeN(item);
834 }
835
836
837 void bvhcache_free(BVHCache *cache)
838 {
839         BLI_linklist_free(*cache, (LinkNodeFreeFP)bvhcacheitem_free);
840         *cache = NULL;
841 }
842
843