Beispiel #1
0
IGL_INLINE void igl::face_areas(
  const Eigen::MatrixBase<DerivedL>& L,
  const typename DerivedL::Scalar doublearea_nan_replacement,
  Eigen::PlainObjectBase<DerivedA>& A)
{
  using namespace Eigen;
  assert(L.cols() == 6);
  const int m = L.rows();
  // (unsigned) face Areas (opposite vertices: 1 2 3 4)
  Matrix<typename DerivedA::Scalar,Dynamic,1> 
    A0(m,1), A1(m,1), A2(m,1), A3(m,1);
  Matrix<typename DerivedA::Scalar,Dynamic,3> 
    L0(m,3), L1(m,3), L2(m,3), L3(m,3);
  L0<<L.col(1),L.col(2),L.col(3);
  L1<<L.col(0),L.col(2),L.col(4);
  L2<<L.col(0),L.col(1),L.col(5);
  L3<<L.col(3),L.col(4),L.col(5);
  doublearea(L0,doublearea_nan_replacement,A0);
  doublearea(L1,doublearea_nan_replacement,A1);
  doublearea(L2,doublearea_nan_replacement,A2);
  doublearea(L3,doublearea_nan_replacement,A3);
  A.resize(m,4);
  A.col(0) = 0.5*A0;
  A.col(1) = 0.5*A1;
  A.col(2) = 0.5*A2;
  A.col(3) = 0.5*A3;
}
IGL_INLINE void igl::directed_edge_parents(
  const Eigen::PlainObjectBase<DerivedE> & E,
  Eigen::PlainObjectBase<DerivedP> & P)
{
  using namespace Eigen;
  using namespace std;
  VectorXi I = VectorXi::Constant(E.maxCoeff()+1,1,-1);
  //I(E.col(1)) = 0:E.rows()-1
  slice_into(colon<int>(0,E.rows()-1),E.col(1).eval(),I);
  VectorXi roots,_;
  setdiff(E.col(0).eval(),E.col(1).eval(),roots,_);
  for_each(roots.data(),roots.data()+roots.size(),[&](int r){I(r)=-1;});
  slice(I,E.col(0).eval(),P);
}
IGL_INLINE void igl::internal_angles(
  const Eigen::PlainObjectBase<DerivedL>& L,
  Eigen::PlainObjectBase<DerivedK> & K)
{
  assert(L.cols() == 3 && "Edge-lengths should come from triangles");
  K.resize(L.rows(),L.cols());
  for(int d = 0;d<3;d++)
  {
    const auto & s1 = L.col(d).array();
    const auto & s2 = L.col((d+1)%3).array();
    const auto & s3 = L.col((d+2)%3).array();
    K.col(d) = ((s3.square() + s2.square() - s1.square())/(2.*s3*s2)).acos();
  }
}
Beispiel #4
0
IGL_INLINE void igl::doublearea( 
  const Eigen::PlainObjectBase<DerivedV> & V, 
  const Eigen::PlainObjectBase<DerivedF> & F, 
  Eigen::PlainObjectBase<DeriveddblA> & dblA)
{
  const int dim = V.cols();
  // Only support triangles
  assert(F.cols() == 3);
  const int m = F.rows();
  // Compute edge lengths
  Eigen::PlainObjectBase<DerivedV> l;
  // "Lecture Notes on Geometric Robustness" Shewchuck 09, Section 3.1
  // http://www.cs.berkeley.edu/~jrs/meshpapers/robnotes.pdf
  switch(dim)
  {
    case 3:
    {
      dblA = Eigen::PlainObjectBase<DeriveddblA>::Zero(m,1);
      for(int d = 0;d<3;d++)
      {
        Eigen::Matrix<typename DerivedV::Scalar, DerivedV::RowsAtCompileTime, 2> Vd(V.rows(),2);
        Vd << V.col(d), V.col((d+1)%3);
        Eigen::PlainObjectBase<DeriveddblA> dblAd;
        doublearea(Vd,F,dblAd);
        dblA.array() += dblAd.array().square();
      }
      dblA = dblA.array().sqrt().eval();
      break;
    }
    case 2:
    {
      dblA.resize(m,1);
      for(int f = 0;f<m;f++)
      {
        auto r = V.row(F(f,0))-V.row(F(f,2));
        auto s = V.row(F(f,1))-V.row(F(f,2));
        dblA(f) = r(0)*s(1) - r(1)*s(0);
      }
      break;
    }
    default:
    {
      edge_lengths(V,F,l);
      return doublearea(l,dblA);
    }
  }
}
IGL_INLINE void igl::barycentric_coordinates(
  const Eigen::MatrixBase<DerivedP> & P,
  const Eigen::MatrixBase<DerivedA> & A,
  const Eigen::MatrixBase<DerivedB> & B,
  const Eigen::MatrixBase<DerivedC> & C,
  Eigen::PlainObjectBase<DerivedL> & L)
{
  using namespace Eigen;
#ifndef NDEBUG
  const int DIM = P.cols();
  assert(A.cols() == DIM && "corners must be in same dimension as query");
  assert(B.cols() == DIM && "corners must be in same dimension as query");
  assert(C.cols() == DIM && "corners must be in same dimension as query");
  assert(P.rows() == A.rows() && "Must have same number of queries as corners");
  assert(A.rows() == B.rows() && "Corners must be same size");
  assert(A.rows() == C.rows() && "Corners must be same size");
#endif

  // http://gamedev.stackexchange.com/a/23745
  typedef 
    Eigen::Array<
      typename DerivedP::Scalar,
               DerivedP::RowsAtCompileTime,
               DerivedP::ColsAtCompileTime>
    ArrayS;
  typedef 
    Eigen::Array<
      typename DerivedP::Scalar,
               DerivedP::RowsAtCompileTime,
               1>
    VectorS;

  const ArrayS v0 = B.array() - A.array();
  const ArrayS v1 = C.array() - A.array();
  const ArrayS v2 = P.array() - A.array();
  VectorS d00 = (v0*v0).rowwise().sum();
  VectorS d01 = (v0*v1).rowwise().sum();
  VectorS d11 = (v1*v1).rowwise().sum();
  VectorS d20 = (v2*v0).rowwise().sum();
  VectorS d21 = (v2*v1).rowwise().sum();
  VectorS denom = d00 * d11 - d01 * d01;
  L.resize(P.rows(),3);
  L.col(1) = (d11 * d20 - d01 * d21) / denom;
  L.col(2) = (d00 * d21 - d01 * d20) / denom;
  L.col(0) = 1.0f -(L.col(1) + L.col(2)).array();
}
Beispiel #6
0
IGL_INLINE bool igl::harmonic(
  const Eigen::PlainObjectBase<DerivedV> & V,
  const Eigen::PlainObjectBase<DerivedF> & F,
  const Eigen::PlainObjectBase<Derivedb> & b,
  const Eigen::PlainObjectBase<Derivedbc> & bc,
  const int k,
  Eigen::PlainObjectBase<DerivedW> & W)
{
  using namespace Eigen;
  typedef typename DerivedV::Scalar Scalar;
  typedef Matrix<Scalar,Dynamic,1> VectorXS;
  SparseMatrix<Scalar> L,M,Mi;
  cotmatrix(V,F,L);
  switch(F.cols())
  {
    case 3:
      massmatrix(V,F,MASSMATRIX_TYPE_VORONOI,M);
      break;
    case 4:
    default:
      massmatrix(V,F,MASSMATRIX_TYPE_BARYCENTRIC,M);
      break;
  }
  invert_diag(M,Mi);
  SparseMatrix<Scalar> Q = -L;
  for(int p = 1;p<k;p++)
  {
    Q = (Q*Mi*-L).eval();
  }
  const VectorXS B = VectorXS::Zero(V.rows(),1);
  min_quad_with_fixed_data<Scalar> data;
  min_quad_with_fixed_precompute(Q,b,SparseMatrix<Scalar>(),true,data);
  W.resize(V.rows(),bc.cols());
  for(int w = 0;w<bc.cols();w++)
  {
    const VectorXS bcw = bc.col(w);
    VectorXS Ww;
    if(!min_quad_with_fixed_solve(data,B,bcw,VectorXS(),Ww))
    {
      return false;
    }
    W.col(w) = Ww;
  }
  return true;
}
Beispiel #7
0
void igl::copyleft::offset_surface(
  const Eigen::MatrixBase<DerivedV> & V,
  const Eigen::MatrixBase<DerivedF> & F,
  const isolevelType isolevel,
  const typename Derivedside::Scalar s,
  const SignedDistanceType & signed_distance_type,
  Eigen::PlainObjectBase<DerivedSV> & SV,
  Eigen::PlainObjectBase<DerivedSF> & SF,
  Eigen::PlainObjectBase<DerivedGV> &   GV,
  Eigen::PlainObjectBase<Derivedside> & side,
  Eigen::PlainObjectBase<DerivedS> & S)
{
  typedef typename DerivedV::Scalar Scalar;
  typedef typename DerivedF::Scalar Index;
  {
    Eigen::AlignedBox<Scalar,3> box;
    typedef Eigen::Matrix<Scalar,1,3> RowVector3S;
    assert(V.cols() == 3 && "V must contain positions in 3D");
    RowVector3S min_ext = V.colwise().minCoeff().array() - isolevel;
    RowVector3S max_ext = V.colwise().maxCoeff().array() + isolevel;
    box.extend(min_ext.transpose());
    box.extend(max_ext.transpose());
    igl::voxel_grid(box,s,1,GV,side);
  }

  const Scalar h = 
    (GV.col(0).maxCoeff()-GV.col(0).minCoeff())/((Scalar)(side(0)-1));
  const Scalar lower_bound = isolevel-sqrt(3.0)*h;
  const Scalar upper_bound = isolevel+sqrt(3.0)*h;
  {
    Eigen::Matrix<Index,Eigen::Dynamic,1> I;
    Eigen::Matrix<typename DerivedV::Scalar,Eigen::Dynamic,3> C,N;
    igl::signed_distance(
      GV,V,F,signed_distance_type,lower_bound,upper_bound,S,I,C,N);
  }
  igl::flood_fill(side,S);
  
  DerivedS SS = S.array()-isolevel;
  igl::copyleft::marching_cubes(SS,GV,side(0),side(1),side(2),SV,SF);
}
void append(
    Eigen::MatrixXd& A,
    const Eigen::PlainObjectBase<Derived1>& B)
{
  int n = A.rows(), m = A.cols();
  if (n <= 0 && m <= 0) {
    A = B;
  } else {
    assertion(B.rows() == n, B.rows(), A.rows());
    A.conservativeResize(n, m + B.cols());
    for(auto i = m; i < m + B.cols(); i++)
      A.col(i) = B.col(i-m);
  }
}
Beispiel #9
0
IGL_INLINE void igl::sort2(
  const Eigen::PlainObjectBase<DerivedX>& X,
  const int dim,
  const bool ascending,
  Eigen::PlainObjectBase<DerivedY>& Y,
  Eigen::PlainObjectBase<DerivedIX>& IX)
{
  using namespace Eigen;
  using namespace std;
  typedef typename Eigen::PlainObjectBase<DerivedY>::Scalar YScalar;
  Y = X.template cast<YScalar>();
  // get number of columns (or rows)
  int num_outer = (dim == 1 ? X.cols() : X.rows() );
  // get number of rows (or columns)
  int num_inner = (dim == 1 ? X.rows() : X.cols() );
  assert(num_inner == 2);(void)num_inner;
  typedef typename Eigen::PlainObjectBase<DerivedIX>::Scalar Index;
  IX.resize(X.rows(),X.cols());
  if(dim==1)
  {
    IX.row(0).setConstant(0);// = Eigen::PlainObjectBase<DerivedIX>::Zero(1,IX.cols());
    IX.row(1).setConstant(1);// = Eigen::PlainObjectBase<DerivedIX>::Ones (1,IX.cols());
  }else
  {
    IX.col(0).setConstant(0);// = Eigen::PlainObjectBase<DerivedIX>::Zero(IX.rows(),1);
    IX.col(1).setConstant(1);// = Eigen::PlainObjectBase<DerivedIX>::Ones (IX.rows(),1);
  }
  // loop over columns (or rows)
  for(int i = 0;i<num_outer;i++)
  {
    YScalar & a = (dim==1 ? Y(0,i) : Y(i,0));
    YScalar & b = (dim==1 ? Y(1,i) : Y(i,1));
    Index & ai = (dim==1 ? IX(0,i) : IX(i,0));
    Index & bi = (dim==1 ? IX(1,i) : IX(i,1));
    if((ascending && a>b) || (!ascending && a<b))
    {
      std::swap(a,b);
      std::swap(ai,bi);
    }
  }
}
Beispiel #10
0
IGL_INLINE bool igl::arap_solve(
    const Eigen::PlainObjectBase<Derivedbc> & bc,
    ARAPData & data,
    Eigen::PlainObjectBase<DerivedU> & U)
{
    using namespace Eigen;
    using namespace std;
    assert(data.b.size() == bc.rows());
    if(bc.size() > 0)
    {
        assert(bc.cols() == data.dim && "bc.cols() match data.dim");
    }
    const int n = data.n;
    int iter = 0;
    if(U.size() == 0)
    {
        // terrible initial guess.. should at least copy input mesh
#ifndef NDEBUG
        cerr<<"arap_solve: Using terrible initial guess for U. Try U = V."<<endl;
#endif
        U = MatrixXd::Zero(data.n,data.dim);
    } else
    {
        assert(U.cols() == data.dim && "U.cols() match data.dim");
    }
    // changes each arap iteration
    MatrixXd U_prev = U;
    // doesn't change for fixed with_dynamics timestep
    MatrixXd U0;
    if(data.with_dynamics)
    {
        U0 = U_prev;
    }
    while(iter < data.max_iter)
    {
        U_prev = U;
        // enforce boundary conditions exactly
        for(int bi = 0; bi<bc.rows(); bi++)
        {
            U.row(data.b(bi)) = bc.row(bi);
        }

        const auto & Udim = U.replicate(data.dim,1);
        assert(U.cols() == data.dim);
        // As if U.col(2) was 0
        MatrixXd S = data.CSM * Udim;
        // THIS NORMALIZATION IS IMPORTANT TO GET SINGLE PRECISION SVD CODE TO WORK
        // CORRECTLY.
        S /= S.array().abs().maxCoeff();

        const int Rdim = data.dim;
        MatrixXd R(Rdim,data.CSM.rows());
        if(R.rows() == 2)
        {
            fit_rotations_planar(S,R);
        } else
        {
            fit_rotations(S,true,R);
//#ifdef __SSE__ // fit_rotations_SSE will convert to float if necessary
//      fit_rotations_SSE(S,R);
//#else
//      fit_rotations(S,true,R);
//#endif
        }
        //for(int k = 0;k<(data.CSM.rows()/dim);k++)
        //{
        //  R.block(0,dim*k,dim,dim) = MatrixXd::Identity(dim,dim);
        //}


        // Number of rotations: #vertices or #elements
        int num_rots = data.K.cols()/Rdim/Rdim;
        // distribute group rotations to vertices in each group
        MatrixXd eff_R;
        if(data.G.size() == 0)
        {
            // copy...
            eff_R = R;
        } else
        {
            eff_R.resize(Rdim,num_rots*Rdim);
            for(int r = 0; r<num_rots; r++)
            {
                eff_R.block(0,Rdim*r,Rdim,Rdim) =
                    R.block(0,Rdim*data.G(r),Rdim,Rdim);
            }
        }

        MatrixXd Dl;
        if(data.with_dynamics)
        {
            assert(data.M.rows() == n &&
                   "No mass matrix. Call arap_precomputation if changing with_dynamics");
            const double h = data.h;
            assert(h != 0);
            //Dl = 1./(h*h*h)*M*(-2.*V0 + Vm1) - fext;
            // data.vel = (V0-Vm1)/h
            // h*data.vel = (V0-Vm1)
            // -h*data.vel = -V0+Vm1)
            // -V0-h*data.vel = -2V0+Vm1
            const double dw = (1./data.ym)*(h*h);
            Dl = dw * (1./(h*h)*data.M*(-U0 - h*data.vel) - data.f_ext);
        }

        VectorXd Rcol;
        columnize(eff_R,num_rots,2,Rcol);
        VectorXd Bcol = -data.K * Rcol;
        assert(Bcol.size() == data.n*data.dim);
        for(int c = 0; c<data.dim; c++)
        {
            VectorXd Uc,Bc,bcc,Beq;
            Bc = Bcol.block(c*n,0,n,1);
            if(data.with_dynamics)
            {
                Bc += Dl.col(c);
            }
            if(bc.size()>0)
            {
                bcc = bc.col(c);
            }
            min_quad_with_fixed_solve(
                data.solver_data,
                Bc,bcc,Beq,
                Uc);
            U.col(c) = Uc;
        }

        iter++;
    }
    if(data.with_dynamics)
    {
        // Keep track of velocity for next time
        data.vel = (U-U0)/data.h;
    }

    return true;
}
Beispiel #11
0
IGL_INLINE void igl::all_edges(
  const Eigen::PlainObjectBase<DerivedF> & F,
  Eigen::PlainObjectBase<DerivedE> & E)
{
  E.resize(F.rows()*F.cols(),F.cols()-1);
  switch(F.cols())
  {
    case 4:
      E.block(0*F.rows(),0,F.rows(),1) = F.col(1);
      E.block(0*F.rows(),1,F.rows(),1) = F.col(3);
      E.block(0*F.rows(),2,F.rows(),1) = F.col(2);

      E.block(1*F.rows(),0,F.rows(),1) = F.col(0);
      E.block(1*F.rows(),1,F.rows(),1) = F.col(2);
      E.block(1*F.rows(),2,F.rows(),1) = F.col(3);

      E.block(2*F.rows(),0,F.rows(),1) = F.col(0);
      E.block(2*F.rows(),1,F.rows(),1) = F.col(3);
      E.block(2*F.rows(),2,F.rows(),1) = F.col(1);

      E.block(3*F.rows(),0,F.rows(),1) = F.col(0);
      E.block(3*F.rows(),1,F.rows(),1) = F.col(1);
      E.block(3*F.rows(),2,F.rows(),1) = F.col(2);
      return;
    case 3:
      E.block(0*F.rows(),0,F.rows(),1) = F.col(1);
      E.block(0*F.rows(),1,F.rows(),1) = F.col(2);
      E.block(1*F.rows(),0,F.rows(),1) = F.col(2);
      E.block(1*F.rows(),1,F.rows(),1) = F.col(0);
      E.block(2*F.rows(),0,F.rows(),1) = F.col(0);
      E.block(2*F.rows(),1,F.rows(),1) = F.col(1);
      return;
  }
}
IGL_INLINE void igl::orientable_patches(
  const Eigen::PlainObjectBase<DerivedF> & F,
  Eigen::PlainObjectBase<DerivedC> & C,
  Eigen::SparseMatrix<AScalar> & A)
{
  using namespace Eigen;
  using namespace std;

  // simplex size
  assert(F.cols() == 3);

  // List of all "half"-edges: 3*#F by 2
  Matrix<typename DerivedF::Scalar, Dynamic, 2> allE,sortallE,uE;
  allE.resize(F.rows()*3,2);
  Matrix<int,Dynamic,2> IX;
  VectorXi IA,IC;
  allE.block(0*F.rows(),0,F.rows(),1) = F.col(1);
  allE.block(0*F.rows(),1,F.rows(),1) = F.col(2);
  allE.block(1*F.rows(),0,F.rows(),1) = F.col(2);
  allE.block(1*F.rows(),1,F.rows(),1) = F.col(0);
  allE.block(2*F.rows(),0,F.rows(),1) = F.col(0);
  allE.block(2*F.rows(),1,F.rows(),1) = F.col(1);
  // Sort each row
  sort(allE,2,true,sortallE,IX);
  //IC(i) tells us where to find sortallE(i,:) in uE: 
  // so that sortallE(i,:) = uE(IC(i),:)
  unique_rows(sortallE,uE,IA,IC);
  // uE2FT(e,f) = 1 means face f is adjacent to unique edge e
  vector<Triplet<AScalar> > uE2FTijv(IC.rows());
  for(int e = 0;e<IC.rows();e++)
  {
    uE2FTijv[e] = Triplet<AScalar>(e%F.rows(),IC(e),1);
  }
  SparseMatrix<AScalar> uE2FT(F.rows(),uE.rows());
  uE2FT.setFromTriplets(uE2FTijv.begin(),uE2FTijv.end());
  // kill non-manifold edges
  for(int j=0; j<(int)uE2FT.outerSize();j++)
  {
    int degree = 0;
    for(typename SparseMatrix<AScalar>::InnerIterator it (uE2FT,j); it; ++it)
    {
      degree++;
    }
    // Iterate over inside
    if(degree > 2)
    {
      for(typename SparseMatrix<AScalar>::InnerIterator it (uE2FT,j); it; ++it)
      {
        uE2FT.coeffRef(it.row(),it.col()) = 0;
      }
    }
  }
  // Face-face Adjacency matrix
  SparseMatrix<AScalar> uE2F;
  uE2F = uE2FT.transpose().eval();
  A = uE2FT*uE2F;
  // All ones
  for(int j=0; j<A.outerSize();j++)
  {
    // Iterate over inside
    for(typename SparseMatrix<AScalar>::InnerIterator it (A,j); it; ++it)
    {
      if(it.value() > 1)
      {
        A.coeffRef(it.row(),it.col()) = 1;
      }
    }
  }
  //% Connected components are patches
  //%C = components(A); % alternative to graphconncomp from matlab_bgl
  //[~,C] = graphconncomp(A);
  // graph connected components using boost
  components(A,C);

}
Beispiel #13
0
IGL_INLINE void igl::sort_new(
  const Eigen::PlainObjectBase<DerivedX>& X,
  const int dim,
  const bool ascending,
  Eigen::PlainObjectBase<DerivedY>& Y,
  Eigen::PlainObjectBase<DerivedIX>& IX)
{
  // get number of rows (or columns)
  int num_inner = (dim == 1 ? X.rows() : X.cols() );
  // Special case for swapping
  if(num_inner == 2)
  {
    return igl::sort2(X,dim,ascending,Y,IX);
  }
  using namespace Eigen;
  // get number of columns (or rows)
  int num_outer = (dim == 1 ? X.cols() : X.rows() );
  // dim must be 2 or 1
  assert(dim == 1 || dim == 2);
  // Resize output
  Y.resize(X.rows(),X.cols());
  IX.resize(X.rows(),X.cols());
  // idea is to process each column (or row) as a std vector
  // loop over columns (or rows)
  for(int i = 0; i<num_outer;i++)
  {
    Eigen::VectorXi ix;
    colon(0,num_inner-1,ix);
    // Sort the index map, using unsorted for comparison
    if(dim == 1)
    {
      std::sort(
        ix.data(),
        ix.data()+ix.size(),
        igl::IndexVectorLessThan<const typename DerivedX::ConstColXpr >(X.col(i)));
    }else
    {
      std::sort(
        ix.data(),
        ix.data()+ix.size(),
        igl::IndexVectorLessThan<const typename DerivedX::ConstRowXpr >(X.row(i)));
    }
    // if not ascending then reverse
    if(!ascending)
    {
      std::reverse(ix.data(),ix.data()+ix.size());
    }
    for(int j = 0;j<num_inner;j++)
    {
      if(dim == 1)
      {
        Y(j,i) = X(ix[j],i);
        IX(j,i) = ix[j];
      }else
      {
        Y(i,j) = X(i,ix[j]);
        IX(i,j) = ix[j];
      }
    }
  }
}
Beispiel #14
0
IGL_INLINE bool igl::arap_precomputation(
    const Eigen::PlainObjectBase<DerivedV> & V,
    const Eigen::PlainObjectBase<DerivedF> & F,
    const int dim,
    const Eigen::PlainObjectBase<Derivedb> & b,
    ARAPData & data)
{
    using namespace std;
    using namespace Eigen;
    typedef typename DerivedV::Scalar Scalar;
    // number of vertices
    const int n = V.rows();
    data.n = n;
    assert((b.size() == 0 || b.maxCoeff() < n) && "b out of bounds");
    assert((b.size() == 0 || b.minCoeff() >=0) && "b out of bounds");
    // remember b
    data.b = b;
    //assert(F.cols() == 3 && "For now only triangles");
    // dimension
    //const int dim = V.cols();
    assert((dim == 3 || dim ==2) && "dim should be 2 or 3");
    data.dim = dim;
    //assert(dim == 3 && "Only 3d supported");
    // Defaults
    data.f_ext = MatrixXd::Zero(n,data.dim);

    assert(data.dim <= V.cols() && "solve dim should be <= embedding");
    bool flat = (V.cols() - data.dim)==1;

    DerivedV plane_V;
    DerivedF plane_F;
    typedef SparseMatrix<Scalar> SparseMatrixS;
    SparseMatrixS ref_map,ref_map_dim;
    if(flat)
    {
        project_isometrically_to_plane(V,F,plane_V,plane_F,ref_map);
        repdiag(ref_map,dim,ref_map_dim);
    }
    const PlainObjectBase<DerivedV>& ref_V = (flat?plane_V:V);
    const PlainObjectBase<DerivedF>& ref_F = (flat?plane_F:F);
    SparseMatrixS L;
    cotmatrix(V,F,L);

    ARAPEnergyType eff_energy = data.energy;
    if(eff_energy == ARAP_ENERGY_TYPE_DEFAULT)
    {
        switch(F.cols())
        {
        case 3:
            if(data.dim == 3)
            {
                eff_energy = ARAP_ENERGY_TYPE_SPOKES_AND_RIMS;
            } else
            {
                eff_energy = ARAP_ENERGY_TYPE_ELEMENTS;
            }
            break;
        case 4:
            eff_energy = ARAP_ENERGY_TYPE_ELEMENTS;
            break;
        default:
            assert(false);
        }
    }


    // Get covariance scatter matrix, when applied collects the covariance
    // matrices used to fit rotations to during optimization
    covariance_scatter_matrix(ref_V,ref_F,eff_energy,data.CSM);
    if(flat)
    {
        data.CSM = (data.CSM * ref_map_dim.transpose()).eval();
    }
    assert(data.CSM.cols() == V.rows()*data.dim);

    // Get group sum scatter matrix, when applied sums all entries of the same
    // group according to G
    SparseMatrix<double> G_sum;
    if(data.G.size() == 0)
    {
        if(eff_energy == ARAP_ENERGY_TYPE_ELEMENTS)
        {
            speye(F.rows(),G_sum);
        } else
        {
            speye(n,G_sum);
        }
    } else
    {
        // groups are defined per vertex, convert to per face using mode
        if(eff_energy == ARAP_ENERGY_TYPE_ELEMENTS)
        {
            Eigen::Matrix<int,Eigen::Dynamic,1> GG;
            MatrixXi GF(F.rows(),F.cols());
            for(int j = 0; j<F.cols(); j++)
            {
                Matrix<int,Eigen::Dynamic,1> GFj;
                slice(data.G,F.col(j),GFj);
                GF.col(j) = GFj;
            }
            mode<int>(GF,2,GG);
            data.G=GG;
        }
        //printf("group_sum_matrix()\n");
        group_sum_matrix(data.G,G_sum);
    }
    SparseMatrix<double> G_sum_dim;
    repdiag(G_sum,data.dim,G_sum_dim);
    assert(G_sum_dim.cols() == data.CSM.rows());
    data.CSM = (G_sum_dim * data.CSM).eval();


    arap_rhs(ref_V,ref_F,data.dim,eff_energy,data.K);
    if(flat)
    {
        data.K = (ref_map_dim * data.K).eval();
    }
    assert(data.K.rows() == data.n*data.dim);

    SparseMatrix<double> Q = (-L).eval();

    if(data.with_dynamics)
    {
        const double h = data.h;
        assert(h != 0);
        SparseMatrix<double> M;
        massmatrix(V,F,MASSMATRIX_TYPE_DEFAULT,data.M);
        const double dw = (1./data.ym)*(h*h);
        SparseMatrix<double> DQ = dw * 1./(h*h)*data.M;
        Q += DQ;
        // Dummy external forces
        data.f_ext = MatrixXd::Zero(n,data.dim);
        data.vel = MatrixXd::Zero(n,data.dim);
    }

    return min_quad_with_fixed_precompute(
               Q,b,SparseMatrix<double>(),true,data.solver_data);
}
IGL_INLINE double igl::polyvector_field_poisson_reconstruction(
  const Eigen::PlainObjectBase<DerivedV> &Vcut,
  const Eigen::PlainObjectBase<DerivedF> &Fcut,
  const Eigen::PlainObjectBase<DerivedS> &sol3D_combed,
  Eigen::PlainObjectBase<DerivedSF> &scalars,
  Eigen::PlainObjectBase<DerivedS> &sol3D_recon,
  Eigen::PlainObjectBase<DerivedE> &max_error )
  {
    Eigen::SparseMatrix<typename DerivedV::Scalar> gradMatrix;
    igl::grad(Vcut, Fcut, gradMatrix);

    Eigen::VectorXd FAreas;
    igl::doublearea(Vcut, Fcut, FAreas);
    FAreas = FAreas.array() * .5;

    int nf = FAreas.rows();
    Eigen::SparseMatrix<typename DerivedV::Scalar> M,M1;
    Eigen::VectorXi II = igl::colon<int>(0, nf-1);

    igl::sparse(II, II, FAreas, M1);
    igl::repdiag(M1, 3, M) ;

    int half_degree = sol3D_combed.cols()/3;

    sol3D_recon.setZero(sol3D_combed.rows(),sol3D_combed.cols());

    int numF = Fcut.rows();
    scalars.setZero(Vcut.rows(),half_degree);

    Eigen::SparseMatrix<typename DerivedV::Scalar> Q = gradMatrix.transpose()* M *gradMatrix;

    //fix one point at Ik=fix, value at fixed xk=0
    int fix = 0;
    Eigen::VectorXi Ik(1);Ik<<fix;
    Eigen::VectorXd xk(1);xk<<0;

    //unknown indices
    Eigen::VectorXi Iu(Vcut.rows()-1,1);
    Iu<<igl::colon<int>(0, fix-1),  igl::colon<int>(fix+1,Vcut.rows()-1);

    Eigen::SparseMatrix<typename DerivedV::Scalar> Quu, Quk;
    igl::slice(Q, Iu, Iu, Quu);
    igl::slice(Q, Iu, Ik, Quk);
    Eigen::SimplicialLDLT<Eigen::SparseMatrix<typename DerivedV::Scalar> > solver;
    solver.compute(Quu);


    Eigen::VectorXd vec; vec.setZero(3*numF,1);
    for (int i =0; i<half_degree; ++i)
    {
      vec<<sol3D_combed.col(i*3+0),sol3D_combed.col(i*3+1),sol3D_combed.col(i*3+2);
      Eigen::VectorXd b = gradMatrix.transpose()* M * vec;
      Eigen::VectorXd bu = igl::slice(b, Iu);

      Eigen::VectorXd rhs = bu-Quk*xk;
      Eigen::MatrixXd yu = solver.solve(rhs);

      Eigen::VectorXi index = i*Eigen::VectorXi::Ones(Iu.rows(),1);
      igl::slice_into(yu, Iu, index, scalars);scalars(Ik[0],i)=xk[0];
    }

    //    evaluate gradient of found scalar function
    for (int i =0; i<half_degree; ++i)
    {
      Eigen::VectorXd vec_poisson = gradMatrix*scalars.col(i);
      sol3D_recon.col(i*3+0) = vec_poisson.segment(0*numF, numF);
      sol3D_recon.col(i*3+1) = vec_poisson.segment(1*numF, numF);
      sol3D_recon.col(i*3+2) = vec_poisson.segment(2*numF, numF);
    }

    max_error.setZero(numF,1);
    for (int i =0; i<half_degree; ++i)
    {
      Eigen::VectorXd diff = (sol3D_recon.block(0, i*3, numF, 3)-sol3D_combed.block(0, i*3, numF, 3)).rowwise().norm();
      diff = diff.array() / sol3D_combed.block(0, i*3, numF, 3).rowwise().norm().array();
      max_error = max_error.cwiseMax(diff.cast<typename DerivedE::Scalar>());
    }

    return max_error.mean();
  }
