Bone Heat Weighting
[blender-staging.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 <string.h>
35
36 #include "MEM_guardedalloc.h"
37
38 #include "DNA_listBase.h"
39 #include "DNA_object_types.h"
40 #include "DNA_mesh_types.h"
41 #include "DNA_meshdata_types.h"
42
43 #include "BLI_arithb.h"
44 #include "BLI_edgehash.h"
45
46 #include "BKE_utildefines.h"
47
48 #include "BIF_editdeform.h"
49 #include "BIF_meshlaplacian.h"
50 #include "BIF_meshtools.h"
51 #include "BIF_toolbox.h"
52
53 #ifdef RIGID_DEFORM
54 #include "BLI_editVert.h"
55 #include "BLI_polardecomp.h"
56 #endif
57
58 #include "RE_raytrace.h"
59
60 #include "ONL_opennl.h"
61
62 /************************** Laplacian System *****************************/
63
64 struct LaplacianSystem {
65         NLContext context;      /* opennl context */
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         int nlbegun;                    /* nlBegin(NL_SYSTEM/NL_MATRIX) done */
78
79         EdgeHash *edgehash;             /* edge hash for construction */
80
81         struct HeatWeighting {
82                 Mesh *mesh;
83                 float (*verts)[3];      /* vertex coordinates */
84                 float (*vnors)[3];      /* vertex normals */
85
86                 float (*root)[3];       /* bone root */
87                 float (*tip)[3];        /* bone tip */
88                 int numbones;
89
90                 float *H;                       /* diagonal H matrix */
91                 float *p;                       /* values from all p vectors */
92                 float *mindist;         /* minimum distance to a bone for all vertices */
93                 
94                 RayTree *raytree;       /* ray tracing acceleration structure */
95                 MFace **vface;          /* a face that the vertex belongs to */
96         } heat;
97
98 #ifdef RIGID_DEFORM
99         struct RigidDeformation {
100                 EditMesh *mesh;
101
102                 float (*R)[3][3];
103                 float (*rhs)[3];
104                 float (*origco)[3];
105                 int thrownerror;
106         } rigid;
107 #endif
108 };
109
110 /* Laplacian matrix construction */
111
112 /* Computation of these weights for the laplacian is based on:
113    "Discrete Differential-Geometry Operators for Triangulated 2-Manifolds",
114    Meyer et al, 2002. Section 3.5, formula (8).
115    
116    We do it a bit different by going over faces instead of going over each
117    vertex and adjacent faces, since we don't store this adjacency. Also, the
118    formulas are tweaked a bit to work for non-manifold meshes. */
119
120 static void laplacian_increase_edge_count(EdgeHash *edgehash, int v1, int v2)
121 {
122         void **p = BLI_edgehash_lookup_p(edgehash, v1, v2);
123
124         if(p)
125                 *p = (void*)((long)*p + (long)1);
126         else
127                 BLI_edgehash_insert(edgehash, v1, v2, (void*)(long)1);
128 }
129
130 static int laplacian_edge_count(EdgeHash *edgehash, int v1, int v2)
131 {
132         return (int)(long)BLI_edgehash_lookup(edgehash, v1, v2);
133 }
134
135 static float cotan_weight(float *v1, float *v2, float *v3)
136 {
137         float a[3], b[3], c[3], clen;
138
139         VecSubf(a, v2, v1);
140         VecSubf(b, v3, v1);
141         Crossf(c, a, b);
142
143         clen = VecLength(c);
144
145         if (clen == 0.0f)
146                 return 0.0f;
147         
148         return Inpf(a, b)/clen;
149 }
150
151 static void laplacian_triangle_area(LaplacianSystem *sys, int i1, int i2, int i3)
152 {
153         float t1, t2, t3, len1, len2, len3, area;
154         float *varea= sys->varea, *v1, *v2, *v3;
155         int obtuse = 0;
156
157         v1= sys->verts[i1];
158         v2= sys->verts[i2];
159         v3= sys->verts[i3];
160
161         t1= cotan_weight(v1, v2, v3);
162         t2= cotan_weight(v2, v3, v1);
163         t3= cotan_weight(v3, v1, v2);
164
165         if(VecAngle3(v2, v1, v3) > 90) obtuse= 1;
166         else if(VecAngle3(v1, v2, v3) > 90) obtuse= 2;
167         else if(VecAngle3(v1, v3, v2) > 90) obtuse= 3;
168
169         if (obtuse > 0) {
170                 area= AreaT3Dfl(v1, v2, v3);
171
172                 varea[i1] += (obtuse == 1)? area: area*0.5;
173                 varea[i2] += (obtuse == 2)? area: area*0.5;
174                 varea[i3] += (obtuse == 3)? area: area*0.5;
175         }
176         else {
177                 len1= VecLenf(v2, v3);
178                 len2= VecLenf(v1, v3);
179                 len3= VecLenf(v1, v2);
180
181                 t1 *= len1*len1;
182                 t2 *= len2*len2;
183                 t3 *= len3*len3;
184
185                 varea[i1] += (t2 + t3)*0.25f;
186                 varea[i2] += (t1 + t3)*0.25f;
187                 varea[i3] += (t1 + t2)*0.25f;
188         }
189 }
190
191 static void laplacian_triangle_weights(LaplacianSystem *sys, int f, int i1, int i2, int i3)
192 {
193         float t1, t2, t3;
194         float *varea= sys->varea, *v1, *v2, *v3;
195
196         v1= sys->verts[i1];
197         v2= sys->verts[i2];
198         v3= sys->verts[i3];
199
200         /* instead of *0.5 we divided by the number of faces of the edge, it still
201            needs to be varified that this is indeed the correct thing to do! */
202         t1= cotan_weight(v1, v2, v3)/laplacian_edge_count(sys->edgehash, i2, i3);
203         t2= cotan_weight(v2, v3, v1)/laplacian_edge_count(sys->edgehash, i3, i1);
204         t3= cotan_weight(v3, v1, v2)/laplacian_edge_count(sys->edgehash, i1, i2);
205
206         nlMatrixAdd(i1, i1, (t2+t3)*varea[i1]);
207         nlMatrixAdd(i2, i2, (t1+t3)*varea[i2]);
208         nlMatrixAdd(i3, i3, (t1+t2)*varea[i3]);
209
210         nlMatrixAdd(i1, i2, -t3*varea[i1]);
211         nlMatrixAdd(i2, i1, -t3*varea[i2]);
212
213         nlMatrixAdd(i2, i3, -t1*varea[i2]);
214         nlMatrixAdd(i3, i2, -t1*varea[i3]);
215
216         nlMatrixAdd(i3, i1, -t2*varea[i3]);
217         nlMatrixAdd(i1, i3, -t2*varea[i1]);
218
219         if(sys->storeweights) {
220                 sys->fweights[f][0]= t1*varea[i1];
221                 sys->fweights[f][1]= t2*varea[i2];
222                 sys->fweights[f][2]= t3*varea[i3];
223         }
224 }
225
226 LaplacianSystem *laplacian_system_construct_begin(int totvert, int totface)
227 {
228         LaplacianSystem *sys;
229
230         sys= MEM_callocN(sizeof(LaplacianSystem), "LaplacianSystem");
231
232         sys->verts= MEM_callocN(sizeof(float*)*totvert, "LaplacianSystemVerts");
233         sys->vpinned= MEM_callocN(sizeof(char)*totvert, "LaplacianSystemVpinned");
234         sys->faces= MEM_callocN(sizeof(int)*3*totface, "LaplacianSystemFaces");
235
236         sys->totvert= 0;
237         sys->totface= 0;
238
239         sys->areaweights= 1;
240         sys->storeweights= 0;
241
242         /* create opennl context */
243         nlNewContext();
244         nlSolverParameteri(NL_NB_VARIABLES, totvert);
245
246         sys->context= nlGetCurrent();
247
248         return sys;
249 }
250
251 void laplacian_add_vertex(LaplacianSystem *sys, float *co, int pinned)
252 {
253         sys->verts[sys->totvert]= co;
254         sys->vpinned[sys->totvert]= pinned;
255         sys->totvert++;
256 }
257
258 void laplacian_add_triangle(LaplacianSystem *sys, int v1, int v2, int v3)
259 {
260         sys->faces[sys->totface][0]= v1;
261         sys->faces[sys->totface][1]= v2;
262         sys->faces[sys->totface][2]= v3;
263         sys->totface++;
264 }
265
266 void laplacian_system_construct_end(LaplacianSystem *sys)
267 {
268         int (*face)[3];
269         int a, totvert=sys->totvert, totface=sys->totface;
270
271         laplacian_begin_solve(sys, 0);
272
273         sys->varea= MEM_callocN(sizeof(float)*totvert, "LaplacianSystemVarea");
274
275         sys->edgehash= BLI_edgehash_new();
276         for(a=0, face=sys->faces; a<sys->totface; a++, face++) {
277                 laplacian_increase_edge_count(sys->edgehash, (*face)[0], (*face)[1]);
278                 laplacian_increase_edge_count(sys->edgehash, (*face)[1], (*face)[2]);
279                 laplacian_increase_edge_count(sys->edgehash, (*face)[2], (*face)[0]);
280         }
281
282         if(sys->areaweights)
283                 for(a=0, face=sys->faces; a<sys->totface; a++, face++)
284                         laplacian_triangle_area(sys, (*face)[0], (*face)[1], (*face)[2]);
285         
286         for(a=0; a<totvert; a++) {
287                 if(sys->areaweights) {
288                         if(sys->varea[a] != 0.0f)
289                                 sys->varea[a]= 0.5f/sys->varea[a];
290                 }
291                 else
292                         sys->varea[a]= 1.0f;
293
294                 /* for heat weighting */
295                 if(sys->heat.H)
296                         nlMatrixAdd(a, a, sys->heat.H[a]);
297         }
298
299         if(sys->storeweights)
300                 sys->fweights= MEM_callocN(sizeof(float)*3*totface, "LaplacianFWeight");
301         
302         for(a=0, face=sys->faces; a<totface; a++, face++)
303                 laplacian_triangle_weights(sys, a, (*face)[0], (*face)[1], (*face)[2]);
304
305         MEM_freeN(sys->faces);
306         sys->faces= NULL;
307
308         if(sys->varea) {
309                 MEM_freeN(sys->varea);
310                 sys->varea= NULL;
311         }
312
313         BLI_edgehash_free(sys->edgehash, NULL);
314         sys->edgehash= NULL;
315 }
316
317 void laplacian_system_delete(LaplacianSystem *sys)
318 {
319         if(sys->verts) MEM_freeN(sys->verts);
320         if(sys->varea) MEM_freeN(sys->varea);
321         if(sys->vpinned) MEM_freeN(sys->vpinned);
322         if(sys->faces) MEM_freeN(sys->faces);
323         if(sys->fweights) MEM_freeN(sys->fweights);
324
325         nlDeleteContext(sys->context);
326         MEM_freeN(sys);
327 }
328
329 void laplacian_begin_solve(LaplacianSystem *sys, int index)
330 {
331         int a;
332
333         if (!sys->nlbegun) {
334                 nlBegin(NL_SYSTEM);
335
336                 if(index >= 0) {
337                         for(a=0; a<sys->totvert; a++) {
338                                 if(sys->vpinned[a]) {
339                                         nlSetVariable(a, sys->verts[a][index]);
340                                         nlLockVariable(a);
341                                 }
342                         }
343                 }
344
345                 nlBegin(NL_MATRIX);
346                 sys->nlbegun = 1;
347         }
348 }
349
350 void laplacian_add_right_hand_side(LaplacianSystem *sys, int v, float value)
351 {
352         nlRightHandSideAdd(v, value);
353 }
354
355 int laplacian_system_solve(LaplacianSystem *sys)
356 {
357         nlEnd(NL_MATRIX);
358         nlEnd(NL_SYSTEM);
359         sys->nlbegun = 0;
360
361         //nlPrintMatrix();
362
363         return nlSolveAdvanced(NULL, NL_TRUE);
364 }
365
366 float laplacian_system_get_solution(int v)
367 {
368         return nlGetVariable(v);
369 }
370
371 /************************* Heat Bone Weighting ******************************/
372 /* From "Automatic Rigging and Animation of 3D Characters"
373          Ilya Baran and Jovan Popovic, SIGGRAPH 2007 */
374
375 #define C_WEIGHT                        1.0f
376 #define WEIGHT_LIMIT            0.05f
377 #define DISTANCE_EPSILON        1e-4f
378
379 /* Raytracing for vertex to bone visibility */
380
381 static LaplacianSystem *HeatSys = NULL;
382
383 static void heat_ray_coords_func(RayFace *face, float **v1, float **v2, float **v3, float **v4)
384 {
385         MFace *mface= (MFace*)face;
386         float (*verts)[3]= HeatSys->heat.verts;
387
388         *v1= verts[mface->v1];
389         *v2= verts[mface->v2];
390         *v3= verts[mface->v3];
391         *v4= (mface->v4)? verts[mface->v4]: NULL;
392 }
393
394 static int heat_ray_check_func(Isect *is, RayFace *face)
395 {
396         float *v1, *v2, *v3, *v4, nor[3];
397
398         /* don't intersect if the ray faces along the face normal */
399         heat_ray_coords_func(face, &v1, &v2, &v3, &v4);
400
401         if(v4) CalcNormFloat4(v1, v2, v3, v4, nor);
402         else CalcNormFloat(v1, v2, v3, nor);
403         
404         return (INPR(nor, is->vec) < 0);
405 }
406
407 static void heat_ray_tree_create(LaplacianSystem *sys)
408 {
409         Mesh *me = sys->heat.mesh;
410         RayTree *tree;
411         MFace *mface;
412         float min[3], max[3];
413         int a;
414
415         /* create a raytrace tree from the mesh */
416         INIT_MINMAX(min, max);
417
418         for(a=0; a<me->totvert; a++)
419                 DO_MINMAX(sys->heat.verts[a], min, max);
420
421         tree= RE_ray_tree_create(64, me->totface, min, max,
422                 heat_ray_coords_func, heat_ray_check_func);
423         
424         sys->heat.vface= MEM_callocN(sizeof(MFace*)*me->totvert, "HeatVFaces");
425
426         HeatSys= sys;
427
428         for(a=0, mface=me->mface; a<me->totface; a++, mface++) {
429                 RE_ray_tree_add_face(tree, mface);
430
431                 sys->heat.vface[mface->v1]= mface;
432                 sys->heat.vface[mface->v2]= mface;
433                 sys->heat.vface[mface->v3]= mface;
434                 if(mface->v4) sys->heat.vface[mface->v4]= mface;
435         }
436
437         HeatSys= NULL;
438         
439         RE_ray_tree_done(tree);
440
441         sys->heat.raytree= tree;
442 }
443
444 static int heat_ray_bone_visible(LaplacianSystem *sys, int vertex, int bone)
445 {
446         Isect isec;
447         MFace *mface;
448         float dir[3];
449         int visible;
450
451         mface= sys->heat.vface[vertex];
452         if(!mface)
453                 return 1;
454
455         /* setup isec */
456         isec.mode= RE_RAY_SHADOW;
457         isec.lay= -1;
458         isec.face_last= NULL;
459         isec.faceorig= mface;
460
461         VECCOPY(isec.start, sys->heat.verts[vertex]);
462         PclosestVL3Dfl(isec.end, isec.start,
463                 sys->heat.root[bone], sys->heat.tip[bone]);
464
465         /* add an extra offset to the start position to avoid self intersection */
466         VECSUB(dir, isec.end, isec.start);
467         Normalize(dir);
468         VecMulf(dir, 1e-5);
469         VecAddf(isec.start, isec.start, dir);
470         
471         HeatSys= sys;
472         visible= !RE_ray_tree_intersect(sys->heat.raytree, &isec);
473         HeatSys= NULL;
474
475         return visible;
476 }
477
478 static float heat_bone_distance(LaplacianSystem *sys, int vertex, int bone)
479 {
480         float closest[3], d[3], dist, cosine;
481         
482         /* compute euclidian distance */
483         PclosestVL3Dfl(closest, sys->heat.verts[vertex],
484                 sys->heat.root[bone], sys->heat.tip[bone]);
485
486         VecSubf(d, sys->heat.verts[vertex], closest);
487         dist= Normalize(d);
488
489         /* if the vertex normal does not point along the bone, increase distance */
490         cosine= INPR(d, sys->heat.vnors[vertex]);
491
492         return dist/(0.5f*(cosine + 1.001f));
493 }
494
495 static int heat_bone_closest(LaplacianSystem *sys, int vertex, int bone)
496 {
497         float dist;
498         
499         dist= heat_bone_distance(sys, vertex, bone);
500
501         if(dist <= sys->heat.mindist[vertex]*(1.0f + DISTANCE_EPSILON))
502                 if(heat_ray_bone_visible(sys, vertex, bone))
503                         return 1;
504         
505         return 0;
506 }
507
508 static void heat_set_H(LaplacianSystem *sys, int vertex)
509 {
510         float dist, mindist, h;
511         int j, numclosest = 0;
512
513         mindist= 1e10;
514
515         /* compute minimum distance */
516         for(j=0; j<sys->heat.numbones; j++) {
517                 dist= heat_bone_distance(sys, vertex, j);
518
519                 if(dist < mindist)
520                         mindist= dist;
521         }
522
523         sys->heat.mindist[vertex]= mindist;
524
525         /* count number of bones with approximately this minimum distance */
526         for(j=0; j<sys->heat.numbones; j++)
527                 if(heat_bone_closest(sys, vertex, j))
528                         numclosest++;
529
530         sys->heat.p[vertex]= (numclosest > 0)? 1.0f/numclosest: 0.0f;
531
532         /* compute H entry */
533         if(numclosest > 0) {
534                 if(mindist > 1e-5)
535                         h= numclosest*C_WEIGHT/(mindist*mindist);
536                 else
537                         h= 1e10f;
538         }
539         else
540                 h= 0.0f;
541         
542         sys->heat.H[vertex]= h;
543 }
544
545 void heat_calc_vnormals(LaplacianSystem *sys)
546 {
547         float fnor[3];
548         int a, v1, v2, v3, (*face)[3];
549
550         sys->heat.vnors= MEM_callocN(sizeof(float)*3*sys->totvert, "HeatVNors");
551
552         for(a=0, face=sys->faces; a<sys->totface; a++, face++) {
553                 v1= (*face)[0];
554                 v2= (*face)[1];
555                 v3= (*face)[2];
556
557                 CalcNormFloat(sys->verts[v1], sys->verts[v2], sys->verts[v3], fnor);
558                 
559                 VecAddf(sys->heat.vnors[v1], sys->heat.vnors[v1], fnor);
560                 VecAddf(sys->heat.vnors[v2], sys->heat.vnors[v2], fnor);
561                 VecAddf(sys->heat.vnors[v3], sys->heat.vnors[v3], fnor);
562         }
563
564         for(a=0; a<sys->totvert; a++)
565                 Normalize(sys->heat.vnors[a]);
566 }
567
568 static void heat_laplacian_create(LaplacianSystem *sys)
569 {
570         Mesh *me = sys->heat.mesh;
571         MFace *mface;
572         int a;
573
574         /* heat specific definitions */
575         sys->heat.mindist= MEM_callocN(sizeof(float)*me->totvert, "HeatMinDist");
576         sys->heat.H= MEM_callocN(sizeof(float)*me->totvert, "HeatH");
577         sys->heat.p= MEM_callocN(sizeof(float)*me->totvert, "HeatP");
578
579         /* add verts and faces to laplacian */
580         for(a=0; a<me->totvert; a++)
581                 laplacian_add_vertex(sys, sys->heat.verts[a], 0);
582
583         for(a=0, mface=me->mface; a<me->totface; a++, mface++) {
584                 laplacian_add_triangle(sys, mface->v1, mface->v2, mface->v3);
585                 if(mface->v4)
586                         laplacian_add_triangle(sys, mface->v1, mface->v3, mface->v4);
587         }
588
589         /* for distance computation in set_H */
590         heat_calc_vnormals(sys);
591
592         for(a=0; a<me->totvert; a++)
593                 heat_set_H(sys, a);
594 }
595
596 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)
597 {
598         LaplacianSystem *sys;
599         MFace *mface;
600         float solution;
601         int a, aflip, totface, j, thrownerror = 0;
602
603         /* count triangles */
604         for(totface=0, a=0, mface=me->mface; a<me->totface; a++, mface++) {
605                 totface++;
606                 if(mface->v4) totface++;
607         }
608
609         /* create laplacian */
610         sys = laplacian_system_construct_begin(me->totvert, totface);
611
612         sys->heat.mesh= me;
613         sys->heat.verts= verts;
614         sys->heat.root= root;
615         sys->heat.tip= tip;
616         sys->heat.numbones= numbones;
617
618         heat_ray_tree_create(sys);
619         heat_laplacian_create(sys);
620
621         laplacian_system_construct_end(sys);
622
623         /* compute weights per bone */
624         for(j=0; j<numbones; j++) {
625                 if(!selected[j])
626                         continue;
627
628                 laplacian_begin_solve(sys, -1);
629
630                 for(a=0; a<me->totvert; a++)
631                         if(heat_bone_closest(sys, a, j))
632                                 laplacian_add_right_hand_side(sys, a,
633                                         sys->heat.H[a]*sys->heat.p[a]);
634
635                 if(laplacian_system_solve(sys)) {
636                         for(a=0; a<me->totvert; a++) {
637                                 solution= laplacian_system_get_solution(a);
638
639                                 if(solution > WEIGHT_LIMIT)
640                                         add_vert_to_defgroup(ob, dgrouplist[j], a, solution,
641                                                 WEIGHT_REPLACE);
642                                 else
643                                         remove_vert_defgroup(ob, dgrouplist[j], a);
644
645                                 /* do same for mirror */
646                                 aflip = (dgroupflip)? mesh_get_x_mirror_vert(ob, a): 0;
647                                 if (dgroupflip && dgroupflip[j] && aflip >= 0) {
648                                         if(solution > WEIGHT_LIMIT)
649                                                 add_vert_to_defgroup(ob, dgroupflip[j], aflip,
650                                                         solution, WEIGHT_REPLACE);
651                                         else
652                                                 remove_vert_defgroup(ob, dgroupflip[j], aflip);
653                                 }
654                         }
655                 }
656                 else if(!thrownerror) {
657                         error("Bone Heat Weighting:"
658                                 " failed to find solution for one or more bones");
659                         thrownerror= 1;
660                         break;
661                 }
662         }
663
664         /* free */
665         RE_ray_tree_free(sys->heat.raytree);
666         MEM_freeN(sys->heat.vface);
667
668         MEM_freeN(sys->heat.mindist);
669         MEM_freeN(sys->heat.H);
670         MEM_freeN(sys->heat.p);
671         MEM_freeN(sys->heat.vnors);
672
673         laplacian_system_delete(sys);
674 }
675
676 #ifdef RIGID_DEFORM
677 /********************** As-Rigid-As-Possible Deformation ******************/
678 /* From "As-Rigid-As-Possible Surface Modeling",
679         Olga Sorkine and Marc Alexa, ESGP 2007. */
680
681 /* investigate:
682    - transpose R in orthogonal
683    - flipped normals and per face adding
684    - move cancelling to transform, make origco pointer
685 */
686
687 static LaplacianSystem *RigidDeformSystem = NULL;
688
689 static void rigid_add_half_edge_to_R(LaplacianSystem *sys, EditVert *v1, EditVert *v2, float w)
690 {
691         float e[3], e_[3];
692         int i;
693
694         VecSubf(e, sys->rigid.origco[v1->tmp.l], sys->rigid.origco[v2->tmp.l]);
695         VecSubf(e_, v1->co, v2->co);
696
697         /* formula (5) */
698         for (i=0; i<3; i++) {
699                 sys->rigid.R[v1->tmp.l][i][0] += w*e[0]*e_[i];
700                 sys->rigid.R[v1->tmp.l][i][1] += w*e[1]*e_[i];
701                 sys->rigid.R[v1->tmp.l][i][2] += w*e[2]*e_[i];
702         }
703 }
704
705 static void rigid_add_edge_to_R(LaplacianSystem *sys, EditVert *v1, EditVert *v2, float w)
706 {
707         rigid_add_half_edge_to_R(sys, v1, v2, w);
708         rigid_add_half_edge_to_R(sys, v2, v1, w);
709 }
710
711 static void rigid_orthogonalize_R(float R[][3])
712 {
713         HMatrix M, Q, S;
714
715         Mat4CpyMat3(M, R);
716         polar_decomp(M, Q, S);
717         Mat3CpyMat4(R, Q);
718 }
719
720 static void rigid_add_half_edge_to_rhs(LaplacianSystem *sys, EditVert *v1, EditVert *v2, float w)
721 {
722         /* formula (8) */
723         float Rsum[3][3], rhs[3];
724
725         if (sys->vpinned[v1->tmp.l])
726                 return;
727
728         Mat3AddMat3(Rsum, sys->rigid.R[v1->tmp.l], sys->rigid.R[v2->tmp.l]);
729         Mat3Transp(Rsum);
730
731         VecSubf(rhs, sys->rigid.origco[v1->tmp.l], sys->rigid.origco[v2->tmp.l]);
732         Mat3MulVecfl(Rsum, rhs);
733         VecMulf(rhs, 0.5f);
734         VecMulf(rhs, w);
735
736         VecAddf(sys->rigid.rhs[v1->tmp.l], sys->rigid.rhs[v1->tmp.l], rhs);
737 }
738
739 static void rigid_add_edge_to_rhs(LaplacianSystem *sys, EditVert *v1, EditVert *v2, float w)
740 {
741         rigid_add_half_edge_to_rhs(sys, v1, v2, w);
742         rigid_add_half_edge_to_rhs(sys, v2, v1, w);
743 }
744
745 void rigid_deform_iteration()
746 {
747         LaplacianSystem *sys= RigidDeformSystem;
748         EditMesh *em;
749         EditVert *eve;
750         EditFace *efa;
751         int a, i;
752
753         if(!sys)
754                 return;
755         
756         nlMakeCurrent(sys->context);
757         em= sys->rigid.mesh;
758
759         /* compute R */
760         memset(sys->rigid.R, 0, sizeof(float)*3*3*sys->totvert);
761         memset(sys->rigid.rhs, 0, sizeof(float)*3*sys->totvert);
762
763         for(a=0, efa=em->faces.first; efa; efa=efa->next, a++) {
764                 rigid_add_edge_to_R(sys, efa->v1, efa->v2, sys->fweights[a][2]);
765                 rigid_add_edge_to_R(sys, efa->v2, efa->v3, sys->fweights[a][0]);
766                 rigid_add_edge_to_R(sys, efa->v3, efa->v1, sys->fweights[a][1]);
767
768                 if(efa->v4) {
769                         a++;
770                         rigid_add_edge_to_R(sys, efa->v1, efa->v3, sys->fweights[a][2]);
771                         rigid_add_edge_to_R(sys, efa->v3, efa->v4, sys->fweights[a][0]);
772                         rigid_add_edge_to_R(sys, efa->v4, efa->v1, sys->fweights[a][1]);
773                 }
774         }
775
776         for(a=0, eve=em->verts.first; eve; eve=eve->next, a++) {
777                 rigid_orthogonalize_R(sys->rigid.R[a]);
778                 eve->tmp.l= a;
779         }
780         
781         /* compute right hand sides for solving */
782         for(a=0, efa=em->faces.first; efa; efa=efa->next, a++) {
783                 rigid_add_edge_to_rhs(sys, efa->v1, efa->v2, sys->fweights[a][2]);
784                 rigid_add_edge_to_rhs(sys, efa->v2, efa->v3, sys->fweights[a][0]);
785                 rigid_add_edge_to_rhs(sys, efa->v3, efa->v1, sys->fweights[a][1]);
786
787                 if(efa->v4) {
788                         a++;
789                         rigid_add_edge_to_rhs(sys, efa->v1, efa->v3, sys->fweights[a][2]);
790                         rigid_add_edge_to_rhs(sys, efa->v3, efa->v4, sys->fweights[a][0]);
791                         rigid_add_edge_to_rhs(sys, efa->v4, efa->v1, sys->fweights[a][1]);
792                 }
793         }
794
795         /* solve for positions, for X,Y and Z separately */
796         for(i=0; i<3; i++) {
797                 laplacian_begin_solve(sys, i);
798
799                 for(a=0; a<sys->totvert; a++)
800                         if(!sys->vpinned[a]) {
801                                 /*if (i==0)
802                                         printf("rhs %f\n", sys->rigid.rhs[a][0]);*/
803                                 laplacian_add_right_hand_side(sys, a, sys->rigid.rhs[a][i]);
804                         }
805
806                 if(laplacian_system_solve(sys)) {
807                         for(a=0, eve=em->verts.first; eve; eve=eve->next, a++)
808                                 eve->co[i]= laplacian_system_get_solution(a);
809                 }
810                 else {
811                         if(!sys->rigid.thrownerror) {
812                                 error("RigidDeform: failed to find solution.");
813                                 sys->rigid.thrownerror= 1;
814                         }
815                         break;
816                 }
817         }
818
819         /*printf("\n--------------------------------------------\n\n");*/
820 }
821
822 static void rigid_laplacian_create(LaplacianSystem *sys)
823 {
824         EditMesh *em = sys->rigid.mesh;
825         EditVert *eve;
826         EditFace *efa;
827         int a;
828
829         /* add verts and faces to laplacian */
830         for(a=0, eve=em->verts.first; eve; eve=eve->next, a++) {
831                 laplacian_add_vertex(sys, eve->co, eve->pinned);
832                 eve->tmp.l= a;
833         }
834
835         for(efa=em->faces.first; efa; efa=efa->next) {
836                 laplacian_add_triangle(sys,
837                         efa->v1->tmp.l, efa->v2->tmp.l, efa->v3->tmp.l);
838                 if(efa->v4)
839                         laplacian_add_triangle(sys,
840                                 efa->v1->tmp.l, efa->v3->tmp.l, efa->v4->tmp.l);
841         }
842 }
843
844 void rigid_deform_begin(EditMesh *em)
845 {
846         LaplacianSystem *sys;
847         EditVert *eve;
848         EditFace *efa;
849         int a, totvert, totface;
850
851         /* count vertices, triangles */
852         for(totvert=0, eve=em->verts.first; eve; eve=eve->next)
853                 totvert++;
854
855         for(totface=0, efa=em->faces.first; efa; efa=efa->next) {
856                 totface++;
857                 if(efa->v4) totface++;
858         }
859
860         /* create laplacian */
861         sys = laplacian_system_construct_begin(totvert, totface);
862
863         sys->rigid.mesh= em;
864         sys->rigid.R = MEM_callocN(sizeof(float)*3*3*totvert, "RigidDeformR");
865         sys->rigid.rhs = MEM_callocN(sizeof(float)*3*totvert, "RigidDeformRHS");
866         sys->rigid.origco = MEM_callocN(sizeof(float)*3*totvert, "RigidDeformCo");
867
868         for(a=0, eve=em->verts.first; eve; eve=eve->next, a++)
869                 VecCopyf(sys->rigid.origco[a], eve->co);
870
871         sys->areaweights= 0;
872         sys->storeweights= 1;
873
874         rigid_laplacian_create(sys);
875
876         laplacian_system_construct_end(sys);
877
878         RigidDeformSystem = sys;
879 }
880
881 void rigid_deform_end(int cancel)
882 {
883         LaplacianSystem *sys = RigidDeformSystem;
884
885         if(sys) {
886                 EditMesh *em = sys->rigid.mesh;
887                 EditVert *eve;
888                 int a;
889
890                 if(cancel)
891                         for(a=0, eve=em->verts.first; eve; eve=eve->next, a++)
892                                 if(!eve->pinned)
893                                         VecCopyf(eve->co, sys->rigid.origco[a]);
894
895                 if(sys->rigid.R) MEM_freeN(sys->rigid.R);
896                 if(sys->rigid.rhs) MEM_freeN(sys->rigid.rhs);
897                 if(sys->rigid.origco) MEM_freeN(sys->rigid.origco);
898
899                 /* free */
900                 laplacian_system_delete(sys);
901         }
902
903         RigidDeformSystem = NULL;
904 }
905 #endif
906