// tcg includes

#include "tcg/tcg_misc.h"

// tlin includes

#include "tlin/tlin.h"

// STD includes

#include <assert.h></assert.h>

#include <memory></memory>

#include "ext/plasticdeformer.h"

#include <cstring></cstring>

/*! \file plasticdeformer.cpp

  This file contains the implementation of a Mesh Deformer as specified in the

  Igarashi-Moscovich-Hughes paper "As-Rigid-As-Possible Shape Manipulation",

  with some slight modifications.

  \n\n

  Notably, the choice of handle points is not limited to mesh vertices, but free

  along the mesh surface. This can be achieved by constraining the classical

  least-squares minimization problems on a linear hyperplane, in the form:

  
 \f$

  \left{ \begin{array}{c}

    v^t H v + v^t f + c   \rightarrow   \mbox{min} \\

    A v + d = 0

  \end{array} \right.

  \f$ 

  which can be solved using the Lagrange Multipliers theorem:

  
 \f$

  \Rightarrow \left{ \begin{array}{c}

    H v + f = A^t \lambda

    A v + d = 0

  \end{array} \right.

  \Rightarrow \left( \begin{array}{c} H \\ A \end{array} \begin{array}{c} -A^t \\ 0 \end{array} \right)

              \left( \begin{array}{c} v \\ \lambda \end{array} \right) = 

              \left( \begin{array}{c} -f \\ -d \end{array} \right)

  \f$ 

*/

//#define GL_DEBUG                                    // Debug using OpenGL. Must deform while drawing.

#ifdef GL_DEBUG  // NOTE: You should force deformations,

#include "tgl.h" // see plasticdeformersstorage.cpp

#endif

//******************************************************************************************

//    Local structures

//******************************************************************************************

namespace

{

//! A linear constraint of the kind   k[0] * v0 + k[1] * v1 + k[2] * v2 = b;     (b is not included though)

struct LinearConstraint {

	int m_h;	   //!< Original handle index corresponding to this constraint

	int m_v[3];	//!< The mesh vertex indices v0, v1, v2

	double m_k[3]; //!< Constraint coefficients

};

//-------------------------------------------------------------------------------------------

// Forced to implement a sloppy substitute to C++11's std::unique_ptr -- since we're still compiling

// C++03 on MAC... ARGH !

template <typename d="" t,="" typename=""></typename>

class UniquePtr

{

	T *m_t;

public:

	UniquePtr() : m_t() {}

	UniquePtr(T *t) : m_t(t) {}

	~UniquePtr() { D::destroy(m_t); }

	void reset(T *t = 0)

	{

		D::destroy(m_t);

		m_t = t;

	}

	const T *get() const { return m_t; }

	T *get() { return m_t; }

	const T *operator->() const { return m_t; }

	T *operator->() { return m_t; }

	const T &operator*() const { return *m_t; }

	T &operator*() { return *m_t; }

	const T &operator[](size_t i) const { return m_t[i]; }

	T &operator[](size_t i) { return m_t[i]; }

};

//-------------------------------------------------------------------------------------------

struct SuperFactors_free {

	static inline void destroy(tlin::SuperFactors *f) { tlin::freeF(f); }

};

struct freer {

	static inline void destroy(void *d) { free(d); }

};

struct deleter {

	static inline void destroy(void *d) { delete d; }

};

//-------------------------------------------------------------------------------------------

typedef UniquePtr<tlin::superfactors, superfactors_free=""> SuperFactorsPtr;</tlin::superfactors,>

typedef UniquePtr<double, freer=""> DoublePtr;</double,>

typedef UniquePtr<tpointd, deleter=""> TPointDPtr;</tpointd,>

} // namespace

//******************************************************************************************

//    Local functions

//******************************************************************************************

namespace

