resolve some compiler warnings with intel c/c++ compiler
[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
34 #include "BKE_bvhutils.h"
35
36 #include "DNA_object_types.h"
37 #include "DNA_modifier_types.h"
38 #include "DNA_meshdata_types.h"
39
40 #include "BKE_DerivedMesh.h"
41 #include "BKE_utildefines.h"
42 #include "BKE_deform.h"
43 #include "BKE_cdderivedmesh.h"
44 #include "BKE_displist.h"
45 #include "BKE_global.h"
46
47 #include "BLI_arithb.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 m_dist, const float *v0, const float *v1, const float *v2)
52 {
53         float dist;
54
55         if(RayIntersectsTriangle((float*)ray->origin, (float*)ray->direction, (float*)v0, (float*)v1, (float*)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, const float *v1, const float *v2)
62 {
63         
64         float idist;
65         float p1[3];
66         float plane_normal[3], hit_point[3];
67
68         CalcNormFloat((float*)v0, (float*)v1, (float*)v2, plane_normal);
69
70         VECADDFAC( p1, ray->origin, ray->direction, m_dist);
71         if(SweepingSphereIntersectsTriangleUV((float*)ray->origin, p1, radius, (float*)v0, (float*)v1, (float*)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,const float *v1,const float *v2,const float *p, int *v, int *e, float *nearest )
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         VECSUB(diff, v0, p);
102         VECSUB(e0, v1, v0);
103         VECSUB(e1, v2, v0);
104         
105         A00 = INPR ( e0, e0 );
106         A01 = INPR( e0, e1 );
107         A11 = INPR ( e1, e1 );
108         B0 = INPR( diff, e0 );
109         B1 = INPR( diff, e1 );
110         C = INPR( 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 = (float)1.0;
127                                                 sqrDist = A00 + 2.0f * B0 + C;
128                                                 lv = 1;
129                                         }
130                                         else
131                                         {
132                                                 if(fabs(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(fabs(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(fabs(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(fabs(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(fabs(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(fabs(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(fabs(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(fabs(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(fabs(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(fabs(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                 VECCOPY(w, v0);
383                 VECCOPY(x, e0);
384                 VecMulf(x, S);
385                 VECCOPY(y, e1);
386                 VecMulf(y, T);
387                 VECADD(z, w, x);
388                 VECADD(z, z, y);
389                 //VECSUB(d, p, z);
390                 VECCOPY(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, 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                         VECCOPY(nearest->co, nearest_tmp);
430                         CalcNormFloat(t0, t1, t2, nearest->no);
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                         VECADDFAC(hit->co, ray->origin, ray->direction, dist);
468
469                         CalcNormFloat(t0, t1, t2, hit->no);
470                 }
471
472                 t1 = t2;
473                 t2 = t3;
474                 t3 = NULL;
475
476         } while(t2);
477 }
478
479 /*
480  * BVH builders
481  */
482 // Builds a bvh tree.. where nodes are the vertexs of the given mesh
483 void bvhtree_from_mesh_verts(BVHTreeFromMesh *data, DerivedMesh *mesh, float epsilon, int tree_type, int axis)
484 {
485         int i;
486         int numVerts= mesh->getNumVerts(mesh);
487         MVert *vert     = mesh->getVertDataArray(mesh, CD_MVERT);
488         BVHTree *tree = NULL;
489
490         memset(data, 0, sizeof(*data));
491
492         if(vert == NULL)
493         {
494                 printf("bvhtree cant be build: cant get a vertex array");
495                 return;
496         }
497
498         tree = BLI_bvhtree_new(numVerts, epsilon, tree_type, axis);
499         if(tree != NULL)
500         {
501                 for(i = 0; i < numVerts; i++)
502                         BLI_bvhtree_insert(tree, i, vert[i].co, 1);
503
504                 BLI_bvhtree_balance(tree);
505
506                 data->tree = tree;
507
508                 //a NULL nearest callback works fine
509                 //remeber the min distance to point is the same as the min distance to BV of point
510                 data->nearest_callback = NULL;
511                 data->raycast_callback = NULL;
512
513                 data->mesh = mesh;
514                 data->vert = mesh->getVertDataArray(mesh, CD_MVERT);
515                 data->face = mesh->getFaceDataArray(mesh, CD_MFACE);
516
517                 data->sphere_radius = epsilon;
518         }
519 }
520
521 // Builds a bvh tree.. where nodes are the faces of the given mesh.
522 void bvhtree_from_mesh_faces(BVHTreeFromMesh *data, DerivedMesh *mesh, float epsilon, int tree_type, int axis)
523 {
524         int i;
525         int numFaces= mesh->getNumFaces(mesh);
526         MVert *vert     = mesh->getVertDataArray(mesh, CD_MVERT);
527         MFace *face = mesh->getFaceDataArray(mesh, CD_MFACE);
528         BVHTree *tree = NULL;
529
530         memset(data, 0, sizeof(*data));
531
532         if(vert == NULL && face == NULL)
533         {
534                 printf("bvhtree cant be build: cant get a vertex/face array");
535                 return;
536         }
537
538         /* Create a bvh-tree of the given target */
539         tree = BLI_bvhtree_new(numFaces, epsilon, tree_type, axis);
540         if(tree != NULL)
541         {
542                 for(i = 0; i < numFaces; i++)
543                 {
544                         float co[4][3];
545                         VECCOPY(co[0], vert[ face[i].v1 ].co);
546                         VECCOPY(co[1], vert[ face[i].v2 ].co);
547                         VECCOPY(co[2], vert[ face[i].v3 ].co);
548                         if(face[i].v4)
549                                 VECCOPY(co[3], vert[ face[i].v4 ].co);
550                         
551                         BLI_bvhtree_insert(tree, i, co[0], face[i].v4 ? 4 : 3);
552                 }
553                 BLI_bvhtree_balance(tree);
554
555                 data->tree = tree;
556                 data->nearest_callback = mesh_faces_nearest_point;
557                 data->raycast_callback = mesh_faces_spherecast;
558
559                 data->mesh = mesh;
560                 data->vert = mesh->getVertDataArray(mesh, CD_MVERT);
561                 data->face = mesh->getFaceDataArray(mesh, CD_MFACE);
562
563                 data->sphere_radius = epsilon;
564         }
565 }
566
567 // Frees data allocated by a call to bvhtree_from_mesh_*.
568 void free_bvhtree_from_mesh(struct BVHTreeFromMesh *data)
569 {
570         if(data->tree)
571         {
572                 BLI_bvhtree_free(data->tree);
573                 memset( data, 0, sizeof(data) );
574         }
575 }
576
577