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