{

inline void vertices(const TTextureMesh &mesh, int faceIdx,

					 const TPointD *&p0, const TPointD *&p1, const TPointD *&p2)

{

	int v0, v1, v2;

	mesh.faceVertices(faceIdx, v0, v1, v2);

	p0 = &mesh.vertex(v0).P(), p1 = &mesh.vertex(v1).P(), p2 = &mesh.vertex(v2).P();

}

//-------------------------------------------------------------------------------------------

inline void buildTriangularCoordinates(const TTextureMesh &mesh, int &faceIdx, const TPointD &p,

									   double &c0, double &c1, double &c2)

{

	TPointD coords;

	const TPointD *p0, *p1, *p2;

	// Follow the hint first

	if (faceIdx >= 0 && faceIdx < mesh.facesCount() &&

		mesh.faceContains(faceIdx, p)) {

		vertices(mesh, faceIdx, p0, p1, p2);

		coords = tcg::point_ops::triCoords(p, *p0, *p1, *p2);

		c1 = coords.x, c2 = coords.y, c0 = 1.0 - c1 - c2;

		return;

	}

	// Find out the face searching on the mesh

	faceIdx = mesh.faceContaining(p);

	if (faceIdx >= 0) {

		vertices(mesh, faceIdx, p0, p1, p2);

		coords = tcg::point_ops::triCoords(p, *p0, *p1, *p2);

		c1 = coords.x, c2 = coords.y, c0 = 1.0 - c1 - c2;

		return;

	}

}

//-------------------------------------------------------------------------------------------

inline void addConstraint1d(int j, const LinearConstraint &cnstr, tlin::spmat &mat)

{

	int i, var, nVars = 3;

	for (i = 0; i < nVars; ++i) {

		var = cnstr.m_v[i];

		mat.at(j, var) += cnstr.m_k[i];

		mat.at(var, j) = mat(j, var);

	}

}

//-------------------------------------------------------------------------------------------

inline void addConstraint2d(int j, const LinearConstraint &cnstr, tlin::spmat &mat)

{

	int i, varX, varY, k = j + 1, nVars = 3;

	for (i = 0; i < nVars; ++i) {

		varX = 2 * cnstr.m_v[i], varY = varX + 1;

		mat.at(j, varX) += cnstr.m_k[i];

		mat.at(k, varY) += cnstr.m_k[i];

		mat.at(varX, j) = mat(j, varX);

		mat.at(varY, k) = mat(k, varY);

	}

}

//-------------------------------------------------------------------------------------------

inline void addGValues(

	int v0x, int v0y, int v1x, int v1y, int v2x, int v2y,

	double px, double py, double w, tlin::spmat &G)

{

	double sqPy = py * py, one_px = 1.0 - px;

	double a = w * (one_px * one_px + sqPy),

		   b = w * (px * one_px - sqPy),

		   c = w * (py * one_px + px * py),

		   d = w * (px * px + sqPy);

	sqPy *= w;

	one_px *= w;

	px *= w;

	py *= w;

	G.at(v0x, v0x) += a;

	G.at(v0x, v1x) += b;

	G.at(v0x, v1y) += c;

	G.at(v0x, v2x) -= one_px;

	G.at(v0x, v2y) -= py;

	G.at(v0y, v0y) += a;

	G.at(v0y, v1x) -= py;

	G.at(v0y, v1y) += b;

	G.at(v0y, v2x) += py;

	G.at(v0y, v2y) -= one_px;

	G.at(v1x, v0x) += b;

	G.at(v1x, v0y) += -py;

	G.at(v1x, v1x) += d;

	G.at(v1x, v2x) += -px;

	G.at(v1x, v2y) += py;

	G.at(v1y, v0x) += c;

	G.at(v1y, v0y) += b;

	G.at(v1y, v1y) += d;

	G.at(v1y, v2x) -= py;

	G.at(v1y, v2y) -= px;

	G.at(v2x, v0x) -= one_px;

	G.at(v2x, v0y) += py;

	G.at(v2x, v1x) -= px;

	G.at(v2x, v1y) -= py;

	G.at(v2x, v2x) += w;

	G.at(v2y, v0x) -= py;

	G.at(v2y, v0y) -= one_px;

	G.at(v2y, v1x) += py;

	G.at(v2y, v1y) -= px;

	G.at(v2y, v2y) += w;

}

//-------------------------------------------------------------------------------------------

void buildF(double px, double py, tlin::spmat &F)

{

	double one_px = 1.0 - px, sqPy = py * py;

	F.at(0, 0) = 1.0 + one_px * one_px + sqPy;

	F.at(1, 1) = F(0, 0);

	F.at(0, 2) = px * one_px - sqPy;

	F.at(0, 3) = py * one_px + px * py;

	F.at(1, 2) = -F(0, 3);

	F.at(1, 3) = F(0, 2);

	F.at(2, 0) = F(0, 2);

	F.at(2, 1) = F(1, 2);

	F.at(3, 0) = F(0, 3);

	F.at(3, 1) = F(1, 3);

	F.at(2, 2) = 1.0 + px * px + sqPy;

	F.at(3, 3) = F(2, 2);

}

//-------------------------------------------------------------------------------------------

void build_c(double v0x, double v0y, double v1x, double v1y, double v2x, double v2y, double px, double py, double *c)

{

	c[0] = (v0x + v2x * (1.0 - px) + v2y * py);

	c[1] = (v0y - v2x * py + v2y * (1.0 - px));

	c[2] = (v1x + v2x * px - v2y * py);

	c[3] = (v1y + v2y * px + v2x * py);

}

//-------------------------------------------------------------------------------------------

inline void addHValues(int v0, int v1, double w, tlin::spmat &H)

{

	H.at(v0, v0) += w;

	H.at(v1, v0) -= w;

	H.at(v0, v1) -= w;

	H.at(v1, v1) += w;

}

//-------------------------------------------------------------------------------------------

inline void add_f_values(int v0, int v1, double fit0, double fit1, double w, double *f)

{

	double f0_f1 = w * (fit0 - fit1);

	f[v0] += f0_f1;

	f[v1] -= f0_f1;

}

} // namespace

