Merging Shrinkwrap Constraint!
[blender.git] / source / blender / blenkernel / intern / bvhutils.c
1 /**
2  *
3  * $Id$
4  *
5  * ***** BEGIN GPL LICENSE BLOCK *****
6  *
7  * This program is free software; you can redistribute it and/or
8  * modify it under the terms of the GNU General Public License
9  * as published by the Free Software Foundation; either version 2
10  * of the License, or (at your option) any later version.
11  *
12  * This program is distributed in the hope that it will be useful,
13  * but WITHOUT ANY WARRANTY; without even the implied warranty of
14  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
15  * GNU General Public License for more details.
16  *
17  * You should have received a copy of the GNU General Public License
18  * along with this program; if not, write to the Free Software Foundation,
19  * Inc., 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.
20  *
21  * The Original Code is Copyright (C) Blender Foundation.
22  * All rights reserved.
23  *
24  * The Original Code is: all of this file.
25  *
26  * Contributor(s): AndrĂ© Pinto.
27  *
28  * ***** END GPL LICENSE BLOCK *****
29  */
30 #include <stdio.h>
31 #include <string.h>
32 #include <math.h>
33 #include <assert.h>
34
35 #include "BKE_bvhutils.h"
36
37 #include "DNA_object_types.h"
38 #include "DNA_modifier_types.h"
39 #include "DNA_meshdata_types.h"
40
41 #include "BKE_DerivedMesh.h"
42 #include "BKE_utildefines.h"
43 #include "BKE_deform.h"
44 #include "BKE_cdderivedmesh.h"
45 #include "BKE_displist.h"
46 #include "BKE_global.h"
47
48 #include "BLI_arithb.h"
49 #include "BLI_linklist.h"
50 #include "MEM_guardedalloc.h"
51
52 /* Math stuff for ray casting on mesh faces and for nearest surface */
53
54 static float ray_tri_intersection(const BVHTreeRay *ray, const float m_dist, const float *v0, const float *v1, const float *v2)
55 {
56         float dist;
57
58         if(RayIntersectsTriangle((float*)ray->origin, (float*)ray->direction, (float*)v0, (float*)v1, (float*)v2, &dist, NULL))
59                 return dist;
60
61         return FLT_MAX;
62 }
63
64 static float sphereray_tri_intersection(const BVHTreeRay *ray, float radius, const float m_dist, const float *v0, const float *v1, const float *v2)
65 {
66         
67         float idist;
68         float p1[3];
69         float plane_normal[3], hit_point[3];
70
71         CalcNormFloat((float*)v0, (float*)v1, (float*)v2, plane_normal);
72
73         VECADDFAC( p1, ray->origin, ray->direction, m_dist);
74         if(SweepingSphereIntersectsTriangleUV((float*)ray->origin, p1, radius, (float*)v0, (float*)v1, (float*)v2, &idist, hit_point))
75         {
76                 return idist * m_dist;
77         }
78
79         return FLT_MAX;
80 }
81
82
83 /*
84  * Function adapted from David Eberly's distance tools (LGPL)
85  * http://www.geometrictools.com/LibFoundation/Distance/Distance.html
86  */
87 static float nearest_point_in_tri_surface(const float *v0,const float *v1,const float *v2,const float *p, int *v, int *e, float *nearest )
88 {
89         float diff[3];
90         float e0[3];
91         float e1[3];
92         float A00;
93         float A01;
94         float A11;
95         float B0;
96         float B1;
97         float C;
98         float Det;
99         float S;
100         float T;
101         float sqrDist;
102         int lv = -1, le = -1;
103         
104         VECSUB(diff, v0, p);
105         VECSUB(e0, v1, v0);
106         VECSUB(e1, v2, v0);
107         
108         A00 = INPR ( e0, e0 );
109         A01 = INPR( e0, e1 );
110         A11 = INPR ( e1, e1 );
111         B0 = INPR( diff, e0 );
112         B1 = INPR( diff, e1 );
113         C = INPR( diff, diff );
114         Det = fabs( A00 * A11 - A01 * A01 );
115         S = A01 * B1 - A11 * B0;
116         T = A01 * B0 - A00 * B1;
117
118         if ( S + T <= Det )
119         {
120                 if ( S < 0.0f )
121                 {
122                         if ( T < 0.0f )  // Region 4
123                         {
124                                 if ( B0 < 0.0f )
125                                 {
126                                         T = 0.0f;
127                                         if ( -B0 >= A00 )
128                                         {
129                                                 S = (float)1.0;
130                                                 sqrDist = A00 + 2.0f * B0 + C;
131                                                 lv = 1;
132                                         }
133                                         else
134                                         {
135                                                 if(fabs(A00) > FLT_EPSILON)
136                                                         S = -B0/A00;
137                                                 else
138                                                         S = 0.0f;
139                                                 sqrDist = B0 * S + C;
140                                                 le = 0;
141                                         }
142                                 }
143                                 else
144                                 {
145                                         S = 0.0f;
146                                         if ( B1 >= 0.0f )
147                                         {
148                                                 T = 0.0f;
149                                                 sqrDist = C;
150                                                 lv = 0;
151                                         }
152                                         else if ( -B1 >= A11 )
153                                         {
154                                                 T = 1.0f;
155                                                 sqrDist = A11 + 2.0f * B1 + C;
156                                                 lv = 2;
157                                         }
158                                         else
159                                         {
160                                                 if(fabs(A11) > FLT_EPSILON)
161                                                         T = -B1 / A11;
162                                                 else
163                                                         T = 0.0f;
164                                                 sqrDist = B1 * T + C;
165                                                 le = 1;
166                                         }
167                                 }
168                         }
169                         else  // Region 3
170                         {
171                                 S = 0.0f;
172                                 if ( B1 >= 0.0f )
173                                 {
174                                         T = 0.0f;
175                                         sqrDist = C;
176                                         lv = 0;
177                                 }
178                                 else if ( -B1 >= A11 )
179                                 {
180                                         T = 1.0f;
181                                         sqrDist = A11 + 2.0f * B1 + C;
182                                         lv = 2;
183                                 }
184                                 else
185                                 {
186                                         if(fabs(A11) > FLT_EPSILON)
187                                                 T = -B1 / A11;
188                                         else
189                                                 T = 0.0;
190                                         sqrDist = B1 * T + C;
191                                         le = 1;
192                                 }
193                         }
194                 }
195                 else if ( T < 0.0f )  // Region 5
196                 {
197                         T = 0.0f;
198                         if ( B0 >= 0.0f )
199                         {
200                                 S = 0.0f;
201                                 sqrDist = C;
202                                 lv = 0;
203                         }
204                         else if ( -B0 >= A00 )
205                         {
206                                 S = 1.0f;
207                                 sqrDist = A00 + 2.0f * B0 + C;
208                                 lv = 1;
209                         }
210                         else
211                         {
212                                 if(fabs(A00) > FLT_EPSILON)
213                                         S = -B0 / A00;
214                                 else
215                                         S = 0.0f;
216                                 sqrDist = B0 * S + C;
217                                 le = 0;
218                         }
219                 }
220                 else  // Region 0
221                 {
222             // Minimum at interior lv
223                         float invDet;
224                         if(fabs(Det) > FLT_EPSILON)
225                                 invDet = 1.0f / Det;
226                         else
227                                 invDet = 0.0f;
228                         S *= invDet;
229                         T *= invDet;
230                         sqrDist = S * ( A00 * S + A01 * T + 2.0f * B0) +
231                                         T * ( A01 * S + A11 * T + 2.0f * B1 ) + C;
232                 }
233         }
234         else
235         {
236                 float tmp0, tmp1, numer, denom;
237
238                 if ( S < 0.0f )  // Region 2
239                 {
240                         tmp0 = A01 + B0;
241                         tmp1 = A11 + B1;
242                         if ( tmp1 > tmp0 )
243                         {
244                                 numer = tmp1 - tmp0;
245                                 denom = A00 - 2.0f * A01 + A11;
246                                 if ( numer >= denom )
247                                 {
248                                         S = 1.0f;
249                                         T = 0.0f;
250                                         sqrDist = A00 + 2.0f * B0 + C;
251                                         lv = 1;
252                                 }
253                                 else
254                                 {
255                                         if(fabs(denom) > FLT_EPSILON)
256                                                 S = numer / denom;
257                                         else
258                                                 S = 0.0f;
259                                         T = 1.0f - S;
260                                         sqrDist = S * ( A00 * S + A01 * T + 2.0f * B0 ) +
261                                                         T * ( A01 * S + A11 * T + 2.0f * B1 ) + C;
262                                         le = 2;
263                                 }
264                         }
265                         else
266                         {
267                                 S = 0.0f;
268                                 if ( tmp1 <= 0.0f )
269                                 {
270                                         T = 1.0f;
271                                         sqrDist = A11 + 2.0f * B1 + C;
272                                         lv = 2;
273                                 }
274                                 else if ( B1 >= 0.0f )
275                                 {
276                                         T = 0.0f;
277                                         sqrDist = C;
278                                         lv = 0;
279                                 }
280                                 else
281                                 {
282                                         if(fabs(A11) > FLT_EPSILON)
283                                                 T = -B1 / A11;
284                                         else
285                                                 T = 0.0f;
286                                         sqrDist = B1 * T + C;
287                                         le = 1;
288                                 }
289                         }
290                 }
291                 else if ( T < 0.0f )  // Region 6
292                 {
293                         tmp0 = A01 + B1;
294                         tmp1 = A00 + B0;
295                         if ( tmp1 > tmp0 )
296                         {
297                                 numer = tmp1 - tmp0;
298                                 denom = A00 - 2.0f * A01 + A11;
299                                 if ( numer >= denom )
300                                 {
301                                         T = 1.0f;
302                                         S = 0.0f;
303                                         sqrDist = A11 + 2.0f * B1 + C;
304                                         lv = 2;
305                                 }
306                                 else
307                                 {
308                                         if(fabs(denom) > FLT_EPSILON)
309                                                 T = numer / denom;
310                                         else
311                                                 T = 0.0f;
312                                         S = 1.0f - T;
313                                         sqrDist = S * ( A00 * S + A01 * T + 2.0f * B0 ) +
314                                                         T * ( A01 * S + A11 * T + 2.0f * B1 ) + C;
315                                         le = 2;
316                                 }
317                         }
318                         else
319                         {
320                                 T = 0.0f;
321                                 if ( tmp1 <= 0.0f )
322                                 {
323                                         S = 1.0f;
324                                         sqrDist = A00 + 2.0f * B0 + C;
325                                         lv = 1;
326                                 }
327                                 else if ( B0 >= 0.0f )
328                                 {
329                                         S = 0.0f;
330                                         sqrDist = C;
331                                         lv = 0;
332                                 }
333                                 else
334                                 {
335                                         if(fabs(A00) > FLT_EPSILON)
336                                                 S = -B0 / A00;
337                                         else
338                                                 S = 0.0f;
339                                         sqrDist = B0 * S + C;
340                                         le = 0;
341                                 }
342                         }
343                 }
344                 else  // Region 1
345                 {
346                         numer = A11 + B1 - A01 - B0;
347                         if ( numer <= 0.0f )
348                         {
349                                 S = 0.0f;
350                                 T = 1.0f;
351                                 sqrDist = A11 + 2.0f * B1 + C;
352                                 lv = 2;
353                         }
354                         else
355                         {
356                                 denom = A00 - 2.0f * A01 + A11;
357                                 if ( numer >= denom )
358                                 {
359                                         S = 1.0f;
360                                         T = 0.0f;
361                                         sqrDist = A00 + 2.0f * B0 + C;
362                                         lv = 1;
363                                 }
364                                 else
365                                 {
366                                         if(fabs(denom) > FLT_EPSILON)
367                                                 S = numer / denom;
368                                         else
369                                                 S = 0.0f;
370                                         T = 1.0f - S;
371                                         sqrDist = S * ( A00 * S + A01 * T + 2.0f * B0 ) +
372                                                         T * ( A01 * S + A11 * T + 2.0f * B1 ) + C;
373                                         le = 2;
374                                 }
375                         }
376                 }
377         }
378
379         // Account for numerical round-off error
380         if ( sqrDist < FLT_EPSILON )
381                 sqrDist = 0.0f;
382         
383         {
384                 float w[3], x[3], y[3], z[3];
385                 VECCOPY(w, v0);
386                 VECCOPY(x, e0);
387                 VecMulf(x, S);
388                 VECCOPY(y, e1);
389                 VecMulf(y, T);
390                 VECADD(z, w, x);
391                 VECADD(z, z, y);
392                 //VECSUB(d, p, z);
393                 VECCOPY(nearest, z);
394                 // d = p - ( v0 + S * e0 + T * e1 );
395         }
396         *v = lv;
397         *e = le;
398
399         return sqrDist;
400 }
401
402
403 /*
404  * BVH from meshs callbacks
405  */
406
407 // Callback to bvh tree nearest point. The tree must bust have been built using bvhtree_from_mesh_faces.
408 // userdata must be a BVHMeshCallbackUserdata built from the same mesh as the tree.
409 static void mesh_faces_nearest_point(void *userdata, int index, const float *co, BVHTreeNearest *nearest)
410 {
411         const BVHTreeFromMesh *data = (BVHTreeFromMesh*) userdata;
412         MVert *vert     = data->vert;
413         MFace *face = data->face + index;
414
415         float *t0, *t1, *t2, *t3;
416         t0 = vert[ face->v1 ].co;
417         t1 = vert[ face->v2 ].co;
418         t2 = vert[ face->v3 ].co;
419         t3 = face->v4 ? vert[ face->v4].co : NULL;
420
421         
422         do
423         {       
424                 float nearest_tmp[3], dist;
425                 int vertex, edge;
426                 
427                 dist = nearest_point_in_tri_surface(t0, t1, t2, co, &vertex, &edge, nearest_tmp);
428                 if(dist < nearest->dist)
429                 {
430                         nearest->index = index;
431                         nearest->dist = dist;
432                         VECCOPY(nearest->co, nearest_tmp);
433                         CalcNormFloat(t0, t1, t2, nearest->no);
434                 }
435
436                 t1 = t2;
437                 t2 = t3;
438                 t3 = NULL;
439
440         } while(t2);
441 }
442
443 // Callback to bvh tree raycast. The tree must bust have been built using bvhtree_from_mesh_faces.
444 // userdata must be a BVHMeshCallbackUserdata built from the same mesh as the tree.
445 static void mesh_faces_spherecast(void *userdata, int index, const BVHTreeRay *ray, BVHTreeRayHit *hit)
446 {
447         const BVHTreeFromMesh *data = (BVHTreeFromMesh*) userdata;
448         MVert *vert     = data->vert;
449         MFace *face = data->face + index;
450
451         float *t0, *t1, *t2, *t3;
452         t0 = vert[ face->v1 ].co;
453         t1 = vert[ face->v2 ].co;
454         t2 = vert[ face->v3 ].co;
455         t3 = face->v4 ? vert[ face->v4].co : NULL;
456
457         
458         do
459         {       
460                 float dist;
461                 if(data->sphere_radius == 0.0f)
462                         dist = ray_tri_intersection(ray, hit->dist, t0, t1, t2);
463                 else
464                         dist = sphereray_tri_intersection(ray, data->sphere_radius, hit->dist, t0, t1, t2);
465
466                 if(dist >= 0 && dist < hit->dist)
467                 {
468                         hit->index = index;
469                         hit->dist = dist;
470                         VECADDFAC(hit->co, ray->origin, ray->direction, dist);
471
472                         CalcNormFloat(t0, t1, t2, hit->no);
473                 }
474
475                 t1 = t2;
476                 t2 = t3;
477                 t3 = NULL;
478
479         } while(t2);
480 }
481
482 /*
483  * BVH builders
484  */
485 // Builds a bvh tree.. where nodes are the vertexs of the given mesh
486 BVHTree* bvhtree_from_mesh_verts(BVHTreeFromMesh *data, DerivedMesh *mesh, float epsilon, int tree_type, int axis)
487 {
488         BVHTree *tree = bvhcache_find(&mesh->bvhCache, BVHTREE_FROM_VERTICES);
489
490         //Not in cache
491         if(tree == NULL)
492         {
493                 int i;
494                 int numVerts= mesh->getNumVerts(mesh);
495                 MVert *vert     = mesh->getVertDataArray(mesh, CD_MVERT);
496
497                 if(vert != NULL)
498                 {
499                         tree = BLI_bvhtree_new(numVerts, epsilon, tree_type, axis);
500
501                         if(tree != NULL)
502                         {
503                                 for(i = 0; i < numVerts; i++)
504                                         BLI_bvhtree_insert(tree, i, vert[i].co, 1);
505
506                                 BLI_bvhtree_balance(tree);
507
508                                 //Save on cache for later use
509 //                              printf("BVHTree built and saved on cache\n");
510                                 bvhcache_insert(&mesh->bvhCache, tree, BVHTREE_FROM_VERTICES);
511                         }
512                 }
513         }
514         else
515         {
516 //              printf("BVHTree is already build, using cached tree\n");
517         }
518
519
520         //Setup BVHTreeFromMesh
521         memset(data, 0, sizeof(*data));
522         data->tree = tree;
523
524         if(data->tree)
525         {
526                 data->cached = TRUE;
527
528                 //a NULL nearest callback works fine
529                 //remeber the min distance to point is the same as the min distance to BV of point
530                 data->nearest_callback = NULL;
531                 data->raycast_callback = NULL;
532
533                 data->mesh = mesh;
534                 data->vert = mesh->getVertDataArray(mesh, CD_MVERT);
535                 data->face = mesh->getFaceDataArray(mesh, CD_MFACE);
536
537                 data->sphere_radius = epsilon;
538         }
539
540         return data->tree;
541 }
542
543 // Builds a bvh tree.. where nodes are the faces of the given mesh.
544 BVHTree* bvhtree_from_mesh_faces(BVHTreeFromMesh *data, DerivedMesh *mesh, float epsilon, int tree_type, int axis)
545 {
546         BVHTree *tree = bvhcache_find(&mesh->bvhCache, BVHTREE_FROM_FACES);
547
548         //Not in cache
549         if(tree == NULL)
550         {
551                 int i;
552                 int numFaces= mesh->getNumFaces(mesh);
553                 MVert *vert     = mesh->getVertDataArray(mesh, CD_MVERT);
554                 MFace *face = mesh->getFaceDataArray(mesh, CD_MFACE);
555
556                 if(vert != NULL && face != NULL)
557                 {
558                         /* Create a bvh-tree of the given target */
559                         tree = BLI_bvhtree_new(numFaces, epsilon, tree_type, axis);
560                         if(tree != NULL)
561                         {
562                                 for(i = 0; i < numFaces; i++)
563                                 {
564                                         float co[4][3];
565                                         VECCOPY(co[0], vert[ face[i].v1 ].co);
566                                         VECCOPY(co[1], vert[ face[i].v2 ].co);
567                                         VECCOPY(co[2], vert[ face[i].v3 ].co);
568                                         if(face[i].v4)
569                                                 VECCOPY(co[3], vert[ face[i].v4 ].co);
570                         
571                                         BLI_bvhtree_insert(tree, i, co[0], face[i].v4 ? 4 : 3);
572                                 }
573                                 BLI_bvhtree_balance(tree);
574
575                                 //Save on cache for later use
576 //                              printf("BVHTree built and saved on cache\n");
577                                 bvhcache_insert(&mesh->bvhCache, tree, BVHTREE_FROM_FACES);
578                         }
579                 }
580         }
581         else
582         {
583 //              printf("BVHTree is already build, using cached tree\n");
584         }
585
586
587         //Setup BVHTreeFromMesh
588         memset(data, 0, sizeof(*data));
589         data->tree = tree;
590
591         if(data->tree)
592         {
593                 data->cached = TRUE;
594
595                 data->nearest_callback = mesh_faces_nearest_point;
596                 data->raycast_callback = mesh_faces_spherecast;
597
598                 data->mesh = mesh;
599                 data->vert = mesh->getVertDataArray(mesh, CD_MVERT);
600                 data->face = mesh->getFaceDataArray(mesh, CD_MFACE);
601
602                 data->sphere_radius = epsilon;
603         }
604         return data->tree;
605
606 }
607
608 // Frees data allocated by a call to bvhtree_from_mesh_*.
609 void free_bvhtree_from_mesh(struct BVHTreeFromMesh *data)
610 {
611         if(data->tree)
612         {
613                 if(!data->cached)
614                         BLI_bvhtree_free(data->tree);
615
616                 memset( data, 0, sizeof(data) );
617         }
618 }
619
620
621 /* BVHCache */
622 typedef struct BVHCacheItem
623 {
624         int type;
625         BVHTree *tree;
626
627 } BVHCacheItem;
628
629 static void bvhcacheitem_set_if_match(void *_cached, void *_search)
630 {
631         BVHCacheItem * cached = (BVHCacheItem *)_cached;
632         BVHCacheItem * search = (BVHCacheItem *)_search;
633
634         if(search->type == cached->type)
635         {
636                 search->tree = cached->tree;            
637         }
638
639
640 BVHTree *bvhcache_find(BVHCache *cache, int type)
641 {
642         BVHCacheItem item;
643         item.type = type;
644         item.tree = NULL;
645
646         BLI_linklist_apply(*cache, bvhcacheitem_set_if_match, &item);
647         return item.tree;
648 }
649
650 void bvhcache_insert(BVHCache *cache, BVHTree *tree, int type)
651 {
652         BVHCacheItem *item = NULL;
653
654         assert( tree != NULL );
655         assert( bvhcache_find(cache, type) == NULL );
656
657         item = MEM_mallocN(sizeof(BVHCacheItem), "BVHCacheItem");
658         assert( item != NULL );
659
660         item->type = type;
661         item->tree = tree;
662
663         BLI_linklist_prepend( cache, item );
664 }
665
666
667 void bvhcache_init(BVHCache *cache)
668 {
669         *cache = NULL;
670 }
671
672 static void bvhcacheitem_free(void *_item)
673 {
674         BVHCacheItem *item = (BVHCacheItem *)_item;
675
676         BLI_bvhtree_free(item->tree);
677         MEM_freeN(item);
678 }
679
680
681 void bvhcache_free(BVHCache *cache)
682 {
683         BLI_linklist_free(*cache, (LinkNodeFreeFP)bvhcacheitem_free);
684         *cache = NULL;
685 }
686
687