Beispiel #16
0
IGL_INLINE void igl::slice_tets(
  const Eigen::PlainObjectBase<DerivedV>& V,
  const Eigen::PlainObjectBase<DerivedT>& T,
  const Eigen::PlainObjectBase<Derivedplane> & plane,
  Eigen::PlainObjectBase<DerivedU>& U,
  Eigen::PlainObjectBase<DerivedG>& G,
  Eigen::PlainObjectBase<DerivedJ>& J,
  Eigen::SparseMatrix<BCType> & BC)
{
  using namespace Eigen;
  using namespace std;
  assert(V.cols() == 3 && "V should be #V by 3");
  assert(T.cols() == 4 && "T should be #T by 4");
  assert(plane.size() == 4 && "Plane equation should be 4 coefficients");

  // number of tets
  const size_t m = T.rows();

  typedef typename DerivedV::Scalar Scalar;
  typedef typename DerivedT::Scalar Index;
  typedef Matrix<Scalar,Dynamic,1> VectorXS;
  typedef Matrix<Scalar,Dynamic,4> MatrixX4S;
  typedef Matrix<Scalar,Dynamic,3> MatrixX3S;
  typedef Matrix<Scalar,Dynamic,2> MatrixX2S;
  typedef Matrix<Index,Dynamic,4> MatrixX4I;
  typedef Matrix<Index,Dynamic,3> MatrixX3I;
  typedef Matrix<Index,Dynamic,1> VectorXI;
  typedef Matrix<bool,Dynamic,1> VectorXb;
  
  // Value of plane's implicit function at all vertices
  VectorXS IV = 
    (V.col(0)*plane(0) + 
     V.col(1)*plane(1) + 
     V.col(2)*plane(2)).array()
    + plane(3);
  MatrixX4S IT(m,4);
  for(size_t t = 0;t<m;t++)
  {
    for(size_t c = 0;c<4;c++)
    {
      IT(t,c) = IV(T(t,c));
    }
  }

  const auto & extract_rows = [](
    const PlainObjectBase<DerivedT> & T,
    const MatrixX4S & IT,
    const VectorXb & I,
    MatrixX4I  & TI,
    MatrixX4S & ITI,
    VectorXI & JI)
  {
    const Index num_I = std::count(I.data(),I.data()+I.size(),true);
    TI.resize(num_I,4);
    ITI.resize(num_I,4);
    JI.resize(num_I,1);
    {
      size_t k = 0;
      for(size_t t = 0;t<(size_t)T.rows();t++)
      {
        if(I(t))
        {
          TI.row(k) = T.row(t);
          ITI.row(k) = IT.row(t);
          JI(k) = t;
          k++;
        }
      }
      assert(k == num_I);
    }
  };

  VectorXb I13 = (IT.array()<0).rowwise().count()==1;
  VectorXb I31 = (IT.array()>0).rowwise().count()==1;
  VectorXb I22 = (IT.array()<0).rowwise().count()==2;
  MatrixX4I T13,T31,T22;
  MatrixX4S IT13,IT31,IT22;
  VectorXI J13,J31,J22;
  extract_rows(T,IT,I13,T13,IT13,J13);
  extract_rows(T,IT,I31,T31,IT31,J31);
  extract_rows(T,IT,I22,T22,IT22,J22);

  const auto & apply_sort = [] (
     const MatrixX4I & T, 
     const MatrixX4I & sJ, 
     MatrixX4I & sT)
  {
    sT.resize(T.rows(),4);
    for(size_t t = 0;t<(size_t)T.rows();t++)
    {
      for(size_t c = 0;c<4;c++)
      {
        sT(t,c) = T(t,sJ(t,c));
      }
    }
  };

  const auto & one_below = [&V,&apply_sort](
    const MatrixX4I & T,
    const MatrixX4S & IT,
    MatrixX3I & G,
    SparseMatrix<BCType> & BC)
  {
    // Number of tets
    const size_t m = T.rows();
    MatrixX4S sIT;
    MatrixX4I sJ;
    sort(IT,2,true,sIT,sJ);
    MatrixX4I sT;
    apply_sort(T,sJ,sT);
    MatrixX3S lambda = 
      sIT.rightCols(3).array() /
      (sIT.rightCols(3).colwise()-sIT.col(0)).array();
    vector<Triplet<BCType> > IJV;
    IJV.reserve(m*3*2);
    for(size_t c = 0;c<3;c++)
    {
      for(size_t t = 0;t<(size_t)m;t++)
      {
        IJV.push_back(Triplet<BCType>(c*m+t,  sT(t,0),  lambda(t,c)));
        IJV.push_back(Triplet<BCType>(c*m+t,sT(t,c+1),1-lambda(t,c)));
      }
    }
    BC.resize(m*3,V.rows());
    BC.reserve(m*3*2);
    BC.setFromTriplets(IJV.begin(),IJV.end());
    G.resize(m,3);
    for(size_t c = 0;c<3;c++)
    {
      G.col(c).setLinSpaced(m,0+c*m,(m-1)+c*m);
    }
  };

  const auto & two_below = [&V,&apply_sort](
    const MatrixX4I & T,
    const MatrixX4S & IT,
    MatrixX3I & G,
    SparseMatrix<BCType> & BC)
  {
    // Number of tets
    const size_t m = T.rows();
    MatrixX4S sIT;
    MatrixX4I sJ;
    sort(IT,2,true,sIT,sJ);
    MatrixX4I sT;
    apply_sort(T,sJ,sT);
    MatrixX2S lambda = 
      sIT.rightCols(2).array() /
      (sIT.rightCols(2).colwise()-sIT.col(0)).array();
    MatrixX2S gamma = 
      sIT.rightCols(2).array() /
      (sIT.rightCols(2).colwise()-sIT.col(1)).array();
    vector<Triplet<BCType> > IJV;
    IJV.reserve(m*4*2);
    for(size_t c = 0;c<2;c++)
    {
      for(size_t t = 0;t<(size_t)m;t++)
      {
        IJV.push_back(Triplet<BCType>(0*2*m+c*m+t,  sT(t,0),  lambda(t,c)));
        IJV.push_back(Triplet<BCType>(0*2*m+c*m+t,sT(t,c+2),1-lambda(t,c)));
        IJV.push_back(Triplet<BCType>(1*2*m+c*m+t,  sT(t,1),   gamma(t,c)));
        IJV.push_back(Triplet<BCType>(1*2*m+c*m+t,sT(t,c+2),1- gamma(t,c)));
      }
    }
    BC.resize(m*4,V.rows());
    BC.reserve(m*4*2);
    BC.setFromTriplets(IJV.begin(),IJV.end());
    G.resize(2*m,3);
    G.block(0,0,m,1) = VectorXI::LinSpaced(m,0+0*m,(m-1)+0*m);
    G.block(0,1,m,1) = VectorXI::LinSpaced(m,0+1*m,(m-1)+1*m);
    G.block(0,2,m,1) = VectorXI::LinSpaced(m,0+3*m,(m-1)+3*m);
    G.block(m,0,m,1) = VectorXI::LinSpaced(m,0+0*m,(m-1)+0*m);
    G.block(m,1,m,1) = VectorXI::LinSpaced(m,0+3*m,(m-1)+3*m);
    G.block(m,2,m,1) = VectorXI::LinSpaced(m,0+2*m,(m-1)+2*m);
  };

  MatrixX3I G13,G31,G22;
  SparseMatrix<BCType> BC13,BC31,BC22;
  one_below(T13,IT13,G13,BC13);
  one_below(T31,-IT31,G31,BC31);
  two_below(T22,IT22,G22,BC22);

  BC = cat(1,cat(1,BC13,BC31),BC22);
  U = BC*V;
  G.resize(G13.rows()+G31.rows()+G22.rows(),3);
  G<<G13,(G31.array()+BC13.rows()),(G22.array()+BC13.rows()+BC31.rows());
  MatrixX3S N;
  per_face_normals(U,G,N);
  Matrix<Scalar,1,3> planeN(plane(0),plane(1),plane(2));
  VectorXb flip = (N.array().rowwise() * planeN.array()).rowwise().sum()<0;
  for(size_t g = 0;g<(size_t)G.rows();g++)
  {
    if(flip(g))
    {
      G.row(g) = G.row(g).reverse().eval();
    }
  }

  J.resize(G.rows());
  J<<J13,J31,J22,J22;
}
Beispiel #17
0
IGL_INLINE bool igl::arap_solve(
  const Eigen::PlainObjectBase<Derivedbc> & bc,
  ARAPData & data,
  Eigen::PlainObjectBase<DerivedU> & U)
{
  using namespace igl;
  using namespace Eigen;
  using namespace std;
  assert(data.b.size() == bc.rows());
  const int dim = bc.cols();
  const int n = data.n;
  int iter = 0;
  if(U.size() == 0)
  {
    // terrible initial guess.. should at least copy input mesh
    U = MatrixXd::Zero(data.n,dim);
  }
  // changes each arap iteration
  MatrixXd U_prev = U;
  // doesn't change for fixed with_dynamics timestep
  MatrixXd U0;
  if(data.with_dynamics)
  {
    U0 = U_prev;
  }
  while(iter < data.max_iter)
  {
    U_prev = U;
    // enforce boundary conditions exactly
    for(int bi = 0;bi<bc.rows();bi++)
    {
      U.row(data.b(bi)) = bc.row(bi);
    }

    MatrixXd S = data.CSM * U.replicate(dim,1);
    MatrixXd R(dim,data.CSM.rows());
#ifdef __SSE__ // fit_rotations_SSE will convert to float if necessary
    fit_rotations_SSE(S,R);
#else
    fit_rotations(S,R);
#endif
    //for(int k = 0;k<(data.CSM.rows()/dim);k++)
    //{
    //  R.block(0,dim*k,dim,dim) = MatrixXd::Identity(dim,dim);
    //}


    // Number of rotations: #vertices or #elements
    int num_rots = data.K.cols()/dim/dim;
    // distribute group rotations to vertices in each group
    MatrixXd eff_R;
    if(data.G.size() == 0)
    {
      // copy...
      eff_R = R;
    }else
    {
      eff_R.resize(dim,num_rots*dim);
      for(int r = 0;r<num_rots;r++)
      {
        eff_R.block(0,dim*r,dim,dim) = 
          R.block(0,dim*data.G(r),dim,dim);
      }
    }

    MatrixXd Dl;
    if(data.with_dynamics)
    {
      assert(M.rows() == n && 
        "No mass matrix. Call arap_precomputation if changing with_dynamics");
      const double h = data.h;
      assert(h != 0);
      //Dl = 1./(h*h*h)*M*(-2.*V0 + Vm1) - fext;
      // data.vel = (V0-Vm1)/h
      // h*data.vel = (V0-Vm1)
      // -h*data.vel = -V0+Vm1)
      // -V0-h*data.vel = -2V0+Vm1
      Dl = 1./(h*h)*data.M*(-U0 - h*data.vel) - data.f_ext;
    }

    VectorXd Rcol;
    columnize(eff_R,num_rots,2,Rcol);
    VectorXd Bcol = -data.K * Rcol;
    for(int c = 0;c<dim;c++)
    {
      VectorXd Uc,Bc,bcc,Beq;
      Bc = Bcol.block(c*n,0,n,1);
      if(data.with_dynamics)
      {
        Bc += Dl.col(c);
      }
      bcc = bc.col(c);
      min_quad_with_fixed_solve(
        data.solver_data,
        Bc,bcc,Beq,
        Uc);
      U.col(c) = Uc;
    }

    iter++;
  }
  if(data.with_dynamics)
  {
    // Keep track of velocity for next time
    data.vel = (U-U0)/data.h;
  }
  return true;
}
Beispiel #18
0
IGL_INLINE bool igl::bbw(
  const Eigen::PlainObjectBase<DerivedV> & V, 
  const Eigen::PlainObjectBase<DerivedEle> & Ele, 
  const Eigen::PlainObjectBase<Derivedb> & b, 
  const Eigen::PlainObjectBase<Derivedbc> & bc, 
  igl::BBWData & data,
  Eigen::PlainObjectBase<DerivedW> & W
  )
{
  using namespace igl;
  using namespace std;
  using namespace Eigen;

  // number of domain vertices
  int n = V.rows();
  // number of handles
  int m = bc.cols();

  SparseMatrix<typename DerivedW::Scalar> L;
  cotmatrix(V,Ele,L);
  MassMatrixType mmtype = MASSMATRIX_VORONOI;
  if(Ele.cols() == 4)
  {
    mmtype = MASSMATRIX_BARYCENTRIC;
  }
  SparseMatrix<typename DerivedW::Scalar> M;
  SparseMatrix<typename DerivedW::Scalar> Mi;
  massmatrix(V,Ele,mmtype,M);

  invert_diag(M,Mi);

  // Biharmonic operator
  SparseMatrix<typename DerivedW::Scalar> Q = L.transpose() * Mi * L;

  W.derived().resize(n,m);
  if(data.partition_unity)
  {
    // Not yet implemented
    assert(false);
  }else
  {
    // No linear terms
    VectorXd c = VectorXd::Zero(n);
    // No linear constraints
    SparseMatrix<typename DerivedW::Scalar> A(0,n),Aeq(0,n),Aieq(0,n);
    VectorXd uc(0,1),Beq(0,1),Bieq(0,1),lc(0,1);
    // Upper and lower box constraints (Constant bounds)
    VectorXd ux = VectorXd::Ones(n);
    VectorXd lx = VectorXd::Zero(n);
    active_set_params eff_params = data.active_set_params;
    switch(data.qp_solver)
    {
      case QP_SOLVER_IGL_ACTIVE_SET:
      {
        //if(data.verbosity >= 1)
        //{
          cout<<"BBW: max_iter: "<<eff_params.max_iter<<endl;
        //}
        if(data.verbosity >= 1)
        {
          cout<<"BBW: Computing initial weights for "<<m<<" handle"<<
            (m!=1?"s":"")<<"."<<endl;
        }
        min_quad_with_fixed_data<typename DerivedW::Scalar > mqwf;
        min_quad_with_fixed_precompute(Q,b,Aeq,true,mqwf);
        min_quad_with_fixed_solve(mqwf,c,bc,Beq,W);
        // decrement
        eff_params.max_iter--;
        bool error = false;
        // Loop over handles
#pragma omp parallel for
        for(int i = 0;i<m;i++)
        {
          // Quicker exit for openmp
          if(error)
          {
            continue;
          }
          if(data.verbosity >= 1)
          {
#pragma omp critical
            cout<<"BBW: Computing weight for handle "<<i+1<<" out of "<<m<<
              "."<<endl;
          }
          VectorXd bci = bc.col(i);
          VectorXd Wi;
          // use initial guess
          Wi = W.col(i);
          SolverStatus ret = active_set(
              Q,c,b,bci,Aeq,Beq,Aieq,Bieq,lx,ux,eff_params,Wi);
          switch(ret)
          {
            case SOLVER_STATUS_CONVERGED:
              break;
            case SOLVER_STATUS_MAX_ITER:
              cerr<<"active_set: max iter without convergence."<<endl;
              break;
            case SOLVER_STATUS_ERROR:
            default:
              cerr<<"active_set error."<<endl;
              error = true;
          }
          W.col(i) = Wi;
        }
        if(error)
        {
          return false;
        }
        break;
      }
      case QP_SOLVER_MOSEK:
      {
#ifdef IGL_NO_MOSEK
        assert(false && "Use another QPSolver. Recompile without IGL_NO_MOSEK defined.");
        cerr<<"Use another QPSolver. Recompile without IGL_NO_MOSEK defined."<<endl;
        return false;
#else
        // Loop over handles
        for(int i = 0;i<m;i++)
        {
          if(data.verbosity >= 1)
          {
            cout<<"BBW: Computing weight for handle "<<i+1<<" out of "<<m<<
              "."<<endl;
          }
          VectorXd bci = bc.col(i);
          VectorXd Wi;
          // impose boundary conditions via bounds
          slice_into(bci,b,ux);
          slice_into(bci,b,lx);
          bool r = mosek_quadprog(Q,c,0,A,lc,uc,lx,ux,data.mosek_data,Wi);
          if(!r)
          {
            return false;
          }
          W.col(i) = Wi;
        }
#endif
        break;
      }
      default:
      {
        assert(false && "Unknown qp_solver");
        return false;
      }
    }
#ifndef NDEBUG
    const double min_rowsum = W.rowwise().sum().array().abs().minCoeff();
    if(min_rowsum < 0.1)
    {
      cerr<<"bbw.cpp: Warning, minimum row sum is very low. Consider more "
        "active set iterations or enforcing partition of unity."<<endl;
    }
#endif
  }

  return true;
}
Beispiel #19
0
IGL_INLINE void igl::dihedral_angles_intrinsic(
  const Eigen::PlainObjectBase<DerivedL>& L,
  const Eigen::PlainObjectBase<DerivedA>& A,
  Eigen::PlainObjectBase<Derivedtheta>& theta,
  Eigen::PlainObjectBase<Derivedcos_theta>& cos_theta)
{
  using namespace Eigen;
  const int m = L.rows();
  assert(m == A.rows());
  // Law of cosines
  // http://math.stackexchange.com/a/49340/35376
  Matrix<typename Derivedtheta::Scalar,Dynamic,6> H_sqr(m,6);
  H_sqr.col(0) = (1./16.) * (4. * L.col(3).array().square() * L.col(0).array().square() - 
                                ((L.col(1).array().square() + L.col(4).array().square()) -
                                 (L.col(2).array().square() + L.col(5).array().square())).square());
  H_sqr.col(1) = (1./16.) * (4. * L.col(4).array().square() * L.col(1).array().square() - 
                                ((L.col(2).array().square() + L.col(5).array().square()) -
                                 (L.col(3).array().square() + L.col(0).array().square())).square());
  H_sqr.col(2) = (1./16.) * (4. * L.col(5).array().square() * L.col(2).array().square() - 
                                ((L.col(3).array().square() + L.col(0).array().square()) -
                                 (L.col(4).array().square() + L.col(1).array().square())).square());
  H_sqr.col(3) = (1./16.) * (4. * L.col(0).array().square() * L.col(3).array().square() - 
                                ((L.col(4).array().square() + L.col(1).array().square()) -
                                 (L.col(5).array().square() + L.col(2).array().square())).square());
  H_sqr.col(4) = (1./16.) * (4. * L.col(1).array().square() * L.col(4).array().square() - 
                                ((L.col(5).array().square() + L.col(2).array().square()) -
                                 (L.col(0).array().square() + L.col(3).array().square())).square());
  H_sqr.col(5) = (1./16.) * (4. * L.col(2).array().square() * L.col(5).array().square() - 
                                ((L.col(0).array().square() + L.col(3).array().square()) -
                                 (L.col(1).array().square() + L.col(4).array().square())).square());
  cos_theta.resize(m,6);
  cos_theta.col(0) = (H_sqr.col(0).array() - 
      A.col(1).array().square() - A.col(2).array().square()).array() / 
      (-2.*A.col(1).array() * A.col(2).array());
  cos_theta.col(1) = (H_sqr.col(1).array() - 
      A.col(2).array().square() - A.col(0).array().square()).array() / 
      (-2.*A.col(2).array() * A.col(0).array());
  cos_theta.col(2) = (H_sqr.col(2).array() - 
      A.col(0).array().square() - A.col(1).array().square()).array() / 
      (-2.*A.col(0).array() * A.col(1).array());
  cos_theta.col(3) = (H_sqr.col(3).array() - 
      A.col(3).array().square() - A.col(0).array().square()).array() / 
      (-2.*A.col(3).array() * A.col(0).array());
  cos_theta.col(4) = (H_sqr.col(4).array() - 
      A.col(3).array().square() - A.col(1).array().square()).array() / 
      (-2.*A.col(3).array() * A.col(1).array());
  cos_theta.col(5) = (H_sqr.col(5).array() - 
      A.col(3).array().square() - A.col(2).array().square()).array() / 
      (-2.*A.col(3).array() * A.col(2).array());

  theta = cos_theta.array().acos();

  cos_theta.resize(m,6);

}