BVH-KDOP update (merge from shrinkwrap branch): supports raytracing, nearest neighbou...
[blender-staging.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 nearest_point_in_tri_surface(const float *point, const float *v0, const float *v1, const float *v2, float *nearest);
52
53 #define ISECT_EPSILON 1e-6
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(ray->origin, ray->direction, v0, v1, 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(ray->origin, p1, radius, v0, v1, v2, &idist, &hit_point))
75         {
76                 return idist * m_dist;
77         }
78
79         return FLT_MAX;
80 }
81
82 /*
83  * This calculates the distance from point to the plane
84  * Distance is negative if point is on the back side of plane
85  */
86 static float point_plane_distance(const float *point, const float *plane_point, const float *plane_normal)
87 {
88         float pp[3];
89         VECSUB(pp, point, plane_point);
90         return INPR(pp, plane_normal);
91 }
92 static float choose_nearest(const float v0[2], const float v1[2], const float point[2], float closest[2])
93 {
94         float d[2][2], sdist[2];
95         VECSUB2D(d[0], v0, point);
96         VECSUB2D(d[1], v1, point);
97
98         sdist[0] = d[0][0]*d[0][0] + d[0][1]*d[0][1];
99         sdist[1] = d[1][0]*d[1][0] + d[1][1]*d[1][1];
100
101         if(sdist[0] < sdist[1])
102         {
103                 if(closest)
104                         VECCOPY2D(closest, v0);
105                 return sdist[0];
106         }
107         else
108         {
109                 if(closest)
110                         VECCOPY2D(closest, v1);
111                 return sdist[1];
112         }
113 }
114 /*
115  * calculates the closest point between point-tri (2D)
116  * returns that tri must be right-handed
117  * Returns square distance
118  */
119 static float closest_point_in_tri2D(const float point[2], /*const*/ float tri[3][2], float closest[2])
120 {
121         float edge_di[2];
122         float v_point[2];
123         float proj[2];                                  //point projected over edge-dir, edge-normal (witouth normalized edge)
124         const float *v0 = tri[2], *v1;
125         float edge_slen, d;                             //edge squared length
126         int i;
127         const float *nearest_vertex = NULL;
128
129
130         //for each edge
131         for(i=0, v0=tri[2], v1=tri[0]; i < 3; v0=tri[i++], v1=tri[i])
132         {
133                 VECSUB2D(edge_di,    v1, v0);
134                 VECSUB2D(v_point, point, v0);
135
136                 proj[1] =  v_point[0]*edge_di[1] - v_point[1]*edge_di[0];       //dot product with edge normal
137
138                 //point inside this edge
139                 if(proj[1] < 0)
140                         continue;
141
142                 proj[0] = v_point[0]*edge_di[0] + v_point[1]*edge_di[1];
143
144                 //closest to this edge is v0
145                 if(proj[0] < 0)
146                 {
147                         if(nearest_vertex == NULL || nearest_vertex == v0)
148                                 nearest_vertex = v0;
149                         else
150                         {
151                                 //choose nearest
152                                 return choose_nearest(nearest_vertex, v0, point, closest);
153                         }
154                         i++;    //We can skip next edge
155                         continue;
156                 }
157
158                 edge_slen = edge_di[0]*edge_di[0] + edge_di[1]*edge_di[1];      //squared edge len
159                 //closest to this edge is v1
160                 if(proj[0] > edge_slen)
161                 {
162                         if(nearest_vertex == NULL || nearest_vertex == v1)
163                                 nearest_vertex = v1;
164                         else
165                         {
166                                 return choose_nearest(nearest_vertex, v1, point, closest);
167                         }
168                         continue;
169                 }
170
171                 //nearest is on this edge
172                 d= proj[1] / edge_slen;
173                 closest[0] = point[0] - edge_di[1] * d;
174                 closest[1] = point[1] + edge_di[0] * d;
175
176                 return proj[1]*proj[1]/edge_slen;
177         }
178
179         if(nearest_vertex)
180         {
181                 VECSUB2D(v_point, nearest_vertex, point);
182                 VECCOPY2D(closest, nearest_vertex);
183                 return v_point[0]*v_point[0] + v_point[1]*v_point[1];
184         }
185         else
186         {
187                 VECCOPY(closest, point);        //point is already inside
188                 return 0.0f;
189         }
190 }
191
192 /*
193  * Returns the square of the minimum distance between the point and a triangle surface
194  * If nearest is not NULL the nearest surface point is written on it
195  */
196 static float nearest_point_in_tri_surface(const float *point, const float *v0, const float *v1, const float *v2, float *nearest)
197 {
198         //Lets solve the 2D problem (closest point-tri)
199         float normal_dist, plane_sdist, plane_offset;
200         float du[3], dv[3], dw[3];      //orthogonal axis (du=(v0->v1), dw=plane normal)
201
202         float p_2d[2], tri_2d[3][2], nearest_2d[2];
203
204         CalcNormFloat((float*)v0, (float*)v1, (float*)v2, dw);
205
206         //point-plane distance and calculate axis
207         normal_dist = point_plane_distance(point, v0, dw);
208
209         // OPTIMIZATION
210         //      if we are only interested in nearest distance if its closer than some distance already found
211         //  we can:
212         //              if(normal_dist*normal_dist >= best_dist_so_far) return FLOAT_MAX;
213         //
214
215         VECSUB(du, v1, v0);
216         Normalize(du);
217         Crossf(dv, dw, du);
218         plane_offset = INPR(v0, dw);
219
220         //project stuff to 2d
221         tri_2d[0][0] = INPR(du, v0);
222         tri_2d[0][1] = INPR(dv, v0);
223
224         tri_2d[1][0] = INPR(du, v1);
225         tri_2d[1][1] = INPR(dv, v1);
226
227         tri_2d[2][0] = INPR(du, v2);
228         tri_2d[2][1] = INPR(dv, v2);
229
230         p_2d[0] = INPR(du, point);
231         p_2d[1] = INPR(dv, point);
232
233         //we always have a right-handed tri
234         //this should always happen because of the way normal is calculated
235         plane_sdist = closest_point_in_tri2D(p_2d, tri_2d, nearest_2d);
236
237         //project back to 3d
238         if(nearest)
239         {
240                 nearest[0] = du[0]*nearest_2d[0] + dv[0] * nearest_2d[1] + dw[0] * plane_offset;
241                 nearest[1] = du[1]*nearest_2d[0] + dv[1] * nearest_2d[1] + dw[1] * plane_offset;
242                 nearest[2] = du[2]*nearest_2d[0] + dv[2] * nearest_2d[1] + dw[2] * plane_offset;
243         }
244
245         return plane_sdist + normal_dist*normal_dist;
246 }
247
248
249 /*
250  * BVH from meshs callbacks
251  */
252
253 // Callback to bvh tree nearest point. The tree must bust have been built using bvhtree_from_mesh_faces.
254 // userdata must be a BVHMeshCallbackUserdata built from the same mesh as the tree.
255 static void mesh_faces_nearest_point(void *userdata, int index, const float *co, BVHTreeNearest *nearest)
256 {
257         const BVHTreeFromMesh *data = (BVHTreeFromMesh*) userdata;
258         MVert *vert     = data->vert;
259         MFace *face = data->face + index;
260
261         float *t0, *t1, *t2, *t3;
262         t0 = vert[ face->v1 ].co;
263         t1 = vert[ face->v2 ].co;
264         t2 = vert[ face->v3 ].co;
265         t3 = face->v4 ? vert[ face->v4].co : NULL;
266
267         
268         do
269         {       
270                 float nearest_tmp[3], dist;
271
272                 dist = nearest_point_in_tri_surface(co,t0, t1, t2, nearest_tmp);
273                 if(dist < nearest->dist)
274                 {
275                         nearest->index = index;
276                         nearest->dist = dist;
277                         VECCOPY(nearest->co, nearest_tmp);
278                         CalcNormFloat((float*)t0, (float*)t1, (float*)t2, nearest->no); //TODO.. (interpolate normals from the vertexs coordinates?
279                 }
280
281
282                 t1 = t2;
283                 t2 = t3;
284                 t3 = NULL;
285
286         } while(t2);
287 }
288
289 // Callback to bvh tree raycast. The tree must bust have been built using bvhtree_from_mesh_faces.
290 // userdata must be a BVHMeshCallbackUserdata built from the same mesh as the tree.
291 static void mesh_faces_spherecast(void *userdata, int index, const BVHTreeRay *ray, BVHTreeRayHit *hit)
292 {
293         const BVHTreeFromMesh *data = (BVHTreeFromMesh*) userdata;
294         MVert *vert     = data->vert;
295         MFace *face = data->face + index;
296
297         float *t0, *t1, *t2, *t3;
298         t0 = vert[ face->v1 ].co;
299         t1 = vert[ face->v2 ].co;
300         t2 = vert[ face->v3 ].co;
301         t3 = face->v4 ? vert[ face->v4].co : NULL;
302
303         
304         do
305         {       
306                 float dist;
307                 if(data->sphere_radius == 0.0f)
308                         dist = ray_tri_intersection(ray, hit->dist, t0, t1, t2);
309                 else
310                         dist = sphereray_tri_intersection(ray, data->sphere_radius, hit->dist, t0, t1, t2);
311
312                 if(dist >= 0 && dist < hit->dist)
313                 {
314                         hit->index = index;
315                         hit->dist = dist;
316                         VECADDFAC(hit->co, ray->origin, ray->direction, dist);
317
318                         CalcNormFloat(t0, t1, t2, hit->no);
319                 }
320
321                 t1 = t2;
322                 t2 = t3;
323                 t3 = NULL;
324
325         } while(t2);
326 }
327
328 /*
329  * BVH builders
330  */
331 // Builds a bvh tree.. where nodes are the vertexs of the given mesh
332 void bvhtree_from_mesh_verts(BVHTreeFromMesh *data, DerivedMesh *mesh, float epsilon, int tree_type, int axis)
333 {
334         int i;
335         int numVerts= mesh->getNumVerts(mesh);
336         MVert *vert     = mesh->getVertDataArray(mesh, CD_MVERT);
337         BVHTree *tree = NULL;
338
339         memset(data, 0, sizeof(*data));
340
341         if(vert == NULL)
342         {
343                 printf("bvhtree cant be build: cant get a vertex array");
344                 return;
345         }
346
347         tree = BLI_bvhtree_new(numVerts, epsilon, tree_type, axis);
348         if(tree != NULL)
349         {
350                 for(i = 0; i < numVerts; i++)
351                         BLI_bvhtree_insert(tree, i, vert[i].co, 1);
352
353                 BLI_bvhtree_balance(tree);
354
355                 data->tree = tree;
356
357                 //a NULL nearest callback works fine
358                 //remeber the min distance to point is the same as the min distance to BV of point
359                 data->nearest_callback = NULL;
360                 data->raycast_callback = NULL;
361
362                 data->mesh = mesh;
363                 data->vert = mesh->getVertDataArray(mesh, CD_MVERT);
364                 data->face = mesh->getFaceDataArray(mesh, CD_MFACE);
365
366                 data->sphere_radius = epsilon;
367         }
368 }
369
370 // Builds a bvh tree.. where nodes are the faces of the given mesh.
371 void bvhtree_from_mesh_faces(BVHTreeFromMesh *data, DerivedMesh *mesh, float epsilon, int tree_type, int axis)
372 {
373         int i;
374         int numFaces= mesh->getNumFaces(mesh);
375         MVert *vert     = mesh->getVertDataArray(mesh, CD_MVERT);
376         MFace *face = mesh->getFaceDataArray(mesh, CD_MFACE);
377         BVHTree *tree = NULL;
378
379         memset(data, 0, sizeof(*data));
380
381         if(vert == NULL && face == NULL)
382         {
383                 printf("bvhtree cant be build: cant get a vertex/face array");
384                 return;
385         }
386
387         /* Create a bvh-tree of the given target */
388         tree = BLI_bvhtree_new(numFaces, epsilon, tree_type, axis);
389         if(tree != NULL)
390         {
391                 for(i = 0; i < numFaces; i++)
392                 {
393                         float co[4][3];
394                         VECCOPY(co[0], vert[ face[i].v1 ].co);
395                         VECCOPY(co[1], vert[ face[i].v2 ].co);
396                         VECCOPY(co[2], vert[ face[i].v3 ].co);
397                         if(face[i].v4)
398                                 VECCOPY(co[3], vert[ face[i].v4 ].co);
399
400                         BLI_bvhtree_insert(tree, i, co[0], face[i].v4 ? 4 : 3);
401                 }
402                 BLI_bvhtree_balance(tree);
403
404                 data->tree = tree;
405                 data->nearest_callback = mesh_faces_nearest_point;
406                 data->raycast_callback = mesh_faces_spherecast;
407
408                 data->mesh = mesh;
409                 data->vert = mesh->getVertDataArray(mesh, CD_MVERT);
410                 data->face = mesh->getFaceDataArray(mesh, CD_MFACE);
411
412                 data->sphere_radius = epsilon;
413         }
414 }
415
416 // Frees data allocated by a call to bvhtree_from_mesh_*.
417 void free_bvhtree_from_mesh(struct BVHTreeFromMesh *data)
418 {
419         if(data->tree)
420         {
421                 BLI_bvhtree_free(data->tree);
422                 memset( data, 0, sizeof(data) );
423         }
424 }
425
426