Added merge option to shrinkwrap when using projection mode (bruteforce for now)
[blender.git] / source / blender / blenkernel / intern / shrinkwrap.c
1 /**
2  * shrinkwrap.c
3  *
4  * ***** BEGIN GPL LICENSE BLOCK *****
5  *
6  * This program is free software; you can redistribute it and/or
7  * modify it under the terms of the GNU General Public License
8  * as published by the Free Software Foundation; either version 2
9  * of the License, or (at your option) any later version.
10  *
11  * This program is distributed in the hope that it will be useful,
12  * but WITHOUT ANY WARRANTY; without even the implied warranty of
13  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
14  * GNU General Public License for more details.
15  *
16  * You should have received a copy of the GNU General Public License
17  * along with this program; if not, write to the Free Software Foundation,
18  * Inc., 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.
19  *
20  * The Original Code is Copyright (C) Blender Foundation.
21  * All rights reserved.
22  *
23  * The Original Code is: all of this file.
24  *
25  * Contributor(s): AndrĂ© Pinto
26  *
27  * ***** END GPL LICENSE BLOCK *****
28  */
29 #include <string.h>
30 #include <float.h>
31 #include <math.h>
32 #include <memory.h>
33 #include <stdio.h>
34 #include <time.h>
35
36 #include "DNA_object_types.h"
37 #include "DNA_modifier_types.h"
38 #include "DNA_meshdata_types.h"
39
40 #include "BKE_shrinkwrap.h"
41 #include "BKE_DerivedMesh.h"
42 #include "BKE_utildefines.h"
43 #include "BKE_deform.h"
44 #include "BKE_cdderivedmesh.h"
45 #include "BKE_global.h"
46
47 #include "BLI_arithb.h"
48 #include "BLI_kdtree.h"
49 #include "BLI_kdopbvh.h"
50
51 #include "RE_raytrace.h"
52 #include "MEM_guardedalloc.h"
53
54
55 /* Util macros */
56 #define TO_STR(a)       #a
57 #define JOIN(a,b)       a##b
58
59 #define OUT_OF_MEMORY() ((void)printf("Shrinkwrap: Out of memory\n"))
60
61 /* Benchmark macros */
62 #if 0
63
64 #define BENCH(a)        \
65         do {                    \
66                 clock_t _clock_init = clock();  \
67                 (a);                                                    \
68                 printf("%s: %fms\n", #a, (float)(clock()-_clock_init)*1000/CLOCKS_PER_SEC);     \
69         } while(0)
70
71 #define BENCH_VAR(name)         clock_t JOIN(_bench_step,name) = 0, JOIN(_bench_total,name) = 0
72 #define BENCH_BEGIN(name)       JOIN(_bench_step, name) = clock()
73 #define BENCH_END(name)         JOIN(_bench_total,name) += clock() - JOIN(_bench_step,name)
74 #define BENCH_RESET(name)       JOIN(_bench_total, name) = 0
75 #define BENCH_REPORT(name)      printf("%s: %fms\n", TO_STR(name), JOIN(_bench_total,name)*1000.0f/CLOCKS_PER_SEC)
76
77 #else
78
79 #define BENCH(a)        (a)
80 #define BENCH_VAR(name)
81 #define BENCH_BEGIN(name)
82 #define BENCH_END(name)
83 #define BENCH_RESET(name)
84 #define BENCH_REPORT(name)
85
86 #endif
87
88 typedef void ( *Shrinkwrap_ForeachVertexCallback) (DerivedMesh *target, float *co, float *normal);
89
90 static float nearest_point_in_tri_surface(const float *point, const float *v0, const float *v1, const float *v2, float *nearest);
91
92 static void normal_short2float(const short *ns, float *nf)
93 {
94         nf[0] = ns[0] / 32767.0f;
95         nf[1] = ns[1] / 32767.0f;
96         nf[2] = ns[2] / 32767.0f;
97 }
98
99 static float vertexgroup_get_weight(MDeformVert *dvert, int index, int vgroup)
100 {
101         if(dvert && vgroup >= 0)
102         {
103                 int j;
104                 for(j = 0; j < dvert[index].totweight; j++)
105                         if(dvert[index].dw[j].def_nr == vgroup)
106                                 return dvert[index].dw[j].weight;
107         }
108         return 1.0;
109 }
110
111 /*
112  * BVH tree from mesh vertices
113  */
114 static BVHTree* bvhtree_from_mesh_verts(DerivedMesh *mesh)
115 {
116         int i;
117         int numVerts= mesh->getNumVerts(mesh);
118         MVert *vert     = mesh->getVertDataArray(mesh, CD_MVERT);
119
120         BVHTree *tree = BLI_bvhtree_new(numVerts, 0, 2, 6);
121         if(tree != NULL)
122         {
123                 for(i = 0; i < numVerts; i++)
124                         BLI_bvhtree_insert(tree, i, vert[i].co, 1);
125
126                 BLI_bvhtree_balance(tree);
127         }
128
129         return tree;
130 }
131
132 static BVHTree* bvhtree_from_mesh_tri(DerivedMesh *mesh)
133 {
134         int i;
135         int numFaces= mesh->getNumFaces(mesh), totFaces;
136         MVert *vert     = mesh->getVertDataArray(mesh, CD_MVERT);
137         MFace *face = mesh->getFaceDataArray(mesh, CD_MFACE);
138         BVHTree *tree= NULL;
139
140         /* Count needed faces */
141         for(totFaces=numFaces, i=0; i<numFaces; i++)
142                 if(face[i].v4) totFaces++;
143
144         /* Create a bvh-tree of the given target */
145         tree = BLI_bvhtree_new(totFaces, 0, 2, 6);
146         if(tree != NULL)
147         {
148                 for(i = 0; i < numFaces; i++)
149                 {
150                         float co[3][3];
151
152                         VECCOPY(co[0], vert[ face[i].v1 ].co);
153                         VECCOPY(co[1], vert[ face[i].v2 ].co);
154                         VECCOPY(co[2], vert[ face[i].v3 ].co);
155                         BLI_bvhtree_insert(tree, 2*i, co[0], 3);
156                         if(face[i].v4)
157                         {
158                                 /* second face is v1,v3,v4 */
159                                 VECCOPY(co[1], vert[ face[i].v3 ].co);
160                                 VECCOPY(co[2], vert[ face[i].v4 ].co);
161                                 BLI_bvhtree_insert(tree, 2*i+1, co[0], 3);
162                         }
163                 }
164
165                 BLI_bvhtree_balance(tree);
166         }
167
168         return tree;
169 }
170
171 static float mesh_tri_nearest_point(void *userdata, int index, const float *co, float *nearest)
172 {
173         DerivedMesh *mesh = (DerivedMesh*)(userdata);
174         MVert *vert     = (MVert*)mesh->getVertDataArray(mesh, CD_MVERT);
175         MFace *face = (MFace*)mesh->getFaceDataArray(mesh, CD_MFACE) + index/2;
176
177         if(index & 1)
178                 return nearest_point_in_tri_surface(co, vert[ face->v1 ].co, vert[ face->v3 ].co, vert[ face->v4 ].co, nearest);
179         else
180                 return nearest_point_in_tri_surface(co, vert[ face->v1 ].co, vert[ face->v2 ].co, vert[ face->v3 ].co, nearest);
181 }
182
183 /*
184  * Raytree from mesh
185  */
186 static MVert *raytree_from_mesh_verts = NULL;
187 static MFace *raytree_from_mesh_faces = NULL;
188
189 static int raytree_check_always(Isect *is, int ob, RayFace *face)
190 {
191         return TRUE;
192 }
193
194 static void raytree_from_mesh_get_coords(RayFace *face, float **v1, float **v2, float **v3, float **v4)
195 {
196         MFace *mface= raytree_from_mesh_faces + (int)face/2 - 1 ;
197
198         if(face == (RayFace*)(-1))
199         {
200                 *v1 = NULL;
201                 *v2 = NULL;
202                 *v3 = NULL;
203                 *v4 = NULL;
204                 return;
205         }
206
207         //Nasty quad splitting
208         if(((int)face) & 1)     // we want the 2 triangle of the quad
209         {
210                 *v1= raytree_from_mesh_verts[mface->v1].co;
211                 *v2= raytree_from_mesh_verts[mface->v3].co;
212                 *v3= raytree_from_mesh_verts[mface->v4].co;
213                 *v4= NULL;
214         }
215         else
216         {
217                 *v1= raytree_from_mesh_verts[mface->v1].co;
218                 *v2= raytree_from_mesh_verts[mface->v2].co;
219                 *v3= raytree_from_mesh_verts[mface->v3].co;
220                 *v4= NULL;
221         }
222 }
223
224 /*
225  * Creates a raytree from the given mesh
226  * No copy of the mesh is done, so it must exist and remain
227  * imutable as long the tree is intended to be used
228  *
229  * No more than 1 raytree can exist.. since this code uses a static variable
230  * to pass data to raytree_from_mesh_get_coords
231  */
232 static RayTree* raytree_create_from_mesh(DerivedMesh *mesh)
233 {
234         int i;
235         float min[3], max[3];
236
237         RayTree*tree= NULL;
238
239         int numFaces= mesh->getNumFaces(mesh);
240         MFace *face = mesh->getFaceDataArray(mesh, CD_MFACE);
241         int numVerts= mesh->getNumVerts(mesh);
242
243         //Initialize static vars
244         raytree_from_mesh_verts = mesh->getVertDataArray(mesh, CD_MVERT);
245         raytree_from_mesh_faces = face;
246
247
248         //calculate bounding box
249         INIT_MINMAX(min, max);
250
251         for(i=0; i<numVerts; i++)
252                 DO_MINMAX(raytree_from_mesh_verts[i].co, min, max);
253         
254         tree = RE_ray_tree_create(64, numFaces, min, max, raytree_from_mesh_get_coords, raytree_check_always, NULL, NULL);
255         if(tree == NULL)
256                 return NULL;
257
258         //Add faces to the RayTree (RayTree uses face=0, with some special value to setup things)
259         for(i=1; i<=numFaces; i++)
260         {
261                 RE_ray_tree_add_face(tree, 0, (RayFace*)(i*2) );
262
263                  //Theres some nasty thing with non-coplanar quads (that I can't find the issue)
264                  //so we split quads (an odd numbered face represents the second triangle of the quad)
265                 if(face[i-1].v4)
266                         RE_ray_tree_add_face(tree, 0, (RayFace*)(i*2+1));
267         }
268
269         RE_ray_tree_done(tree);
270
271         return tree;
272 }
273
274 static void free_raytree_from_mesh(RayTree *tree)
275 {
276         raytree_from_mesh_verts = NULL;
277         RE_ray_tree_free(tree);
278 }
279
280 /*
281  * Cast a ray on the specified direction
282  * Returns the distance the ray must travel until intersect something
283  * Returns FLT_MAX in case of nothing intersection
284  * if facenormal is given, it will be overwritted with the normal of the face the ray collided with
285  */
286 static float raytree_cast_ray(RayTree *tree, const float *coord, const float *direction, float *facenormal)
287 {
288         Isect isec;
289         float *v1, *v2, *v3, *v4;
290
291         /* Setup intersection */
292         isec.mode               = RE_RAY_MIRROR; /* We want closest intersection */
293         isec.lay                = -1;
294         isec.face_last  = NULL;
295         isec.faceorig   = (RayFace*)(-1);
296         isec.labda              = 1e10f;
297
298         VECCOPY(isec.start, coord);
299         VECCOPY(isec.vec, direction);
300         VECADDFAC(isec.end, isec.start, isec.vec, isec.labda);
301
302         if(!RE_ray_tree_intersect(tree, &isec))
303                 return FLT_MAX;
304
305         if(facenormal)
306         {
307                 raytree_from_mesh_get_coords( isec.face, &v1, &v2, &v3, &v4);
308                 CalcNormFloat(v1, v2, v3, facenormal);
309         }
310
311         isec.labda = ABS(isec.labda);
312         VECADDFAC(isec.end, isec.start, isec.vec, isec.labda);
313         return VecLenf((float*)coord, (float*)isec.end);
314 }
315
316 /*
317  * Returns the squared distance between two given points
318  */
319 static float squared_dist(const float *a, const float *b)
320 {
321         float tmp[3];
322         VECSUB(tmp, a, b);
323         return INPR(tmp, tmp);
324 }
325
326 /*
327  *
328  */
329 static void derivedmesh_mergeNearestPoints(DerivedMesh *dm, float mdist, BitSet skipVert)
330 {
331         if(mdist > 0.0f)
332         {
333                 int i, j, merged;
334                 int     numVerts = dm->getNumVerts(dm);
335                 int *translate_vert = MEM_mallocN( sizeof(int)*numVerts, "merge points array");
336
337                 MVert *vert = dm->getVertDataArray(dm, CD_MVERT);
338
339                 if(!translate_vert) return;
340
341                 merged = 0;
342                 for(i=0; i<numVerts; i++)
343                 {
344                         translate_vert[i] = i;
345
346                         if(skipVert && bitset_get(skipVert, i)) continue;
347
348                         for(j = 0; j<i; j++)
349                         {
350                                 if(skipVert && bitset_get(skipVert, j)) continue;
351                                 if(squared_dist(vert[i].co, vert[j].co) < mdist)
352                                 {
353                                         translate_vert[i] = j;
354                                         merged++;
355                                         break;
356                                 }
357                         }
358                 }
359
360                 //some vertexs were merged.. recalculate structure (edges and faces)
361                 if(merged > 0)
362                 {
363                         int     numFaces = dm->getNumFaces(dm);
364                         int freeVert;
365                         MFace *face = dm->getFaceDataArray(dm, CD_MFACE);
366
367
368                         //Adjust vertexs using the translation_table.. only translations to back indexs are allowed
369                         //which means t[i] <= i must always verify
370                         for(i=0, freeVert = 0; i<numVerts; i++)
371                         {
372                                 if(translate_vert[i] == i)
373                                 {
374                                         memcpy(&vert[freeVert], &vert[i], sizeof(*vert));
375                                         translate_vert[i] = freeVert++;
376                                 }
377                                 else translate_vert[i] = translate_vert[ translate_vert[i] ];
378                         }
379
380                         CDDM_lower_num_verts(dm, numVerts - merged);
381
382                         for(i=0; i<numFaces; i++)
383                         {
384                                 MFace *f = face+i;
385                                 f->v1 = translate_vert[f->v1];
386                                 f->v2 = translate_vert[f->v2];
387                                 f->v3 = translate_vert[f->v3];
388                                 //TODO be carefull with vertexs v4 being translated to 0
389                                 f->v4 = translate_vert[f->v4];
390                         }
391
392                         //TODO: maybe update edges could be done outside this function
393                         CDDM_calc_edges(dm);
394                         //CDDM_calc_normals(dm);
395                 }
396
397                 if(translate_vert) MEM_freeN( translate_vert );
398         }
399 }
400
401
402 /*
403  * This calculates the distance (in dir units) that the ray must travel to intersect plane
404  * It can return negative values
405  *
406  * TODO theres probably something like this on blender code
407  *
408  * Returns FLT_MIN in parallel case
409  */
410 static float ray_intersect_plane(const float *point, const float *dir, const float *plane_point, const float *plane_normal)
411 {
412                 float pp[3];
413                 float a, pp_dist;
414
415                 a = INPR(dir, plane_normal);
416
417                 if(fabs(a) < 1e-5f) return FLT_MIN;
418
419                 VECSUB(pp, point, plane_point);
420                 pp_dist = INPR(pp, plane_normal);
421
422                 return -pp_dist/a;
423 }
424
425 /*
426  * This calculates the distance from point to the plane
427  * Distance is negative if point is on the back side of plane
428  */
429 static float point_plane_distance(const float *point, const float *plane_point, const float *plane_normal)
430 {
431         float pp[3];
432         VECSUB(pp, point, plane_point);
433         return INPR(pp, plane_normal);
434 }
435 static float choose_nearest(const float v0[2], const float v1[2], const float point[2], float closest[2])
436 {
437         float d[2][2], sdist[2];
438         VECSUB2D(d[0], v0, point);
439         VECSUB2D(d[1], v1, point);
440
441         sdist[0] = d[0][0]*d[0][0] + d[0][1]*d[0][1];
442         sdist[1] = d[1][0]*d[1][0] + d[1][1]*d[1][1];
443
444         if(sdist[0] < sdist[1])
445         {
446                 if(closest)
447                         VECCOPY2D(closest, v0);
448                 return sdist[0];
449         }
450         else
451         {
452                 if(closest)
453                         VECCOPY2D(closest, v1);
454                 return sdist[1];
455         }
456 }
457 /*
458  * calculates the closest point between point-tri (2D)
459  * returns that tri must be right-handed
460  * Returns square distance
461  */
462 static float closest_point_in_tri2D(const float point[2], /*const*/ float tri[3][2], float closest[2])
463 {
464         float edge_di[2];
465         float v_point[2];
466         float proj[2];                                  //point projected over edge-dir, edge-normal (witouth normalized edge)
467         const float *v0 = tri[2], *v1;
468         float edge_slen, d;                             //edge squared length
469         int i;
470         const float *nearest_vertex = NULL;
471
472
473         //for each edge
474         for(i=0, v0=tri[2], v1=tri[0]; i < 3; v0=tri[i++], v1=tri[i])
475         {
476                 VECSUB2D(edge_di,    v1, v0);
477                 VECSUB2D(v_point, point, v0);
478
479                 proj[1] =  v_point[0]*edge_di[1] - v_point[1]*edge_di[0];       //dot product with edge normal
480
481                 //point inside this edge
482                 if(proj[1] < 0)
483                         continue;
484
485                 proj[0] = v_point[0]*edge_di[0] + v_point[1]*edge_di[1];
486
487                 //closest to this edge is v0
488                 if(proj[0] < 0)
489                 {
490                         if(nearest_vertex == NULL || nearest_vertex == v0)
491                                 nearest_vertex = v0;
492                         else
493                         {
494                                 //choose nearest
495                                 return choose_nearest(nearest_vertex, v0, point, closest);
496                         }
497                         i++;    //We can skip next edge
498                         continue;
499                 }
500
501                 edge_slen = edge_di[0]*edge_di[0] + edge_di[1]*edge_di[1];      //squared edge len
502                 //closest to this edge is v1
503                 if(proj[0] > edge_slen)
504                 {
505                         if(nearest_vertex == NULL || nearest_vertex == v1)
506                                 nearest_vertex = v1;
507                         else
508                         {
509                                 return choose_nearest(nearest_vertex, v1, point, closest);
510                         }
511                         continue;
512                 }
513
514                 //nearest is on this edge
515                 d= proj[1] / edge_slen;
516                 closest[0] = point[0] - edge_di[1] * d;
517                 closest[1] = point[1] + edge_di[0] * d;
518
519                 return proj[1]*proj[1]/edge_slen;
520         }
521
522         if(nearest_vertex)
523         {
524                 VECSUB2D(v_point, nearest_vertex, point);
525                 VECCOPY2D(closest, nearest_vertex);
526                 return v_point[0]*v_point[0] + v_point[1]*v_point[1];
527         }
528         else
529         {
530                 VECCOPY(closest, point);        //point is already inside
531                 return 0.0f;
532         }
533 }
534
535 /*
536  * Returns the square of the minimum distance between the point and a triangle surface
537  * If nearest is not NULL the nearest surface point is written on it
538  */
539 static float nearest_point_in_tri_surface(const float *point, const float *v0, const float *v1, const float *v2, float *nearest)
540 {
541         //Lets solve the 2D problem (closest point-tri)
542         float normal_dist, plane_sdist, plane_offset;
543         float du[3], dv[3], dw[3];      //orthogonal axis (du=(v0->v1), dw=plane normal)
544
545         float p_2d[2], tri_2d[3][2], nearest_2d[2];
546
547         CalcNormFloat((float*)v0, (float*)v1, (float*)v2, dw);
548
549         //point-plane distance and calculate axis
550         normal_dist = point_plane_distance(point, v0, dw);
551
552         VECSUB(du, v1, v0);
553         Normalize(du);
554         Crossf(dv, dw, du);
555         plane_offset = INPR(v0, dw);
556
557         //project stuff to 2d
558         tri_2d[0][0] = INPR(du, v0);
559         tri_2d[0][1] = INPR(dv, v0);
560
561         tri_2d[1][0] = INPR(du, v1);
562         tri_2d[1][1] = INPR(dv, v1);
563
564         tri_2d[2][0] = INPR(du, v2);
565         tri_2d[2][1] = INPR(dv, v2);
566
567         p_2d[0] = INPR(du, point);
568         p_2d[1] = INPR(dv, point);
569
570         //we always have a right-handed tri
571         //this should always happen because of the way normal is calculated
572         plane_sdist = closest_point_in_tri2D(p_2d, tri_2d, nearest_2d);
573
574         //project back to 3d
575         if(nearest)
576         {
577                 nearest[0] = du[0]*nearest_2d[0] + dv[0] * nearest_2d[1] + dw[0] * plane_offset;
578                 nearest[1] = du[1]*nearest_2d[0] + dv[1] * nearest_2d[1] + dw[1] * plane_offset;
579                 nearest[2] = du[2]*nearest_2d[0] + dv[2] * nearest_2d[1] + dw[2] * plane_offset;
580         }
581
582         return plane_sdist + normal_dist*normal_dist;
583 }
584
585
586
587 /*
588  * Shrink to nearest surface point on target mesh
589  */
590 static void bruteforce_shrinkwrap_calc_nearest_surface_point(DerivedMesh *target, float *co, float *unused)
591 {
592         float minDist = FLT_MAX;
593         float orig_co[3];
594
595         int i;
596         int     numFaces = target->getNumFaces(target);
597         MVert *vert = target->getVertDataArray(target, CD_MVERT);
598         MFace *face = target->getFaceDataArray(target, CD_MFACE);
599
600         VECCOPY(orig_co, co);   
601
602         for (i = 0; i < numFaces; i++)
603         {
604                 float *v0, *v1, *v2, *v3;
605
606                 v0 = vert[ face[i].v1 ].co;
607                 v1 = vert[ face[i].v2 ].co;
608                 v2 = vert[ face[i].v3 ].co;
609                 v3 = face[i].v4 ? vert[ face[i].v4 ].co : 0;
610
611                 while(v2)
612                 {
613                         float dist;
614                         float tmp[3];
615
616                         dist = nearest_point_in_tri_surface(orig_co, v0, v1, v2, tmp);
617
618                         if(dist < minDist)
619                         {
620                                 minDist = dist;
621                                 VECCOPY(co, tmp);
622                         }
623
624                         v1 = v2;
625                         v2 = v3;
626                         v3 = 0;
627                 }
628         }
629 }
630
631 /*
632  * Projects the vertex on the normal direction over the target mesh
633  */
634 static void bruteforce_shrinkwrap_calc_normal_projection(DerivedMesh *target, float *co, float *vnormal)
635 {
636         //TODO: this should use raycast code probably existent in blender
637         float minDist = FLT_MAX;
638         float orig_co[3];
639
640         int i;
641         int     numFaces = target->getNumFaces(target);
642         MVert *vert = target->getVertDataArray(target, CD_MVERT);
643         MFace *face = target->getFaceDataArray(target, CD_MFACE);
644
645         VECCOPY(orig_co, co);
646
647         for (i = 0; i < numFaces; i++)
648         {
649                 float *v0, *v1, *v2, *v3;
650
651                 v0 = vert[ face[i].v1 ].co;
652                 v1 = vert[ face[i].v2 ].co;
653                 v2 = vert[ face[i].v3 ].co;
654                 v3 = face[i].v4 ? vert[ face[i].v4 ].co : 0;
655
656                 while(v2)
657                 {
658                         float dist;
659                         float pnormal[3];
660
661                         CalcNormFloat(v0, v1, v2, pnormal);
662                         dist =  ray_intersect_plane(orig_co, vnormal, v0, pnormal);
663
664                         if(fabs(dist) < minDist)
665                         {
666                                 float tmp[3], nearest[3];
667                                 VECADDFAC(tmp, orig_co, vnormal, dist);
668
669                                 if( fabs(nearest_point_in_tri_surface(tmp, v0, v1, v2, nearest)) < 0.0001)
670                                 {
671                                         minDist = fabs(dist);
672                                         VECCOPY(co, nearest);
673                                 }
674                         }
675                         v1 = v2;
676                         v2 = v3;
677                         v3 = 0;
678                 }
679         }
680 }
681
682 /*
683  * Shrink to nearest vertex on target mesh
684  */
685 static void bruteforce_shrinkwrap_calc_nearest_vertex(DerivedMesh *target, float *co, float *unused)
686 {
687         float minDist = FLT_MAX;
688         float orig_co[3];
689
690         int i;
691         int     numVerts = target->getNumVerts(target);
692         MVert *vert = target->getVertDataArray(target, CD_MVERT);
693
694         VECCOPY(orig_co, co);
695
696         for (i = 0; i < numVerts; i++)
697         {
698                 float sdist = squared_dist( orig_co, vert[i].co);
699                 
700                 if(sdist < minDist)
701                 {
702                         minDist = sdist;
703                         VECCOPY(co, vert[i].co);
704                 }
705         }
706 }
707
708
709 static void shrinkwrap_calc_foreach_vertex(ShrinkwrapCalcData *calc, Shrinkwrap_ForeachVertexCallback callback)
710 {
711         int i;
712         int vgroup              = get_named_vertexgroup_num(calc->ob, calc->smd->vgroup_name);
713         int     numVerts        = 0;
714
715         MDeformVert *dvert = NULL;
716         MVert           *vert  = NULL;
717
718         numVerts = calc->final->getNumVerts(calc->final);
719         dvert = calc->final->getVertDataArray(calc->final, CD_MDEFORMVERT);
720         vert  = calc->final->getVertDataArray(calc->final, CD_MVERT);
721
722         //Shrink (calculate each vertex final position)
723         for(i = 0; i<numVerts; i++)
724         {
725                 float weight = vertexgroup_get_weight(dvert, i, vgroup);
726
727                 float orig[3], final[3]; //Coords relative to target
728                 float normal[3];
729                 float dist;
730
731                 if(weight == 0.0f) continue;    //Skip vertexs where we have no influence
732
733                 VecMat4MulVecfl(orig, calc->local2target, vert[i].co);
734                 VECCOPY(final, orig);
735
736                 //We also need to apply the rotation to normal
737                 if(calc->smd->shrinkType == MOD_SHRINKWRAP_NORMAL)
738                 {
739                         normal_short2float(vert[i].no, normal);
740                         Mat4Mul3Vecfl(calc->local2target, normal);
741                         Normalize(normal);      //Watch out for scaling (TODO: do we really needed a unit-len normal?)
742                 }
743                 (callback)(calc->target, final, normal);
744
745                 VecMat4MulVecfl(final, calc->target2local, final);
746
747                 dist = VecLenf(vert[i].co, final);
748                 if(dist > 1e-5) weight *= (dist - calc->keptDist)/dist;
749                 VecLerpf(vert[i].co, vert[i].co, final, weight);        //linear interpolation
750         }
751 }
752
753
754 /*
755  * This function removes Unused faces, vertexs and edges from calc->target
756  *
757  * This function may modify calc->final. As so no data retrieved from
758  * it before the call to this function  can be considered valid
759  * In case it creates a new DerivedMesh, the old calc->final is freed
760  */
761 //TODO memory checks on allocs
762 static void shrinkwrap_removeUnused(ShrinkwrapCalcData *calc)
763 {
764         int i, t;
765
766         DerivedMesh *old = calc->final, *new = NULL;
767         MFace *new_face = NULL;
768         MVert *new_vert  = NULL;
769
770         int numVerts= old->getNumVerts(old);
771         MVert *vert = old->getVertDataArray(old, CD_MVERT);
772
773         int     numFaces= old->getNumFaces(old);
774         MFace *face = old->getFaceDataArray(old, CD_MFACE);
775
776         BitSet moved_verts = calc->moved;
777
778         //Arrays to translate to new vertexs indexs
779         int *vert_index = (int*)MEM_callocN(sizeof(int)*(numVerts), "shrinkwrap used verts");
780         BitSet used_faces = bitset_new(numFaces, "shrinkwrap used faces");
781         int numUsedFaces = 0;
782
783
784         //calculate which vertexs need to be used
785         //even unmoved vertices might need to be used if theres a face that needs it
786         //calc real number of faces, and vertices
787         //Count used faces
788         for(i=0; i<numFaces; i++)
789         {
790                 char res = 0;
791                 if(bitset_get(moved_verts, face[i].v1)) res++;
792                 if(bitset_get(moved_verts, face[i].v2)) res++;
793                 if(bitset_get(moved_verts, face[i].v3)) res++;
794                 if(face[i].v4 && bitset_get(moved_verts, face[i].v4)) res++;
795
796                 //Ignore a face were not a single vertice moved
797                 if(res == 0) continue;
798
799                 //Only 1 vertice moved.. (if its a quad.. remove the vertice oposite to it)
800                 if(res == 1 && face[i].v4)
801                 {
802                         if(bitset_get(moved_verts, face[i].v1))
803                         {
804                                 //remove vertex 3
805                                 face[i].v3 = face[i].v4;
806                         }
807                         else if(bitset_get(moved_verts, face[i].v2))
808                         {
809                                 //remove vertex 4
810                         }
811                         else if(bitset_get(moved_verts, face[i].v3))
812                         {
813                                 //remove vertex 1
814                                 face[i].v1 = face[i].v4;
815                         }
816                         else if(bitset_get(moved_verts, face[i].v4))
817                         {
818                                 //remove vertex 2
819                                 face[i].v2 = face[i].v3;
820                                 face[i].v3 = face[i].v4;
821                         }
822
823                         face[i].v4 = 0; //this quad turned on a tri
824                 }
825
826                 bitset_set(used_faces, i);      //Mark face to maintain
827                 numUsedFaces++;
828
829                 //Mark vertices are needed
830                 vert_index[face[i].v1] = 1;
831                 vert_index[face[i].v2] = 1;
832                 vert_index[face[i].v3] = 1;
833                 if(face[i].v4) vert_index[face[i].v4] = 1;
834         }
835
836         //DP: Accumulate vertexs indexs.. (will calculate the new vertex index with a 1 offset)
837         for(i=1; i<numVerts; i++)
838                 vert_index[i] += vert_index[i-1];
839                 
840         
841         //Start creating the clean mesh
842         new = CDDM_new(vert_index[numVerts-1], 0, numUsedFaces);
843
844         //Copy vertexs (unused are are removed)
845         new_vert  = new->getVertDataArray(new, CD_MVERT);
846         for(i=0, t=0; i<numVerts; i++)
847         {
848                 
849                 if(vert_index[i] != t)
850                 {
851                         t = vert_index[i];
852                         memcpy(new_vert++, vert+i, sizeof(MVert));
853
854                         if(bitset_get(moved_verts, i))
855                                 bitset_set(moved_verts, t-1);
856                         else
857                                 bitset_unset(moved_verts, t-1);
858                 }
859         }
860
861         //Copy faces
862         new_face = new->getFaceDataArray(new, CD_MFACE);
863         for(i=0, t=0; i<numFaces; i++)
864         {
865                 if(bitset_get(used_faces, i))
866                 {
867                         memcpy(new_face, face+i, sizeof(MFace));
868                         //update vertices indexs
869                         new_face->v1 = vert_index[new_face->v1]-1;
870                         new_face->v2 = vert_index[new_face->v2]-1;
871                         new_face->v3 = vert_index[new_face->v3]-1;
872                         if(new_face->v4)
873                         {
874                                 new_face->v4 = vert_index[new_face->v4]-1;
875
876                                 //Ups translated vertex ended on 0 .. TODO fix this
877                                 if(new_face->v4 == 0)
878                                 {
879                                 }
880                         }                       
881                         new_face++;
882                 }
883         }
884
885         //Free memory
886         bitset_free(used_faces);
887         MEM_freeN(vert_index);
888         old->release(old);
889
890         //Update edges
891         CDDM_calc_edges(new);
892         CDDM_calc_normals(new);
893
894         calc->final = new;
895 }
896
897
898 /* Main shrinkwrap function */
899 DerivedMesh *shrinkwrapModifier_do(ShrinkwrapModifierData *smd, Object *ob, DerivedMesh *dm, int useRenderParams, int isFinalCalc)
900 {
901
902         ShrinkwrapCalcData calc;
903         memset(&calc, 0, sizeof(calc));
904
905         //Init Shrinkwrap calc data
906         calc.smd = smd;
907
908         calc.ob = ob;
909         calc.original = dm;
910         calc.final = CDDM_copy(calc.original);
911
912         if(!calc.final)
913         {
914                 OUT_OF_MEMORY();
915                 return dm;
916         }
917
918         if(smd->target)
919         {
920                 calc.target = (DerivedMesh *)smd->target->derivedFinal;
921
922                 if(!calc.target)
923                 {
924                         printf("Target derived mesh is null! :S\n");
925                 }
926
927                 //TODO should we reduce the number of matrix mults? by choosing applying matrixs to target or to derived mesh?
928                 //Calculate matrixs for local <-> target
929                 Mat4Invert (smd->target->imat, smd->target->obmat);     //inverse is outdated
930                 Mat4MulSerie(calc.local2target, smd->target->imat, ob->obmat, 0, 0, 0, 0, 0, 0);
931                 Mat4Invert(calc.target2local, calc.local2target);
932         
933                 calc.keptDist = smd->keptDist;  //TODO: smd->keptDist is in global units.. must change to local
934         }
935
936         //Projecting target defined - lets work!
937         if(calc.target)
938         {
939 /*
940                 printf("Shrinkwrap (%s)%d over (%s)%d\n",
941                         calc.ob->id.name,                       calc.final->getNumVerts(calc.final),
942                         calc.smd->target->id.name,      calc.target->getNumVerts(calc.target)
943                 );
944 */
945
946                 switch(smd->shrinkType)
947                 {
948                         case MOD_SHRINKWRAP_NEAREST_SURFACE:
949                                 BENCH(shrinkwrap_calc_nearest_surface_point(&calc));
950 //                              BENCH(shrinkwrap_calc_foreach_vertex(&calc, bruteforce_shrinkwrap_calc_nearest_surface_point));
951                         break;
952
953                         case MOD_SHRINKWRAP_NORMAL:
954                                 BENCH(shrinkwrap_calc_normal_projection(&calc));
955 //                              BENCH(shrinkwrap_calc_foreach_vertex(&calc, bruteforce_shrinkwrap_calc_normal_projection));
956                         break;
957
958                         case MOD_SHRINKWRAP_NEAREST_VERTEX:
959                                 BENCH(shrinkwrap_calc_nearest_vertex(&calc));
960 //                              BENCH(shrinkwrap_calc_foreach_vertex(&calc, bruteforce_shrinkwrap_calc_nearest_vertex));
961                         break;
962                 }
963
964         }
965
966         if(calc.moved)
967         {
968                 //Destroy faces, edges and stuff
969                 shrinkwrap_removeUnused(&calc);
970
971                 if(calc.moved)
972                         derivedmesh_mergeNearestPoints(calc.final, calc.smd->mergeDist, calc.moved);
973         }
974
975         CDDM_calc_normals(calc.final);
976
977         //clean memory
978         if(calc.moved)
979                 bitset_free(calc.moved);
980
981
982         return calc.final;
983 }
984
985
986 /*
987  * Shrinkwrap to the nearest vertex
988  *
989  * it builds a kdtree of vertexs we can attach to and then
990  * for each vertex on performs a nearest vertex search on the tree
991  */
992 void shrinkwrap_calc_nearest_vertex(ShrinkwrapCalcData *calc)
993 {
994         int i;
995         int vgroup              = get_named_vertexgroup_num(calc->ob, calc->smd->vgroup_name);
996         float tmp_co[3];
997
998         BVHTree *tree   = NULL;
999         BVHTreeNearest nearest;
1000
1001         int     numVerts;
1002         MVert *vert = NULL;
1003         MDeformVert *dvert = NULL;
1004
1005         BENCH_VAR(query);
1006
1007
1008         BENCH(tree = bvhtree_from_mesh_verts(calc->target));
1009         if(tree == NULL) return OUT_OF_MEMORY();
1010
1011         //Setup nearest
1012         nearest.index = -1;
1013         nearest.dist = FLT_MAX;
1014
1015
1016         //Find the nearest vertex 
1017         numVerts= calc->final->getNumVerts(calc->final);
1018         vert    = calc->final->getVertDataArray(calc->final, CD_MVERT); 
1019         dvert   = calc->final->getVertDataArray(calc->final, CD_MDEFORMVERT);
1020
1021         BENCH_BEGIN(query);
1022         for(i=0; i<numVerts; i++)
1023         {
1024                 int index;
1025                 float weight = vertexgroup_get_weight(dvert, i, vgroup);
1026                 if(weight == 0.0f) continue;
1027
1028                 VecMat4MulVecfl(tmp_co, calc->local2target, vert[i].co);
1029
1030                 if(nearest.index != -1)
1031                 {
1032                         nearest.dist = squared_dist(tmp_co, nearest.nearest);
1033                 }
1034                 else nearest.dist = FLT_MAX;
1035
1036                 index = BLI_bvhtree_find_nearest(tree, tmp_co, &nearest, NULL, NULL);
1037
1038                 if(index != -1)
1039                 {
1040                         float dist;
1041
1042                         VecMat4MulVecfl(tmp_co, calc->target2local, nearest.nearest);
1043                         dist = VecLenf(vert[i].co, tmp_co);
1044                         if(dist > 1e-5) weight *= (dist - calc->keptDist)/dist;
1045                         VecLerpf(vert[i].co, vert[i].co, tmp_co, weight);       //linear interpolation
1046                 }
1047         }
1048         BENCH_END(query);
1049         BENCH_REPORT(query);
1050
1051         BLI_bvhtree_free(tree);
1052 }
1053
1054 /*
1055  * Shrinkwrap projecting vertexs allong their normals over the target
1056  *
1057  * it builds a RayTree from the target mesh and then performs a
1058  * raycast for each vertex (ray direction = normal)
1059  */
1060 void shrinkwrap_calc_normal_projection(ShrinkwrapCalcData *calc)
1061 {
1062         int i;
1063         int vgroup              = get_named_vertexgroup_num(calc->ob, calc->smd->vgroup_name);
1064         char use_normal = calc->smd->shrinkOpts;
1065         RayTree *target = NULL;
1066
1067         int     numVerts;
1068         MVert *vert = NULL;
1069         MDeformVert *dvert = NULL;
1070         float tmp_co[3], tmp_no[3];
1071
1072         if( (use_normal & (MOD_SHRINKWRAP_ALLOW_INVERTED_NORMAL | MOD_SHRINKWRAP_ALLOW_DEFAULT_NORMAL)) == 0)
1073                 return; //Nothing todo
1074
1075         //setup raytracing
1076         target = raytree_create_from_mesh(calc->target);
1077         if(target == NULL) return OUT_OF_MEMORY();
1078
1079
1080
1081         //Project each vertex along normal
1082         numVerts= calc->final->getNumVerts(calc->final);
1083         vert    = calc->final->getVertDataArray(calc->final, CD_MVERT); 
1084         dvert   = calc->final->getVertDataArray(calc->final, CD_MDEFORMVERT);
1085
1086         if(calc->smd->shrinkOpts & MOD_SHRINKWRAP_REMOVE_UNPROJECTED_FACES)
1087                 calc->moved = bitset_new(numVerts, "shrinkwrap bitset data");
1088
1089         for(i=0; i<numVerts; i++)
1090         {
1091                 float dist = FLT_MAX;
1092                 float weight = vertexgroup_get_weight(dvert, i, vgroup);
1093                 float face_normal[3];
1094                 if(weight == 0.0f) continue;
1095
1096                 //Transform coordinates local->target
1097                 VecMat4MulVecfl(tmp_co, calc->local2target, vert[i].co);
1098
1099                 normal_short2float(vert[i].no, tmp_no);
1100                 Mat4Mul3Vecfl(calc->local2target, tmp_no);      //Watch out for scaling on normal
1101                 Normalize(tmp_no);                                                      //(TODO: do we really needed a unit-len normal? and we could know the scale factor before hand?)
1102
1103
1104                 if(use_normal & MOD_SHRINKWRAP_ALLOW_DEFAULT_NORMAL)
1105                 {
1106                         dist = raytree_cast_ray(target, tmp_co, tmp_no, face_normal);
1107
1108                         if((calc->smd->shrinkOpts & MOD_SHRINKWRAP_CULL_TARGET_FRONTFACE) && INPR(tmp_no, face_normal) < 0)
1109                                 dist = FLT_MAX;
1110                         if((calc->smd->shrinkOpts & MOD_SHRINKWRAP_CULL_TARGET_BACKFACE) && INPR(tmp_no, face_normal) > 0)
1111                                 dist = FLT_MAX;
1112                 }
1113
1114                 normal_short2float(vert[i].no, tmp_no);
1115                 Mat4Mul3Vecfl(calc->local2target, tmp_no);      //Watch out for scaling on normal
1116                 Normalize(tmp_no);                                                      //(TODO: do we really needed a unit-len normal? and we could know the scale factor before hand?)
1117
1118                 if(use_normal & MOD_SHRINKWRAP_ALLOW_INVERTED_NORMAL)
1119                 {
1120                         float inv[3]; // = {-tmp_no[0], -tmp_no[1], -tmp_no[2]};
1121                         float tdist;
1122
1123                         inv[0] = -tmp_no[0];
1124                         inv[1] = -tmp_no[1];
1125                         inv[2] = -tmp_no[2];
1126
1127                         tdist = raytree_cast_ray(target, tmp_co, inv, 0);
1128
1129                         if((calc->smd->shrinkOpts & MOD_SHRINKWRAP_CULL_TARGET_FRONTFACE) && INPR(tmp_no, face_normal) < 0)
1130                                 tdist = FLT_MAX;
1131                         if((calc->smd->shrinkOpts & MOD_SHRINKWRAP_CULL_TARGET_BACKFACE) && INPR(tmp_no, face_normal) > 0)
1132                                 tdist = FLT_MAX;
1133
1134                         if(ABS(tdist) < ABS(dist))
1135                                 dist = -tdist;
1136                 }
1137
1138                 if(ABS(dist) != FLT_MAX)
1139                 {
1140                         float dist_t;
1141
1142                         VECADDFAC(tmp_co, tmp_co, tmp_no, dist);
1143                         VecMat4MulVecfl(tmp_co, calc->target2local, tmp_co);
1144
1145                         dist_t = VecLenf(vert[i].co, tmp_co);
1146                         if(dist_t > 1e-5) weight *= (dist_t - calc->keptDist)/dist_t;
1147                         VecLerpf(vert[i].co, vert[i].co, tmp_co, weight);       //linear interpolation
1148
1149                         if(calc->moved)
1150                                 bitset_set(calc->moved, i);
1151                 }
1152
1153         }
1154
1155         free_raytree_from_mesh(target);
1156 }
1157
1158 /*
1159  * Shrinkwrap moving vertexs to the nearest surface point on the target
1160  *
1161  * it builds a BVHTree from the target mesh and then performs a
1162  * NN matchs for each vertex
1163  */
1164 void shrinkwrap_calc_nearest_surface_point(ShrinkwrapCalcData *calc)
1165 {
1166         int i;
1167         int vgroup              = get_named_vertexgroup_num(calc->ob, calc->smd->vgroup_name);
1168         float tmp_co[3];
1169
1170         BVHTree *tree   = NULL;
1171         BVHTreeNearest nearest;
1172
1173         int     numVerts;
1174         MVert *vert = NULL;
1175         MDeformVert *dvert = NULL;
1176
1177
1178         //Create a bvh-tree of the given target
1179         tree = bvhtree_from_mesh_tri(calc->target);
1180         if(tree == NULL) return OUT_OF_MEMORY();
1181
1182         //Setup nearest
1183         nearest.index = -1;
1184         nearest.dist = FLT_MAX;
1185
1186
1187         //Find the nearest vertex 
1188         numVerts= calc->final->getNumVerts(calc->final);
1189         vert    = calc->final->getVertDataArray(calc->final, CD_MVERT); 
1190         dvert   = calc->final->getVertDataArray(calc->final, CD_MDEFORMVERT);
1191
1192         for(i=0; i<numVerts; i++)
1193         {
1194                 int index;
1195                 float weight = vertexgroup_get_weight(dvert, i, vgroup);
1196                 if(weight == 0.0f) continue;
1197
1198                 VecMat4MulVecfl(tmp_co, calc->local2target, vert[i].co);
1199
1200                 if(nearest.index != -1)
1201                 {
1202                         nearest.dist = squared_dist(tmp_co, nearest.nearest);
1203                 }
1204                 else nearest.dist = FLT_MAX;
1205
1206                 index = BLI_bvhtree_find_nearest(tree, tmp_co, &nearest, mesh_tri_nearest_point, calc->target);
1207
1208                 if(index != -1)
1209                 {
1210                         float dist;
1211
1212                         VecMat4MulVecfl(tmp_co, calc->target2local, nearest.nearest);
1213                         dist = VecLenf(vert[i].co, tmp_co);
1214                         if(dist > 1e-5) weight *= (dist - calc->keptDist)/dist;
1215                         VecLerpf(vert[i].co, vert[i].co, tmp_co, weight);       //linear interpolation
1216                 }
1217         }
1218
1219         BLI_bvhtree_free(tree);
1220 }
1221