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