//******************************************************************************************

//    PlasticDeformer::Imp  definition

//******************************************************************************************

class PlasticDeformer::Imp

{

public:

	TTextureMeshP m_mesh;										  //!< Deformed mesh (cannot be changed)

	std::vector<plastichandle> m_handles;						  //!< Compiled handles</plastichandle>

	std::vector<linearconstraint> m_constraints1, m_constraints3; //!< Compiled constraints (depends on the above)</linearconstraint>

	bool m_compiled;											  //!< Whether the deformer is ready to deform()

public:

	Imp();

	void initialize(const TTextureMeshP &mesh);

	void compile(const std::vector<plastichandle> &handles, int *faceHints);</plastichandle>

	void deform(const TPointD *dstHandles, double *dstVerticesCoords);

	void copyOriginals(double *dstVerticesCoords);

public:

	tlin::spmat m_G;		//!< Pre-initialized entries for the 1st

							//!< linear system

	SuperFactorsPtr m_invC; //!< C's factors (C is G plus linear constraints)

	DoublePtr m_q;			//!< Step 1's known term

	DoublePtr m_out;		//!< Step 1's result

	// Step 1 members:

	//   The first step of a MeshDeformer instance is about building the desired vertices configuration.

	void initializeStep1();

	void compileStep1(const std::vector<plastichandle> &handles);</plastichandle>

	void deformStep1(const TPointD *dstHandles, double *dstVerticesCoords);

	void releaseInitializedData();

public:

	std::vector<superfactorsptr> m_invF; //!< Each of step 2's systems factorizations</superfactorsptr>

	TPointDPtr m_relativeCoords; //!< Faces' p2 coordinates in (p0, p1)'s orthogonal reference

	double m_v[4], m_c[4];		 //!< Known term and output coordinates

	TPointDPtr m_fitTriangles; //!< Output face coordinates

	// Step 2 members:

	//   The second step of MeshDeformer rigidly maps neighbourhoods of the original mesh

