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