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