Bone Heat Weighting

===================

This is a new automatic vertex weighting method, next to the existing
envelope based method. The details are here:

http://www.blender.org/development/current-projects/changes-since-244/skinning/

This is based on section 4 of the paper:

"Automatic Rigging and Animation of 3D Characters"
Ilya Baran and Jovan Popovic, SIGGRAPH 2007

Implementation Notes:

- Generic code for making mesh laplacian matrices has been added, which
  is only used by bone heat weighting at the moment.
- Bone to vertex visibility checking is done with the raytracing code.
- Fixed an issue in the subsurf limit calculation function, where the
  position of vertices on boundary edges was wrong. It is still not the
  correct position, but at least it's in the neighbourhood now.

1 files changed, 906 insertions, 0 deletions

diff --git a/source/blender/src/meshlaplacian.c b/source/blender/src/meshlaplacian.c
new file mode 100644
index 00000000000..a660b42996e
--- /dev/null
+++ b/source/blender/src/meshlaplacian.c
@@ -0,0 +1,906 @@
+/**
+ * $Id$
+ *
+ * ***** BEGIN GPL/BL DUAL LICENSE BLOCK *****
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * as published by the Free Software Foundation; either version 2
+ * of the License, or (at your option) any later version. The Blender
+ * Foundation also sells licenses for use in proprietary software under
+ * the Blender License.  See http://www.blender.org/BL/ for information
+ * about this.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software Foundation,
+ * Inc., 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.
+ *
+ * The Original Code is Copyright (C) 2001-2002 by NaN Holding BV.
+ * All rights reserved.
+ *
+ * The Original Code is: all of this file.
+ *
+ * Contributor(s): none yet.
+ *
+ * ***** END GPL/BL DUAL LICENSE BLOCK *****
+ * meshlaplacian.c: Algorithms using the mesh laplacian.
+ */
+
+#include <string.h>
+
+#include "MEM_guardedalloc.h"
+
+#include "DNA_listBase.h"
+#include "DNA_object_types.h"
+#include "DNA_mesh_types.h"
+#include "DNA_meshdata_types.h"
+
+#include "BLI_arithb.h"
+#include "BLI_edgehash.h"
+
+#include "BKE_utildefines.h"
+
+#include "BIF_editdeform.h"
+#include "BIF_meshlaplacian.h"
+#include "BIF_meshtools.h"
+#include "BIF_toolbox.h"
+
+#ifdef RIGID_DEFORM
+#include "BLI_editVert.h"
+#include "BLI_polardecomp.h"
+#endif
+
+#include "RE_raytrace.h"
+
+#include "ONL_opennl.h"
+
+/************************** Laplacian System *****************************/
+
+struct LaplacianSystem {
+	NLContext context;	/* opennl context */
+
+	int totvert, totface;
+
+	float **verts;			/* vertex coordinates */
+	float *varea;			/* vertex weights for laplacian computation */
+	char *vpinned;			/* vertex pinning */
+	int (*faces)[3];		/* face vertex indices */
+	float (*fweights)[3];	/* cotangent weights per face */
+
+	int areaweights;		/* use area in cotangent weights? */
+	int storeweights;		/* store cotangent weights in fweights */
+	int nlbegun;			/* nlBegin(NL_SYSTEM/NL_MATRIX) done */
+
+	EdgeHash *edgehash;		/* edge hash for construction */
+
+	struct HeatWeighting {
+		Mesh *mesh;
+		float (*verts)[3];	/* vertex coordinates */
+		float (*vnors)[3];	/* vertex normals */
+
+		float (*root)[3];	/* bone root */
+		float (*tip)[3];	/* bone tip */
+		int numbones;
+
+		float *H;			/* diagonal H matrix */
+		float *p;			/* values from all p vectors */
+		float *mindist;		/* minimum distance to a bone for all vertices */
+		
+		RayTree *raytree;	/* ray tracing acceleration structure */
+		MFace **vface;		/* a face that the vertex belongs to */
+	} heat;
+
+#ifdef RIGID_DEFORM
+	struct RigidDeformation {
+		EditMesh *mesh;
+
+		float (*R)[3][3];
+		float (*rhs)[3];
+		float (*origco)[3];
+		int thrownerror;
+	} rigid;
+#endif
+};
+
+/* Laplacian matrix construction */
+
+/* Computation of these weights for the laplacian is based on:
+   "Discrete Differential-Geometry Operators for Triangulated 2-Manifolds",
+   Meyer et al, 2002. Section 3.5, formula (8).
+   
+   We do it a bit different by going over faces instead of going over each
+   vertex and adjacent faces, since we don't store this adjacency. Also, the
+   formulas are tweaked a bit to work for non-manifold meshes. */
+
+static void laplacian_increase_edge_count(EdgeHash *edgehash, int v1, int v2)
+{
+	void **p = BLI_edgehash_lookup_p(edgehash, v1, v2);
+
+	if(p)
+		*p = (void*)((long)*p + (long)1);
+	else
+		BLI_edgehash_insert(edgehash, v1, v2, (void*)(long)1);
+}
+
+static int laplacian_edge_count(EdgeHash *edgehash, int v1, int v2)
+{
+	return (int)(long)BLI_edgehash_lookup(edgehash, v1, v2);
+}
+
+static float cotan_weight(float *v1, float *v2, float *v3)
+{
+	float a[3], b[3], c[3], clen;
+
+	VecSubf(a, v2, v1);
+	VecSubf(b, v3, v1);
+	Crossf(c, a, b);
+
+	clen = VecLength(c);
+
+	if (clen == 0.0f)
+		return 0.0f;
+	
+	return Inpf(a, b)/clen;
+}
+
+static void laplacian_triangle_area(LaplacianSystem *sys, int i1, int i2, int i3)
+{
+	float t1, t2, t3, len1, len2, len3, area;
+	float *varea= sys->varea, *v1, *v2, *v3;
+	int obtuse = 0;
+
+	v1= sys->verts[i1];
+	v2= sys->verts[i2];
+	v3= sys->verts[i3];
+
+	t1= cotan_weight(v1, v2, v3);
+	t2= cotan_weight(v2, v3, v1);
+	t3= cotan_weight(v3, v1, v2);
+
+	if(VecAngle3(v2, v1, v3) > 90) obtuse= 1;
+	else if(VecAngle3(v1, v2, v3) > 90) obtuse= 2;
+	else if(VecAngle3(v1, v3, v2) > 90) obtuse= 3;
+
+	if (obtuse > 0) {
+		area= AreaT3Dfl(v1, v2, v3);
+
+		varea[i1] += (obtuse == 1)? area: area*0.5;
+		varea[i2] += (obtuse == 2)? area: area*0.5;
+		varea[i3] += (obtuse == 3)? area: area*0.5;
+	}
+	else {
+		len1= VecLenf(v2, v3);
+		len2= VecLenf(v1, v3);
+		len3= VecLenf(v1, v2);
+
+		t1 *= len1*len1;
+		t2 *= len2*len2;
+		t3 *= len3*len3;
+
+		varea[i1] += (t2 + t3)*0.25f;
+		varea[i2] += (t1 + t3)*0.25f;
+		varea[i3] += (t1 + t2)*0.25f;
+	}
+}
+
+static void laplacian_triangle_weights(LaplacianSystem *sys, int f, int i1, int i2, int i3)
+{
+	float t1, t2, t3;
+	float *varea= sys->varea, *v1, *v2, *v3;
+
+	v1= sys->verts[i1];
+	v2= sys->verts[i2];
+	v3= sys->verts[i3];
+
+	/* instead of *0.5 we divided by the number of faces of the edge, it still
+	   needs to be varified that this is indeed the correct thing to do! */
+	t1= cotan_weight(v1, v2, v3)/laplacian_edge_count(sys->edgehash, i2, i3);
+	t2= cotan_weight(v2, v3, v1)/laplacian_edge_count(sys->edgehash, i3, i1);
+	t3= cotan_weight(v3, v1, v2)/laplacian_edge_count(sys->edgehash, i1, i2);
+
+	nlMatrixAdd(i1, i1, (t2+t3)*varea[i1]);
+	nlMatrixAdd(i2, i2, (t1+t3)*varea[i2]);
+	nlMatrixAdd(i3, i3, (t1+t2)*varea[i3]);
+
+	nlMatrixAdd(i1, i2, -t3*varea[i1]);
+	nlMatrixAdd(i2, i1, -t3*varea[i2]);
+
+	nlMatrixAdd(i2, i3, -t1*varea[i2]);
+	nlMatrixAdd(i3, i2, -t1*varea[i3]);
+
+	nlMatrixAdd(i3, i1, -t2*varea[i3]);
+	nlMatrixAdd(i1, i3, -t2*varea[i1]);
+
+	if(sys->storeweights) {
+		sys->fweights[f][0]= t1*varea[i1];
+		sys->fweights[f][1]= t2*varea[i2];
+		sys->fweights[f][2]= t3*varea[i3];
+	}
+}
+
+LaplacianSystem *laplacian_system_construct_begin(int totvert, int totface)
+{
+	LaplacianSystem *sys;
+
+	sys= MEM_callocN(sizeof(LaplacianSystem), "LaplacianSystem");
+
+	sys->verts= MEM_callocN(sizeof(float*)*totvert, "LaplacianSystemVerts");
+	sys->vpinned= MEM_callocN(sizeof(char)*totvert, "LaplacianSystemVpinned");
+	sys->faces= MEM_callocN(sizeof(int)*3*totface, "LaplacianSystemFaces");
+
+	sys->totvert= 0;
+	sys->totface= 0;
+
+	sys->areaweights= 1;
+	sys->storeweights= 0;
+
+	/* create opennl context */
+	nlNewContext();
+	nlSolverParameteri(NL_NB_VARIABLES, totvert);
+
+	sys->context= nlGetCurrent();
+
+	return sys;
+}
+
+void laplacian_add_vertex(LaplacianSystem *sys, float *co, int pinned)
+{
+	sys->verts[sys->totvert]= co;
+	sys->vpinned[sys->totvert]= pinned;
+	sys->totvert++;
+}
+
+void laplacian_add_triangle(LaplacianSystem *sys, int v1, int v2, int v3)
+{
+	sys->faces[sys->totface][0]= v1;
+	sys->faces[sys->totface][1]= v2;
+	sys->faces[sys->totface][2]= v3;
+	sys->totface++;
+}
+
+void laplacian_system_construct_end(LaplacianSystem *sys)
+{
+	int (*face)[3];
+	int a, totvert=sys->totvert, totface=sys->totface;
+
+	laplacian_begin_solve(sys, 0);
+
+	sys->varea= MEM_callocN(sizeof(float)*totvert, "LaplacianSystemVarea");
+
+	sys->edgehash= BLI_edgehash_new();
+	for(a=0, face=sys->faces; a<sys->totface; a++, face++) {
+		laplacian_increase_edge_count(sys->edgehash, (*face)[0], (*face)[1]);
+		laplacian_increase_edge_count(sys->edgehash, (*face)[1], (*face)[2]);
+		laplacian_increase_edge_count(sys->edgehash, (*face)[2], (*face)[0]);
+	}
+
+	if(sys->areaweights)
+		for(a=0, face=sys->faces; a<sys->totface; a++, face++)
+			laplacian_triangle_area(sys, (*face)[0], (*face)[1], (*face)[2]);
+	
+	for(a=0; a<totvert; a++) {
+		if(sys->areaweights) {
+			if(sys->varea[a] != 0.0f)
+				sys->varea[a]= 0.5f/sys->varea[a];
+		}
+		else
+			sys->varea[a]= 1.0f;
+
+		/* for heat weighting */
+		if(sys->heat.H)
+			nlMatrixAdd(a, a, sys->heat.H[a]);
+	}
+
+	if(sys->storeweights)
+		sys->fweights= MEM_callocN(sizeof(float)*3*totface, "LaplacianFWeight");
+	
+	for(a=0, face=sys->faces; a<totface; a++, face++)
+		laplacian_triangle_weights(sys, a, (*face)[0], (*face)[1], (*face)[2]);
+
+	MEM_freeN(sys->faces);
+	sys->faces= NULL;
+
+	if(sys->varea) {
+		MEM_freeN(sys->varea);
+		sys->varea= NULL;
+	}
+
+	BLI_edgehash_free(sys->edgehash, NULL);
+	sys->edgehash= NULL;
+}
+
+void laplacian_system_delete(LaplacianSystem *sys)
+{
+	if(sys->verts) MEM_freeN(sys->verts);
+	if(sys->varea) MEM_freeN(sys->varea);
+	if(sys->vpinned) MEM_freeN(sys->vpinned);
+	if(sys->faces) MEM_freeN(sys->faces);
+	if(sys->fweights) MEM_freeN(sys->fweights);
+
+	nlDeleteContext(sys->context);
+	MEM_freeN(sys);
+}
+
+void laplacian_begin_solve(LaplacianSystem *sys, int index)
+{
+	int a;
+
+	if (!sys->nlbegun) {
+		nlBegin(NL_SYSTEM);
+
+		if(index >= 0) {
+			for(a=0; a<sys->totvert; a++) {
+				if(sys->vpinned[a]) {
+					nlSetVariable(a, sys->verts[a][index]);
+					nlLockVariable(a);
+				}
+			}
+		}
+
+		nlBegin(NL_MATRIX);
+		sys->nlbegun = 1;
+	}
+}
+
+void laplacian_add_right_hand_side(LaplacianSystem *sys, int v, float value)
+{
+	nlRightHandSideAdd(v, value);
+}
+
+int laplacian_system_solve(LaplacianSystem *sys)
+{
+	nlEnd(NL_MATRIX);
+	nlEnd(NL_SYSTEM);
+	sys->nlbegun = 0;
+
+	//nlPrintMatrix();
+
+	return nlSolveAdvanced(NULL, NL_TRUE);
+}
+
+float laplacian_system_get_solution(int v)
+{
+	return nlGetVariable(v);
+}
+
+/************************* Heat Bone Weighting ******************************/
+/* From "Automatic Rigging and Animation of 3D Characters"
+         Ilya Baran and Jovan Popovic, SIGGRAPH 2007 */
+
+#define C_WEIGHT			1.0f
+#define WEIGHT_LIMIT		0.05f
+#define DISTANCE_EPSILON	1e-4f
+
+/* Raytracing for vertex to bone visibility */
+
+static LaplacianSystem *HeatSys = NULL;
+
+static void heat_ray_coords_func(RayFace *face, float **v1, float **v2, float **v3, float **v4)
+{
+	MFace *mface= (MFace*)face;
+	float (*verts)[3]= HeatSys->heat.verts;
+
+	*v1= verts[mface->v1];
+	*v2= verts[mface->v2];
+	*v3= verts[mface->v3];
+	*v4= (mface->v4)? verts[mface->v4]: NULL;
+}
+
+static int heat_ray_check_func(Isect *is, RayFace *face)
+{
+	float *v1, *v2, *v3, *v4, nor[3];
+
+	/* don't intersect if the ray faces along the face normal */
+	heat_ray_coords_func(face, &v1, &v2, &v3, &v4);
+
+	if(v4) CalcNormFloat4(v1, v2, v3, v4, nor);
+	else CalcNormFloat(v1, v2, v3, nor);
+	
+	return (INPR(nor, is->vec) < 0);
+}
+
+static void heat_ray_tree_create(LaplacianSystem *sys)
+{
+	Mesh *me = sys->heat.mesh;
+	RayTree *tree;
+	MFace *mface;
+	float min[3], max[3];
+	int a;
+
+	/* create a raytrace tree from the mesh */
+	INIT_MINMAX(min, max);
+
+	for(a=0; a<me->totvert; a++)
+		DO_MINMAX(sys->heat.verts[a], min, max);
+
+	tree= RE_ray_tree_create(64, me->totface, min, max,
+		heat_ray_coords_func, heat_ray_check_func);
+	
+	sys->heat.vface= MEM_callocN(sizeof(MFace*)*me->totvert, "HeatVFaces");
+
+	HeatSys= sys;
+
+	for(a=0, mface=me->mface; a<me->totface; a++, mface++) {
+		RE_ray_tree_add_face(tree, mface);
+
+		sys->heat.vface[mface->v1]= mface;
+		sys->heat.vface[mface->v2]= mface;
+		sys->heat.vface[mface->v3]= mface;
+		if(mface->v4) sys->heat.vface[mface->v4]= mface;
+	}
+
+	HeatSys= NULL;
+	
+	RE_ray_tree_done(tree);
+
+	sys->heat.raytree= tree;
+}
+
+static int heat_ray_bone_visible(LaplacianSystem *sys, int vertex, int bone)
+{
+	Isect isec;
+	MFace *mface;
+	float dir[3];
+	int visible;
+
+	mface= sys->heat.vface[vertex];
+	if(!mface)
+		return 1;
+
+	/* setup isec */
+	isec.mode= RE_RAY_SHADOW;
+	isec.lay= -1;
+	isec.face_last= NULL;
+	isec.faceorig= mface;
+
+	VECCOPY(isec.start, sys->heat.verts[vertex]);
+	PclosestVL3Dfl(isec.end, isec.start,
+		sys->heat.root[bone], sys->heat.tip[bone]);
+
+	/* add an extra offset to the start position to avoid self intersection */
+	VECSUB(dir, isec.end, isec.start);
+	Normalize(dir);
+	VecMulf(dir, 1e-5);
+	VecAddf(isec.start, isec.start, dir);
+	
+	HeatSys= sys;
+	visible= !RE_ray_tree_intersect(sys->heat.raytree, &isec);
+	HeatSys= NULL;
+
+	return visible;
+}
+
+static float heat_bone_distance(LaplacianSystem *sys, int vertex, int bone)
+{
+	float closest[3], d[3], dist, cosine;
+	
+	/* compute euclidian distance */
+	PclosestVL3Dfl(closest, sys->heat.verts[vertex],
+		sys->heat.root[bone], sys->heat.tip[bone]);
+
+	VecSubf(d, sys->heat.verts[vertex], closest);
+	dist= Normalize(d);
+
+	/* if the vertex normal does not point along the bone, increase distance */
+	cosine= INPR(d, sys->heat.vnors[vertex]);
+
+	return dist/(0.5f*(cosine + 1.001f));
+}
+
+static int heat_bone_closest(LaplacianSystem *sys, int vertex, int bone)
+{
+	float dist;
+	
+	dist= heat_bone_distance(sys, vertex, bone);
+
+	if(dist <= sys->heat.mindist[vertex]*(1.0f + DISTANCE_EPSILON))
+		if(heat_ray_bone_visible(sys, vertex, bone))
+			return 1;
+	
+	return 0;
+}
+
+static void heat_set_H(LaplacianSystem *sys, int vertex)
+{
+	float dist, mindist, h;
+	int j, numclosest = 0;
+
+	mindist= 1e10;
+
+	/* compute minimum distance */
+	for(j=0; j<sys->heat.numbones; j++) {
+		dist= heat_bone_distance(sys, vertex, j);
+
+		if(dist < mindist)
+			mindist= dist;
+	}
+
+	sys->heat.mindist[vertex]= mindist;
+
+	/* count number of bones with approximately this minimum distance */
+	for(j=0; j<sys->heat.numbones; j++)
+		if(heat_bone_closest(sys, vertex, j))
+			numclosest++;
+
+	sys->heat.p[vertex]= (numclosest > 0)? 1.0f/numclosest: 0.0f;
+
+	/* compute H entry */
+	if(numclosest > 0) {
+		if(mindist > 1e-5)
+			h= numclosest*C_WEIGHT/(mindist*mindist);
+		else
+			h= 1e10f;
+	}
+	else
+		h= 0.0f;
+	
+	sys->heat.H[vertex]= h;
+}
+
+void heat_calc_vnormals(LaplacianSystem *sys)
+{
+	float fnor[3];
+	int a, v1, v2, v3, (*face)[3];
+
+	sys->heat.vnors= MEM_callocN(sizeof(float)*3*sys->totvert, "HeatVNors");
+
+	for(a=0, face=sys->faces; a<sys->totface; a++, face++) {
+		v1= (*face)[0];
+		v2= (*face)[1];
+		v3= (*face)[2];
+
+		CalcNormFloat(sys->verts[v1], sys->verts[v2], sys->verts[v3], fnor);
+		
+		VecAddf(sys->heat.vnors[v1], sys->heat.vnors[v1], fnor);
+		VecAddf(sys->heat.vnors[v2], sys->heat.vnors[v2], fnor);
+		VecAddf(sys->heat.vnors[v3], sys->heat.vnors[v3], fnor);
+	}
+
+	for(a=0; a<sys->totvert; a++)
+		Normalize(sys->heat.vnors[a]);
+}
+
+static void heat_laplacian_create(LaplacianSystem *sys)
+{
+	Mesh *me = sys->heat.mesh;
+	MFace *mface;
+	int a;
+
+	/* heat specific definitions */
+	sys->heat.mindist= MEM_callocN(sizeof(float)*me->totvert, "HeatMinDist");
+	sys->heat.H= MEM_callocN(sizeof(float)*me->totvert, "HeatH");
+	sys->heat.p= MEM_callocN(sizeof(float)*me->totvert, "HeatP");
+
+	/* add verts and faces to laplacian */
+	for(a=0; a<me->totvert; a++)
+		laplacian_add_vertex(sys, sys->heat.verts[a], 0);
+
+	for(a=0, mface=me->mface; a<me->totface; a++, mface++) {
+		laplacian_add_triangle(sys, mface->v1, mface->v2, mface->v3);
+		if(mface->v4)
+			laplacian_add_triangle(sys, mface->v1, mface->v3, mface->v4);
+	}
+
+	/* for distance computation in set_H */
+	heat_calc_vnormals(sys);
+
+	for(a=0; a<me->totvert; a++)
+		heat_set_H(sys, a);
+}
+
+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)
+{
+	LaplacianSystem *sys;
+	MFace *mface;
+	float solution;
+	int a, aflip, totface, j, thrownerror = 0;
+
+	/* count triangles */
+	for(totface=0, a=0, mface=me->mface; a<me->totface; a++, mface++) {
+		totface++;
+		if(mface->v4) totface++;
+	}
+
+	/* create laplacian */
+	sys = laplacian_system_construct_begin(me->totvert, totface);
+
+	sys->heat.mesh= me;
+	sys->heat.verts= verts;
+	sys->heat.root= root;
+	sys->heat.tip= tip;
+	sys->heat.numbones= numbones;
+
+	heat_ray_tree_create(sys);
+	heat_laplacian_create(sys);
+
+	laplacian_system_construct_end(sys);
+
+	/* compute weights per bone */
+	for(j=0; j<numbones; j++) {
+		if(!selected[j])
+			continue;
+
+		laplacian_begin_solve(sys, -1);
+
+		for(a=0; a<me->totvert; a++)
+			if(heat_bone_closest(sys, a, j))
+				laplacian_add_right_hand_side(sys, a,
+					sys->heat.H[a]*sys->heat.p[a]);
+
+		if(laplacian_system_solve(sys)) {
+			for(a=0; a<me->totvert; a++) {
+				solution= laplacian_system_get_solution(a);
+
+				if(solution > WEIGHT_LIMIT)
+					add_vert_to_defgroup(ob, dgrouplist[j], a, solution,
+						WEIGHT_REPLACE);
+				else
+					remove_vert_defgroup(ob, dgrouplist[j], a);
+
+				/* do same for mirror */
+				aflip = (dgroupflip)? mesh_get_x_mirror_vert(ob, a): 0;
+				if (dgroupflip && dgroupflip[j] && aflip >= 0) {
+					if(solution > WEIGHT_LIMIT)
+						add_vert_to_defgroup(ob, dgroupflip[j], aflip,
+							solution, WEIGHT_REPLACE);
+					else
+						remove_vert_defgroup(ob, dgroupflip[j], aflip);
+				}
+			}
+		}
+		else if(!thrownerror) {
+			error("Bone Heat Weighting:"
+				" failed to find solution for one or more bones");
+			thrownerror= 1;
+			break;
+		}
+	}
+
+	/* free */
+	RE_ray_tree_free(sys->heat.raytree);
+	MEM_freeN(sys->heat.vface);
+
+	MEM_freeN(sys->heat.mindist);
+	MEM_freeN(sys->heat.H);
+	MEM_freeN(sys->heat.p);
+	MEM_freeN(sys->heat.vnors);
+
+	laplacian_system_delete(sys);
+}
+
+#ifdef RIGID_DEFORM
+/********************** As-Rigid-As-Possible Deformation ******************/
+/* From "As-Rigid-As-Possible Surface Modeling",
+        Olga Sorkine and Marc Alexa, ESGP 2007. */
+
+/* investigate:
+   - transpose R in orthogonal
+   - flipped normals and per face adding
+   - move cancelling to transform, make origco pointer
+*/
+
+static LaplacianSystem *RigidDeformSystem = NULL;
+
+static void rigid_add_half_edge_to_R(LaplacianSystem *sys, EditVert *v1, EditVert *v2, float w)
+{
+	float e[3], e_[3];
+	int i;
+
+	VecSubf(e, sys->rigid.origco[v1->tmp.l], sys->rigid.origco[v2->tmp.l]);
+	VecSubf(e_, v1->co, v2->co);
+
+	/* formula (5) */
+	for (i=0; i<3; i++) {
+		sys->rigid.R[v1->tmp.l][i][0] += w*e[0]*e_[i];
+		sys->rigid.R[v1->tmp.l][i][1] += w*e[1]*e_[i];
+		sys->rigid.R[v1->tmp.l][i][2] += w*e[2]*e_[i];
+	}
+}
+
+static void rigid_add_edge_to_R(LaplacianSystem *sys, EditVert *v1, EditVert *v2, float w)
+{
+	rigid_add_half_edge_to_R(sys, v1, v2, w);
+	rigid_add_half_edge_to_R(sys, v2, v1, w);
+}
+
+static void rigid_orthogonalize_R(float R[][3])
+{
+	HMatrix M, Q, S;
+
+	Mat4CpyMat3(M, R);
+	polar_decomp(M, Q, S);
+	Mat3CpyMat4(R, Q);
+}
+
+static void rigid_add_half_edge_to_rhs(LaplacianSystem *sys, EditVert *v1, EditVert *v2, float w)
+{
+	/* formula (8) */
+	float Rsum[3][3], rhs[3];
+
+	if (sys->vpinned[v1->tmp.l])
+		return;
+
+	Mat3AddMat3(Rsum, sys->rigid.R[v1->tmp.l], sys->rigid.R[v2->tmp.l]);
+	Mat3Transp(Rsum);
+
+	VecSubf(rhs, sys->rigid.origco[v1->tmp.l], sys->rigid.origco[v2->tmp.l]);
+	Mat3MulVecfl(Rsum, rhs);
+	VecMulf(rhs, 0.5f);
+	VecMulf(rhs, w);
+
+	VecAddf(sys->rigid.rhs[v1->tmp.l], sys->rigid.rhs[v1->tmp.l], rhs);
+}
+
+static void rigid_add_edge_to_rhs(LaplacianSystem *sys, EditVert *v1, EditVert *v2, float w)
+{
+	rigid_add_half_edge_to_rhs(sys, v1, v2, w);
+	rigid_add_half_edge_to_rhs(sys, v2, v1, w);
+}
+
+void rigid_deform_iteration()
+{
+	LaplacianSystem *sys= RigidDeformSystem;
+	EditMesh *em;
+	EditVert *eve;
+	EditFace *efa;
+	int a, i;
+
+	if(!sys)
+		return;
+	
+	nlMakeCurrent(sys->context);
+	em= sys->rigid.mesh;
+
+	/* compute R */
+	memset(sys->rigid.R, 0, sizeof(float)*3*3*sys->totvert);
+	memset(sys->rigid.rhs, 0, sizeof(float)*3*sys->totvert);
+
+	for(a=0, efa=em->faces.first; efa; efa=efa->next, a++) {
+		rigid_add_edge_to_R(sys, efa->v1, efa->v2, sys->fweights[a][2]);
+		rigid_add_edge_to_R(sys, efa->v2, efa->v3, sys->fweights[a][0]);
+		rigid_add_edge_to_R(sys, efa->v3, efa->v1, sys->fweights[a][1]);
+
+		if(efa->v4) {
+			a++;
+			rigid_add_edge_to_R(sys, efa->v1, efa->v3, sys->fweights[a][2]);
+			rigid_add_edge_to_R(sys, efa->v3, efa->v4, sys->fweights[a][0]);
+			rigid_add_edge_to_R(sys, efa->v4, efa->v1, sys->fweights[a][1]);
+		}
+	}
+
+	for(a=0, eve=em->verts.first; eve; eve=eve->next, a++) {
+		rigid_orthogonalize_R(sys->rigid.R[a]);
+		eve->tmp.l= a;
+	}
+	
+	/* compute right hand sides for solving */
+	for(a=0, efa=em->faces.first; efa; efa=efa->next, a++) {
+		rigid_add_edge_to_rhs(sys, efa->v1, efa->v2, sys->fweights[a][2]);
+		rigid_add_edge_to_rhs(sys, efa->v2, efa->v3, sys->fweights[a][0]);
+		rigid_add_edge_to_rhs(sys, efa->v3, efa->v1, sys->fweights[a][1]);
+
+		if(efa->v4) {
+			a++;
+			rigid_add_edge_to_rhs(sys, efa->v1, efa->v3, sys->fweights[a][2]);
+			rigid_add_edge_to_rhs(sys, efa->v3, efa->v4, sys->fweights[a][0]);
+			rigid_add_edge_to_rhs(sys, efa->v4, efa->v1, sys->fweights[a][1]);
+		}
+	}
+
+	/* solve for positions, for X,Y and Z separately */
+	for(i=0; i<3; i++) {
+		laplacian_begin_solve(sys, i);
+
+		for(a=0; a<sys->totvert; a++)
+			if(!sys->vpinned[a]) {
+				/*if (i==0)
+					printf("rhs %f\n", sys->rigid.rhs[a][0]);*/
+				laplacian_add_right_hand_side(sys, a, sys->rigid.rhs[a][i]);
+			}
+
+		if(laplacian_system_solve(sys)) {
+			for(a=0, eve=em->verts.first; eve; eve=eve->next, a++)
+				eve->co[i]= laplacian_system_get_solution(a);
+		}
+		else {
+			if(!sys->rigid.thrownerror) {
+				error("RigidDeform: failed to find solution.");
+				sys->rigid.thrownerror= 1;
+			}
+			break;
+		}
+	}
+
+	/*printf("\n--------------------------------------------\n\n");*/
+}
+
+static void rigid_laplacian_create(LaplacianSystem *sys)
+{
+	EditMesh *em = sys->rigid.mesh;
+	EditVert *eve;
+	EditFace *efa;
+	int a;
+
+	/* add verts and faces to laplacian */
+	for(a=0, eve=em->verts.first; eve; eve=eve->next, a++) {
+		laplacian_add_vertex(sys, eve->co, eve->pinned);
+		eve->tmp.l= a;
+	}
+
+	for(efa=em->faces.first; efa; efa=efa->next) {
+		laplacian_add_triangle(sys,
+			efa->v1->tmp.l, efa->v2->tmp.l, efa->v3->tmp.l);
+		if(efa->v4)
+			laplacian_add_triangle(sys,
+				efa->v1->tmp.l, efa->v3->tmp.l, efa->v4->tmp.l);
+	}
+}
+
+void rigid_deform_begin(EditMesh *em)
+{
+	LaplacianSystem *sys;
+	EditVert *eve;
+	EditFace *efa;
+	int a, totvert, totface;
+
+	/* count vertices, triangles */
+	for(totvert=0, eve=em->verts.first; eve; eve=eve->next)
+		totvert++;
+
+	for(totface=0, efa=em->faces.first; efa; efa=efa->next) {
+		totface++;
+		if(efa->v4) totface++;
+	}
+
+	/* create laplacian */
+	sys = laplacian_system_construct_begin(totvert, totface);
+
+	sys->rigid.mesh= em;
+	sys->rigid.R = MEM_callocN(sizeof(float)*3*3*totvert, "RigidDeformR");
+	sys->rigid.rhs = MEM_callocN(sizeof(float)*3*totvert, "RigidDeformRHS");
+	sys->rigid.origco = MEM_callocN(sizeof(float)*3*totvert, "RigidDeformCo");
+
+	for(a=0, eve=em->verts.first; eve; eve=eve->next, a++)
+		VecCopyf(sys->rigid.origco[a], eve->co);
+
+	sys->areaweights= 0;
+	sys->storeweights= 1;
+
+	rigid_laplacian_create(sys);
+
+	laplacian_system_construct_end(sys);
+
+	RigidDeformSystem = sys;
+}
+
+void rigid_deform_end(int cancel)
+{
+	LaplacianSystem *sys = RigidDeformSystem;
+
+	if(sys) {
+		EditMesh *em = sys->rigid.mesh;
+		EditVert *eve;
+		int a;
+
+		if(cancel)
+			for(a=0, eve=em->verts.first; eve; eve=eve->next, a++)
+				if(!eve->pinned)
+					VecCopyf(eve->co, sys->rigid.origco[a]);
+
+		if(sys->rigid.R) MEM_freeN(sys->rigid.R);
+		if(sys->rigid.rhs) MEM_freeN(sys->rigid.rhs);
+		if(sys->rigid.origco) MEM_freeN(sys->rigid.origco);
+
+		/* free */
+		laplacian_system_delete(sys);
+	}
+
+	RigidDeformSystem = NULL;
+}
+#endif
+