style cleanup: follow style guide for formatting of if/for/while loops, and else...
[blender.git] / source / blender / editors / armature / meshlaplacian.c
1 /*
2  * ***** BEGIN GPL LICENSE BLOCK *****
3  *
4  * This program is free software; you can redistribute it and/or
5  * modify it under the terms of the GNU General Public License
6  * as published by the Free Software Foundation; either version 2
7  * of the License, or (at your option) any later version.
8  *
9  * This program is distributed in the hope that it will be useful,
10  * but WITHOUT ANY WARRANTY; without even the implied warranty of
11  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
12  * GNU General Public License for more details.
13  *
14  * You should have received a copy of the GNU General Public License
15  * along with this program; if not, write to the Free Software Foundation,
16  * Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
17  *
18  * The Original Code is Copyright (C) 2001-2002 by NaN Holding BV.
19  * All rights reserved.
20  *
21  * The Original Code is: all of this file.
22  *
23  * Contributor(s): none yet.
24  *
25  * ***** END GPL LICENSE BLOCK *****
26  * meshlaplacian.c: Algorithms using the mesh laplacian.
27  */
28
29 /** \file blender/editors/armature/meshlaplacian.c
30  *  \ingroup edarmature
31  */
32
33
34 #include <math.h>
35 #include <string.h>
36
37 #include "MEM_guardedalloc.h"
38
39 #include "DNA_object_types.h"
40 #include "DNA_mesh_types.h"
41 #include "DNA_meshdata_types.h"
42 #include "DNA_scene_types.h"
43
44 #include "BLI_utildefines.h"
45 #include "BLI_math.h"
46 #include "BLI_edgehash.h"
47 #include "BLI_memarena.h"
48 #include "BLI_string.h"
49
50 #include "BLF_translation.h"
51
52 #include "BKE_DerivedMesh.h"
53 #include "BKE_modifier.h"
54 #include "BKE_mesh.h"
55
56
57 #ifdef RIGID_DEFORM
58 #include "BLI_polardecomp.h"
59 #endif
60
61 #include "ONL_opennl.h"
62
63 #include "BLO_sys_types.h" // for intptr_t support
64
65 #include "ED_mesh.h"
66 #include "ED_armature.h"
67
68 #include "meshlaplacian.h"
69
70
71 /* ************* XXX *************** */
72 static void waitcursor(int UNUSED(val)) {}
73 static void progress_bar(int UNUSED(dummy_val), const char *UNUSED(dummy)) {}
74 static void start_progress_bar(void) {}
75 static void end_progress_bar(void) {}
76 static void error(const char *str) { printf("error: %s\n", str); }
77 /* ************* XXX *************** */
78
79
80 /************************** Laplacian System *****************************/
81
82 struct LaplacianSystem {
83         NLContext context;      /* opennl context */
84
85         int totvert, totface;
86
87         float **verts;                  /* vertex coordinates */
88         float *varea;                   /* vertex weights for laplacian computation */
89         char *vpinned;                  /* vertex pinning */
90         int (*faces)[3];                /* face vertex indices */
91         float (*fweights)[3];   /* cotangent weights per face */
92
93         int areaweights;                /* use area in cotangent weights? */
94         int storeweights;               /* store cotangent weights in fweights */
95         int nlbegun;                    /* nlBegin(NL_SYSTEM/NL_MATRIX) done */
96
97         EdgeHash *edgehash;             /* edge hash for construction */
98
99         struct HeatWeighting {
100                 MFace *mface;
101                 int totvert;
102                 int totface;
103                 float (*verts)[3];      /* vertex coordinates */
104                 float (*vnors)[3];      /* vertex normals */
105
106                 float (*root)[3];       /* bone root */
107                 float (*tip)[3];        /* bone tip */
108                 float (*source)[3]; /* vertex source */
109                 int numsource;
110
111                 float *H;                       /* diagonal H matrix */
112                 float *p;                       /* values from all p vectors */
113                 float *mindist;         /* minimum distance to a bone for all vertices */
114                 
115                 BVHTree   *bvhtree;     /* ray tracing acceleration structure */
116                 MFace     **vface;      /* a face that the vertex belongs to */
117         } heat;
118
119 #ifdef RIGID_DEFORM
120         struct RigidDeformation {
121                 EditMesh *mesh;
122
123                 float (*R)[3][3];
124                 float (*rhs)[3];
125                 float (*origco)[3];
126                 int thrownerror;
127         } rigid;
128 #endif
129 };
130
131 /* Laplacian matrix construction */
132
133 /* Computation of these weights for the laplacian is based on:
134  * "Discrete Differential-Geometry Operators for Triangulated 2-Manifolds",
135  * Meyer et al, 2002. Section 3.5, formula (8).
136  * 
137  * We do it a bit different by going over faces instead of going over each
138  * vertex and adjacent faces, since we don't store this adjacency. Also, the
139  * formulas are tweaked a bit to work for non-manifold meshes. */
140
141 static void laplacian_increase_edge_count(EdgeHash *edgehash, int v1, int v2)
142 {
143         void **p = BLI_edgehash_lookup_p(edgehash, v1, v2);
144
145         if (p)
146                 *p = (void*)((intptr_t)*p + (intptr_t)1);
147         else
148                 BLI_edgehash_insert(edgehash, v1, v2, (void*)(intptr_t)1);
149 }
150
151 static int laplacian_edge_count(EdgeHash *edgehash, int v1, int v2)
152 {
153         return (int)(intptr_t)BLI_edgehash_lookup(edgehash, v1, v2);
154 }
155
156 static float cotan_weight(float *v1, float *v2, float *v3)
157 {
158         float a[3], b[3], c[3], clen;
159
160         sub_v3_v3v3(a, v2, v1);
161         sub_v3_v3v3(b, v3, v1);
162         cross_v3_v3v3(c, a, b);
163
164         clen = len_v3(c);
165
166         if (clen == 0.0f)
167                 return 0.0f;
168         
169         return dot_v3v3(a, b)/clen;
170 }
171
172 static void laplacian_triangle_area(LaplacianSystem *sys, int i1, int i2, int i3)
173 {
174         float t1, t2, t3, len1, len2, len3, area;
175         float *varea= sys->varea, *v1, *v2, *v3;
176         int obtuse = 0;
177
178         v1= sys->verts[i1];
179         v2= sys->verts[i2];
180         v3= sys->verts[i3];
181
182         t1= cotan_weight(v1, v2, v3);
183         t2= cotan_weight(v2, v3, v1);
184         t3= cotan_weight(v3, v1, v2);
185
186         if     (angle_v3v3v3(v2, v1, v3) > DEG2RADF(90.0f)) obtuse= 1;
187         else if (angle_v3v3v3(v1, v2, v3) > DEG2RADF(90.0f)) obtuse= 2;
188         else if (angle_v3v3v3(v1, v3, v2) > DEG2RADF(90.0f)) obtuse= 3;
189
190         if (obtuse > 0) {
191                 area= area_tri_v3(v1, v2, v3);
192
193                 varea[i1] += (obtuse == 1)? area: area*0.5f;
194                 varea[i2] += (obtuse == 2)? area: area*0.5f;
195                 varea[i3] += (obtuse == 3)? area: area*0.5f;
196         }
197         else {
198                 len1= len_v3v3(v2, v3);
199                 len2= len_v3v3(v1, v3);
200                 len3= len_v3v3(v1, v2);
201
202                 t1 *= len1*len1;
203                 t2 *= len2*len2;
204                 t3 *= len3*len3;
205
206                 varea[i1] += (t2 + t3)*0.25f;
207                 varea[i2] += (t1 + t3)*0.25f;
208                 varea[i3] += (t1 + t2)*0.25f;
209         }
210 }
211
212 static void laplacian_triangle_weights(LaplacianSystem *sys, int f, int i1, int i2, int i3)
213 {
214         float t1, t2, t3;
215         float *varea= sys->varea, *v1, *v2, *v3;
216
217         v1= sys->verts[i1];
218         v2= sys->verts[i2];
219         v3= sys->verts[i3];
220
221         /* instead of *0.5 we divided by the number of faces of the edge, it still
222          * needs to be verified that this is indeed the correct thing to do! */
223         t1= cotan_weight(v1, v2, v3)/laplacian_edge_count(sys->edgehash, i2, i3);
224         t2= cotan_weight(v2, v3, v1)/laplacian_edge_count(sys->edgehash, i3, i1);
225         t3= cotan_weight(v3, v1, v2)/laplacian_edge_count(sys->edgehash, i1, i2);
226
227         nlMatrixAdd(i1, i1, (t2+t3)*varea[i1]);
228         nlMatrixAdd(i2, i2, (t1+t3)*varea[i2]);
229         nlMatrixAdd(i3, i3, (t1+t2)*varea[i3]);
230
231         nlMatrixAdd(i1, i2, -t3*varea[i1]);
232         nlMatrixAdd(i2, i1, -t3*varea[i2]);
233
234         nlMatrixAdd(i2, i3, -t1*varea[i2]);
235         nlMatrixAdd(i3, i2, -t1*varea[i3]);
236
237         nlMatrixAdd(i3, i1, -t2*varea[i3]);
238         nlMatrixAdd(i1, i3, -t2*varea[i1]);
239
240         if (sys->storeweights) {
241                 sys->fweights[f][0]= t1*varea[i1];
242                 sys->fweights[f][1]= t2*varea[i2];
243                 sys->fweights[f][2]= t3*varea[i3];
244         }
245 }
246
247 static LaplacianSystem *laplacian_system_construct_begin(int totvert, int totface, int lsq)
248 {
249         LaplacianSystem *sys;
250
251         sys= MEM_callocN(sizeof(LaplacianSystem), "LaplacianSystem");
252
253         sys->verts= MEM_callocN(sizeof(float*)*totvert, "LaplacianSystemVerts");
254         sys->vpinned= MEM_callocN(sizeof(char)*totvert, "LaplacianSystemVpinned");
255         sys->faces= MEM_callocN(sizeof(int)*3*totface, "LaplacianSystemFaces");
256
257         sys->totvert= 0;
258         sys->totface= 0;
259
260         sys->areaweights= 1;
261         sys->storeweights= 0;
262
263         /* create opennl context */
264         nlNewContext();
265         nlSolverParameteri(NL_NB_VARIABLES, totvert);
266         if (lsq)
267                 nlSolverParameteri(NL_LEAST_SQUARES, NL_TRUE);
268
269         sys->context= nlGetCurrent();
270
271         return sys;
272 }
273
274 void laplacian_add_vertex(LaplacianSystem *sys, float *co, int pinned)
275 {
276         sys->verts[sys->totvert]= co;
277         sys->vpinned[sys->totvert]= pinned;
278         sys->totvert++;
279 }
280
281 void laplacian_add_triangle(LaplacianSystem *sys, int v1, int v2, int v3)
282 {
283         sys->faces[sys->totface][0]= v1;
284         sys->faces[sys->totface][1]= v2;
285         sys->faces[sys->totface][2]= v3;
286         sys->totface++;
287 }
288
289 static void laplacian_system_construct_end(LaplacianSystem *sys)
290 {
291         int (*face)[3];
292         int a, totvert=sys->totvert, totface=sys->totface;
293
294         laplacian_begin_solve(sys, 0);
295
296         sys->varea= MEM_callocN(sizeof(float)*totvert, "LaplacianSystemVarea");
297
298         sys->edgehash= BLI_edgehash_new();
299         for (a=0, face=sys->faces; a<sys->totface; a++, face++) {
300                 laplacian_increase_edge_count(sys->edgehash, (*face)[0], (*face)[1]);
301                 laplacian_increase_edge_count(sys->edgehash, (*face)[1], (*face)[2]);
302                 laplacian_increase_edge_count(sys->edgehash, (*face)[2], (*face)[0]);
303         }
304
305         if (sys->areaweights)
306                 for (a=0, face=sys->faces; a<sys->totface; a++, face++)
307                         laplacian_triangle_area(sys, (*face)[0], (*face)[1], (*face)[2]);
308         
309         for (a=0; a<totvert; a++) {
310                 if (sys->areaweights) {
311                         if (sys->varea[a] != 0.0f)
312                                 sys->varea[a]= 0.5f/sys->varea[a];
313                 }
314                 else
315                         sys->varea[a]= 1.0f;
316
317                 /* for heat weighting */
318                 if (sys->heat.H)
319                         nlMatrixAdd(a, a, sys->heat.H[a]);
320         }
321
322         if (sys->storeweights)
323                 sys->fweights= MEM_callocN(sizeof(float)*3*totface, "LaplacianFWeight");
324         
325         for (a=0, face=sys->faces; a<totface; a++, face++)
326                 laplacian_triangle_weights(sys, a, (*face)[0], (*face)[1], (*face)[2]);
327
328         MEM_freeN(sys->faces);
329         sys->faces= NULL;
330
331         if (sys->varea) {
332                 MEM_freeN(sys->varea);
333                 sys->varea= NULL;
334         }
335
336         BLI_edgehash_free(sys->edgehash, NULL);
337         sys->edgehash= NULL;
338 }
339
340 static void laplacian_system_delete(LaplacianSystem *sys)
341 {
342         if (sys->verts) MEM_freeN(sys->verts);
343         if (sys->varea) MEM_freeN(sys->varea);
344         if (sys->vpinned) MEM_freeN(sys->vpinned);
345         if (sys->faces) MEM_freeN(sys->faces);
346         if (sys->fweights) MEM_freeN(sys->fweights);
347
348         nlDeleteContext(sys->context);
349         MEM_freeN(sys);
350 }
351
352 void laplacian_begin_solve(LaplacianSystem *sys, int index)
353 {
354         int a;
355
356         if (!sys->nlbegun) {
357                 nlBegin(NL_SYSTEM);
358
359                 if (index >= 0) {
360                         for (a=0; a<sys->totvert; a++) {
361                                 if (sys->vpinned[a]) {
362                                         nlSetVariable(0, a, sys->verts[a][index]);
363                                         nlLockVariable(a);
364                                 }
365                         }
366                 }
367
368                 nlBegin(NL_MATRIX);
369                 sys->nlbegun = 1;
370         }
371 }
372
373 void laplacian_add_right_hand_side(LaplacianSystem *UNUSED(sys), int v, float value)
374 {
375         nlRightHandSideAdd(0, v, value);
376 }
377
378 int laplacian_system_solve(LaplacianSystem *sys)
379 {
380         nlEnd(NL_MATRIX);
381         nlEnd(NL_SYSTEM);
382         sys->nlbegun = 0;
383
384         //nlPrintMatrix();
385
386         return nlSolveAdvanced(NULL, NL_TRUE);
387 }
388
389 float laplacian_system_get_solution(int v)
390 {
391         return nlGetVariable(0, v);
392 }
393
394 /************************* Heat Bone Weighting ******************************/
395 /* From "Automatic Rigging and Animation of 3D Characters"
396  * Ilya Baran and Jovan Popovic, SIGGRAPH 2007 */
397
398 #define C_WEIGHT                        1.0f
399 #define WEIGHT_LIMIT_START      0.05f
400 #define WEIGHT_LIMIT_END        0.025f
401 #define DISTANCE_EPSILON        1e-4f
402
403 typedef struct BVHCallbackUserData {
404         float start[3];
405         float vec[3];
406         LaplacianSystem *sys;
407 } BVHCallbackUserData;
408
409 static void bvh_callback(void *userdata, int index, const BVHTreeRay *UNUSED(ray), BVHTreeRayHit *hit)
410 {
411         BVHCallbackUserData *data = (struct BVHCallbackUserData*)userdata;
412         MFace *mf = data->sys->heat.mface + index;
413         float (*verts)[3] = data->sys->heat.verts;
414         float lambda, uv[2], n[3], dir[3];
415
416         mul_v3_v3fl(dir, data->vec, hit->dist);
417
418         if (isect_ray_tri_v3(data->start, dir, verts[mf->v1], verts[mf->v2], verts[mf->v3], &lambda, uv)) {
419                 normal_tri_v3(n, verts[mf->v1], verts[mf->v2], verts[mf->v3]);
420                 if (lambda < 1.0f && dot_v3v3(n, data->vec) < -1e-5f) {
421                         hit->index = index;
422                         hit->dist *= lambda;
423                 }
424         }
425
426         mul_v3_v3fl(dir, data->vec, hit->dist);
427
428         if (isect_ray_tri_v3(data->start, dir, verts[mf->v1], verts[mf->v3], verts[mf->v4], &lambda, uv)) {
429                 normal_tri_v3(n, verts[mf->v1], verts[mf->v3], verts[mf->v4]);
430                 if (lambda < 1.0f && dot_v3v3(n, data->vec) < -1e-5f) {
431                         hit->index = index;
432                         hit->dist *= lambda;
433                 }
434         }
435 }
436
437 /* Raytracing for vertex to bone/vertex visibility */
438 static void heat_ray_tree_create(LaplacianSystem *sys)
439 {
440         MFace *mface = sys->heat.mface;
441         float (*verts)[3] = sys->heat.verts;
442         int totface = sys->heat.totface;
443         int totvert = sys->heat.totvert;
444         int a;
445
446         sys->heat.bvhtree = BLI_bvhtree_new(totface, 0.0f, 4, 6);
447         sys->heat.vface = MEM_callocN(sizeof(MFace*)*totvert, "HeatVFaces");
448
449         for (a=0; a<totface; a++) {
450                 MFace *mf = mface+a;
451                 float bb[6];
452
453                 INIT_MINMAX(bb, bb+3);
454                 DO_MINMAX(verts[mf->v1], bb, bb+3);
455                 DO_MINMAX(verts[mf->v2], bb, bb+3);
456                 DO_MINMAX(verts[mf->v3], bb, bb+3);
457                 if (mf->v4) {
458                         DO_MINMAX(verts[mf->v4], bb, bb+3);
459                 }
460
461                 BLI_bvhtree_insert(sys->heat.bvhtree, a, bb, 2);
462                 
463                 //Setup inverse pointers to use on isect.orig
464                 sys->heat.vface[mf->v1]= mf;
465                 sys->heat.vface[mf->v2]= mf;
466                 sys->heat.vface[mf->v3]= mf;
467                 if (mf->v4) sys->heat.vface[mf->v4]= mf;
468         }
469
470         BLI_bvhtree_balance(sys->heat.bvhtree); 
471 }
472
473 static int heat_ray_source_visible(LaplacianSystem *sys, int vertex, int source)
474 {
475         BVHTreeRayHit hit;
476         BVHCallbackUserData data;
477         MFace *mface;
478         float end[3];
479         int visible;
480
481         mface= sys->heat.vface[vertex];
482         if (!mface)
483                 return 1;
484
485         data.sys= sys;
486         copy_v3_v3(data.start, sys->heat.verts[vertex]);
487
488         if (sys->heat.root) /* bone */
489                 closest_to_line_segment_v3(end, data.start,
490                         sys->heat.root[source], sys->heat.tip[source]);
491         else /* vertex */
492                 copy_v3_v3(end, sys->heat.source[source]);
493
494         sub_v3_v3v3(data.vec, end, data.start);
495         madd_v3_v3v3fl(data.start, data.start, data.vec, 1e-5);
496         mul_v3_fl(data.vec, 1.0f - 2e-5f);
497
498         /* pass normalized vec + distance to bvh */
499         hit.index = -1;
500         hit.dist = normalize_v3(data.vec);
501
502         visible= BLI_bvhtree_ray_cast(sys->heat.bvhtree, data.start, data.vec, 0.0f, &hit, bvh_callback, (void*)&data) == -1;
503
504         return visible;
505 }
506
507 static float heat_source_distance(LaplacianSystem *sys, int vertex, int source)
508 {
509         float closest[3], d[3], dist, cosine;
510         
511         /* compute euclidian distance */
512         if (sys->heat.root) /* bone */
513                 closest_to_line_segment_v3(closest, sys->heat.verts[vertex],
514                         sys->heat.root[source], sys->heat.tip[source]);
515         else /* vertex */
516                 copy_v3_v3(closest, sys->heat.source[source]);
517
518         sub_v3_v3v3(d, sys->heat.verts[vertex], closest);
519         dist= normalize_v3(d);
520
521         /* if the vertex normal does not point along the bone, increase distance */
522         cosine= dot_v3v3(d, sys->heat.vnors[vertex]);
523
524         return dist/(0.5f*(cosine + 1.001f));
525 }
526
527 static int heat_source_closest(LaplacianSystem *sys, int vertex, int source)
528 {
529         float dist;
530
531         dist= heat_source_distance(sys, vertex, source);
532
533         if (dist <= sys->heat.mindist[vertex]*(1.0f + DISTANCE_EPSILON))
534                 if (heat_ray_source_visible(sys, vertex, source))
535                         return 1;
536                 
537         return 0;
538 }
539
540 static void heat_set_H(LaplacianSystem *sys, int vertex)
541 {
542         float dist, mindist, h;
543         int j, numclosest = 0;
544
545         mindist= 1e10;
546
547         /* compute minimum distance */
548         for (j=0; j<sys->heat.numsource; j++) {
549                 dist= heat_source_distance(sys, vertex, j);
550
551                 if (dist < mindist)
552                         mindist= dist;
553         }
554
555         sys->heat.mindist[vertex]= mindist;
556
557         /* count number of sources with approximately this minimum distance */
558         for (j=0; j<sys->heat.numsource; j++)
559                 if (heat_source_closest(sys, vertex, j))
560                         numclosest++;
561
562         sys->heat.p[vertex]= (numclosest > 0)? 1.0f/numclosest: 0.0f;
563
564         /* compute H entry */
565         if (numclosest > 0) {
566                 mindist= maxf(mindist, 1e-4f);
567                 h= numclosest*C_WEIGHT/(mindist*mindist);
568         }
569         else
570                 h= 0.0f;
571         
572         sys->heat.H[vertex]= h;
573 }
574
575 static void heat_calc_vnormals(LaplacianSystem *sys)
576 {
577         float fnor[3];
578         int a, v1, v2, v3, (*face)[3];
579
580         sys->heat.vnors= MEM_callocN(sizeof(float)*3*sys->totvert, "HeatVNors");
581
582         for (a=0, face=sys->faces; a<sys->totface; a++, face++) {
583                 v1= (*face)[0];
584                 v2= (*face)[1];
585                 v3= (*face)[2];
586
587                 normal_tri_v3( fnor,sys->verts[v1], sys->verts[v2], sys->verts[v3]);
588                 
589                 add_v3_v3(sys->heat.vnors[v1], fnor);
590                 add_v3_v3(sys->heat.vnors[v2], fnor);
591                 add_v3_v3(sys->heat.vnors[v3], fnor);
592         }
593
594         for (a=0; a<sys->totvert; a++)
595                 normalize_v3(sys->heat.vnors[a]);
596 }
597
598 static void heat_laplacian_create(LaplacianSystem *sys)
599 {
600         MFace *mface = sys->heat.mface, *mf;
601         int totface= sys->heat.totface;
602         int totvert= sys->heat.totvert;
603         int a;
604
605         /* heat specific definitions */
606         sys->heat.mindist= MEM_callocN(sizeof(float)*totvert, "HeatMinDist");
607         sys->heat.H= MEM_callocN(sizeof(float)*totvert, "HeatH");
608         sys->heat.p= MEM_callocN(sizeof(float)*totvert, "HeatP");
609
610         /* add verts and faces to laplacian */
611         for (a=0; a<totvert; a++)
612                 laplacian_add_vertex(sys, sys->heat.verts[a], 0);
613
614         for (a=0, mf=mface; a<totface; a++, mf++) {
615                 laplacian_add_triangle(sys, mf->v1, mf->v2, mf->v3);
616                 if (mf->v4)
617                         laplacian_add_triangle(sys, mf->v1, mf->v3, mf->v4);
618         }
619
620         /* for distance computation in set_H */
621         heat_calc_vnormals(sys);
622
623         for (a=0; a<totvert; a++)
624                 heat_set_H(sys, a);
625 }
626
627 static void heat_system_free(LaplacianSystem *sys)
628 {
629         BLI_bvhtree_free(sys->heat.bvhtree);
630         MEM_freeN(sys->heat.vface);
631
632         MEM_freeN(sys->heat.mindist);
633         MEM_freeN(sys->heat.H);
634         MEM_freeN(sys->heat.p);
635         MEM_freeN(sys->heat.vnors);
636 }
637
638 static float heat_limit_weight(float weight)
639 {
640         float t;
641
642         if (weight < WEIGHT_LIMIT_END) {
643                 return 0.0f;
644         }
645         else if (weight < WEIGHT_LIMIT_START) {
646                 t= (weight - WEIGHT_LIMIT_END)/(WEIGHT_LIMIT_START - WEIGHT_LIMIT_END);
647                 return t*WEIGHT_LIMIT_START;
648         }
649         else
650                 return weight;
651 }
652
653 void heat_bone_weighting(Object *ob, Mesh *me, float (*verts)[3], int numsource, bDeformGroup **dgrouplist, bDeformGroup **dgroupflip, float (*root)[3], float (*tip)[3], int *selected, const char **err_str)
654 {
655         LaplacianSystem *sys;
656         MPoly *mp;
657         MLoop *ml;
658         MFace *mf;
659         float solution, weight;
660         int *vertsflipped = NULL, *mask= NULL;
661         int a, tottri, j, bbone, firstsegment, lastsegment;
662
663         MVert *mvert = me->mvert;
664         int use_vert_sel= FALSE;
665         int use_face_sel= FALSE;
666
667         *err_str= NULL;
668
669         /* count triangles and create mask */
670         if (     (use_face_sel= (me->editflag & ME_EDIT_PAINT_MASK) != 0) ||
671                 (use_vert_sel= ((me->editflag & ME_EDIT_VERT_SEL) != 0)))
672         {
673                 mask= MEM_callocN(sizeof(int)*me->totvert, "heat_bone_weighting mask");
674         }
675
676         for (a = 0, mp=me->mpoly; a < me->totpoly; mp++, a++) {
677                 /*  (added selectedVerts content for vertex mask, they used to just equal 1) */
678                 if (use_vert_sel) {
679                         for (j = 0, ml = me->mloop + mp->loopstart; j < mp->totloop; j++, ml++) {
680                                 if (use_vert_sel) {
681                                         mask[ml->v] = (mvert[ml->v].flag & SELECT) != 0;
682                                 }
683                         }
684                 }
685                 else if (use_face_sel) {
686                         if (mp->flag & ME_FACE_SEL) {
687                                 for (j = 0, ml = me->mloop + mp->loopstart; j < mp->totloop; j++, ml++) {
688                                         mask[ml->v] = 1;
689                                 }
690                         }
691                 }
692         }
693
694         /* bone heat needs triangulated faces */
695         BKE_mesh_tessface_ensure(me);
696
697         for (tottri = 0, a = 0, mf = me->mface; a < me->totface; mf++, a++) {
698                 tottri++;
699                 if (mf->v4) tottri++;
700         }
701
702         /* create laplacian */
703         sys = laplacian_system_construct_begin(me->totvert, tottri, 1);
704
705         sys->heat.mface= me->mface;
706         sys->heat.totface= me->totface;
707         sys->heat.totvert= me->totvert;
708         sys->heat.verts= verts;
709         sys->heat.root= root;
710         sys->heat.tip= tip;
711         sys->heat.numsource= numsource;
712
713         heat_ray_tree_create(sys);
714         heat_laplacian_create(sys);
715
716         laplacian_system_construct_end(sys);
717
718         if (dgroupflip) {
719                 vertsflipped = MEM_callocN(sizeof(int)*me->totvert, "vertsflipped");
720                 for (a=0; a<me->totvert; a++)
721                         vertsflipped[a] = mesh_get_x_mirror_vert(ob, a);
722         }
723         
724         /* compute weights per bone */
725         for (j=0; j<numsource; j++) {
726                 if (!selected[j])
727                         continue;
728
729                 firstsegment= (j == 0 || dgrouplist[j-1] != dgrouplist[j]);
730                 lastsegment= (j == numsource-1 || dgrouplist[j] != dgrouplist[j+1]);
731                 bbone= !(firstsegment && lastsegment);
732
733                 /* clear weights */
734                 if (bbone && firstsegment) {
735                         for (a=0; a<me->totvert; a++) {
736                                 if (mask && !mask[a])
737                                         continue;
738
739                                 ED_vgroup_vert_remove(ob, dgrouplist[j], a);
740                                 if (vertsflipped && dgroupflip[j] && vertsflipped[a] >= 0)
741                                         ED_vgroup_vert_remove(ob, dgroupflip[j], vertsflipped[a]);
742                         }
743                 }
744
745                 /* fill right hand side */
746                 laplacian_begin_solve(sys, -1);
747
748                 for (a=0; a<me->totvert; a++)
749                         if (heat_source_closest(sys, a, j))
750                                 laplacian_add_right_hand_side(sys, a,
751                                         sys->heat.H[a]*sys->heat.p[a]);
752
753                 /* solve */
754                 if (laplacian_system_solve(sys)) {
755                         /* load solution into vertex groups */
756                         for (a=0; a<me->totvert; a++) {
757                                 if (mask && !mask[a])
758                                         continue;
759
760                                 solution= laplacian_system_get_solution(a);
761                                 
762                                 if (bbone) {
763                                         if (solution > 0.0f)
764                                                 ED_vgroup_vert_add(ob, dgrouplist[j], a, solution,
765                                                         WEIGHT_ADD);
766                                 }
767                                 else {
768                                         weight= heat_limit_weight(solution);
769                                         if (weight > 0.0f)
770                                                 ED_vgroup_vert_add(ob, dgrouplist[j], a, weight,
771                                                         WEIGHT_REPLACE);
772                                         else
773                                                 ED_vgroup_vert_remove(ob, dgrouplist[j], a);
774                                 }
775
776                                 /* do same for mirror */
777                                 if (vertsflipped && dgroupflip[j] && vertsflipped[a] >= 0) {
778                                         if (bbone) {
779                                                 if (solution > 0.0f)
780                                                         ED_vgroup_vert_add(ob, dgroupflip[j], vertsflipped[a],
781                                                                 solution, WEIGHT_ADD);
782                                         }
783                                         else {
784                                                 weight= heat_limit_weight(solution);
785                                                 if (weight > 0.0f)
786                                                         ED_vgroup_vert_add(ob, dgroupflip[j], vertsflipped[a],
787                                                                 weight, WEIGHT_REPLACE);
788                                                 else
789                                                         ED_vgroup_vert_remove(ob, dgroupflip[j], vertsflipped[a]);
790                                         }
791                                 }
792                         }
793                 }
794                 else if (*err_str == NULL) {
795                         *err_str= "Bone Heat Weighting: failed to find solution for one or more bones";
796                         break;
797                 }
798
799                 /* remove too small vertex weights */
800                 if (bbone && lastsegment) {
801                         for (a=0; a<me->totvert; a++) {
802                                 if (mask && !mask[a])
803                                         continue;
804
805                                 weight= ED_vgroup_vert_weight(ob, dgrouplist[j], a);
806                                 weight= heat_limit_weight(weight);
807                                 if (weight <= 0.0f)
808                                         ED_vgroup_vert_remove(ob, dgrouplist[j], a);
809
810                                 if (vertsflipped && dgroupflip[j] && vertsflipped[a] >= 0) {
811                                         weight= ED_vgroup_vert_weight(ob, dgroupflip[j], vertsflipped[a]);
812                                         weight= heat_limit_weight(weight);
813                                         if (weight <= 0.0f)
814                                                 ED_vgroup_vert_remove(ob, dgroupflip[j], vertsflipped[a]);
815                                 }
816                         }
817                 }
818         }
819
820         /* free */
821         if (vertsflipped) MEM_freeN(vertsflipped);
822         if (mask) MEM_freeN(mask);
823
824         heat_system_free(sys);
825
826         laplacian_system_delete(sys);
827 }
828
829 #ifdef RIGID_DEFORM
830 /********************** As-Rigid-As-Possible Deformation ******************/
831 /* From "As-Rigid-As-Possible Surface Modeling",
832  * Olga Sorkine and Marc Alexa, ESGP 2007. */
833
834 /* investigate:
835  * - transpose R in orthogonal
836  * - flipped normals and per face adding
837  * - move canceling to transform, make origco pointer
838  */
839
840 static LaplacianSystem *RigidDeformSystem = NULL;
841
842 static void rigid_add_half_edge_to_R(LaplacianSystem *sys, EditVert *v1, EditVert *v2, float w)
843 {
844         float e[3], e_[3];
845         int i;
846
847         sub_v3_v3v3(e, sys->rigid.origco[v1->tmp.l], sys->rigid.origco[v2->tmp.l]);
848         sub_v3_v3v3(e_, v1->co, v2->co);
849
850         /* formula (5) */
851         for (i=0; i<3; i++) {
852                 sys->rigid.R[v1->tmp.l][i][0] += w*e[0]*e_[i];
853                 sys->rigid.R[v1->tmp.l][i][1] += w*e[1]*e_[i];
854                 sys->rigid.R[v1->tmp.l][i][2] += w*e[2]*e_[i];
855         }
856 }
857
858 static void rigid_add_edge_to_R(LaplacianSystem *sys, EditVert *v1, EditVert *v2, float w)
859 {
860         rigid_add_half_edge_to_R(sys, v1, v2, w);
861         rigid_add_half_edge_to_R(sys, v2, v1, w);
862 }
863
864 static void rigid_orthogonalize_R(float R[][3])
865 {
866         HMatrix M, Q, S;
867
868         copy_m4_m3(M, R);
869         polar_decomp(M, Q, S);
870         copy_m3_m4(R, Q);
871 }
872
873 static void rigid_add_half_edge_to_rhs(LaplacianSystem *sys, EditVert *v1, EditVert *v2, float w)
874 {
875         /* formula (8) */
876         float Rsum[3][3], rhs[3];
877
878         if (sys->vpinned[v1->tmp.l])
879                 return;
880
881         add_m3_m3m3(Rsum, sys->rigid.R[v1->tmp.l], sys->rigid.R[v2->tmp.l]);
882         transpose_m3(Rsum);
883
884         sub_v3_v3v3(rhs, sys->rigid.origco[v1->tmp.l], sys->rigid.origco[v2->tmp.l]);
885         mul_m3_v3(Rsum, rhs);
886         mul_v3_fl(rhs, 0.5f);
887         mul_v3_fl(rhs, w);
888
889         add_v3_v3(sys->rigid.rhs[v1->tmp.l], rhs);
890 }
891
892 static void rigid_add_edge_to_rhs(LaplacianSystem *sys, EditVert *v1, EditVert *v2, float w)
893 {
894         rigid_add_half_edge_to_rhs(sys, v1, v2, w);
895         rigid_add_half_edge_to_rhs(sys, v2, v1, w);
896 }
897
898 void rigid_deform_iteration()
899 {
900         LaplacianSystem *sys= RigidDeformSystem;
901         EditMesh *em;
902         EditVert *eve;
903         EditFace *efa;
904         int a, i;
905
906         if (!sys)
907                 return;
908         
909         nlMakeCurrent(sys->context);
910         em= sys->rigid.mesh;
911
912         /* compute R */
913         memset(sys->rigid.R, 0, sizeof(float)*3*3*sys->totvert);
914         memset(sys->rigid.rhs, 0, sizeof(float)*3*sys->totvert);
915
916         for (a=0, efa=em->faces.first; efa; efa=efa->next, a++) {
917                 rigid_add_edge_to_R(sys, efa->v1, efa->v2, sys->fweights[a][2]);
918                 rigid_add_edge_to_R(sys, efa->v2, efa->v3, sys->fweights[a][0]);
919                 rigid_add_edge_to_R(sys, efa->v3, efa->v1, sys->fweights[a][1]);
920
921                 if (efa->v4) {
922                         a++;
923                         rigid_add_edge_to_R(sys, efa->v1, efa->v3, sys->fweights[a][2]);
924                         rigid_add_edge_to_R(sys, efa->v3, efa->v4, sys->fweights[a][0]);
925                         rigid_add_edge_to_R(sys, efa->v4, efa->v1, sys->fweights[a][1]);
926                 }
927         }
928
929         for (a=0, eve=em->verts.first; eve; eve=eve->next, a++) {
930                 rigid_orthogonalize_R(sys->rigid.R[a]);
931                 eve->tmp.l= a;
932         }
933         
934         /* compute right hand sides for solving */
935         for (a=0, efa=em->faces.first; efa; efa=efa->next, a++) {
936                 rigid_add_edge_to_rhs(sys, efa->v1, efa->v2, sys->fweights[a][2]);
937                 rigid_add_edge_to_rhs(sys, efa->v2, efa->v3, sys->fweights[a][0]);
938                 rigid_add_edge_to_rhs(sys, efa->v3, efa->v1, sys->fweights[a][1]);
939
940                 if (efa->v4) {
941                         a++;
942                         rigid_add_edge_to_rhs(sys, efa->v1, efa->v3, sys->fweights[a][2]);
943                         rigid_add_edge_to_rhs(sys, efa->v3, efa->v4, sys->fweights[a][0]);
944                         rigid_add_edge_to_rhs(sys, efa->v4, efa->v1, sys->fweights[a][1]);
945                 }
946         }
947
948         /* solve for positions, for X,Y and Z separately */
949         for (i=0; i<3; i++) {
950                 laplacian_begin_solve(sys, i);
951
952                 for (a=0; a<sys->totvert; a++)
953                         if (!sys->vpinned[a])
954                                 laplacian_add_right_hand_side(sys, a, sys->rigid.rhs[a][i]);
955
956                 if (laplacian_system_solve(sys)) {
957                         for (a=0, eve=em->verts.first; eve; eve=eve->next, a++)
958                                 eve->co[i]= laplacian_system_get_solution(a);
959                 }
960                 else {
961                         if (!sys->rigid.thrownerror) {
962                                 error("RigidDeform: failed to find solution");
963                                 sys->rigid.thrownerror= 1;
964                         }
965                         break;
966                 }
967         }
968 }
969
970 static void rigid_laplacian_create(LaplacianSystem *sys)
971 {
972         EditMesh *em = sys->rigid.mesh;
973         EditVert *eve;
974         EditFace *efa;
975         int a;
976
977         /* add verts and faces to laplacian */
978         for (a=0, eve=em->verts.first; eve; eve=eve->next, a++) {
979                 laplacian_add_vertex(sys, eve->co, eve->pinned);
980                 eve->tmp.l= a;
981         }
982
983         for (efa=em->faces.first; efa; efa=efa->next) {
984                 laplacian_add_triangle(sys,
985                         efa->v1->tmp.l, efa->v2->tmp.l, efa->v3->tmp.l);
986                 if (efa->v4)
987                         laplacian_add_triangle(sys,
988                                 efa->v1->tmp.l, efa->v3->tmp.l, efa->v4->tmp.l);
989         }
990 }
991
992 void rigid_deform_begin(EditMesh *em)
993 {
994         LaplacianSystem *sys;
995         EditVert *eve;
996         EditFace *efa;
997         int a, totvert, totface;
998
999         /* count vertices, triangles */
1000         for (totvert=0, eve=em->verts.first; eve; eve=eve->next)
1001                 totvert++;
1002
1003         for (totface=0, efa=em->faces.first; efa; efa=efa->next) {
1004                 totface++;
1005                 if (efa->v4) totface++;
1006         }
1007
1008         /* create laplacian */
1009         sys = laplacian_system_construct_begin(totvert, totface, 0);
1010
1011         sys->rigid.mesh= em;
1012         sys->rigid.R = MEM_callocN(sizeof(float)*3*3*totvert, "RigidDeformR");
1013         sys->rigid.rhs = MEM_callocN(sizeof(float)*3*totvert, "RigidDeformRHS");
1014         sys->rigid.origco = MEM_callocN(sizeof(float)*3*totvert, "RigidDeformCo");
1015
1016         for (a=0, eve=em->verts.first; eve; eve=eve->next, a++)
1017                 copy_v3_v3(sys->rigid.origco[a], eve->co);
1018
1019         sys->areaweights= 0;
1020         sys->storeweights= 1;
1021
1022         rigid_laplacian_create(sys);
1023
1024         laplacian_system_construct_end(sys);
1025
1026         RigidDeformSystem = sys;
1027 }
1028
1029 void rigid_deform_end(int cancel)
1030 {
1031         LaplacianSystem *sys = RigidDeformSystem;
1032
1033         if (sys) {
1034                 EditMesh *em = sys->rigid.mesh;
1035                 EditVert *eve;
1036                 int a;
1037
1038                 if (cancel)
1039                         for (a=0, eve=em->verts.first; eve; eve=eve->next, a++)
1040                                 if (!eve->pinned)
1041                                         copy_v3_v3(eve->co, sys->rigid.origco[a]);
1042
1043                 if (sys->rigid.R) MEM_freeN(sys->rigid.R);
1044                 if (sys->rigid.rhs) MEM_freeN(sys->rigid.rhs);
1045                 if (sys->rigid.origco) MEM_freeN(sys->rigid.origco);
1046
1047                 /* free */
1048                 laplacian_system_delete(sys);
1049         }
1050
1051         RigidDeformSystem = NULL;
1052 }
1053 #endif
1054
1055 /************************** Harmonic Coordinates ****************************/
1056 /* From "Harmonic Coordinates for Character Articulation",
1057  * Pushkar Joshi, Mark Meyer, Tony DeRose, Brian Green and Tom Sanocki,
1058  * SIGGRAPH 2007. */
1059
1060 #define EPSILON 0.0001f
1061
1062 #define MESHDEFORM_TAG_UNTYPED  0
1063 #define MESHDEFORM_TAG_BOUNDARY 1
1064 #define MESHDEFORM_TAG_INTERIOR 2
1065 #define MESHDEFORM_TAG_EXTERIOR 3
1066
1067 #define MESHDEFORM_LEN_THRESHOLD 1e-6f
1068
1069 #define MESHDEFORM_MIN_INFLUENCE 0.0005f
1070
1071 static int MESHDEFORM_OFFSET[7][3] =
1072                 {{0,0,0}, {1,0,0}, {-1,0,0}, {0,1,0}, {0,-1,0}, {0,0,1}, {0,0,-1}};
1073
1074 typedef struct MDefBoundIsect {
1075         float co[3], uvw[4];
1076         int nvert, v[4], facing;
1077         float len;
1078 } MDefBoundIsect;
1079
1080 typedef struct MDefBindInfluence {
1081         struct MDefBindInfluence *next;
1082         float weight;
1083         int vertex;
1084 } MDefBindInfluence;
1085
1086 typedef struct MeshDeformBind {
1087         /* grid dimensions */
1088         float min[3], max[3];
1089         float width[3], halfwidth[3];
1090         int size, size3;
1091
1092         /* meshes */
1093         DerivedMesh *cagedm;
1094         float (*cagecos)[3];
1095         float (*vertexcos)[3];
1096         int totvert, totcagevert;
1097
1098         /* grids */
1099         MemArena *memarena;
1100         MDefBoundIsect *(*boundisect)[6];
1101         int *semibound;
1102         int *tag;
1103         float *phi, *totalphi;
1104
1105         /* mesh stuff */
1106         int *inside;
1107         float *weights;
1108         MDefBindInfluence **dyngrid;
1109         float cagemat[4][4];
1110
1111         /* direct solver */
1112         int *varidx;
1113 } MeshDeformBind;
1114
1115 typedef struct MeshDeformIsect {
1116         float start[3];
1117         float vec[3];
1118         float labda;
1119
1120         void *face;
1121         int isect;
1122         float u, v;
1123         
1124 } MeshDeformIsect;
1125
1126 /* ray intersection */
1127
1128 /* our own triangle intersection, so we can fully control the epsilons and
1129  * prevent corner case from going wrong*/
1130 static int meshdeform_tri_intersect(float orig[3], float end[3], float vert0[3],
1131         float vert1[3], float vert2[3], float *isectco, float *uvw)
1132 {
1133         float edge1[3], edge2[3], tvec[3], pvec[3], qvec[3];
1134         float det,inv_det, u, v, dir[3], isectdir[3];
1135
1136         sub_v3_v3v3(dir, end, orig);
1137
1138         /* find vectors for two edges sharing vert0 */
1139         sub_v3_v3v3(edge1, vert1, vert0);
1140         sub_v3_v3v3(edge2, vert2, vert0);
1141
1142         /* begin calculating determinant - also used to calculate U parameter */
1143         cross_v3_v3v3(pvec, dir, edge2);
1144
1145         /* if determinant is near zero, ray lies in plane of triangle */
1146         det = dot_v3v3(edge1, pvec);
1147
1148         if (det == 0.0f)
1149                 return 0;
1150         inv_det = 1.0f / det;
1151
1152         /* calculate distance from vert0 to ray origin */
1153         sub_v3_v3v3(tvec, orig, vert0);
1154
1155         /* calculate U parameter and test bounds */
1156         u = dot_v3v3(tvec, pvec) * inv_det;
1157         if (u < -EPSILON || u > 1.0f+EPSILON)
1158                 return 0;
1159
1160         /* prepare to test V parameter */
1161         cross_v3_v3v3(qvec, tvec, edge1);
1162
1163         /* calculate V parameter and test bounds */
1164         v = dot_v3v3(dir, qvec) * inv_det;
1165         if (v < -EPSILON || u + v > 1.0f+EPSILON)
1166                 return 0;
1167
1168         isectco[0]= (1.0f - u - v)*vert0[0] + u*vert1[0] + v*vert2[0];
1169         isectco[1]= (1.0f - u - v)*vert0[1] + u*vert1[1] + v*vert2[1];
1170         isectco[2]= (1.0f - u - v)*vert0[2] + u*vert1[2] + v*vert2[2];
1171
1172         uvw[0]= 1.0f - u - v;
1173         uvw[1]= u;
1174         uvw[2]= v;
1175
1176         /* check if it is within the length of the line segment */
1177         sub_v3_v3v3(isectdir, isectco, orig);
1178
1179         if (dot_v3v3(dir, isectdir) < -EPSILON)
1180                 return 0;
1181         
1182         if (dot_v3v3(dir, dir) + EPSILON < dot_v3v3(isectdir, isectdir))
1183                 return 0;
1184
1185         return 1;
1186 }
1187
1188 static int meshdeform_intersect(MeshDeformBind *mdb, MeshDeformIsect *isec)
1189 {
1190         MFace *mface;
1191         float face[4][3], co[3], uvw[3], len, nor[3], end[3];
1192         int f, hit, is= 0, totface;
1193
1194         isec->labda= 1e10;
1195
1196         mface= mdb->cagedm->getTessFaceArray(mdb->cagedm);
1197         totface= mdb->cagedm->getNumTessFaces(mdb->cagedm);
1198
1199         add_v3_v3v3(end, isec->start, isec->vec);
1200
1201         for (f=0; f<totface; f++, mface++) {
1202                 copy_v3_v3(face[0], mdb->cagecos[mface->v1]);
1203                 copy_v3_v3(face[1], mdb->cagecos[mface->v2]);
1204                 copy_v3_v3(face[2], mdb->cagecos[mface->v3]);
1205
1206                 if (mface->v4) {
1207                         copy_v3_v3(face[3], mdb->cagecos[mface->v4]);
1208                         hit = meshdeform_tri_intersect(isec->start, end, face[0], face[1], face[2], co, uvw);
1209
1210                         if (hit) {
1211                                 normal_tri_v3( nor,face[0], face[1], face[2]);
1212                         }
1213                         else {
1214                                 hit= meshdeform_tri_intersect(isec->start, end, face[0], face[2], face[3], co, uvw);
1215                                 normal_tri_v3( nor,face[0], face[2], face[3]);
1216                         }
1217                 }
1218                 else {
1219                         hit= meshdeform_tri_intersect(isec->start, end, face[0], face[1], face[2], co, uvw);
1220                         normal_tri_v3( nor,face[0], face[1], face[2]);
1221                 }
1222
1223                 if (hit) {
1224                         len= len_v3v3(isec->start, co)/len_v3v3(isec->start, end);
1225                         if (len < isec->labda) {
1226                                 isec->labda= len;
1227                                 isec->face = mface;
1228                                 isec->isect= (dot_v3v3(isec->vec, nor) <= 0.0f);
1229                                 is= 1;
1230                         }
1231                 }
1232         }
1233
1234         return is;
1235 }
1236
1237 static MDefBoundIsect *meshdeform_ray_tree_intersect(MeshDeformBind *mdb, float *co1, float *co2)
1238 {
1239         MDefBoundIsect *isect;
1240         MeshDeformIsect isec;
1241         float (*cagecos)[3];
1242         MFace *mface;
1243         float vert[4][3], len, end[3];
1244         static float epsilon[3]= {0, 0, 0}; //1e-4, 1e-4, 1e-4};
1245
1246         /* setup isec */
1247         memset(&isec, 0, sizeof(isec));
1248         isec.labda= 1e10f;
1249
1250         add_v3_v3v3(isec.start, co1, epsilon);
1251         add_v3_v3v3(end, co2, epsilon);
1252         sub_v3_v3v3(isec.vec, end, isec.start);
1253
1254         if (meshdeform_intersect(mdb, &isec)) {
1255                 len= isec.labda;
1256                 mface=(MFace*)isec.face;
1257
1258                 /* create MDefBoundIsect */
1259                 isect= BLI_memarena_alloc(mdb->memarena, sizeof(*isect));
1260
1261                 /* compute intersection coordinate */
1262                 isect->co[0]= co1[0] + isec.vec[0]*len;
1263                 isect->co[1]= co1[1] + isec.vec[1]*len;
1264                 isect->co[2]= co1[2] + isec.vec[2]*len;
1265
1266                 isect->len= len_v3v3(co1, isect->co);
1267                 if (isect->len < MESHDEFORM_LEN_THRESHOLD)
1268                         isect->len= MESHDEFORM_LEN_THRESHOLD;
1269
1270                 isect->v[0]= mface->v1;
1271                 isect->v[1]= mface->v2;
1272                 isect->v[2]= mface->v3;
1273                 isect->v[3]= mface->v4;
1274                 isect->nvert= (mface->v4)? 4: 3;
1275
1276                 isect->facing= isec.isect;
1277
1278                 /* compute mean value coordinates for interpolation */
1279                 cagecos= mdb->cagecos;
1280                 copy_v3_v3(vert[0], cagecos[mface->v1]);
1281                 copy_v3_v3(vert[1], cagecos[mface->v2]);
1282                 copy_v3_v3(vert[2], cagecos[mface->v3]);
1283                 if (mface->v4) copy_v3_v3(vert[3], cagecos[mface->v4]);
1284                 interp_weights_poly_v3( isect->uvw,vert, isect->nvert, isect->co);
1285
1286                 return isect;
1287         }
1288
1289         return NULL;
1290 }
1291
1292 static int meshdeform_inside_cage(MeshDeformBind *mdb, float *co)
1293 {
1294         MDefBoundIsect *isect;
1295         float outside[3], start[3], dir[3];
1296         int i;
1297
1298         for (i=1; i<=6; i++) {
1299                 outside[0] = co[0] + (mdb->max[0] - mdb->min[0] + 1.0f)*MESHDEFORM_OFFSET[i][0];
1300                 outside[1] = co[1] + (mdb->max[1] - mdb->min[1] + 1.0f)*MESHDEFORM_OFFSET[i][1];
1301                 outside[2] = co[2] + (mdb->max[2] - mdb->min[2] + 1.0f)*MESHDEFORM_OFFSET[i][2];
1302
1303                 copy_v3_v3(start, co);
1304                 sub_v3_v3v3(dir, outside, start);
1305                 normalize_v3(dir);
1306                 
1307                 isect = meshdeform_ray_tree_intersect(mdb, start, outside);
1308                 if (isect && !isect->facing)
1309                         return 1;
1310         }
1311
1312         return 0;
1313 }
1314
1315 /* solving */
1316
1317 static int meshdeform_index(MeshDeformBind *mdb, int x, int y, int z, int n)
1318 {
1319         int size= mdb->size;
1320         
1321         x += MESHDEFORM_OFFSET[n][0];
1322         y += MESHDEFORM_OFFSET[n][1];
1323         z += MESHDEFORM_OFFSET[n][2];
1324
1325         if (x < 0 || x >= mdb->size)
1326                 return -1;
1327         if (y < 0 || y >= mdb->size)
1328                 return -1;
1329         if (z < 0 || z >= mdb->size)
1330                 return -1;
1331
1332         return x + y*size + z*size*size;
1333 }
1334
1335 static void meshdeform_cell_center(MeshDeformBind *mdb, int x, int y, int z, int n, float *center)
1336 {
1337         x += MESHDEFORM_OFFSET[n][0];
1338         y += MESHDEFORM_OFFSET[n][1];
1339         z += MESHDEFORM_OFFSET[n][2];
1340
1341         center[0]= mdb->min[0] + x*mdb->width[0] + mdb->halfwidth[0];
1342         center[1]= mdb->min[1] + y*mdb->width[1] + mdb->halfwidth[1];
1343         center[2]= mdb->min[2] + z*mdb->width[2] + mdb->halfwidth[2];
1344 }
1345
1346 static void meshdeform_add_intersections(MeshDeformBind *mdb, int x, int y, int z)
1347 {
1348         MDefBoundIsect *isect;
1349         float center[3], ncenter[3];
1350         int i, a;
1351
1352         a= meshdeform_index(mdb, x, y, z, 0);
1353         meshdeform_cell_center(mdb, x, y, z, 0, center);
1354
1355         /* check each outgoing edge for intersection */
1356         for (i=1; i<=6; i++) {
1357                 if (meshdeform_index(mdb, x, y, z, i) == -1)
1358                         continue;
1359
1360                 meshdeform_cell_center(mdb, x, y, z, i, ncenter);
1361
1362                 isect= meshdeform_ray_tree_intersect(mdb, center, ncenter);
1363                 if (isect) {
1364                         mdb->boundisect[a][i-1]= isect;
1365                         mdb->tag[a]= MESHDEFORM_TAG_BOUNDARY;
1366                 }
1367         }
1368 }
1369
1370 static void meshdeform_bind_floodfill(MeshDeformBind *mdb)
1371 {
1372         int *stack, *tag= mdb->tag;
1373         int a, b, i, xyz[3], stacksize, size= mdb->size;
1374
1375         stack= MEM_callocN(sizeof(int)*mdb->size3, "MeshDeformBindStack");
1376
1377         /* we know lower left corner is EXTERIOR because of padding */
1378         tag[0]= MESHDEFORM_TAG_EXTERIOR;
1379         stack[0]= 0;
1380         stacksize= 1;
1381
1382         /* floodfill exterior tag */
1383         while (stacksize > 0) {
1384                 a= stack[--stacksize];
1385
1386                 xyz[2]= a/(size*size);
1387                 xyz[1]= (a - xyz[2]*size*size)/size;
1388                 xyz[0]= a - xyz[1]*size - xyz[2]*size*size;
1389
1390                 for (i=1; i<=6; i++) {
1391                         b= meshdeform_index(mdb, xyz[0], xyz[1], xyz[2], i);
1392
1393                         if (b != -1) {
1394                                 if (tag[b] == MESHDEFORM_TAG_UNTYPED ||
1395                                    (tag[b] == MESHDEFORM_TAG_BOUNDARY && !mdb->boundisect[a][i-1])) {
1396                                         tag[b]= MESHDEFORM_TAG_EXTERIOR;
1397                                         stack[stacksize++]= b;
1398                                 }
1399                         }
1400                 }
1401         }
1402
1403         /* other cells are interior */
1404         for (a=0; a<size*size*size; a++)
1405                 if (tag[a]==MESHDEFORM_TAG_UNTYPED)
1406                         tag[a]= MESHDEFORM_TAG_INTERIOR;
1407
1408 #if 0
1409         {
1410                 int tb, ti, te, ts;
1411                 tb= ti= te= ts= 0;
1412                 for (a=0; a<size*size*size; a++)
1413                         if (tag[a]==MESHDEFORM_TAG_BOUNDARY)
1414                                 tb++;
1415                         else if (tag[a]==MESHDEFORM_TAG_INTERIOR)
1416                                 ti++;
1417                         else if (tag[a]==MESHDEFORM_TAG_EXTERIOR) {
1418                                 te++;
1419
1420                                 if (mdb->semibound[a])
1421                                         ts++;
1422                         }
1423                 
1424                 printf("interior %d exterior %d boundary %d semi-boundary %d\n", ti, te, tb, ts);
1425         }
1426 #endif
1427
1428         MEM_freeN(stack);
1429 }
1430
1431 static float meshdeform_boundary_phi(MeshDeformBind *UNUSED(mdb), MDefBoundIsect *isect, int cagevert)
1432 {
1433         int a;
1434
1435         for (a=0; a<isect->nvert; a++)
1436                 if (isect->v[a] == cagevert)
1437                         return isect->uvw[a];
1438         
1439         return 0.0f;
1440 }
1441
1442 static float meshdeform_interp_w(MeshDeformBind *mdb, float *gridvec, float *UNUSED(vec), int UNUSED(cagevert))
1443 {
1444         float dvec[3], ivec[3], wx, wy, wz, result=0.0f;
1445         float weight, totweight= 0.0f;
1446         int i, a, x, y, z;
1447
1448         for (i=0; i<3; i++) {
1449                 ivec[i]= (int)gridvec[i];
1450                 dvec[i]= gridvec[i] - ivec[i];
1451         }
1452
1453         for (i=0; i<8; i++) {
1454                 if (i & 1) { x= ivec[0]+1; wx= dvec[0]; }
1455                 else { x= ivec[0]; wx= 1.0f-dvec[0]; } 
1456
1457                 if (i & 2) { y= ivec[1]+1; wy= dvec[1]; }
1458                 else { y= ivec[1]; wy= 1.0f-dvec[1]; } 
1459
1460                 if (i & 4) { z= ivec[2]+1; wz= dvec[2]; }
1461                 else { z= ivec[2]; wz= 1.0f-dvec[2]; } 
1462
1463                 CLAMP(x, 0, mdb->size-1);
1464                 CLAMP(y, 0, mdb->size-1);
1465                 CLAMP(z, 0, mdb->size-1);
1466
1467                 a= meshdeform_index(mdb, x, y, z, 0);
1468                 weight= wx*wy*wz;
1469                 result += weight*mdb->phi[a];
1470                 totweight += weight;
1471         }
1472
1473         if (totweight > 0.0f)
1474                 result /= totweight;
1475
1476         return result;
1477 }
1478
1479 static void meshdeform_check_semibound(MeshDeformBind *mdb, int x, int y, int z)
1480 {
1481         int i, a;
1482
1483         a= meshdeform_index(mdb, x, y, z, 0);
1484         if (mdb->tag[a] != MESHDEFORM_TAG_EXTERIOR)
1485                 return;
1486
1487         for (i=1; i<=6; i++)
1488                 if (mdb->boundisect[a][i-1]) 
1489                         mdb->semibound[a]= 1;
1490 }
1491
1492 static float meshdeform_boundary_total_weight(MeshDeformBind *mdb, int x, int y, int z)
1493 {
1494         float weight, totweight= 0.0f;
1495         int i, a;
1496
1497         a= meshdeform_index(mdb, x, y, z, 0);
1498
1499         /* count weight for neighbor cells */
1500         for (i=1; i<=6; i++) {
1501                 if (meshdeform_index(mdb, x, y, z, i) == -1)
1502                         continue;
1503
1504                 if (mdb->boundisect[a][i-1])
1505                         weight= 1.0f/mdb->boundisect[a][i-1]->len;
1506                 else if (!mdb->semibound[a])
1507                         weight= 1.0f/mdb->width[0];
1508                 else
1509                         weight= 0.0f;
1510
1511                 totweight += weight;
1512         }
1513
1514         return totweight;
1515 }
1516
1517 static void meshdeform_matrix_add_cell(MeshDeformBind *mdb, int x, int y, int z)
1518 {
1519         MDefBoundIsect *isect;
1520         float weight, totweight;
1521         int i, a, acenter;
1522
1523         acenter= meshdeform_index(mdb, x, y, z, 0);
1524         if (mdb->tag[acenter] == MESHDEFORM_TAG_EXTERIOR)
1525                 return;
1526
1527         nlMatrixAdd(mdb->varidx[acenter], mdb->varidx[acenter], 1.0f);
1528         
1529         totweight= meshdeform_boundary_total_weight(mdb, x, y, z);
1530         for (i=1; i<=6; i++) {
1531                 a= meshdeform_index(mdb, x, y, z, i);
1532                 if (a == -1 || mdb->tag[a] == MESHDEFORM_TAG_EXTERIOR)
1533                         continue;
1534
1535                 isect= mdb->boundisect[acenter][i-1];
1536                 if (!isect) {
1537                         weight= (1.0f/mdb->width[0])/totweight;
1538                         nlMatrixAdd(mdb->varidx[acenter], mdb->varidx[a], -weight);
1539                 }
1540         }
1541 }
1542
1543 static void meshdeform_matrix_add_rhs(MeshDeformBind *mdb, int x, int y, int z, int cagevert)
1544 {
1545         MDefBoundIsect *isect;
1546         float rhs, weight, totweight;
1547         int i, a, acenter;
1548
1549         acenter= meshdeform_index(mdb, x, y, z, 0);
1550         if (mdb->tag[acenter] == MESHDEFORM_TAG_EXTERIOR)
1551                 return;
1552
1553         totweight= meshdeform_boundary_total_weight(mdb, x, y, z);
1554         for (i=1; i<=6; i++) {
1555                 a= meshdeform_index(mdb, x, y, z, i);
1556                 if (a == -1)
1557                         continue;
1558
1559                 isect= mdb->boundisect[acenter][i-1];
1560
1561                 if (isect) {
1562                         weight= (1.0f/isect->len)/totweight;
1563                         rhs= weight*meshdeform_boundary_phi(mdb, isect, cagevert);
1564                         nlRightHandSideAdd(0, mdb->varidx[acenter], rhs);
1565                 }
1566         }
1567 }
1568
1569 static void meshdeform_matrix_add_semibound_phi(MeshDeformBind *mdb, int x, int y, int z, int cagevert)
1570 {
1571         MDefBoundIsect *isect;
1572         float rhs, weight, totweight;
1573         int i, a;
1574
1575         a= meshdeform_index(mdb, x, y, z, 0);
1576         if (!mdb->semibound[a])
1577                 return;
1578         
1579         mdb->phi[a]= 0.0f;
1580
1581         totweight= meshdeform_boundary_total_weight(mdb, x, y, z);
1582         for (i=1; i<=6; i++) {
1583                 isect= mdb->boundisect[a][i-1];
1584
1585                 if (isect) {
1586                         weight= (1.0f/isect->len)/totweight;
1587                         rhs= weight*meshdeform_boundary_phi(mdb, isect, cagevert);
1588                         mdb->phi[a] += rhs;
1589                 }
1590         }
1591 }
1592
1593 static void meshdeform_matrix_add_exterior_phi(MeshDeformBind *mdb, int x, int y, int z, int UNUSED(cagevert))
1594 {
1595         float phi, totweight;
1596         int i, a, acenter;
1597
1598         acenter= meshdeform_index(mdb, x, y, z, 0);
1599         if (mdb->tag[acenter] != MESHDEFORM_TAG_EXTERIOR || mdb->semibound[acenter])
1600                 return;
1601
1602         phi= 0.0f;
1603         totweight= 0.0f;
1604         for (i=1; i<=6; i++) {
1605                 a= meshdeform_index(mdb, x, y, z, i);
1606
1607                 if (a != -1 && mdb->semibound[a]) {
1608                         phi += mdb->phi[a];
1609                         totweight += 1.0f;
1610                 }
1611         }
1612
1613         if (totweight != 0.0f)
1614                 mdb->phi[acenter]= phi/totweight;
1615 }
1616
1617 static void meshdeform_matrix_solve(MeshDeformModifierData *mmd, MeshDeformBind *mdb)
1618 {
1619         NLContext *context;
1620         float vec[3], gridvec[3];
1621         int a, b, x, y, z, totvar;
1622         char message[256];
1623
1624         /* setup variable indices */
1625         mdb->varidx= MEM_callocN(sizeof(int)*mdb->size3, "MeshDeformDSvaridx");
1626         for (a=0, totvar=0; a<mdb->size3; a++)
1627                 mdb->varidx[a]= (mdb->tag[a] == MESHDEFORM_TAG_EXTERIOR)? -1: totvar++;
1628
1629         if (totvar == 0) {
1630                 MEM_freeN(mdb->varidx);
1631                 return;
1632         }
1633
1634         progress_bar(0, "Starting mesh deform solve");
1635
1636         /* setup opennl solver */
1637         nlNewContext();
1638         context= nlGetCurrent();
1639
1640         nlSolverParameteri(NL_NB_VARIABLES, totvar);
1641         nlSolverParameteri(NL_NB_ROWS, totvar);
1642         nlSolverParameteri(NL_NB_RIGHT_HAND_SIDES, 1);
1643
1644         nlBegin(NL_SYSTEM);
1645         nlBegin(NL_MATRIX);
1646
1647         /* build matrix */
1648         for (z=0; z<mdb->size; z++)
1649                 for (y=0; y<mdb->size; y++)
1650                         for (x=0; x<mdb->size; x++)
1651                                 meshdeform_matrix_add_cell(mdb, x, y, z);
1652
1653         /* solve for each cage vert */
1654         for (a=0; a<mdb->totcagevert; a++) {
1655                 if (a != 0) {
1656                         nlBegin(NL_SYSTEM);
1657                         nlBegin(NL_MATRIX);
1658                 }
1659
1660                 /* fill in right hand side and solve */
1661                 for (z=0; z<mdb->size; z++)
1662                         for (y=0; y<mdb->size; y++)
1663                                 for (x=0; x<mdb->size; x++)
1664                                         meshdeform_matrix_add_rhs(mdb, x, y, z, a);
1665
1666                 nlEnd(NL_MATRIX);
1667                 nlEnd(NL_SYSTEM);
1668
1669 #if 0
1670                 nlPrintMatrix();
1671 #endif
1672
1673                 if (nlSolveAdvanced(NULL, NL_TRUE)) {
1674                         for (z=0; z<mdb->size; z++)
1675                                 for (y=0; y<mdb->size; y++)
1676                                         for (x=0; x<mdb->size; x++)
1677                                                 meshdeform_matrix_add_semibound_phi(mdb, x, y, z, a);
1678
1679                         for (z=0; z<mdb->size; z++)
1680                                 for (y=0; y<mdb->size; y++)
1681                                         for (x=0; x<mdb->size; x++)
1682                                                 meshdeform_matrix_add_exterior_phi(mdb, x, y, z, a);
1683
1684                         for (b=0; b<mdb->size3; b++) {
1685                                 if (mdb->tag[b] != MESHDEFORM_TAG_EXTERIOR)
1686                                         mdb->phi[b]= nlGetVariable(0, mdb->varidx[b]);
1687                                 mdb->totalphi[b] += mdb->phi[b];
1688                         }
1689
1690                         if (mdb->weights) {
1691                                 /* static bind : compute weights for each vertex */
1692                                 for (b=0; b<mdb->totvert; b++) {
1693                                         if (mdb->inside[b]) {
1694                                                 copy_v3_v3(vec, mdb->vertexcos[b]);
1695                                                 gridvec[0]= (vec[0] - mdb->min[0] - mdb->halfwidth[0])/mdb->width[0];
1696                                                 gridvec[1]= (vec[1] - mdb->min[1] - mdb->halfwidth[1])/mdb->width[1];
1697                                                 gridvec[2]= (vec[2] - mdb->min[2] - mdb->halfwidth[2])/mdb->width[2];
1698
1699                                                 mdb->weights[b*mdb->totcagevert + a]= meshdeform_interp_w(mdb, gridvec, vec, a);
1700                                         }
1701                                 }
1702                         }
1703                         else {
1704                                 MDefBindInfluence *inf;
1705
1706                                 /* dynamic bind */
1707                                 for (b=0; b<mdb->size3; b++) {
1708                                         if (mdb->phi[b] >= MESHDEFORM_MIN_INFLUENCE) {
1709                                                 inf= BLI_memarena_alloc(mdb->memarena, sizeof(*inf));
1710                                                 inf->vertex= a;
1711                                                 inf->weight= mdb->phi[b];
1712                                                 inf->next= mdb->dyngrid[b];
1713                                                 mdb->dyngrid[b]= inf;
1714                                         }
1715                                 }
1716                         }
1717                 }
1718                 else {
1719                         modifier_setError(&mmd->modifier, "%s", TIP_("Failed to find bind solution (increase precision?)."));
1720                         error("Mesh Deform: failed to find bind solution.");
1721                         break;
1722                 }
1723
1724                 BLI_snprintf(message, sizeof(message), "Mesh deform solve %d / %d       |||", a+1, mdb->totcagevert);
1725                 progress_bar((float)(a+1)/(float)(mdb->totcagevert), message);
1726         }
1727
1728 #if 0
1729         /* sanity check */
1730         for (b=0; b<mdb->size3; b++)
1731                 if (mdb->tag[b] != MESHDEFORM_TAG_EXTERIOR)
1732                         if (fabs(mdb->totalphi[b] - 1.0f) > 1e-4)
1733                                 printf("totalphi deficiency [%s|%d] %d: %.10f\n",
1734                                         (mdb->tag[b] == MESHDEFORM_TAG_INTERIOR)? "interior": "boundary", mdb->semibound[b], mdb->varidx[b], mdb->totalphi[b]);
1735 #endif
1736         
1737         /* free */
1738         MEM_freeN(mdb->varidx);
1739
1740         nlDeleteContext(context);
1741 }
1742
1743 static void harmonic_coordinates_bind(Scene *UNUSED(scene), MeshDeformModifierData *mmd, MeshDeformBind *mdb)
1744 {
1745         MDefBindInfluence *inf;
1746         MDefInfluence *mdinf;
1747         MDefCell *cell;
1748         float center[3], vec[3], maxwidth, totweight;
1749         int a, b, x, y, z, totinside, offset;
1750
1751         /* compute bounding box of the cage mesh */
1752         INIT_MINMAX(mdb->min, mdb->max);
1753
1754         for (a=0; a<mdb->totcagevert; a++)
1755                 DO_MINMAX(mdb->cagecos[a], mdb->min, mdb->max);
1756
1757         /* allocate memory */
1758         mdb->size= (2<<(mmd->gridsize-1)) + 2;
1759         mdb->size3= mdb->size*mdb->size*mdb->size;
1760         mdb->tag= MEM_callocN(sizeof(int)*mdb->size3, "MeshDeformBindTag");
1761         mdb->phi= MEM_callocN(sizeof(float)*mdb->size3, "MeshDeformBindPhi");
1762         mdb->totalphi= MEM_callocN(sizeof(float)*mdb->size3, "MeshDeformBindTotalPhi");
1763         mdb->boundisect= MEM_callocN(sizeof(*mdb->boundisect)*mdb->size3, "MDefBoundIsect");
1764         mdb->semibound= MEM_callocN(sizeof(int)*mdb->size3, "MDefSemiBound");
1765
1766         mdb->inside= MEM_callocN(sizeof(int)*mdb->totvert, "MDefInside");
1767
1768         if (mmd->flag & MOD_MDEF_DYNAMIC_BIND)
1769                 mdb->dyngrid= MEM_callocN(sizeof(MDefBindInfluence*)*mdb->size3, "MDefDynGrid");
1770         else
1771                 mdb->weights= MEM_callocN(sizeof(float)*mdb->totvert*mdb->totcagevert, "MDefWeights");
1772
1773         mdb->memarena= BLI_memarena_new(BLI_MEMARENA_STD_BUFSIZE, "harmonic coords arena");
1774         BLI_memarena_use_calloc(mdb->memarena);
1775
1776         /* make bounding box equal size in all directions, add padding, and compute
1777          * width of the cells */
1778         maxwidth = -1.0f;
1779         for (a=0; a<3; a++)
1780                 if (mdb->max[a]-mdb->min[a] > maxwidth)
1781                         maxwidth= mdb->max[a]-mdb->min[a];
1782
1783         for (a=0; a<3; a++) {
1784                 center[a]= (mdb->min[a]+mdb->max[a])*0.5f;
1785                 mdb->min[a]= center[a] - maxwidth*0.5f;
1786                 mdb->max[a]= center[a] + maxwidth*0.5f;
1787
1788                 mdb->width[a]= (mdb->max[a]-mdb->min[a])/(mdb->size-4);
1789                 mdb->min[a] -= 2.1f*mdb->width[a];
1790                 mdb->max[a] += 2.1f*mdb->width[a];
1791
1792                 mdb->width[a]= (mdb->max[a]-mdb->min[a])/mdb->size;
1793                 mdb->halfwidth[a]= mdb->width[a]*0.5f;
1794         }
1795
1796         progress_bar(0, "Setting up mesh deform system");
1797
1798         totinside= 0;
1799         for (a=0; a<mdb->totvert; a++) {
1800                 copy_v3_v3(vec, mdb->vertexcos[a]);
1801                 mdb->inside[a]= meshdeform_inside_cage(mdb, vec);
1802                 if (mdb->inside[a])
1803                         totinside++;
1804         }
1805
1806         /* free temporary MDefBoundIsects */
1807         BLI_memarena_free(mdb->memarena);
1808         mdb->memarena= BLI_memarena_new(BLI_MEMARENA_STD_BUFSIZE, "harmonic coords arena");
1809
1810         /* start with all cells untyped */
1811         for (a=0; a<mdb->size3; a++)
1812                 mdb->tag[a]= MESHDEFORM_TAG_UNTYPED;
1813         
1814         /* detect intersections and tag boundary cells */
1815         for (z=0; z<mdb->size; z++)
1816                 for (y=0; y<mdb->size; y++)
1817                         for (x=0; x<mdb->size; x++)
1818                                 meshdeform_add_intersections(mdb, x, y, z);
1819
1820         /* compute exterior and interior tags */
1821         meshdeform_bind_floodfill(mdb);
1822
1823         for (z=0; z<mdb->size; z++)
1824                 for (y=0; y<mdb->size; y++)
1825                         for (x=0; x<mdb->size; x++)
1826                                 meshdeform_check_semibound(mdb, x, y, z);
1827
1828         /* solve */
1829         meshdeform_matrix_solve(mmd, mdb);
1830
1831         /* assign results */
1832         if (mmd->flag & MOD_MDEF_DYNAMIC_BIND) {
1833                 mmd->totinfluence= 0;
1834                 for (a=0; a<mdb->size3; a++)
1835                         for (inf=mdb->dyngrid[a]; inf; inf=inf->next)
1836                                 mmd->totinfluence++;
1837
1838                 /* convert MDefBindInfluences to smaller MDefInfluences */
1839                 mmd->dyngrid= MEM_callocN(sizeof(MDefCell)*mdb->size3, "MDefDynGrid");
1840                 mmd->dyninfluences= MEM_callocN(sizeof(MDefInfluence)*mmd->totinfluence, "MDefInfluence");
1841                 offset= 0;
1842                 for (a=0; a<mdb->size3; a++) {
1843                         cell= &mmd->dyngrid[a];
1844                         cell->offset= offset;
1845
1846                         totweight= 0.0f;
1847                         mdinf= mmd->dyninfluences + cell->offset;
1848                         for (inf=mdb->dyngrid[a]; inf; inf=inf->next, mdinf++) {
1849                                 mdinf->weight= inf->weight;
1850                                 mdinf->vertex= inf->vertex;
1851                                 totweight += mdinf->weight;
1852                                 cell->totinfluence++;
1853                         }
1854
1855                         if (totweight > 0.0f) {
1856                                 mdinf= mmd->dyninfluences + cell->offset;
1857                                 for (b=0; b<cell->totinfluence; b++, mdinf++)
1858                                         mdinf->weight /= totweight;
1859                         }
1860
1861                         offset += cell->totinfluence;
1862                 }
1863
1864                 mmd->dynverts= mdb->inside;
1865                 mmd->dyngridsize= mdb->size;
1866                 copy_v3_v3(mmd->dyncellmin, mdb->min);
1867                 mmd->dyncellwidth= mdb->width[0];
1868                 MEM_freeN(mdb->dyngrid);
1869         }
1870         else {
1871                 mmd->bindweights= mdb->weights;
1872                 MEM_freeN(mdb->inside);
1873         }
1874
1875         MEM_freeN(mdb->tag);
1876         MEM_freeN(mdb->phi);
1877         MEM_freeN(mdb->totalphi);
1878         MEM_freeN(mdb->boundisect);
1879         MEM_freeN(mdb->semibound);
1880         BLI_memarena_free(mdb->memarena);
1881 }
1882
1883 #if 0
1884 static void heat_weighting_bind(Scene *scene, DerivedMesh *dm, MeshDeformModifierData *mmd, MeshDeformBind *mdb)
1885 {
1886         LaplacianSystem *sys;
1887         MFace *mface= dm->getTessFaceArray(dm), *mf;
1888         int totvert= dm->getNumVerts(dm);
1889         int totface= dm->getNumTessFaces(dm);
1890         float solution, weight;
1891         int a, tottri, j, thrownerror = 0;
1892
1893         mdb->weights= MEM_callocN(sizeof(float)*mdb->totvert*mdb->totcagevert, "MDefWeights");
1894
1895         /* count triangles */
1896         for (tottri=0, a=0, mf=mface; a<totface; a++, mf++) {
1897                 tottri++;
1898                 if (mf->v4) tottri++;
1899         }
1900
1901         /* create laplacian */
1902         sys = laplacian_system_construct_begin(totvert, tottri, 1);
1903
1904         sys->heat.mface= mface;
1905         sys->heat.totface= totface;
1906         sys->heat.totvert= totvert;
1907         sys->heat.verts= mdb->vertexcos;
1908         sys->heat.source = mdb->cagecos;
1909         sys->heat.numsource= mdb->totcagevert;
1910
1911         heat_ray_tree_create(sys);
1912         heat_laplacian_create(sys);
1913
1914         laplacian_system_construct_end(sys);
1915
1916         /* compute weights per bone */
1917         for (j=0; j<mdb->totcagevert; j++) {
1918                 /* fill right hand side */
1919                 laplacian_begin_solve(sys, -1);
1920
1921                 for (a=0; a<totvert; a++)
1922                         if (heat_source_closest(sys, a, j))
1923                                 laplacian_add_right_hand_side(sys, a,
1924                                         sys->heat.H[a]*sys->heat.p[a]);
1925
1926                 /* solve */
1927                 if (laplacian_system_solve(sys)) {
1928                         /* load solution into vertex groups */
1929                         for (a=0; a<totvert; a++) {
1930                                 solution= laplacian_system_get_solution(a);
1931                                 
1932                                 weight= heat_limit_weight(solution);
1933                                 if (weight > 0.0f)
1934                                         mdb->weights[a*mdb->totcagevert + j] = weight;
1935                         }
1936                 }
1937                 else if (!thrownerror) {
1938                         error("Mesh Deform Heat Weighting:"
1939                                 " failed to find solution for one or more vertices");
1940                         thrownerror= 1;
1941                         break;
1942                 }
1943         }
1944
1945         /* free */
1946         heat_system_free(sys);
1947         laplacian_system_delete(sys);
1948
1949         mmd->bindweights= mdb->weights;
1950 }
1951 #endif
1952
1953 void mesh_deform_bind(Scene *scene, MeshDeformModifierData *mmd, float *vertexcos, int totvert, float cagemat[][4])
1954 {
1955         MeshDeformBind mdb;
1956         MVert *mvert;
1957         int a;
1958
1959         waitcursor(1);
1960         start_progress_bar();
1961
1962         memset(&mdb, 0, sizeof(MeshDeformBind));
1963
1964         /* get mesh and cage mesh */
1965         mdb.vertexcos= MEM_callocN(sizeof(float)*3*totvert, "MeshDeformCos");
1966         mdb.totvert= totvert;
1967         
1968         mdb.cagedm= mesh_create_derived_no_deform(scene, mmd->object, NULL, CD_MASK_BAREMESH);
1969         mdb.totcagevert= mdb.cagedm->getNumVerts(mdb.cagedm);
1970         mdb.cagecos= MEM_callocN(sizeof(*mdb.cagecos)*mdb.totcagevert, "MeshDeformBindCos");
1971         copy_m4_m4(mdb.cagemat, cagemat);
1972
1973         mvert= mdb.cagedm->getVertArray(mdb.cagedm);
1974         for (a=0; a<mdb.totcagevert; a++)
1975                 copy_v3_v3(mdb.cagecos[a], mvert[a].co);
1976         for (a=0; a<mdb.totvert; a++)
1977                 mul_v3_m4v3(mdb.vertexcos[a], mdb.cagemat, vertexcos + a*3);
1978
1979         /* solve */
1980 #if 0
1981         if (mmd->mode == MOD_MDEF_VOLUME)
1982                 harmonic_coordinates_bind(scene, mmd, &mdb);
1983         else
1984                 heat_weighting_bind(scene, dm, mmd, &mdb);
1985 #else
1986         harmonic_coordinates_bind(scene, mmd, &mdb);
1987 #endif
1988
1989         /* assign bind variables */
1990         mmd->bindcagecos= (float*)mdb.cagecos;
1991         mmd->totvert= mdb.totvert;
1992         mmd->totcagevert= mdb.totcagevert;
1993         copy_m4_m4(mmd->bindmat, mmd->object->obmat);
1994
1995         /* transform bindcagecos to world space */
1996         for (a=0; a<mdb.totcagevert; a++)
1997                 mul_m4_v3(mmd->object->obmat, mmd->bindcagecos+a*3);
1998
1999         /* free */
2000         mdb.cagedm->release(mdb.cagedm);
2001         MEM_freeN(mdb.vertexcos);
2002
2003         /* compact weights */
2004         modifier_mdef_compact_influences((ModifierData*)mmd);
2005
2006         end_progress_bar();
2007         waitcursor(0);
2008 }
2009