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