svn merge -r41575:41602 ^/trunk/blender
[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[3], const float v1[3], const float v2[3])
52 {
53         float dist;
54
55         if(isect_ray_tri_v3(ray->origin, ray->direction, v0, v1, 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[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 static 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 = 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                 if(numFaces != 0)
578                 {
579                         /* Create a bvh-tree of the given target */
580                         tree = BLI_bvhtree_new(numFaces, epsilon, tree_type, axis);
581                         if(tree != NULL)
582                         {
583                                 BMEditMesh *em= data->em_evil;
584                                 if(em) {
585                                         /*data->em_evil is only set for snapping, and only for the mesh of the object
586                                           which is currently open in edit mode. When set, the bvhtree should not contain
587                                           faces that will interfere with snapping (e.g. faces that are hidden/selected
588                                           or faces that have selected verts).*/
589
590                                         /* XXX, for snap only, em & dm are assumed to be aligned, since dm is the em's cage */
591
592                                         /*Insert BMesh-tesselation triangles into the bvh tree, unless they are hidden
593                                           and/or selected. Even if the faces themselves are not selected for the snapped
594                                           transform, having a vertex selected means the face (and thus it's tesselated
595                                           triangles) will be moving and will not be a good snap targets.*/
596                                         for (i = 0; i < em->tottri; i++) {
597                                                 BMLoop **tri = em->looptris[i];
598                                                 BMFace *f;
599                                                 BMVert *v;
600                                                 BMIter iter;
601                                                 int insert;
602
603                                                 /*Each loop of the triangle points back to the BMFace it was tesselated from.
604                                                   All three should point to the same face, so just use the face from the first
605                                                   loop.*/
606                                                 f = tri[0]->f;
607
608                                                 /*If the looptris is ordered such that all triangles tesselated from a single
609                                                   faces are consecutive elements in the array, then we could speed up the tests
610                                                   below by using the insert value from the previous iteration.*/
611
612                                                 /*Start with the assumption the triangle should be included for snapping.*/
613                                                 insert = 1;
614
615                                                 if (BM_TestHFlag(f, BM_SELECT) || BM_TestHFlag(f, BM_HIDDEN)) {
616                                                         /*Don't insert triangles tesselated from faces that are hidden
617                                                           or selected*/
618                                                         insert = 0;
619                                                 }
620                                                 else {
621                                                         BM_ITER(v, &iter, em->bm, BM_VERTS_OF_FACE, f) {
622                                                                 if (BM_TestHFlag(v, BM_SELECT)) {
623                                                                         /*Don't insert triangles tesselated from faces that have
624                                                                           any selected verts.*/
625                                                                         insert = 0;
626                                                                 }
627                                                         }
628                                                 }
629
630                                                 if (insert)
631                                                 {
632                                                         /*No reason found to block hit-testing the triangle for snap,
633                                                           so insert it now.*/
634                                                         float co[4][3];
635                                                         copy_v3_v3(co[0], tri[0]->v->co);
636                                                         copy_v3_v3(co[1], tri[1]->v->co);
637                                                         copy_v3_v3(co[2], tri[2]->v->co);
638                                         
639                                                         BLI_bvhtree_insert(tree, i, co[0], 3);
640                                                 }
641                                         }
642                                 }
643                                 else {
644                                         MVert *vert     = mesh->getVertDataArray(mesh, CD_MVERT);
645                                         MFace *face = mesh->getTessFaceDataArray(mesh, CD_MFACE);
646
647                                         if (vert != NULL && face != NULL) {
648                                                 for(i = 0; i < numFaces; i++) {
649                                                         float co[4][3];
650                                                         copy_v3_v3(co[0], vert[ face[i].v1 ].co);
651                                                         copy_v3_v3(co[1], vert[ face[i].v2 ].co);
652                                                         copy_v3_v3(co[2], vert[ face[i].v3 ].co);
653                                                         if(face[i].v4)
654                                                                 copy_v3_v3(co[3], vert[ face[i].v4 ].co);
655                                 
656                                                         BLI_bvhtree_insert(tree, i, co[0], face[i].v4 ? 4 : 3);
657                                                 }
658                                         }
659                                 }
660                                 BLI_bvhtree_balance(tree);
661
662                                 //Save on cache for later use
663 //                              printf("BVHTree built and saved on cache\n");
664                                 bvhcache_insert(&mesh->bvhCache, tree, BVHTREE_FROM_FACES);
665                         }
666                 }
667         }
668         else
669         {
670 //              printf("BVHTree is already build, using cached tree\n");
671         }
672
673
674         //Setup BVHTreeFromMesh
675         memset(data, 0, sizeof(*data));
676         data->tree = tree;
677
678         if(data->tree)
679         {
680                 data->cached = TRUE;
681
682                 data->nearest_callback = mesh_faces_nearest_point;
683                 data->raycast_callback = mesh_faces_spherecast;
684
685                 data->mesh = mesh;
686                 data->vert = mesh->getVertDataArray(mesh, CD_MVERT);
687                 data->face = mesh->getTessFaceDataArray(mesh, CD_MFACE);
688
689                 data->sphere_radius = epsilon;
690         }
691         return data->tree;
692
693 }
694
695 // Builds a bvh tree.. where nodes are the faces of the given mesh.
696 BVHTree* bvhtree_from_mesh_edges(BVHTreeFromMesh *data, DerivedMesh *mesh, float epsilon, int tree_type, int axis)
697 {
698         BVHTree *tree = bvhcache_find(&mesh->bvhCache, BVHTREE_FROM_EDGES);
699
700         //Not in cache
701         if(tree == NULL)
702         {
703                 int i;
704                 int numEdges= mesh->getNumEdges(mesh);
705                 MVert *vert     = mesh->getVertDataArray(mesh, CD_MVERT);
706                 MEdge *edge = mesh->getEdgeDataArray(mesh, CD_MEDGE);
707
708                 if(vert != NULL && edge != NULL)
709                 {
710                         /* Create a bvh-tree of the given target */
711                         tree = BLI_bvhtree_new(numEdges, epsilon, tree_type, axis);
712                         if(tree != NULL)
713                         {
714                                 for(i = 0; i < numEdges; i++)
715                                 {
716                                         float co[4][3];
717                                         copy_v3_v3(co[0], vert[ edge[i].v1 ].co);
718                                         copy_v3_v3(co[1], vert[ edge[i].v2 ].co);
719                         
720                                         BLI_bvhtree_insert(tree, i, co[0], 2);
721                                 }
722                                 BLI_bvhtree_balance(tree);
723
724                                 //Save on cache for later use
725 //                              printf("BVHTree built and saved on cache\n");
726                                 bvhcache_insert(&mesh->bvhCache, tree, BVHTREE_FROM_EDGES);
727                         }
728                 }
729         }
730         else
731         {
732 //              printf("BVHTree is already build, using cached tree\n");
733         }
734
735
736         //Setup BVHTreeFromMesh
737         memset(data, 0, sizeof(*data));
738         data->tree = tree;
739
740         if(data->tree)
741         {
742                 data->cached = TRUE;
743
744                 data->nearest_callback = mesh_edges_nearest_point;
745                 data->raycast_callback = NULL;
746
747                 data->mesh = mesh;
748                 data->vert = mesh->getVertDataArray(mesh, CD_MVERT);
749                 data->edge = mesh->getEdgeDataArray(mesh, CD_MEDGE);
750
751                 data->sphere_radius = epsilon;
752         }
753         return data->tree;
754
755 }
756
757 // Frees data allocated by a call to bvhtree_from_mesh_*.
758 void free_bvhtree_from_mesh(struct BVHTreeFromMesh *data)
759 {
760         if(data->tree)
761         {
762                 if(!data->cached)
763                         BLI_bvhtree_free(data->tree);
764
765                 memset( data, 0, sizeof(*data) );
766         }
767 }
768
769
770 /* BVHCache */
771 typedef struct BVHCacheItem
772 {
773         int type;
774         BVHTree *tree;
775
776 } BVHCacheItem;
777
778 static void bvhcacheitem_set_if_match(void *_cached, void *_search)
779 {
780         BVHCacheItem * cached = (BVHCacheItem *)_cached;
781         BVHCacheItem * search = (BVHCacheItem *)_search;
782
783         if(search->type == cached->type)
784         {
785                 search->tree = cached->tree;            
786         }
787
788
789 BVHTree *bvhcache_find(BVHCache *cache, int type)
790 {
791         BVHCacheItem item;
792         item.type = type;
793         item.tree = NULL;
794
795         BLI_linklist_apply(*cache, bvhcacheitem_set_if_match, &item);
796         return item.tree;
797 }
798
799 void bvhcache_insert(BVHCache *cache, BVHTree *tree, int type)
800 {
801         BVHCacheItem *item = NULL;
802
803         assert( tree != NULL );
804         assert( bvhcache_find(cache, type) == NULL );
805
806         item = MEM_mallocN(sizeof(BVHCacheItem), "BVHCacheItem");
807         assert( item != NULL );
808
809         item->type = type;
810         item->tree = tree;
811
812         BLI_linklist_prepend( cache, item );
813 }
814
815
816 void bvhcache_init(BVHCache *cache)
817 {
818         *cache = NULL;
819 }
820
821 static void bvhcacheitem_free(void *_item)
822 {
823         BVHCacheItem *item = (BVHCacheItem *)_item;
824
825         BLI_bvhtree_free(item->tree);
826         MEM_freeN(item);
827 }
828
829
830 void bvhcache_free(BVHCache *cache)
831 {
832         BLI_linklist_free(*cache, (LinkNodeFreeFP)bvhcacheitem_free);
833         *cache = NULL;
834 }
835
836