	//   to fit as much as possible the neighbourhoods in the step 1 result.

	void initializeStep2();

	void compileStep2(const std::vector<plastichandle> &handles);</plastichandle>

	void deformStep2(const TPointD *dstHandles, double *dstVerticesCoords);

public:

	// NOTE: This step accepts separation in the X and Y components

	tlin::spmat m_H;		//!< Step 3's system entries

	SuperFactorsPtr m_invK; //!< System inverse

	DoublePtr m_fx, m_fy; //!< Known terms

	DoublePtr m_x, m_y;   //!< Output values

	// Step 3 members:

	//   The third step of MeshDeformer glues together the mapped neighbourhoods from step2.

	void initializeStep3();

	void compileStep3(const std::vector<plastichandle> &handles);</plastichandle>

	void deformStep3(const TPointD *dstHandles, double *dstVerticesCoords);

};

//=================================================================================

PlasticDeformer::Imp::Imp()

{

}

//-------------------------------------------------------------------------------------------

void PlasticDeformer::Imp::initialize(const TTextureMeshP &mesh)

{

	assert("Input mesh must be squeezed!" &&

		   mesh->verticesCount() == mesh->vertices().nodesCount() &&

		   mesh->edgesCount() == mesh->edges().nodesCount() &&

		   mesh->facesCount() == mesh->faces().nodesCount());

	m_mesh = mesh;

	initializeStep1();

	initializeStep2();

	initializeStep3();

	m_compiled = false; // Compilation is expected after a new initialization

}

//-------------------------------------------------------------------------------------------

void PlasticDeformer::Imp::compile(const std::vector<plastichandle> &handles,</plastichandle>

								   int *faceHints)

{

	assert(m_mesh);

	m_handles.clear(), m_handles.reserve(handles.size());

	m_constraints1.clear(), m_constraints3.clear();

	LinearConstraint constr;

	// Build the linear constraints raising from the mesh-handles pairing

	int h, hCount = handles.size();

	for (h = 0; h < hCount; ++h) {

		int localFaceIdx = -1, &faceIdx = faceHints ? faceHints[h] : localFaceIdx;

		::buildTriangularCoordinates(*m_mesh, faceIdx, handles[h].m_pos, constr.m_k[0], constr.m_k[1], constr.m_k[2]);

		if (faceIdx >= 0) {

			constr.m_h = h;

			m_mesh->faceVertices(faceIdx, constr.m_v[0], constr.m_v[1], constr.m_v[2]);

			m_constraints1.push_back(constr);

			if (handles[h].m_interpolate)

				m_constraints3.push_back(constr);

			// Store the handles to retain precompiled data

			m_handles.push_back(handles[h]);

		}

	}

	//m_handles = handles;                              // NOT directly copied like this: some handles could be

	// (geometrically) OUTSIDE the mesh!

	// Now, invoke the actual compilation procedures

	m_compiled = true;

	if (m_handles.size() < 2)

		return;

	compileStep1(handles); // These may set m_compiled = false. Must still be

	compileStep2(handles); // called even when that happens - as they are

	compileStep3(handles); // responsible for resources reclamation.

}

//-------------------------------------------------------------------------------------------

void PlasticDeformer::Imp::deform(const TPointD *dstHandles, double *dstVerticesCoords)

{

	assert(m_mesh);

	assert(dstVerticesCoords);

	// Deal with trivial cases

	if (!m_compiled || m_handles.size() == 0) {

		// Cannot deform anything - just copy the source mesh vertices into dst ones

		copyOriginals(dstVerticesCoords);

		return;

	}

	assert(dstHandles);

	if (m_handles.size() == 1) {

		// 1 handle inside the mesh - pure translational case

		const PlasticHandle &srcHandle = m_handles.front();

		const TPointD &dstHandlePos = dstHandles[m_constraints1.front().m_h];

		TPointD shift(dstHandlePos - srcHandle.m_pos);

		int v, vCount = m_mesh->verticesCount();

		for (v = 0; v != vCount; ++v, dstVerticesCoords += 2) {

			dstVerticesCoords[0] = m_mesh->vertex(v).P().x + shift.x;

			dstVerticesCoords[1] = m_mesh->vertex(v).P().y + shift.y;

		}

		return;

	}

	deformStep1(dstHandles, dstVerticesCoords);

	deformStep2(dstHandles, dstVerticesCoords);

	deformStep3(dstHandles, dstVerticesCoords);

}

