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