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