//-------------------------------------------------------------------------------------------

void PlasticDeformer::Imp::copyOriginals(double *dstVerticesCoords)

{

	int v, vCount = m_mesh->verticesCount();

	for (v = 0; v != vCount; ++v, dstVerticesCoords += 2) {

		dstVerticesCoords[0] = m_mesh->vertex(v).P().x;

		dstVerticesCoords[1] = m_mesh->vertex(v).P().y;

	}

}

//-------------------------------------------------------------------------------------------

void PlasticDeformer::Imp::releaseInitializedData()

{

	// Release m_G and m_H

	{

		tlin::spmat temp;

		swap(m_G, temp);

	} // Ensure resources release by swapping them

	{

		tlin::spmat temp;

		swap(m_H, temp);

	} // with newly initialized instances

}

//******************************************************************************************

//    Plastic Deformation Step 1

//******************************************************************************************

void PlasticDeformer::Imp::initializeStep1()

{

	const TTextureMesh &mesh = *m_mesh;

	int vCount = mesh.verticesCount(), vCount_2 = 2 * vCount;

	m_G = tlin::spmat(vCount_2, vCount_2);

	// Initialize the linear system indices for the stored mesh

	int f, fCount = mesh.facesCount();

	for (f = 0; f < fCount; ++f) {

		int v0, v1, v2;

		mesh.faceVertices(f, v0, v1, v2);

		int v0x = 2 * v0;  // We'll use 2 params per vertex - with

		int v0y = v0x + 1; // adjacent x and y parameters

		int v1x = 2 * v1;

		int v1y = v1x + 1;

		int v2x = 2 * v2;

		int v2y = v2x + 1;

		const TTextureMesh::vertex_type &vx0 = mesh.vertex(v0);

		const TTextureMesh::vertex_type &vx1 = mesh.vertex(v1);

		const TTextureMesh::vertex_type &vx2 = mesh.vertex(v2);

		TPointD c0, c1, c2;

		c2 = tcg::point_ops::ortCoords(vx2.P(), vx0.P(), vx1.P());

		c0 = tcg::point_ops::ortCoords(vx0.P(), vx1.P(), vx2.P());

		c1 = tcg::point_ops::ortCoords(vx1.P(), vx2.P(), vx0.P());

		addGValues(v0x, v0y, v1x, v1y, v2x, v2y, c2.x, c2.y, vx2.P().rigidity, m_G);

		addGValues(v1x, v1y, v2x, v2y, v0x, v0y, c0.x, c0.y, vx0.P().rigidity, m_G);

		addGValues(v2x, v2y, v0x, v0y, v1x, v1y, c1.x, c1.y, vx1.P().rigidity, m_G);

	}

}

//-------------------------------------------------------------------------------------------

void PlasticDeformer::Imp::compileStep1(const std::vector<plastichandle> &handles)</plastichandle>

