Merge branch 'master' into blender2.8
[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  * Contributor(s): none yet.
19  *
20  * ***** END GPL LICENSE BLOCK *****
21  * meshlaplacian.c: Algorithms using the mesh laplacian.
22  */
23
24 /** \file blender/editors/armature/meshlaplacian.c
25  *  \ingroup edarmature
26  */
27
28 #include "MEM_guardedalloc.h"
29
30 #include "DNA_object_types.h"
31 #include "DNA_mesh_types.h"
32 #include "DNA_scene_types.h"
33
34 #include "BLI_math.h"
35 #include "BLI_edgehash.h"
36 #include "BLI_memarena.h"
37 #include "BLI_string.h"
38 #include "BLI_alloca.h"
39
40 #include "BLT_translation.h"
41
42 #include "BKE_DerivedMesh.h"
43 #include "BKE_modifier.h"
44 #include "BKE_mesh.h"
45
46 #include "ED_mesh.h"
47 #include "ED_armature.h"
48
49 #include "DEG_depsgraph.h"
50
51 #include "eigen_capi.h"
52
53 #include "meshlaplacian.h"
54
55 /* ************* XXX *************** */
56 static void waitcursor(int UNUSED(val)) {}
57 static void progress_bar(int UNUSED(dummy_val), const char *UNUSED(dummy)) {}
58 static void start_progress_bar(void) {}
59 static void end_progress_bar(void) {}
60 static void error(const char *str) { printf("error: %s\n", str); }
61 /* ************* XXX *************** */
62
63
64 /************************** Laplacian System *****************************/
65
66 struct LaplacianSystem {
67         LinearSolver *context;  /* linear solver */
68
69         int totvert, totface;
70
71         float **verts;          /* vertex coordinates */
72         float *varea;           /* vertex weights for laplacian computation */
73         char *vpinned;          /* vertex pinning */
74         int (*faces)[3];        /* face vertex indices */
75         float (*fweights)[3];   /* cotangent weights per face */
76
77         int areaweights;        /* use area in cotangent weights? */
78         int storeweights;       /* store cotangent weights in fweights */
79         bool variablesdone;     /* variables set in linear system */
80
81         EdgeHash *edgehash;     /* edge hash for construction */
82
83         struct HeatWeighting {
84                 const MLoopTri *mlooptri;
85                 const MLoop *mloop;     /* needed to find vertices by index */
86                 int totvert;
87                 int tottri;
88                 float (*verts)[3];  /* vertex coordinates */
89                 float (*vnors)[3];  /* vertex normals */
90
91                 float (*root)[3];   /* bone root */
92                 float (*tip)[3];    /* bone tip */
93                 float (*source)[3]; /* vertex source */
94                 int numsource;
95
96                 float *H;           /* diagonal H matrix */
97                 float *p;           /* values from all p vectors */
98                 float *mindist;     /* minimum distance to a bone for all vertices */
99                 
100                 BVHTree   *bvhtree; /* ray tracing acceleration structure */
101                 const MLoopTri **vltree;  /* a looptri that the vertex belongs to */
102         } heat;
103 };
104
105 /* Laplacian matrix construction */
106
107 /* Computation of these weights for the laplacian is based on:
108  * "Discrete Differential-Geometry Operators for Triangulated 2-Manifolds",
109  * Meyer et al, 2002. Section 3.5, formula (8).
110  * 
111  * We do it a bit different by going over faces instead of going over each
112  * vertex and adjacent faces, since we don't store this adjacency. Also, the
113  * formulas are tweaked a bit to work for non-manifold meshes. */
114
115 static void laplacian_increase_edge_count(EdgeHash *edgehash, int v1, int v2)
116 {
117         void **p;
118
119         if (BLI_edgehash_ensure_p(edgehash, v1, v2, &p))
120                 *p = (void *)((intptr_t)*p + (intptr_t)1);
121         else
122                 *p = (void *)((intptr_t)1);
123 }
124
125 static int laplacian_edge_count(EdgeHash *edgehash, int v1, int v2)
126 {
127         return (int)(intptr_t)BLI_edgehash_lookup(edgehash, v1, v2);
128 }
129
130 static void laplacian_triangle_area(LaplacianSystem *sys, int i1, int i2, int i3)
131 {
132         float t1, t2, t3, len1, len2, len3, area;
133         float *varea = sys->varea, *v1, *v2, *v3;
134         int obtuse = 0;
135
136         v1 = sys->verts[i1];
137         v2 = sys->verts[i2];
138         v3 = sys->verts[i3];
139
140         t1 = cotangent_tri_weight_v3(v1, v2, v3);
141         t2 = cotangent_tri_weight_v3(v2, v3, v1);
142         t3 = cotangent_tri_weight_v3(v3, v1, v2);
143
144         if      (angle_v3v3v3(v2, v1, v3) > DEG2RADF(90.0f)) obtuse = 1;
145         else if (angle_v3v3v3(v1, v2, v3) > DEG2RADF(90.0f)) obtuse = 2;
146         else if (angle_v3v3v3(v1, v3, v2) > DEG2RADF(90.0f)) obtuse = 3;
147
148         if (obtuse > 0) {
149                 area = area_tri_v3(v1, v2, v3);
150
151                 varea[i1] += (obtuse == 1) ? area : area * 0.5f;
152                 varea[i2] += (obtuse == 2) ? area : area * 0.5f;
153                 varea[i3] += (obtuse == 3) ? area : area * 0.5f;
154         }
155         else {
156                 len1 = len_v3v3(v2, v3);
157                 len2 = len_v3v3(v1, v3);
158                 len3 = len_v3v3(v1, v2);
159
160                 t1 *= len1 * len1;
161                 t2 *= len2 * len2;
162                 t3 *= len3 * len3;
163
164                 varea[i1] += (t2 + t3) * 0.25f;
165                 varea[i2] += (t1 + t3) * 0.25f;
166                 varea[i3] += (t1 + t2) * 0.25f;
167         }
168 }
169
170 static void laplacian_triangle_weights(LaplacianSystem *sys, int f, int i1, int i2, int i3)
171 {
172         float t1, t2, t3;
173         float *varea = sys->varea, *v1, *v2, *v3;
174
175         v1 = sys->verts[i1];
176         v2 = sys->verts[i2];
177         v3 = sys->verts[i3];
178
179         /* instead of *0.5 we divided by the number of faces of the edge, it still
180          * needs to be verified that this is indeed the correct thing to do! */
181         t1 = cotangent_tri_weight_v3(v1, v2, v3) / laplacian_edge_count(sys->edgehash, i2, i3);
182         t2 = cotangent_tri_weight_v3(v2, v3, v1) / laplacian_edge_count(sys->edgehash, i3, i1);
183         t3 = cotangent_tri_weight_v3(v3, v1, v2) / laplacian_edge_count(sys->edgehash, i1, i2);
184
185         EIG_linear_solver_matrix_add(sys->context, i1, i1, (t2 + t3) * varea[i1]);
186         EIG_linear_solver_matrix_add(sys->context, i2, i2, (t1 + t3) * varea[i2]);
187         EIG_linear_solver_matrix_add(sys->context, i3, i3, (t1 + t2) * varea[i3]);
188
189         EIG_linear_solver_matrix_add(sys->context, i1, i2, -t3 * varea[i1]);
190         EIG_linear_solver_matrix_add(sys->context, i2, i1, -t3 * varea[i2]);
191
192         EIG_linear_solver_matrix_add(sys->context, i2, i3, -t1 * varea[i2]);
193         EIG_linear_solver_matrix_add(sys->context, i3, i2, -t1 * varea[i3]);
194
195         EIG_linear_solver_matrix_add(sys->context, i3, i1, -t2 * varea[i3]);
196         EIG_linear_solver_matrix_add(sys->context, i1, i3, -t2 * varea[i1]);
197
198         if (sys->storeweights) {
199                 sys->fweights[f][0] = t1 * varea[i1];
200                 sys->fweights[f][1] = t2 * varea[i2];
201                 sys->fweights[f][2] = t3 * varea[i3];
202         }
203 }
204
205 static LaplacianSystem *laplacian_system_construct_begin(int totvert, int totface, int lsq)
206 {
207         LaplacianSystem *sys;
208
209         sys = MEM_callocN(sizeof(LaplacianSystem), "LaplacianSystem");
210
211         sys->verts = MEM_callocN(sizeof(float *) * totvert, "LaplacianSystemVerts");
212         sys->vpinned = MEM_callocN(sizeof(char) * totvert, "LaplacianSystemVpinned");
213         sys->faces = MEM_callocN(sizeof(int) * 3 * totface, "LaplacianSystemFaces");
214
215         sys->totvert = 0;
216         sys->totface = 0;
217
218         sys->areaweights = 1;
219         sys->storeweights = 0;
220
221         /* create linear solver */
222         if (lsq)
223                 sys->context = EIG_linear_least_squares_solver_new(0, totvert, 1);
224         else
225                 sys->context = EIG_linear_solver_new(0, totvert, 1);
226
227         return sys;
228 }
229
230 void laplacian_add_vertex(LaplacianSystem *sys, float *co, int pinned)
231 {
232         sys->verts[sys->totvert] = co;
233         sys->vpinned[sys->totvert] = pinned;
234         sys->totvert++;
235 }
236
237 void laplacian_add_triangle(LaplacianSystem *sys, int v1, int v2, int v3)
238 {
239         sys->faces[sys->totface][0] = v1;
240         sys->faces[sys->totface][1] = v2;
241         sys->faces[sys->totface][2] = v3;
242         sys->totface++;
243 }
244
245 static void laplacian_system_construct_end(LaplacianSystem *sys)
246 {
247         int (*face)[3];
248         int a, totvert = sys->totvert, totface = sys->totface;
249
250         laplacian_begin_solve(sys, 0);
251
252         sys->varea = MEM_callocN(sizeof(float) * totvert, "LaplacianSystemVarea");
253
254         sys->edgehash = BLI_edgehash_new_ex(__func__, BLI_EDGEHASH_SIZE_GUESS_FROM_POLYS(sys->totface));
255         for (a = 0, face = sys->faces; a < sys->totface; a++, face++) {
256                 laplacian_increase_edge_count(sys->edgehash, (*face)[0], (*face)[1]);
257                 laplacian_increase_edge_count(sys->edgehash, (*face)[1], (*face)[2]);
258                 laplacian_increase_edge_count(sys->edgehash, (*face)[2], (*face)[0]);
259         }
260
261         if (sys->areaweights)
262                 for (a = 0, face = sys->faces; a < sys->totface; a++, face++)
263                         laplacian_triangle_area(sys, (*face)[0], (*face)[1], (*face)[2]);
264         
265         for (a = 0; a < totvert; a++) {
266                 if (sys->areaweights) {
267                         if (sys->varea[a] != 0.0f)
268                                 sys->varea[a] = 0.5f / sys->varea[a];
269                 }
270                 else
271                         sys->varea[a] = 1.0f;
272
273                 /* for heat weighting */
274                 if (sys->heat.H)
275                         EIG_linear_solver_matrix_add(sys->context, a, a, sys->heat.H[a]);
276         }
277
278         if (sys->storeweights)
279                 sys->fweights = MEM_callocN(sizeof(float) * 3 * totface, "LaplacianFWeight");
280         
281         for (a = 0, face = sys->faces; a < totface; a++, face++)
282                 laplacian_triangle_weights(sys, a, (*face)[0], (*face)[1], (*face)[2]);
283
284         MEM_freeN(sys->faces);
285         sys->faces = NULL;
286
287         if (sys->varea) {
288                 MEM_freeN(sys->varea);
289                 sys->varea = NULL;
290         }
291
292         BLI_edgehash_free(sys->edgehash, NULL);
293         sys->edgehash = NULL;
294 }
295
296 static void laplacian_system_delete(LaplacianSystem *sys)
297 {
298         if (sys->verts) MEM_freeN(sys->verts);
299         if (sys->varea) MEM_freeN(sys->varea);
300         if (sys->vpinned) MEM_freeN(sys->vpinned);
301         if (sys->faces) MEM_freeN(sys->faces);
302         if (sys->fweights) MEM_freeN(sys->fweights);
303
304         EIG_linear_solver_delete(sys->context);
305         MEM_freeN(sys);
306 }
307
308 void laplacian_begin_solve(LaplacianSystem *sys, int index)
309 {
310         int a;
311
312         if (!sys->variablesdone) {
313                 if (index >= 0) {
314                         for (a = 0; a < sys->totvert; a++) {
315                                 if (sys->vpinned[a]) {
316                                         EIG_linear_solver_variable_set(sys->context, 0, a, sys->verts[a][index]);
317                                         EIG_linear_solver_variable_lock(sys->context, a);
318                                 }
319                         }
320                 }
321
322                 sys->variablesdone = true;
323         }
324 }
325
326 void laplacian_add_right_hand_side(LaplacianSystem *sys, int v, float value)
327 {
328         EIG_linear_solver_right_hand_side_add(sys->context, 0, v, value);
329 }
330
331 int laplacian_system_solve(LaplacianSystem *sys)
332 {
333         sys->variablesdone = false;
334
335         //EIG_linear_solver_print_matrix(sys->context, );
336
337         return EIG_linear_solver_solve(sys->context);
338 }
339
340 float laplacian_system_get_solution(LaplacianSystem *sys, int v)
341 {
342         return EIG_linear_solver_variable_get(sys->context, 0, v);
343 }
344
345 /************************* Heat Bone Weighting ******************************/
346 /* From "Automatic Rigging and Animation of 3D Characters"
347  * Ilya Baran and Jovan Popovic, SIGGRAPH 2007 */
348
349 #define C_WEIGHT            1.0f
350 #define WEIGHT_LIMIT_START  0.05f
351 #define WEIGHT_LIMIT_END    0.025f
352 #define DISTANCE_EPSILON    1e-4f
353
354 typedef struct BVHCallbackUserData {
355         float start[3];
356         float vec[3];
357         LaplacianSystem *sys;
358 } BVHCallbackUserData;
359
360 static void bvh_callback(void *userdata, int index, const BVHTreeRay *ray, BVHTreeRayHit *hit)
361 {
362         BVHCallbackUserData *data = (struct BVHCallbackUserData *)userdata;
363         const MLoopTri *lt = &data->sys->heat.mlooptri[index];
364         const MLoop *mloop = data->sys->heat.mloop;
365         float (*verts)[3] = data->sys->heat.verts;
366         const float *vtri_co[3];
367         float dist_test;
368
369         vtri_co[0] = verts[mloop[lt->tri[0]].v];
370         vtri_co[1] = verts[mloop[lt->tri[1]].v];
371         vtri_co[2] = verts[mloop[lt->tri[2]].v];
372
373 #ifdef USE_KDOPBVH_WATERTIGHT
374         if (isect_ray_tri_watertight_v3(data->start, ray->isect_precalc, UNPACK3(vtri_co), &dist_test, NULL))
375 #else
376         UNUSED_VARS(ray);
377         if (isect_ray_tri_v3(data->start, data->vec, UNPACK3(vtri_co), &dist_test, NULL))
378 #endif
379         {
380                 if (dist_test < hit->dist) {
381                         float n[3];
382                         normal_tri_v3(n, UNPACK3(vtri_co));
383                         if (dot_v3v3(n, data->vec) < -1e-5f) {
384                                 hit->index = index;
385                                 hit->dist = dist_test;
386                         }
387                 }
388         }
389 }
390
391 /* Raytracing for vertex to bone/vertex visibility */
392 static void heat_ray_tree_create(LaplacianSystem *sys)
393 {
394         const MLoopTri *looptri = sys->heat.mlooptri;
395         const MLoop *mloop = sys->heat.mloop;
396         float (*verts)[3] = sys->heat.verts;
397         int tottri = sys->heat.tottri;
398         int totvert = sys->heat.totvert;
399         int a;
400
401         sys->heat.bvhtree = BLI_bvhtree_new(tottri, 0.0f, 4, 6);
402         sys->heat.vltree = MEM_callocN(sizeof(MLoopTri *) * totvert, "HeatVFaces");
403
404         for (a = 0; a < tottri; a++) {
405                 const MLoopTri *lt = &looptri[a];
406                 float bb[6];
407                 int vtri[3];
408                 
409                 vtri[0] = mloop[lt->tri[0]].v;
410                 vtri[1] = mloop[lt->tri[1]].v;
411                 vtri[2] = mloop[lt->tri[2]].v;
412
413                 INIT_MINMAX(bb, bb + 3);
414                 minmax_v3v3_v3(bb, bb + 3, verts[vtri[0]]);
415                 minmax_v3v3_v3(bb, bb + 3, verts[vtri[1]]);
416                 minmax_v3v3_v3(bb, bb + 3, verts[vtri[2]]);
417
418                 BLI_bvhtree_insert(sys->heat.bvhtree, a, bb, 2);
419                 
420                 //Setup inverse pointers to use on isect.orig
421                 sys->heat.vltree[vtri[0]] = lt;
422                 sys->heat.vltree[vtri[1]] = lt;
423                 sys->heat.vltree[vtri[2]] = lt;
424         }
425
426         BLI_bvhtree_balance(sys->heat.bvhtree); 
427 }
428
429 static int heat_ray_source_visible(LaplacianSystem *sys, int vertex, int source)
430 {
431         BVHTreeRayHit hit;
432         BVHCallbackUserData data;
433         const MLoopTri *lt;
434         float end[3];
435         int visible;
436
437         lt = sys->heat.vltree[vertex];
438         if (lt == NULL)
439                 return 1;
440
441         data.sys = sys;
442         copy_v3_v3(data.start, sys->heat.verts[vertex]);
443
444         closest_to_line_segment_v3(end, data.start, sys->heat.root[source], sys->heat.tip[source]);
445
446         sub_v3_v3v3(data.vec, end, data.start);
447         madd_v3_v3v3fl(data.start, data.start, data.vec, 1e-5);
448         mul_v3_fl(data.vec, 1.0f - 2e-5f);
449
450         /* pass normalized vec + distance to bvh */
451         hit.index = -1;
452         hit.dist = normalize_v3(data.vec);
453
454         visible = BLI_bvhtree_ray_cast(sys->heat.bvhtree, data.start, data.vec, 0.0f, &hit, bvh_callback, (void *)&data) == -1;
455
456         return visible;
457 }
458
459 static float heat_source_distance(LaplacianSystem *sys, int vertex, int source)
460 {
461         float closest[3], d[3], dist, cosine;
462         
463         /* compute euclidian distance */
464         closest_to_line_segment_v3(closest, sys->heat.verts[vertex], sys->heat.root[source], sys->heat.tip[source]);
465
466         sub_v3_v3v3(d, sys->heat.verts[vertex], closest);
467         dist = normalize_v3(d);
468
469         /* if the vertex normal does not point along the bone, increase distance */
470         cosine = dot_v3v3(d, sys->heat.vnors[vertex]);
471
472         return dist / (0.5f * (cosine + 1.001f));
473 }
474
475 static int heat_source_closest(LaplacianSystem *sys, int vertex, int source)
476 {
477         float dist;
478
479         dist = heat_source_distance(sys, vertex, source);
480
481         if (dist <= sys->heat.mindist[vertex] * (1.0f + DISTANCE_EPSILON))
482                 if (heat_ray_source_visible(sys, vertex, source))
483                         return 1;
484                 
485         return 0;
486 }
487
488 static void heat_set_H(LaplacianSystem *sys, int vertex)
489 {
490         float dist, mindist, h;
491         int j, numclosest = 0;
492
493         mindist = 1e10;
494
495         /* compute minimum distance */
496         for (j = 0; j < sys->heat.numsource; j++) {
497                 dist = heat_source_distance(sys, vertex, j);
498
499                 if (dist < mindist)
500                         mindist = dist;
501         }
502
503         sys->heat.mindist[vertex] = mindist;
504
505         /* count number of sources with approximately this minimum distance */
506         for (j = 0; j < sys->heat.numsource; j++)
507                 if (heat_source_closest(sys, vertex, j))
508                         numclosest++;
509
510         sys->heat.p[vertex] = (numclosest > 0) ? 1.0f / numclosest : 0.0f;
511
512         /* compute H entry */
513         if (numclosest > 0) {
514                 mindist = max_ff(mindist, 1e-4f);
515                 h = numclosest * C_WEIGHT / (mindist * mindist);
516         }
517         else
518                 h = 0.0f;
519         
520         sys->heat.H[vertex] = h;
521 }
522
523 static void heat_calc_vnormals(LaplacianSystem *sys)
524 {
525         float fnor[3];
526         int a, v1, v2, v3, (*face)[3];
527
528         sys->heat.vnors = MEM_callocN(sizeof(float) * 3 * sys->totvert, "HeatVNors");
529
530         for (a = 0, face = sys->faces; a < sys->totface; a++, face++) {
531                 v1 = (*face)[0];
532                 v2 = (*face)[1];
533                 v3 = (*face)[2];
534
535                 normal_tri_v3(fnor, sys->verts[v1], sys->verts[v2], sys->verts[v3]);
536                 
537                 add_v3_v3(sys->heat.vnors[v1], fnor);
538                 add_v3_v3(sys->heat.vnors[v2], fnor);
539                 add_v3_v3(sys->heat.vnors[v3], fnor);
540         }
541
542         for (a = 0; a < sys->totvert; a++)
543                 normalize_v3(sys->heat.vnors[a]);
544 }
545
546 static void heat_laplacian_create(LaplacianSystem *sys)
547 {
548         const MLoopTri *mlooptri = sys->heat.mlooptri, *lt;
549         const MLoop *mloop = sys->heat.mloop;
550         int tottri = sys->heat.tottri;
551         int totvert = sys->heat.totvert;
552         int a;
553
554         /* heat specific definitions */
555         sys->heat.mindist = MEM_callocN(sizeof(float) * totvert, "HeatMinDist");
556         sys->heat.H = MEM_callocN(sizeof(float) * totvert, "HeatH");
557         sys->heat.p = MEM_callocN(sizeof(float) * totvert, "HeatP");
558
559         /* add verts and faces to laplacian */
560         for (a = 0; a < totvert; a++)
561                 laplacian_add_vertex(sys, sys->heat.verts[a], 0);
562
563         for (a = 0, lt = mlooptri; a < tottri; a++, lt++) {
564                 int vtri[3];
565                 vtri[0] = mloop[lt->tri[0]].v;
566                 vtri[1] = mloop[lt->tri[1]].v;
567                 vtri[2] = mloop[lt->tri[2]].v;
568                 laplacian_add_triangle(sys, UNPACK3(vtri));
569         }
570
571         /* for distance computation in set_H */
572         heat_calc_vnormals(sys);
573
574         for (a = 0; a < totvert; a++)
575                 heat_set_H(sys, a);
576 }
577
578 static void heat_system_free(LaplacianSystem *sys)
579 {
580         BLI_bvhtree_free(sys->heat.bvhtree);
581         MEM_freeN((void *)sys->heat.vltree);
582         MEM_freeN((void *)sys->heat.mlooptri);
583
584         MEM_freeN(sys->heat.mindist);
585         MEM_freeN(sys->heat.H);
586         MEM_freeN(sys->heat.p);
587         MEM_freeN(sys->heat.vnors);
588 }
589
590 static float heat_limit_weight(float weight)
591 {
592         float t;
593
594         if (weight < WEIGHT_LIMIT_END) {
595                 return 0.0f;
596         }
597         else if (weight < WEIGHT_LIMIT_START) {
598                 t = (weight - WEIGHT_LIMIT_END) / (WEIGHT_LIMIT_START - WEIGHT_LIMIT_END);
599                 return t * WEIGHT_LIMIT_START;
600         }
601         else
602                 return weight;
603 }
604
605 void heat_bone_weighting(
606         Object *ob, Mesh *me, float (*verts)[3], int numsource,
607         bDeformGroup **dgrouplist, bDeformGroup **dgroupflip,
608         float (*root)[3], float (*tip)[3], int *selected, const char **err_str)
609 {
610         LaplacianSystem *sys;
611         MLoopTri *mlooptri;
612         MPoly *mp;
613         MLoop *ml;
614         float solution, weight;
615         int *vertsflipped = NULL, *mask = NULL;
616         int a, tottri, j, bbone, firstsegment, lastsegment;
617         bool use_topology = (me->editflag & ME_EDIT_MIRROR_TOPO) != 0;
618
619         MVert *mvert = me->mvert;
620         bool use_vert_sel = (me->editflag & ME_EDIT_PAINT_VERT_SEL) != 0;
621         bool use_face_sel = (me->editflag & ME_EDIT_PAINT_FACE_SEL) != 0;
622
623         *err_str = NULL;
624
625         /* bone heat needs triangulated faces */
626         tottri = poly_to_tri_count(me->totpoly, me->totloop);
627
628         /* count triangles and create mask */
629         if (ob->mode & OB_MODE_WEIGHT_PAINT &&
630             (use_face_sel || use_vert_sel))
631         {
632                 mask = MEM_callocN(sizeof(int) * me->totvert, "heat_bone_weighting mask");
633
634                 /*  (added selectedVerts content for vertex mask, they used to just equal 1) */
635                 if (use_vert_sel) {
636                         for (a = 0, mp = me->mpoly; a < me->totpoly; mp++, a++) {
637                                 for (j = 0, ml = me->mloop + mp->loopstart; j < mp->totloop; j++, ml++) {
638                                         mask[ml->v] = (mvert[ml->v].flag & SELECT) != 0;
639                                 }
640                         }
641                 }
642                 else if (use_face_sel) {
643                         for (a = 0, mp = me->mpoly; a < me->totpoly; mp++, a++) {
644                                 if (mp->flag & ME_FACE_SEL) {
645                                         for (j = 0, ml = me->mloop + mp->loopstart; j < mp->totloop; j++, ml++) {
646                                                 mask[ml->v] = 1;
647                                         }
648                                 }
649                         }
650                 }
651         }
652
653         /* create laplacian */
654         sys = laplacian_system_construct_begin(me->totvert, tottri, 1);
655
656         sys->heat.tottri = poly_to_tri_count(me->totpoly, me->totloop);
657         mlooptri = MEM_mallocN(sizeof(*sys->heat.mlooptri) * sys->heat.tottri, __func__);
658
659         BKE_mesh_recalc_looptri(
660                 me->mloop, me->mpoly,
661                 me->mvert,
662                 me->totloop, me->totpoly,
663                 mlooptri);
664
665         sys->heat.mlooptri = mlooptri;
666         sys->heat.mloop = me->mloop;
667         sys->heat.totvert = me->totvert;
668         sys->heat.verts = verts;
669         sys->heat.root = root;
670         sys->heat.tip = tip;
671         sys->heat.numsource = numsource;
672
673         heat_ray_tree_create(sys);
674         heat_laplacian_create(sys);
675
676         laplacian_system_construct_end(sys);
677
678         if (dgroupflip) {
679                 vertsflipped = MEM_callocN(sizeof(int) * me->totvert, "vertsflipped");
680                 for (a = 0; a < me->totvert; a++)
681                         vertsflipped[a] = mesh_get_x_mirror_vert(ob, NULL, a, use_topology);
682         }
683         
684         /* compute weights per bone */
685         for (j = 0; j < numsource; j++) {
686                 if (!selected[j])
687                         continue;
688
689                 firstsegment = (j == 0 || dgrouplist[j - 1] != dgrouplist[j]);
690                 lastsegment = (j == numsource - 1 || dgrouplist[j] != dgrouplist[j + 1]);
691                 bbone = !(firstsegment && lastsegment);
692
693                 /* clear weights */
694                 if (bbone && firstsegment) {
695                         for (a = 0; a < me->totvert; a++) {
696                                 if (mask && !mask[a])
697                                         continue;
698
699                                 ED_vgroup_vert_remove(ob, dgrouplist[j], a);
700                                 if (vertsflipped && dgroupflip[j] && vertsflipped[a] >= 0)
701                                         ED_vgroup_vert_remove(ob, dgroupflip[j], vertsflipped[a]);
702                         }
703                 }
704
705                 /* fill right hand side */
706                 laplacian_begin_solve(sys, -1);
707
708                 for (a = 0; a < me->totvert; a++)
709                         if (heat_source_closest(sys, a, j))
710                                 laplacian_add_right_hand_side(sys, a,
711                                                               sys->heat.H[a] * sys->heat.p[a]);
712
713                 /* solve */
714                 if (laplacian_system_solve(sys)) {
715                         /* load solution into vertex groups */
716                         for (a = 0; a < me->totvert; a++) {
717                                 if (mask && !mask[a])
718                                         continue;
719
720                                 solution = laplacian_system_get_solution(sys, a);
721                                 
722                                 if (bbone) {
723                                         if (solution > 0.0f)
724                                                 ED_vgroup_vert_add(ob, dgrouplist[j], a, solution,
725                                                                    WEIGHT_ADD);
726                                 }
727                                 else {
728                                         weight = heat_limit_weight(solution);
729                                         if (weight > 0.0f)
730                                                 ED_vgroup_vert_add(ob, dgrouplist[j], a, weight,
731                                                                    WEIGHT_REPLACE);
732                                         else
733                                                 ED_vgroup_vert_remove(ob, dgrouplist[j], a);
734                                 }
735
736                                 /* do same for mirror */
737                                 if (vertsflipped && dgroupflip[j] && vertsflipped[a] >= 0) {
738                                         if (bbone) {
739                                                 if (solution > 0.0f)
740                                                         ED_vgroup_vert_add(ob, dgroupflip[j], vertsflipped[a],
741                                                                            solution, WEIGHT_ADD);
742                                         }
743                                         else {
744                                                 weight = heat_limit_weight(solution);
745                                                 if (weight > 0.0f)
746                                                         ED_vgroup_vert_add(ob, dgroupflip[j], vertsflipped[a],
747                                                                            weight, WEIGHT_REPLACE);
748                                                 else
749                                                         ED_vgroup_vert_remove(ob, dgroupflip[j], vertsflipped[a]);
750                                         }
751                                 }
752                         }
753                 }
754                 else if (*err_str == NULL) {
755                         *err_str = N_("Bone Heat Weighting: failed to find solution for one or more bones");
756                         break;
757                 }
758
759                 /* remove too small vertex weights */
760                 if (bbone && lastsegment) {
761                         for (a = 0; a < me->totvert; a++) {
762                                 if (mask && !mask[a])
763                                         continue;
764
765                                 weight = ED_vgroup_vert_weight(ob, dgrouplist[j], a);
766                                 weight = heat_limit_weight(weight);
767                                 if (weight <= 0.0f)
768                                         ED_vgroup_vert_remove(ob, dgrouplist[j], a);
769
770                                 if (vertsflipped && dgroupflip[j] && vertsflipped[a] >= 0) {
771                                         weight = ED_vgroup_vert_weight(ob, dgroupflip[j], vertsflipped[a]);
772                                         weight = heat_limit_weight(weight);
773                                         if (weight <= 0.0f)
774                                                 ED_vgroup_vert_remove(ob, dgroupflip[j], vertsflipped[a]);
775                                 }
776                         }
777                 }
778         }
779
780         /* free */
781         if (vertsflipped) MEM_freeN(vertsflipped);
782         if (mask) MEM_freeN(mask);
783
784         heat_system_free(sys);
785
786         laplacian_system_delete(sys);
787 }
788
789 /************************** Harmonic Coordinates ****************************/
790 /* From "Harmonic Coordinates for Character Articulation",
791  * Pushkar Joshi, Mark Meyer, Tony DeRose, Brian Green and Tom Sanocki,
792  * SIGGRAPH 2007. */
793
794 #define EPSILON 0.0001f
795
796 #define MESHDEFORM_TAG_UNTYPED  0
797 #define MESHDEFORM_TAG_BOUNDARY 1
798 #define MESHDEFORM_TAG_INTERIOR 2
799 #define MESHDEFORM_TAG_EXTERIOR 3
800
801 /** minimum length for #MDefBoundIsect.len */
802 #define MESHDEFORM_LEN_THRESHOLD 1e-6f
803
804 #define MESHDEFORM_MIN_INFLUENCE 0.0005f
805
806 static const int MESHDEFORM_OFFSET[7][3] = {
807         {0, 0, 0}, {1, 0, 0}, {-1, 0, 0}, {0, 1, 0}, {0, -1, 0}, {0, 0, 1}, {0, 0, -1}
808 };
809
810 typedef struct MDefBoundIsect {
811         /* intersection on the cage 'cagecos' */
812         float co[3];
813         /* non-facing intersections are considered interior */
814         bool facing;
815         /* ray-cast index aligned with MPoly (ray-hit-triangle isn't needed) */
816         int poly_index;
817         /* distance from 'co' to the ray-cast start (clamped to avoid zero division) */
818         float len;
819         /* weights aligned with the MPoly's loop indices */
820         float poly_weights[0];
821 } MDefBoundIsect;
822
823 typedef struct MDefBindInfluence {
824         struct MDefBindInfluence *next;
825         float weight;
826         int vertex;
827 } MDefBindInfluence;
828
829 typedef struct MeshDeformBind {
830         /* grid dimensions */
831         float min[3], max[3];
832         float width[3], halfwidth[3];
833         int size, size3;
834
835         /* meshes */
836         DerivedMesh *cagedm;
837         float (*cagecos)[3];
838         float (*vertexcos)[3];
839         int totvert, totcagevert;
840
841         /* grids */
842         MemArena *memarena;
843         MDefBoundIsect *(*boundisect)[6];
844         int *semibound;
845         int *tag;
846         float *phi, *totalphi;
847
848         /* mesh stuff */
849         int *inside;
850         float *weights;
851         MDefBindInfluence **dyngrid;
852         float cagemat[4][4];
853
854         /* direct solver */
855         int *varidx;
856         
857         BVHTree *bvhtree;
858         BVHTreeFromMesh bvhdata;
859
860         /* avoid DM function calls during intersections */
861         struct {
862                 const MPoly *mpoly;
863                 const MLoop *mloop;
864                 const MLoopTri *looptri;
865                 const float (*poly_nors)[3];
866         } cagedm_cache;
867 } MeshDeformBind;
868
869 typedef struct MeshDeformIsect {
870         float start[3];
871         float vec[3];
872         float vec_length;
873         float lambda;
874
875         bool isect;
876         float u, v;
877         
878 } MeshDeformIsect;
879
880 /* ray intersection */
881
882 struct MeshRayCallbackData {
883         MeshDeformBind *mdb;
884         MeshDeformIsect *isec;
885 };
886
887 static void harmonic_ray_callback(void *userdata, int index, const BVHTreeRay *ray, BVHTreeRayHit *hit)
888 {
889         struct MeshRayCallbackData *data = userdata;
890         MeshDeformBind *mdb = data->mdb;
891         const MLoop *mloop = mdb->cagedm_cache.mloop;
892         const MLoopTri *looptri = mdb->cagedm_cache.looptri, *lt;
893         const float (*poly_nors)[3] = mdb->cagedm_cache.poly_nors;
894         MeshDeformIsect *isec = data->isec;
895         float no[3], co[3], dist;
896         float *face[3];
897         
898         lt = &looptri[index];
899         
900         face[0] = mdb->cagecos[mloop[lt->tri[0]].v];
901         face[1] = mdb->cagecos[mloop[lt->tri[1]].v];
902         face[2] = mdb->cagecos[mloop[lt->tri[2]].v];
903
904         if (!isect_ray_tri_watertight_v3(
905                 ray->origin, ray->isect_precalc, UNPACK3(face), &dist, NULL))
906         {
907                 return;
908         }
909
910         if (poly_nors) {
911                 copy_v3_v3(no, poly_nors[lt->poly]);
912         }
913         else {
914                 normal_tri_v3(no, UNPACK3(face));
915         }
916
917         madd_v3_v3v3fl(co, ray->origin, ray->direction, dist);
918         dist /= isec->vec_length;
919         if (dist < hit->dist) {
920                 hit->index = index;
921                 hit->dist = dist;
922                 copy_v3_v3(hit->co, co);
923                 
924                 isec->isect = (dot_v3v3(no, ray->direction) <= 0.0f);
925                 isec->lambda = dist;
926         }
927 }
928
929 static MDefBoundIsect *meshdeform_ray_tree_intersect(MeshDeformBind *mdb, const float co1[3], const float co2[3])
930 {
931         BVHTreeRayHit hit;
932         MeshDeformIsect isect_mdef;
933         struct MeshRayCallbackData data = {
934                 mdb,
935                 &isect_mdef,
936         };
937         float end[3], vec_normal[3];
938
939         /* happens binding when a cage has no faces */
940         if (UNLIKELY(mdb->bvhtree == NULL))
941                 return NULL;
942
943         /* setup isec */
944         memset(&isect_mdef, 0, sizeof(isect_mdef));
945         isect_mdef.lambda = 1e10f;
946
947         copy_v3_v3(isect_mdef.start, co1);
948         copy_v3_v3(end, co2);
949         sub_v3_v3v3(isect_mdef.vec, end, isect_mdef.start);
950         isect_mdef.vec_length = normalize_v3_v3(vec_normal, isect_mdef.vec);
951
952         hit.index = -1;
953         hit.dist = BVH_RAYCAST_DIST_MAX;
954         if (BLI_bvhtree_ray_cast_ex(mdb->bvhtree, isect_mdef.start, vec_normal,
955                                     0.0, &hit, harmonic_ray_callback, &data, BVH_RAYCAST_WATERTIGHT) != -1)
956         {
957                 const MLoop *mloop = mdb->cagedm_cache.mloop;
958                 const MLoopTri *lt = &mdb->cagedm_cache.looptri[hit.index];
959                 const MPoly *mp = &mdb->cagedm_cache.mpoly[lt->poly];
960                 const float (*cagecos)[3] = mdb->cagecos;
961                 const float len = isect_mdef.lambda;
962                 MDefBoundIsect *isect;
963
964                 float (*mp_cagecos)[3] = BLI_array_alloca(mp_cagecos, mp->totloop);
965                 int i;
966
967                 /* create MDefBoundIsect, and extra for 'poly_weights[]' */
968                 isect = BLI_memarena_alloc(mdb->memarena, sizeof(*isect) + (sizeof(float) * mp->totloop));
969
970                 /* compute intersection coordinate */
971                 madd_v3_v3v3fl(isect->co, co1, isect_mdef.vec, len);
972
973                 isect->facing = isect_mdef.isect;
974
975                 isect->poly_index = lt->poly;
976
977                 isect->len = max_ff(len_v3v3(co1, isect->co), MESHDEFORM_LEN_THRESHOLD);
978
979                 /* compute mean value coordinates for interpolation */
980                 for (i = 0; i < mp->totloop; i++) {
981                         copy_v3_v3(mp_cagecos[i], cagecos[mloop[mp->loopstart + i].v]);
982                 }
983
984                 interp_weights_poly_v3(isect->poly_weights, mp_cagecos, mp->totloop, isect->co);
985
986                 return isect;
987         }
988
989         return NULL;
990 }
991
992 static int meshdeform_inside_cage(MeshDeformBind *mdb, float *co)
993 {
994         MDefBoundIsect *isect;
995         float outside[3], start[3], dir[3];
996         int i;
997
998         for (i = 1; i <= 6; i++) {
999                 outside[0] = co[0] + (mdb->max[0] - mdb->min[0] + 1.0f) * MESHDEFORM_OFFSET[i][0];
1000                 outside[1] = co[1] + (mdb->max[1] - mdb->min[1] + 1.0f) * MESHDEFORM_OFFSET[i][1];
1001                 outside[2] = co[2] + (mdb->max[2] - mdb->min[2] + 1.0f) * MESHDEFORM_OFFSET[i][2];
1002
1003                 copy_v3_v3(start, co);
1004                 sub_v3_v3v3(dir, outside, start);
1005                 normalize_v3(dir);
1006                 
1007                 isect = meshdeform_ray_tree_intersect(mdb, start, outside);
1008                 if (isect && !isect->facing)
1009                         return 1;
1010         }
1011
1012         return 0;
1013 }
1014
1015 /* solving */
1016
1017 BLI_INLINE int meshdeform_index(MeshDeformBind *mdb, int x, int y, int z, int n)
1018 {
1019         int size = mdb->size;
1020         
1021         x += MESHDEFORM_OFFSET[n][0];
1022         y += MESHDEFORM_OFFSET[n][1];
1023         z += MESHDEFORM_OFFSET[n][2];
1024
1025         if (x < 0 || x >= mdb->size)
1026                 return -1;
1027         if (y < 0 || y >= mdb->size)
1028                 return -1;
1029         if (z < 0 || z >= mdb->size)
1030                 return -1;
1031
1032         return x + y * size + z * size * size;
1033 }
1034
1035 BLI_INLINE void meshdeform_cell_center(MeshDeformBind *mdb, int x, int y, int z, int n, float *center)
1036 {
1037         x += MESHDEFORM_OFFSET[n][0];
1038         y += MESHDEFORM_OFFSET[n][1];
1039         z += MESHDEFORM_OFFSET[n][2];
1040
1041         center[0] = mdb->min[0] + x * mdb->width[0] + mdb->halfwidth[0];
1042         center[1] = mdb->min[1] + y * mdb->width[1] + mdb->halfwidth[1];
1043         center[2] = mdb->min[2] + z * mdb->width[2] + mdb->halfwidth[2];
1044 }
1045
1046 static void meshdeform_add_intersections(MeshDeformBind *mdb, int x, int y, int z)
1047 {
1048         MDefBoundIsect *isect;
1049         float center[3], ncenter[3];
1050         int i, a;
1051
1052         a = meshdeform_index(mdb, x, y, z, 0);
1053         meshdeform_cell_center(mdb, x, y, z, 0, center);
1054
1055         /* check each outgoing edge for intersection */
1056         for (i = 1; i <= 6; i++) {
1057                 if (meshdeform_index(mdb, x, y, z, i) == -1)
1058                         continue;
1059
1060                 meshdeform_cell_center(mdb, x, y, z, i, ncenter);
1061
1062                 isect = meshdeform_ray_tree_intersect(mdb, center, ncenter);
1063                 if (isect) {
1064                         mdb->boundisect[a][i - 1] = isect;
1065                         mdb->tag[a] = MESHDEFORM_TAG_BOUNDARY;
1066                 }
1067         }
1068 }
1069
1070 static void meshdeform_bind_floodfill(MeshDeformBind *mdb)
1071 {
1072         int *stack, *tag = mdb->tag;
1073         int a, b, i, xyz[3], stacksize, size = mdb->size;
1074
1075         stack = MEM_callocN(sizeof(int) * mdb->size3, "MeshDeformBindStack");
1076
1077         /* we know lower left corner is EXTERIOR because of padding */
1078         tag[0] = MESHDEFORM_TAG_EXTERIOR;
1079         stack[0] = 0;
1080         stacksize = 1;
1081
1082         /* floodfill exterior tag */
1083         while (stacksize > 0) {
1084                 a = stack[--stacksize];
1085
1086                 xyz[2] = a / (size * size);
1087                 xyz[1] = (a - xyz[2] * size * size) / size;
1088                 xyz[0] = a - xyz[1] * size - xyz[2] * size * size;
1089
1090                 for (i = 1; i <= 6; i++) {
1091                         b = meshdeform_index(mdb, xyz[0], xyz[1], xyz[2], i);
1092
1093                         if (b != -1) {
1094                                 if (tag[b] == MESHDEFORM_TAG_UNTYPED ||
1095                                     (tag[b] == MESHDEFORM_TAG_BOUNDARY && !mdb->boundisect[a][i - 1]))
1096                                 {
1097                                         tag[b] = MESHDEFORM_TAG_EXTERIOR;
1098                                         stack[stacksize++] = b;
1099                                 }
1100                         }
1101                 }
1102         }
1103
1104         /* other cells are interior */
1105         for (a = 0; a < size * size * size; a++)
1106                 if (tag[a] == MESHDEFORM_TAG_UNTYPED)
1107                         tag[a] = MESHDEFORM_TAG_INTERIOR;
1108
1109 #if 0
1110         {
1111                 int tb, ti, te, ts;
1112                 tb = ti = te = ts = 0;
1113                 for (a = 0; a < size * size * size; a++)
1114                         if (tag[a] == MESHDEFORM_TAG_BOUNDARY)
1115                                 tb++;
1116                         else if (tag[a] == MESHDEFORM_TAG_INTERIOR)
1117                                 ti++;
1118                         else if (tag[a] == MESHDEFORM_TAG_EXTERIOR) {
1119                                 te++;
1120
1121                                 if (mdb->semibound[a])
1122                                         ts++;
1123                         }
1124                 
1125                 printf("interior %d exterior %d boundary %d semi-boundary %d\n", ti, te, tb, ts);
1126         }
1127 #endif
1128
1129         MEM_freeN(stack);
1130 }
1131
1132 static float meshdeform_boundary_phi(const MeshDeformBind *mdb, const MDefBoundIsect *isect, int cagevert)
1133 {
1134         const MLoop *mloop = mdb->cagedm_cache.mloop;
1135         const MPoly *mp = &mdb->cagedm_cache.mpoly[isect->poly_index];
1136         int i;
1137
1138         for (i = 0; i < mp->totloop; i++) {
1139                 if (mloop[mp->loopstart + i].v == cagevert) {
1140                         return isect->poly_weights[i];
1141                 }
1142         }
1143
1144         return 0.0f;
1145 }
1146
1147 static float meshdeform_interp_w(MeshDeformBind *mdb, float *gridvec, float *UNUSED(vec), int UNUSED(cagevert))
1148 {
1149         float dvec[3], ivec[3], wx, wy, wz, result = 0.0f;
1150         float weight, totweight = 0.0f;
1151         int i, a, x, y, z;
1152
1153         for (i = 0; i < 3; i++) {
1154                 ivec[i] = (int)gridvec[i];
1155                 dvec[i] = gridvec[i] - ivec[i];
1156         }
1157
1158         for (i = 0; i < 8; i++) {
1159                 if (i & 1) { x = ivec[0] + 1; wx = dvec[0]; }
1160                 else       { x = ivec[0];     wx = 1.0f - dvec[0]; }
1161
1162                 if (i & 2) { y = ivec[1] + 1; wy = dvec[1]; }
1163                 else       { y = ivec[1];     wy = 1.0f - dvec[1]; }
1164
1165                 if (i & 4) { z = ivec[2] + 1; wz = dvec[2]; }
1166                 else       { z = ivec[2];     wz = 1.0f - dvec[2]; }
1167
1168                 CLAMP(x, 0, mdb->size - 1);
1169                 CLAMP(y, 0, mdb->size - 1);
1170                 CLAMP(z, 0, mdb->size - 1);
1171
1172                 a = meshdeform_index(mdb, x, y, z, 0);
1173                 weight = wx * wy * wz;
1174                 result += weight * mdb->phi[a];
1175                 totweight += weight;
1176         }
1177
1178         if (totweight > 0.0f)
1179                 result /= totweight;
1180
1181         return result;
1182 }
1183
1184 static void meshdeform_check_semibound(MeshDeformBind *mdb, int x, int y, int z)
1185 {
1186         int i, a;
1187
1188         a = meshdeform_index(mdb, x, y, z, 0);
1189         if (mdb->tag[a] != MESHDEFORM_TAG_EXTERIOR)
1190                 return;
1191
1192         for (i = 1; i <= 6; i++)
1193                 if (mdb->boundisect[a][i - 1])
1194                         mdb->semibound[a] = 1;
1195 }
1196
1197 static float meshdeform_boundary_total_weight(MeshDeformBind *mdb, int x, int y, int z)
1198 {
1199         float weight, totweight = 0.0f;
1200         int i, a;
1201
1202         a = meshdeform_index(mdb, x, y, z, 0);
1203
1204         /* count weight for neighbor cells */
1205         for (i = 1; i <= 6; i++) {
1206                 if (meshdeform_index(mdb, x, y, z, i) == -1)
1207                         continue;
1208
1209                 if (mdb->boundisect[a][i - 1])
1210                         weight = 1.0f / mdb->boundisect[a][i - 1]->len;
1211                 else if (!mdb->semibound[a])
1212                         weight = 1.0f / mdb->width[0];
1213                 else
1214                         weight = 0.0f;
1215
1216                 totweight += weight;
1217         }
1218
1219         return totweight;
1220 }
1221
1222 static void meshdeform_matrix_add_cell(MeshDeformBind *mdb, LinearSolver *context, int x, int y, int z)
1223 {
1224         MDefBoundIsect *isect;
1225         float weight, totweight;
1226         int i, a, acenter;
1227
1228         acenter = meshdeform_index(mdb, x, y, z, 0);
1229         if (mdb->tag[acenter] == MESHDEFORM_TAG_EXTERIOR)
1230                 return;
1231
1232         EIG_linear_solver_matrix_add(context, mdb->varidx[acenter], mdb->varidx[acenter], 1.0f);
1233         
1234         totweight = meshdeform_boundary_total_weight(mdb, x, y, z);
1235         for (i = 1; i <= 6; i++) {
1236                 a = meshdeform_index(mdb, x, y, z, i);
1237                 if (a == -1 || mdb->tag[a] == MESHDEFORM_TAG_EXTERIOR)
1238                         continue;
1239
1240                 isect = mdb->boundisect[acenter][i - 1];
1241                 if (!isect) {
1242                         weight = (1.0f / mdb->width[0]) / totweight;
1243                         EIG_linear_solver_matrix_add(context, mdb->varidx[acenter], mdb->varidx[a], -weight);
1244                 }
1245         }
1246 }
1247
1248 static void meshdeform_matrix_add_rhs(MeshDeformBind *mdb, LinearSolver *context, int x, int y, int z, int cagevert)
1249 {
1250         MDefBoundIsect *isect;
1251         float rhs, weight, totweight;
1252         int i, a, acenter;
1253
1254         acenter = meshdeform_index(mdb, x, y, z, 0);
1255         if (mdb->tag[acenter] == MESHDEFORM_TAG_EXTERIOR)
1256                 return;
1257
1258         totweight = meshdeform_boundary_total_weight(mdb, x, y, z);
1259         for (i = 1; i <= 6; i++) {
1260                 a = meshdeform_index(mdb, x, y, z, i);
1261                 if (a == -1)
1262                         continue;
1263
1264                 isect = mdb->boundisect[acenter][i - 1];
1265
1266                 if (isect) {
1267                         weight = (1.0f / isect->len) / totweight;
1268                         rhs = weight * meshdeform_boundary_phi(mdb, isect, cagevert);
1269                         EIG_linear_solver_right_hand_side_add(context, 0, mdb->varidx[acenter], rhs);
1270                 }
1271         }
1272 }
1273
1274 static void meshdeform_matrix_add_semibound_phi(MeshDeformBind *mdb, int x, int y, int z, int cagevert)
1275 {
1276         MDefBoundIsect *isect;
1277         float rhs, weight, totweight;
1278         int i, a;
1279
1280         a = meshdeform_index(mdb, x, y, z, 0);
1281         if (!mdb->semibound[a])
1282                 return;
1283         
1284         mdb->phi[a] = 0.0f;
1285
1286         totweight = meshdeform_boundary_total_weight(mdb, x, y, z);
1287         for (i = 1; i <= 6; i++) {
1288                 isect = mdb->boundisect[a][i - 1];
1289
1290                 if (isect) {
1291                         weight = (1.0f / isect->len) / totweight;
1292                         rhs = weight * meshdeform_boundary_phi(mdb, isect, cagevert);
1293                         mdb->phi[a] += rhs;
1294                 }
1295         }
1296 }
1297
1298 static void meshdeform_matrix_add_exterior_phi(MeshDeformBind *mdb, int x, int y, int z, int UNUSED(cagevert))
1299 {
1300         float phi, totweight;
1301         int i, a, acenter;
1302
1303         acenter = meshdeform_index(mdb, x, y, z, 0);
1304         if (mdb->tag[acenter] != MESHDEFORM_TAG_EXTERIOR || mdb->semibound[acenter])
1305                 return;
1306
1307         phi = 0.0f;
1308         totweight = 0.0f;
1309         for (i = 1; i <= 6; i++) {
1310                 a = meshdeform_index(mdb, x, y, z, i);
1311
1312                 if (a != -1 && mdb->semibound[a]) {
1313                         phi += mdb->phi[a];
1314                         totweight += 1.0f;
1315                 }
1316         }
1317
1318         if (totweight != 0.0f)
1319                 mdb->phi[acenter] = phi / totweight;
1320 }
1321
1322 static void meshdeform_matrix_solve(MeshDeformModifierData *mmd, MeshDeformBind *mdb)
1323 {
1324         LinearSolver *context;
1325         float vec[3], gridvec[3];
1326         int a, b, x, y, z, totvar;
1327         char message[256];
1328
1329         /* setup variable indices */
1330         mdb->varidx = MEM_callocN(sizeof(int) * mdb->size3, "MeshDeformDSvaridx");
1331         for (a = 0, totvar = 0; a < mdb->size3; a++)
1332                 mdb->varidx[a] = (mdb->tag[a] == MESHDEFORM_TAG_EXTERIOR) ? -1 : totvar++;
1333
1334         if (totvar == 0) {
1335                 MEM_freeN(mdb->varidx);
1336                 return;
1337         }
1338
1339         progress_bar(0, "Starting mesh deform solve");
1340
1341         /* setup linear solver */
1342         context = EIG_linear_solver_new(totvar, totvar, 1);
1343
1344         /* build matrix */
1345         for (z = 0; z < mdb->size; z++)
1346                 for (y = 0; y < mdb->size; y++)
1347                         for (x = 0; x < mdb->size; x++)
1348                                 meshdeform_matrix_add_cell(mdb, context, x, y, z);
1349
1350         /* solve for each cage vert */
1351         for (a = 0; a < mdb->totcagevert; a++) {
1352                 /* fill in right hand side and solve */
1353                 for (z = 0; z < mdb->size; z++)
1354                         for (y = 0; y < mdb->size; y++)
1355                                 for (x = 0; x < mdb->size; x++)
1356                                         meshdeform_matrix_add_rhs(mdb, context, x, y, z, a);
1357
1358                 if (EIG_linear_solver_solve(context)) {
1359                         for (z = 0; z < mdb->size; z++)
1360                                 for (y = 0; y < mdb->size; y++)
1361                                         for (x = 0; x < mdb->size; x++)
1362                                                 meshdeform_matrix_add_semibound_phi(mdb, x, y, z, a);
1363
1364                         for (z = 0; z < mdb->size; z++)
1365                                 for (y = 0; y < mdb->size; y++)
1366                                         for (x = 0; x < mdb->size; x++)
1367                                                 meshdeform_matrix_add_exterior_phi(mdb, x, y, z, a);
1368
1369                         for (b = 0; b < mdb->size3; b++) {
1370                                 if (mdb->tag[b] != MESHDEFORM_TAG_EXTERIOR)
1371                                         mdb->phi[b] = EIG_linear_solver_variable_get(context, 0, mdb->varidx[b]);
1372                                 mdb->totalphi[b] += mdb->phi[b];
1373                         }
1374
1375                         if (mdb->weights) {
1376                                 /* static bind : compute weights for each vertex */
1377                                 for (b = 0; b < mdb->totvert; b++) {
1378                                         if (mdb->inside[b]) {
1379                                                 copy_v3_v3(vec, mdb->vertexcos[b]);
1380                                                 gridvec[0] = (vec[0] - mdb->min[0] - mdb->halfwidth[0]) / mdb->width[0];
1381                                                 gridvec[1] = (vec[1] - mdb->min[1] - mdb->halfwidth[1]) / mdb->width[1];
1382                                                 gridvec[2] = (vec[2] - mdb->min[2] - mdb->halfwidth[2]) / mdb->width[2];
1383
1384                                                 mdb->weights[b * mdb->totcagevert + a] = meshdeform_interp_w(mdb, gridvec, vec, a);
1385                                         }
1386                                 }
1387                         }
1388                         else {
1389                                 MDefBindInfluence *inf;
1390
1391                                 /* dynamic bind */
1392                                 for (b = 0; b < mdb->size3; b++) {
1393                                         if (mdb->phi[b] >= MESHDEFORM_MIN_INFLUENCE) {
1394                                                 inf = BLI_memarena_alloc(mdb->memarena, sizeof(*inf));
1395                                                 inf->vertex = a;
1396                                                 inf->weight = mdb->phi[b];
1397                                                 inf->next = mdb->dyngrid[b];
1398                                                 mdb->dyngrid[b] = inf;
1399                                         }
1400                                 }
1401                         }
1402                 }
1403                 else {
1404                         modifier_setError(&mmd->modifier, "Failed to find bind solution (increase precision?)");
1405                         error("Mesh Deform: failed to find bind solution.");
1406                         break;
1407                 }
1408
1409                 BLI_snprintf(message, sizeof(message), "Mesh deform solve %d / %d       |||", a + 1, mdb->totcagevert);
1410                 progress_bar((float)(a + 1) / (float)(mdb->totcagevert), message);
1411         }
1412
1413 #if 0
1414         /* sanity check */
1415         for (b = 0; b < mdb->size3; b++)
1416                 if (mdb->tag[b] != MESHDEFORM_TAG_EXTERIOR)
1417                         if (fabsf(mdb->totalphi[b] - 1.0f) > 1e-4f)
1418                                 printf("totalphi deficiency [%s|%d] %d: %.10f\n",
1419                                        (mdb->tag[b] == MESHDEFORM_TAG_INTERIOR) ? "interior" : "boundary", mdb->semibound[b], mdb->varidx[b], mdb->totalphi[b]);
1420 #endif
1421         
1422         /* free */
1423         MEM_freeN(mdb->varidx);
1424
1425         EIG_linear_solver_delete(context);
1426 }
1427
1428 static void harmonic_coordinates_bind(Scene *UNUSED(scene), MeshDeformModifierData *mmd, MeshDeformBind *mdb)
1429 {
1430         MDefBindInfluence *inf;
1431         MDefInfluence *mdinf;
1432         MDefCell *cell;
1433         float center[3], vec[3], maxwidth, totweight;
1434         int a, b, x, y, z, totinside, offset;
1435
1436         /* compute bounding box of the cage mesh */
1437         INIT_MINMAX(mdb->min, mdb->max);
1438
1439         for (a = 0; a < mdb->totcagevert; a++)
1440                 minmax_v3v3_v3(mdb->min, mdb->max, mdb->cagecos[a]);
1441
1442         /* allocate memory */
1443         mdb->size = (2 << (mmd->gridsize - 1)) + 2;
1444         mdb->size3 = mdb->size * mdb->size * mdb->size;
1445         mdb->tag = MEM_callocN(sizeof(int) * mdb->size3, "MeshDeformBindTag");
1446         mdb->phi = MEM_callocN(sizeof(float) * mdb->size3, "MeshDeformBindPhi");
1447         mdb->totalphi = MEM_callocN(sizeof(float) * mdb->size3, "MeshDeformBindTotalPhi");
1448         mdb->boundisect = MEM_callocN(sizeof(*mdb->boundisect) * mdb->size3, "MDefBoundIsect");
1449         mdb->semibound = MEM_callocN(sizeof(int) * mdb->size3, "MDefSemiBound");
1450         mdb->bvhtree = bvhtree_from_mesh_get(&mdb->bvhdata, mdb->cagedm, BVHTREE_FROM_LOOPTRI, 4);
1451         mdb->inside = MEM_callocN(sizeof(int) * mdb->totvert, "MDefInside");
1452
1453         if (mmd->flag & MOD_MDEF_DYNAMIC_BIND)
1454                 mdb->dyngrid = MEM_callocN(sizeof(MDefBindInfluence *) * mdb->size3, "MDefDynGrid");
1455         else
1456                 mdb->weights = MEM_callocN(sizeof(float) * mdb->totvert * mdb->totcagevert, "MDefWeights");
1457
1458         mdb->memarena = BLI_memarena_new(BLI_MEMARENA_STD_BUFSIZE, "harmonic coords arena");
1459         BLI_memarena_use_calloc(mdb->memarena);
1460
1461         /* initialize data from 'cagedm' for reuse */
1462         {
1463                 DerivedMesh *dm = mdb->cagedm;
1464                 mdb->cagedm_cache.mpoly = dm->getPolyArray(dm);
1465                 mdb->cagedm_cache.mloop = dm->getLoopArray(dm);
1466                 mdb->cagedm_cache.looptri = dm->getLoopTriArray(dm);
1467                 mdb->cagedm_cache.poly_nors = dm->getPolyDataArray(dm, CD_NORMAL);  /* can be NULL */
1468         }
1469
1470         /* make bounding box equal size in all directions, add padding, and compute
1471          * width of the cells */
1472         maxwidth = -1.0f;
1473         for (a = 0; a < 3; a++)
1474                 if (mdb->max[a] - mdb->min[a] > maxwidth)
1475                         maxwidth = mdb->max[a] - mdb->min[a];
1476
1477         for (a = 0; a < 3; a++) {
1478                 center[a] = (mdb->min[a] + mdb->max[a]) * 0.5f;
1479                 mdb->min[a] = center[a] - maxwidth * 0.5f;
1480                 mdb->max[a] = center[a] + maxwidth * 0.5f;
1481
1482                 mdb->width[a] = (mdb->max[a] - mdb->min[a]) / (mdb->size - 4);
1483                 mdb->min[a] -= 2.1f * mdb->width[a];
1484                 mdb->max[a] += 2.1f * mdb->width[a];
1485
1486                 mdb->width[a] = (mdb->max[a] - mdb->min[a]) / mdb->size;
1487                 mdb->halfwidth[a] = mdb->width[a] * 0.5f;
1488         }
1489
1490         progress_bar(0, "Setting up mesh deform system");
1491
1492         totinside = 0;
1493         for (a = 0; a < mdb->totvert; a++) {
1494                 copy_v3_v3(vec, mdb->vertexcos[a]);
1495                 mdb->inside[a] = meshdeform_inside_cage(mdb, vec);
1496                 if (mdb->inside[a])
1497                         totinside++;
1498         }
1499
1500         /* free temporary MDefBoundIsects */
1501         BLI_memarena_free(mdb->memarena);
1502         mdb->memarena = BLI_memarena_new(BLI_MEMARENA_STD_BUFSIZE, "harmonic coords arena");
1503
1504         /* start with all cells untyped */
1505         for (a = 0; a < mdb->size3; a++)
1506                 mdb->tag[a] = MESHDEFORM_TAG_UNTYPED;
1507         
1508         /* detect intersections and tag boundary cells */
1509         for (z = 0; z < mdb->size; z++)
1510                 for (y = 0; y < mdb->size; y++)
1511                         for (x = 0; x < mdb->size; x++)
1512                                 meshdeform_add_intersections(mdb, x, y, z);
1513
1514         /* compute exterior and interior tags */
1515         meshdeform_bind_floodfill(mdb);
1516
1517         for (z = 0; z < mdb->size; z++)
1518                 for (y = 0; y < mdb->size; y++)
1519                         for (x = 0; x < mdb->size; x++)
1520                                 meshdeform_check_semibound(mdb, x, y, z);
1521
1522         /* solve */
1523         meshdeform_matrix_solve(mmd, mdb);
1524
1525         /* assign results */
1526         if (mmd->flag & MOD_MDEF_DYNAMIC_BIND) {
1527                 mmd->totinfluence = 0;
1528                 for (a = 0; a < mdb->size3; a++)
1529                         for (inf = mdb->dyngrid[a]; inf; inf = inf->next)
1530                                 mmd->totinfluence++;
1531
1532                 /* convert MDefBindInfluences to smaller MDefInfluences */
1533                 mmd->dyngrid = MEM_callocN(sizeof(MDefCell) * mdb->size3, "MDefDynGrid");
1534                 mmd->dyninfluences = MEM_callocN(sizeof(MDefInfluence) * mmd->totinfluence, "MDefInfluence");
1535                 offset = 0;
1536                 for (a = 0; a < mdb->size3; a++) {
1537                         cell = &mmd->dyngrid[a];
1538                         cell->offset = offset;
1539
1540                         totweight = 0.0f;
1541                         mdinf = mmd->dyninfluences + cell->offset;
1542                         for (inf = mdb->dyngrid[a]; inf; inf = inf->next, mdinf++) {
1543                                 mdinf->weight = inf->weight;
1544                                 mdinf->vertex = inf->vertex;
1545                                 totweight += mdinf->weight;
1546                                 cell->totinfluence++;
1547                         }
1548
1549                         if (totweight > 0.0f) {
1550                                 mdinf = mmd->dyninfluences + cell->offset;
1551                                 for (b = 0; b < cell->totinfluence; b++, mdinf++)
1552                                         mdinf->weight /= totweight;
1553                         }
1554
1555                         offset += cell->totinfluence;
1556                 }
1557
1558                 mmd->dynverts = mdb->inside;
1559                 mmd->dyngridsize = mdb->size;
1560                 copy_v3_v3(mmd->dyncellmin, mdb->min);
1561                 mmd->dyncellwidth = mdb->width[0];
1562                 MEM_freeN(mdb->dyngrid);
1563         }
1564         else {
1565                 mmd->bindweights = mdb->weights;
1566                 MEM_freeN(mdb->inside);
1567         }
1568
1569         MEM_freeN(mdb->tag);
1570         MEM_freeN(mdb->phi);
1571         MEM_freeN(mdb->totalphi);
1572         MEM_freeN(mdb->boundisect);
1573         MEM_freeN(mdb->semibound);
1574         BLI_memarena_free(mdb->memarena);
1575         free_bvhtree_from_mesh(&mdb->bvhdata);
1576 }
1577
1578 void ED_mesh_deform_bind_callback(
1579         Scene *scene, MeshDeformModifierData *mmd, DerivedMesh *cagedm,
1580         float *vertexcos, int totvert, float cagemat[4][4])
1581 {
1582         MeshDeformBind mdb;
1583         MVert *mvert;
1584         int a;
1585
1586         waitcursor(1);
1587         start_progress_bar();
1588
1589         memset(&mdb, 0, sizeof(MeshDeformBind));
1590
1591         /* get mesh and cage mesh */
1592         mdb.vertexcos = MEM_callocN(sizeof(float) * 3 * totvert, "MeshDeformCos");
1593         mdb.totvert = totvert;
1594         
1595         mdb.cagedm = cagedm;
1596         mdb.totcagevert = mdb.cagedm->getNumVerts(mdb.cagedm);
1597         mdb.cagecos = MEM_callocN(sizeof(*mdb.cagecos) * mdb.totcagevert, "MeshDeformBindCos");
1598         copy_m4_m4(mdb.cagemat, cagemat);
1599
1600         mvert = mdb.cagedm->getVertArray(mdb.cagedm);
1601         for (a = 0; a < mdb.totcagevert; a++)
1602                 copy_v3_v3(mdb.cagecos[a], mvert[a].co);
1603         for (a = 0; a < mdb.totvert; a++)
1604                 mul_v3_m4v3(mdb.vertexcos[a], mdb.cagemat, vertexcos + a * 3);
1605
1606         /* solve */
1607         harmonic_coordinates_bind(scene, mmd, &mdb);
1608
1609         /* assign bind variables */
1610         mmd->bindcagecos = (float *)mdb.cagecos;
1611         mmd->totvert = mdb.totvert;
1612         mmd->totcagevert = mdb.totcagevert;
1613         copy_m4_m4(mmd->bindmat, mmd->object->obmat);
1614
1615         /* transform bindcagecos to world space */
1616         for (a = 0; a < mdb.totcagevert; a++)
1617                 mul_m4_v3(mmd->object->obmat, mmd->bindcagecos + a * 3);
1618
1619         /* free */
1620         MEM_freeN(mdb.vertexcos);
1621
1622         /* compact weights */
1623         modifier_mdef_compact_influences((ModifierData *)mmd);
1624
1625         end_progress_bar();
1626         waitcursor(0);
1627 }