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