{

	// First, release resources

	m_invC.reset();

	m_q.reset();

	m_out.reset();

	// Now, start compiling

	const TTextureMesh &mesh = *m_mesh;

	int vCount = mesh.verticesCount(), hCount = m_handles.size();

	int cSize = 2 * (vCount + hCount); // Coefficients count

	// Initialize C with G

	tlin::SuperMatrix *trC = 0;

	{

		tlin::spmat C(cSize, cSize);

		C.entries() = m_G.entries();

		C.entries().hashFunctor().m_cols = C.cols();

		C.entries().rehash(C.entries().buckets().size()); // Rehash to entries according to C's size

		// Add the entries constraining handles

		int i;

		std::vector<linearconstraint>::iterator ht, hEnd(m_constraints1.end());</linearconstraint>

		for (i = 0, ht = m_constraints1.begin(); ht != hEnd; ++i, ++ht)

			addConstraint2d(2 * (vCount + i), *ht, C);

		tlin::traduceS(C, trC);

	}

	// Build m_invC

	tlin::SuperFactors *invC = 0;

	tlin::factorize(trC, invC);

	tlin::freeS(trC);

	if (invC) {

		m_invC.reset(invC);

		// Reallocate arrays

		m_q.reset((double *)malloc(cSize * sizeof(double)));

		m_out.reset((double *)malloc(cSize * sizeof(double)));

		memset(m_q.get(), 0, 2 * vCount * sizeof(double)); // Initialize the system's known term with 0

	} else

		m_compiled = false;

}

//-------------------------------------------------------------------------------------------

void PlasticDeformer::Imp::deformStep1(const TPointD *dstHandles, double *dstVerticesCoords)

{

	int vCount2 = 2 * m_mesh->verticesCount();

	int cSize = vCount2 + 2 * m_handles.size();

	// Copy destination handles into the system's known term

	int i, h;

	for (i = vCount2, h = 0; i < cSize; i += 2, ++h) {

		const TPointD &dstHandlePos = dstHandles[m_constraints1[h].m_h];

		m_q[i] = dstHandlePos.x;	 // NOTE: We could use calloc on-the-fly, sparing

		m_q[i + 1] = dstHandlePos.y; // some memory. Have to benchmark the cost, though...

	}

	// Solve the linear system

	double *out = m_out.get();

	tlin::solve(m_invC.get(), m_q.get(), out);

#ifdef GL_DEBUG

	glColor3d(1.0, 0.0, 0.0); // Red

	glBegin(GL_LINES);

	// Draw deformed mesh edges

	int e, eCount = m_mesh->edgesCount();

	for (e = 0; e < eCount; ++e) {

		const TTextureMesh::edge_type &ed = m_mesh->edge(e);

		double *x0 = out + 2 * ed.vertex(0), *x1 = out + 2 * ed.vertex(1);

		glVertex2d(*x0, *(x0 + 1));

		glVertex2d(*x1, *(x1 + 1));

	}

	glEnd();

#endif

}

//******************************************************************************************

//    Plastic Deformation Step 2

//******************************************************************************************

void PlasticDeformer::Imp::initializeStep2()

{

	const TTextureMesh &mesh = *m_mesh;

	int f, fCount = mesh.facesCount();

	// Clear and re-initialize vars

	tlin::spmat F(4, 4);

	tlin::SuperMatrix *trF;

	std::vector<superfactorsptr>(fCount).swap(m_invF);</superfactorsptr>

	m_relativeCoords.reset(new TPointD[fCount]);

	m_fitTriangles.reset(new TPointD[3 * fCount]);

	// Build step 2's system factorizations (yep, can be done at this point)

	const TPointD *p0, *p1, *p2;

	for (f = 0; f < fCount; ++f) {

		::vertices(mesh, f, p0, p1, p2);

		TPointD c(tcg::point_ops::ortCoords(*p2, *p0, *p1));

		m_relativeCoords[f] = c;

		F.clear();

		buildF(c.x, c.y, F);

		trF = 0;

		tlin::traduceS(F, trF);

		tlin::SuperFactors *invF = 0;

		tlin::factorize(trF, invF);

		m_invF[f].reset(invF);

		tlin::freeS(trF);

	}

}

//-------------------------------------------------------------------------------------------

void PlasticDeformer::Imp::compileStep2(const std::vector<plastichandle> &handles)</plastichandle>

{

	// Nothing to do :)

}

//-------------------------------------------------------------------------------------------

void PlasticDeformer::Imp::deformStep2(const TPointD *dstHandles, double *dstVerticesCoords)

