// 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

};

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

struct SuperFactors_free {

  void operator()(tlin::SuperFactors *f) { tlin::freeF(f); }

};

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

using SuperFactorsPtr = std::unique_ptr<tlin::superfactors, superfactors_free="">;</tlin::superfactors,>

using DoublePtr       = std::unique_ptr<double[]>;</double[]>

using TPointDPtr      = std::unique_ptr<tpointd[]>;</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,</linearconstraint>

      m_constraints3;  //!< Compiled constraints (depends on the above)

  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></superfactorsptr>

      m_invF;  //!< Each of step 2's systems factorizations

  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(new double[cSize]);

    m_out.reset(new double[cSize]);

    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,

                 std::min(p0.rigidity, p1.rigidity), m_fx.get());

    add_f_values(v0, v1, fitTri[0].y, fitTri[1].y,

                 std::min(p0.rigidity, p1.rigidity), m_fy.get());

    add_f_values(v1, v2, fitTri[1].x, fitTri[2].x,

                 std::min(p1.rigidity, p2.rigidity), m_fx.get());

    add_f_values(v1, v2, fitTri[1].y, fitTri[2].y,

                 std::min(p1.rigidity, p2.rigidity), m_fy.get());

    add_f_values(v2, v0, fitTri[2].x, fitTri[0].x,

                 std::min(p2.rigidity, p0.rigidity), m_fx.get());

    add_f_values(v2, v0, fitTri[2].y, fitTri[0].y,

                 std::min(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, std::min(p0.rigidity, p1.rigidity), m_H);

    addHValues(v1, v2, std::min(p1.rigidity, p2.rigidity), m_H);

    addHValues(v2, v0, std::min(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(new double[kSize]);

    m_y.reset(new double[kSize]);

    m_fx.reset(new double[kSize]);

    m_fy.reset(new double[kSize]);

  } 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];

  }

}

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

//    Plastic Deformer  implementation

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

PlasticDeformer::PlasticDeformer() : m_imp(new Imp) {}

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

PlasticDeformer::~PlasticDeformer() {}

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

bool PlasticDeformer::compiled() const { return m_imp->m_compiled; }

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

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

  m_imp->initialize(mesh);

}

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

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

                              int *faceHints) {

  m_imp->compile(handles, faceHints);

  return compiled();

}

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

void PlasticDeformer::deform(const TPointD *dstHandles,

                             double *dstVerticesCoords) const {

  m_imp->deform(dstHandles, dstVerticesCoords);

}