{

	const TTextureMesh &mesh = *m_mesh;

	int vCount = mesh.verticesCount();

	memset(m_fx.get(), 0, vCount * sizeof(double)); // These should be part of step 3...

	memset(m_fy.get(), 0, vCount * sizeof(double)); // They are filled here just for convenience

	// Build fit triangles

	TPointD *fitTri = m_fitTriangles.get(), *relCoord = m_relativeCoords.get();

	double *out1 = m_out.get();

	int f, fCount = mesh.facesCount();

	for (f = 0; f < fCount; ++f, fitTri += 3, ++relCoord) {

		int v0, v1, v2;

		m_mesh->faceVertices(f, v0, v1, v2);

		const RigidPoint &p0 = mesh.vertex(v0).P(), &p1 = mesh.vertex(v1).P(), &p2 = mesh.vertex(v2).P();

		double *v0x = out1 + (v0 << 1), *v0y = v0x + 1,

			   *v1x = out1 + (v1 << 1), *v1y = v1x + 1,

			   *v2x = out1 + (v2 << 1), *v2y = v2x + 1;

		build_c(

			*v0x, *v0y,

			*v1x, *v1y,

			*v2x, *v2y,

			relCoord->x, relCoord->y, m_c);

		double *vPtr = (double *)m_v;

		tlin::solve(m_invF[f].get(), (double *)m_c, vPtr);

		fitTri[0].x = m_v[0], fitTri[0].y = m_v[1];

		fitTri[1].x = m_v[2], fitTri[1].y = m_v[3];

		fitTri[2].x = fitTri[0].x + relCoord->x * (fitTri[1].x - fitTri[0].x) + relCoord->y * (fitTri[1].y - fitTri[0].y);

		fitTri[2].y = fitTri[0].y + relCoord->x * (fitTri[1].y - fitTri[0].y) + relCoord->y * (fitTri[0].x - fitTri[1].x);

		// Scale with respect to baricenter. The baricenter is used since it makes distance from vertices equally

		// weighting - which is the same expected when minimizing positions (by collateral effect) in the next step.

		TPointD baricenter(

			(fitTri[0].x + fitTri[1].x + fitTri[2].x) / 3.0,

			(fitTri[0].y + fitTri[1].y + fitTri[2].y) / 3.0);

		double scale = sqrt(

			norm2(TPointD(p1.x - p0.x, p1.y - p0.y)) /

			norm2(TPointD(fitTri[1].x - fitTri[0].x, fitTri[1].y - fitTri[0].y)));

		fitTri[0] = scale * (fitTri[0] - baricenter) + baricenter;

		fitTri[1] = scale * (fitTri[1] - baricenter) + baricenter;

		fitTri[2] = scale * (fitTri[2] - baricenter) + baricenter;

		// Build f -- note: this should be part of step 3, we're just avoiding the same cycle twice :)

		add_f_values(v0, v1, fitTri[0].x, fitTri[1].x, tmin(p0.rigidity, p1.rigidity), m_fx.get());

		add_f_values(v0, v1, fitTri[0].y, fitTri[1].y, tmin(p0.rigidity, p1.rigidity), m_fy.get());

		add_f_values(v1, v2, fitTri[1].x, fitTri[2].x, tmin(p1.rigidity, p2.rigidity), m_fx.get());

		add_f_values(v1, v2, fitTri[1].y, fitTri[2].y, tmin(p1.rigidity, p2.rigidity), m_fy.get());

		add_f_values(v2, v0, fitTri[2].x, fitTri[0].x, tmin(p2.rigidity, p0.rigidity), m_fx.get());

		add_f_values(v2, v0, fitTri[2].y, fitTri[0].y, tmin(p2.rigidity, p0.rigidity), m_fy.get());

	}

#ifdef GL_DEBUG

	glColor3d(0.0, 0.0, 1.0); // Blue

	// Draw fit triangles

	fitTri = m_fitTriangles.get();

	for (f = 0; f < fCount; ++f, fitTri += 3) {

		glBegin(GL_LINE_LOOP);

		glVertex2d(fitTri[0].x, fitTri[0].y);

		glVertex2d(fitTri[1].x, fitTri[1].y);

		glVertex2d(fitTri[2].x, fitTri[2].y);

		glEnd();

	}

#endif

}

//******************************************************************************************

//    Plastic Deformation Step 3

//******************************************************************************************

void PlasticDeformer::Imp::initializeStep3()

{

	const TTextureMesh &mesh = *m_mesh;

	int vCount = mesh.verticesCount();

	m_H = tlin::spmat(vCount, vCount);

	int f, fCount = mesh.facesCount();

	for (f = 0; f < fCount; ++f) {

		int v0, v1, v2;

		mesh.faceVertices(f, v0, v1, v2);

		const RigidPoint &p0 = mesh.vertex(v0).P(),

						 &p1 = mesh.vertex(v1).P(),

						 &p2 = mesh.vertex(v2).P();

		addHValues(v0, v1, tmin(p0.rigidity, p1.rigidity), m_H);

		addHValues(v1, v2, tmin(p1.rigidity, p2.rigidity), m_H);

		addHValues(v2, v0, tmin(p2.rigidity, p0.rigidity), m_H);

	}

}

//-------------------------------------------------------------------------------------------

void PlasticDeformer::Imp::compileStep3(const std::vector<plastichandle> &handles)</plastichandle>

{

	// First, release resources

	m_invK.reset();

	m_x.reset();

	m_y.reset();

	m_fx.reset();

	m_fy.reset();

	// If compilation already failed, skip

	if (!m_compiled)

		return;

	const TTextureMesh &mesh = *m_mesh;

	int vCount = mesh.verticesCount();

	int kSize = vCount + m_constraints3.size();

	// Initialize K with H

	tlin::SuperMatrix *trK = 0;

	{

		tlin::spmat K(kSize, kSize);

		K.entries() = m_H.entries();

		K.entries().hashFunctor().m_cols = K.cols();

		K.entries().rehash(K.entries().buckets().size()); //Rehash to entries according to K's size

		// Add the entries constraining handles

		int c, cnstrCount = m_constraints3.size();

		for (c = 0; c < cnstrCount; ++c) {

			// Add the linear constraint to the system

			addConstraint1d(vCount + c, m_constraints3[c], K);

		}

		tlin::traduceS(K, trK);

	}

	// Build m_invK

	tlin::SuperFactors *invK = 0;

	tlin::factorize(trK, invK);

	tlin::freeS(trK);

	if (invK) {

		m_invK.reset(invK);

		m_x.reset((double *)malloc(kSize * sizeof(double)));

		m_y.reset((double *)malloc(kSize * sizeof(double)));

		m_fx.reset((double *)malloc(kSize * sizeof(double)));

		m_fy.reset((double *)malloc(kSize * sizeof(double)));

	} else

		m_compiled = false;

}

//-------------------------------------------------------------------------------------------

void PlasticDeformer::Imp::deformStep3(const TPointD *dstHandles, double *dstVerticesCoords)

{

	int v, vCount = m_mesh->verticesCount();

	int c;

	int h, hCount = m_handles.size();

	for (c = 0, h = 0; h < hCount; ++h) {

		if (!m_handles[h].m_interpolate)

			continue;

		const TPointD &dstHandlePos = dstHandles[m_constraints1[h].m_h];

		m_fx[vCount + c] = dstHandlePos.x;

		m_fy[vCount + c] = dstHandlePos.y;

		++c;

	}

	double *x = m_x.get(), *y = m_y.get();

	tlin::solve(m_invK.get(), m_fx.get(), x);

	tlin::solve(m_invK.get(), m_fy.get(), y);

	int i;

	for (i = v = 0; v < vCount; ++v, i += 2) {

		dstVerticesCoords[i] = m_x[v];

		dstVerticesCoords[i + 1] = m_y[v];

	}

}