void projectorFromSvd(const Eigen::MatrixXd& jac, Eigen::JacobiSVD<Eigen::MatrixXd>& svd, Eigen::VectorXd& svdSingular, Eigen::MatrixXd& preResult, Eigen::MatrixXd& result, double epsilon=std::numeric_limits<double>::epsilon(), double minTol=1e-8) { // we are force to compute the Full matrix because of // the nullspace matrix computation svd.compute(jac, Eigen::ComputeFullU | Eigen::ComputeFullV); double tolerance = epsilon*double(std::max(jac.cols(), jac.rows()))* std::abs(svd.singularValues()[0]); tolerance = std::max(tolerance, minTol); svdSingular.setOnes(); for(int i = 0; i < svd.singularValues().rows(); ++i) { svdSingular[i] = svd.singularValues()[i] > tolerance ? 0. : 1.; } preResult.noalias() = svd.matrixV()*svdSingular.asDiagonal(); result.noalias() = preResult*svd.matrixV().adjoint(); }
bool MatrixXr_pseudoInverse(const MatrixXr &a, MatrixXr &a_pinv, double epsilon) { // see : http://en.wikipedia.org/wiki/Moore-Penrose_pseudoinverse#The_general_case_and_the_SVD_method if ( a.rows()<a.cols() ) return false; // SVD Eigen::JacobiSVD<MatrixXr> svdA; svdA.compute(a,Eigen::ComputeThinU|Eigen::ComputeThinV); MatrixXr vSingular = svdA.singularValues(); // Build a diagonal matrix with the Inverted Singular values // The pseudo inverted singular matrix is easy to compute : // is formed by replacing every nonzero entry by its reciprocal (inversing). VectorXr vPseudoInvertedSingular(svdA.matrixV().cols(),1); for (int iRow =0; iRow<vSingular.rows(); iRow++) { if(fabs(vSingular(iRow))<=epsilon) vPseudoInvertedSingular(iRow,0)=0.; else vPseudoInvertedSingular(iRow,0)=1./vSingular(iRow); } // A little optimization here MatrixXr mAdjointU = svdA.matrixU().adjoint().block(0,0,vSingular.rows(),svdA.matrixU().adjoint().cols()); // Pseudo-Inversion : V * S * U' a_pinv = (svdA.matrixV() * vPseudoInvertedSingular.asDiagonal()) * mAdjointU; return true; }
bool pseudoInverse( const _Matrix_Type_ &a, _Matrix_Type_ &result, double epsilon = std::numeric_limits<typename _Matrix_Type_::Scalar>::epsilon()) { if (a.rows() < a.cols()) return false; Eigen::JacobiSVD<_Matrix_Type_> svd = a.jacobiSvd(); typename _Matrix_Type_::Scalar tolerance = epsilon * std::max(a.cols(), a.rows()) * svd.singularValues().array().abs().maxCoeff(); result = svd.matrixV() * _Matrix_Type_( _Matrix_Type_((svd.singularValues().array().abs() > tolerance) .select(svd.singularValues().array().inverse(), 0)).diagonal()) * svd.matrixU().adjoint(); }
void pseudoInverse(const Eigen::MatrixXd& jac, Eigen::JacobiSVD<Eigen::MatrixXd>& svd, Eigen::VectorXd& svdSingular, Eigen::MatrixXd& prePseudoInv, Eigen::MatrixXd& result, double epsilon=std::numeric_limits<double>::epsilon(), double minTol=1e-8) { svd.compute(jac, Eigen::ComputeThinU | Eigen::ComputeThinV); // singular values are sorted in decreasing order // so the first one is the max one double tolerance = epsilon*double(std::max(jac.cols(), jac.rows()))* std::abs(svd.singularValues()[0]); tolerance = std::max(tolerance, minTol); svdSingular = ((svd.singularValues().array().abs() > tolerance). select(svd.singularValues().array().inverse(), 0.)); prePseudoInv.noalias() = svd.matrixV()*svdSingular.asDiagonal(); result.noalias() = prePseudoInv*svd.matrixU().adjoint(); }
int main() { std::ifstream file; file.open("SVD_benchmark"); if (!file) { CGAL_TRACE_STREAM << "Error loading file!\n"; return 0; } int ite = 200000; Eigen::JacobiSVD<Eigen::Matrix3d> svd; Eigen::Matrix3d u, v, cov, r; Eigen::Vector3d w; int matrix_idx = rand()%200; for (int i = 0; i < matrix_idx; i++) { for (int j = 0; j < 3; j++) { for (int k = 0; k < 3; k++) { file >> cov(j, k); } } } CGAL::Timer task_timer; CGAL_TRACE_STREAM << "Start SVD decomposition..."; task_timer.start(); for (int i = 0; i < ite; i++) { svd.compute( cov, Eigen::ComputeFullU | Eigen::ComputeFullV ); u = svd.matrixU(); v = svd.matrixV(); w = svd.singularValues(); r = v*u.transpose(); } task_timer.stop(); file.close(); CGAL_TRACE_STREAM << "done: " << task_timer.time() << "s\n"; return 0; }
// Derived from code by Yohann Solaro ( http://listengine.tuxfamily.org/lists.tuxfamily.org/eigen/2010/01/msg00187.html ) // see : http://en.wikipedia.org/wiki/Moore-Penrose_pseudoinverse#The_general_case_and_the_SVD_method Eigen::MatrixXd pinv( const Eigen::MatrixXd &b, double rcond ) { // TODO: Figure out why it wants fewer rows than columns // if ( a.rows()<a.cols() ) // return false; bool flip = false; Eigen::MatrixXd a; if( a.rows() < a.cols() ) { a = b.transpose(); flip = true; } else a = b; // SVD Eigen::JacobiSVD<Eigen::MatrixXd> svdA; svdA.compute( a, Eigen::ComputeFullU | Eigen::ComputeThinV ); Eigen::JacobiSVD<Eigen::MatrixXd>::SingularValuesType vSingular = svdA.singularValues(); // Build a diagonal matrix with the Inverted Singular values // The pseudo inverted singular matrix is easy to compute : // is formed by replacing every nonzero entry by its reciprocal (inversing). Eigen::VectorXd vPseudoInvertedSingular( svdA.matrixV().cols() ); for (int iRow=0; iRow<vSingular.rows(); iRow++) { if ( fabs(vSingular(iRow)) <= rcond ) { vPseudoInvertedSingular(iRow)=0.; } else vPseudoInvertedSingular(iRow)=1./vSingular(iRow); } // A little optimization here Eigen::MatrixXd mAdjointU = svdA.matrixU().adjoint().block( 0, 0, vSingular.rows(), svdA.matrixU().adjoint().cols() ); // Pseudo-Inversion : V * S * U' Eigen::MatrixXd a_pinv = (svdA.matrixV() * vPseudoInvertedSingular.asDiagonal()) * mAdjointU; if( flip ) { a = a.transpose(); a_pinv = a_pinv.transpose(); } return a_pinv; }
// typename DerivedA::Scalar void pseudo_inverse_svd(const Eigen::MatrixBase<DerivedA>& M, Eigen::MatrixBase<OutputMatrixType>& Minv, typename DerivedA::Scalar epsilon = 1e-6)//std::numeric_limits<typename DerivedA::Scalar>::epsilon()) { // CONTROLIT_INFO << "Method called!\n epsilon = " << epsilon << ", M = \n" << M; // Ensure matrix Minv has the correct size. Its size should be equal to M.transpose(). assert_msg(M.rows() == Minv.cols(), "Minv has invalid number of columns. Expected " << M.rows() << " got " << Minv.cols()); assert_msg(M.cols() == Minv.rows(), "Minv has invalid number of rows. Expected " << M.cols() << " got " << Minv.rows()); // According to Eigen documentation, "If the input matrix has inf or nan coefficients, the result of the // computation is undefined, but the computation is guaranteed to terminate in finite (and reasonable) time." Eigen::JacobiSVD<DerivedA> svd = M.jacobiSvd(Eigen::ComputeFullU | Eigen::ComputeFullV); // Get the max singular value typename DerivedA::Scalar maxSingularValue = svd.singularValues().array().abs().maxCoeff(); // Use Minv to temporarily hold sigma Minv.setZero(); typename DerivedA::Scalar tolerance = 0; // Only compute sigma if the max singular value is greater than zero. if (maxSingularValue > epsilon) { tolerance = epsilon * std::max(M.cols(), M.rows()) * maxSingularValue; // For each singular value of matrix M's SVD decomposition, check if it is greater than // the tolerance value. If it is, save 1/(singular value) in the sigma vector. // Otherwise save zero in the sigma vector. DerivedA sigmaVector = DerivedA( (svd.singularValues().array().abs() > tolerance).select(svd.singularValues().array().inverse(), 0) ); // DerivedA zeroSVs = DerivedA( (svd.singularValues().array().abs() <= tolerance).select(svd.singularValues().array().inverse(), 0) ); // CONTROLIT_INFO << "epsilon: " << epsilon << ", std::max(M.cols(), M.rows()): " << std::max(M.cols(), M.rows()) << ", maxSingularValue: " << maxSingularValue << ", tolerance: " << tolerance; // CONTROLIT_INFO << "sigmaVector = " << sigmaVector.transpose(); // CONTROLIT_INFO << "zeroSigmaVector : "<< zeroSVs.transpose(); Minv.block(0, 0, sigmaVector.rows(), sigmaVector.rows()) = sigmaVector.asDiagonal(); } // Double check to make sure the matrices have the correct dimensions assert_msg(svd.matrixV().cols() == Minv.rows(), "Matrix dimension mismatch, svd.matrixV().cols() = " << svd.matrixV().cols() << ", Minv.rows() = " << Minv.rows() << "."); assert_msg(Minv.cols() == svd.matrixU().adjoint().rows(), "Matrix dimension mismatch, Minv.cols() = " << Minv.cols() << ", svd.matrixU().adjoint().rows() = " << svd.matrixU().adjoint().rows() << "."); Minv = svd.matrixV() * Minv * svd.matrixU().adjoint(); // take the transpose of matrix U // CONTROLIT_INFO << "Done method call! Minv = " << Minv; // typename DerivedA::Scalar errorNorm = std::abs((M * Minv - DerivedA::Identity(M.rows(), Minv.cols())).norm()); // if (tolerance != 0 && errorNorm > tolerance * 10) // { // CONTROLIT_WARN << "Problems computing pseudoinverse. Perhaps the tolerance is too high?\n" // << " - epsilon: " << epsilon << "\n" // << " - tolerance: " << tolerance << "\n" // << " - maxSingularValue: " << maxSingularValue << "\n" // << " - errorNorm: " << errorNorm << "\n" // << " - M:\n" << M << "\n" // << " - Minv:\n" << Minv; // } // return errorNorm; }
IGL_INLINE void igl::min_quad_dense_precompute( const Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic>& A, const Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic>& Aeq, const bool use_lu_decomposition, Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic>& S) { typedef Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic> Mat; // This threshold seems to matter a lot but I'm not sure how to // set it const T treshold = igl::FLOAT_EPS; //const T treshold = igl::DOUBLE_EPS; const int n = A.rows(); assert(A.cols() == n); const int m = Aeq.rows(); assert(Aeq.cols() == n); // Lagrange multipliers method: Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic> LM(n + m, n + m); LM.block(0, 0, n, n) = A; LM.block(0, n, n, m) = Aeq.transpose(); LM.block(n, 0, m, n) = Aeq; LM.block(n, n, m, m).setZero(); Mat LMpinv; if(use_lu_decomposition) { // if LM is close to singular, use at your own risk :) LMpinv = LM.inverse(); }else { // use SVD typedef Eigen::Matrix<T, Eigen::Dynamic, 1> Vec; Vec singValues; Eigen::JacobiSVD<Mat> svd; svd.compute(LM, Eigen::ComputeFullU | Eigen::ComputeFullV ); const Mat& u = svd.matrixU(); const Mat& v = svd.matrixV(); const Vec& singVals = svd.singularValues(); Vec pi_singVals(n + m); int zeroed = 0; for (int i=0; i<n + m; i++) { T sv = singVals(i, 0); assert(sv >= 0); // printf("sv: %lg ? %lg\n",(double) sv,(double)treshold); if (sv > treshold) pi_singVals(i, 0) = T(1) / sv; else { pi_singVals(i, 0) = T(0); zeroed++; } } printf("min_quad_dense_precompute: %i singular values zeroed (threshold = %e)\n", zeroed, treshold); Eigen::DiagonalMatrix<T, Eigen::Dynamic> pi_diag(pi_singVals); LMpinv = v * pi_diag * u.transpose(); } S = LMpinv.block(0, 0, n, n + m); //// debug: //mlinit(&g_pEngine); // //mlsetmatrix(&g_pEngine, "A", A); //mlsetmatrix(&g_pEngine, "Aeq", Aeq); //mlsetmatrix(&g_pEngine, "LM", LM); //mlsetmatrix(&g_pEngine, "u", u); //mlsetmatrix(&g_pEngine, "v", v); //MatrixXd svMat = singVals; //mlsetmatrix(&g_pEngine, "singVals", svMat); //mlsetmatrix(&g_pEngine, "LMpinv", LMpinv); //mlsetmatrix(&g_pEngine, "S", S); //int hu = 1; }
//Do the interpolation calculations bool SIM_SnowSolver::solveGasSubclass(SIM_Engine &engine, SIM_Object *obj, SIM_Time time, SIM_Time framerate){ /// STEP #0: Retrieve all data objects from Houdini //Scalar params freal particle_mass = getPMass(); freal YOUNGS_MODULUS = getYoungsModulus(); freal POISSONS_RATIO = getPoissonsRatio(); freal CRIT_COMPRESS = getCritComp(); freal CRIT_STRETCH = getCritStretch(); freal FLIP_PERCENT = getFlipPercent(); freal HARDENING = getHardening(); freal CFL = getCfl(); freal COF = getCof(); freal division_size = getDivSize(); freal max_vel = getMaxVel(); //Vector params vector3 GRAVITY = getGravity(); vector3 bbox_min_limit = getBboxMin(); vector3 bbox_max_limit = getBboxMax(); //Particle params UT_String s_p, s_vol, s_den, s_vel, s_fe, s_fp; getParticles(s_p); getPVol(s_vol); getPD(s_den); getPVel(s_vel); getPFe(s_fe); getPFp(s_fp); SIM_Geometry* geometry = (SIM_Geometry*) obj->getNamedSubData(s_p); if (!geometry) return true; //Get particle data //Do we use the attribute name??? // GU_DetailHandle gdh = geometry->getGeometry().getWriteableCopy(); GU_DetailHandle gdh = geometry->getOwnGeometry(); const GU_Detail* gdp_in = gdh.readLock(); // Must unlock later GU_Detail* gdp_out = gdh.writeLock(); GA_RWAttributeRef p_ref_position = gdp_out->findPointAttribute("P"); GA_RWHandleT<vector3> p_position(p_ref_position.getAttribute()); GA_RWAttributeRef p_ref_volume = gdp_out->findPointAttribute(s_vol); GA_RWHandleT<freal> p_volume(p_ref_volume.getAttribute()); GA_RWAttributeRef p_ref_density = gdp_out->findPointAttribute(s_den); GA_RWHandleT<freal> p_density(p_ref_density.getAttribute()); GA_RWAttributeRef p_ref_vel = gdp_out->findPointAttribute(s_vel); GA_RWHandleT<vector3> p_vel(p_ref_vel.getAttribute()); GA_RWAttributeRef p_ref_Fe = gdp_out->findPointAttribute(s_fe); GA_RWHandleT<matrix3> p_Fe(p_ref_Fe.getAttribute()); GA_RWAttributeRef p_ref_Fp = gdp_out->findPointAttribute(s_fp); GA_RWHandleT<matrix3> p_Fp(p_ref_Fp.getAttribute()); //EVALUATE PARAMETERS freal mu = YOUNGS_MODULUS/(2+2*POISSONS_RATIO); freal lambda = YOUNGS_MODULUS*POISSONS_RATIO/((1+POISSONS_RATIO)*(1-2*POISSONS_RATIO)); //Get grid data SIM_ScalarField *g_mass_field; SIM_DataArray g_mass_data; getMatchingData(g_mass_data, obj, MPM_G_MASS); g_mass_field = SIM_DATA_CAST(g_mass_data(0), SIM_ScalarField); SIM_VectorField *g_nvel_field; SIM_DataArray g_nvel_data; getMatchingData(g_nvel_data, obj, MPM_G_NVEL); g_nvel_field = SIM_DATA_CAST(g_nvel_data(0), SIM_VectorField); SIM_VectorField *g_ovel_field; SIM_DataArray g_ovel_data; getMatchingData(g_ovel_data, obj, MPM_G_OVEL); g_ovel_field = SIM_DATA_CAST(g_ovel_data(0), SIM_VectorField); SIM_ScalarField *g_active_field; SIM_DataArray g_active_data; getMatchingData(g_active_data, obj, MPM_G_ACTIVE); g_active_field = SIM_DATA_CAST(g_active_data(0), SIM_ScalarField); SIM_ScalarField *g_density_field; SIM_DataArray g_density_data; getMatchingData(g_density_data, obj, MPM_G_DENSITY); g_density_field = SIM_DATA_CAST(g_density_data(0), SIM_ScalarField); SIM_ScalarField *g_col_field; SIM_DataArray g_col_data; getMatchingData(g_col_data, obj, MPM_G_COL); g_col_field = SIM_DATA_CAST(g_col_data(0), SIM_ScalarField); SIM_VectorField *g_colVel_field; SIM_DataArray g_colVel_data; getMatchingData(g_colVel_data, obj, MPM_G_COLVEL); g_colVel_field = SIM_DATA_CAST(g_colVel_data(0), SIM_VectorField); SIM_VectorField *g_extForce_field; SIM_DataArray g_extForce_data; getMatchingData(g_extForce_data, obj, MPM_G_EXTFORCE); g_extForce_field = SIM_DATA_CAST(g_extForce_data(0), SIM_VectorField); UT_VoxelArrayF *g_mass = g_mass_field->getField()->fieldNC(), *g_nvelX = g_nvel_field->getField(0)->fieldNC(), *g_nvelY = g_nvel_field->getField(1)->fieldNC(), *g_nvelZ = g_nvel_field->getField(2)->fieldNC(), *g_ovelX = g_ovel_field->getField(0)->fieldNC(), *g_ovelY = g_ovel_field->getField(1)->fieldNC(), *g_ovelZ = g_ovel_field->getField(2)->fieldNC(), *g_colVelX = g_colVel_field->getField(0)->fieldNC(), *g_colVelY = g_colVel_field->getField(1)->fieldNC(), *g_colVelZ = g_colVel_field->getField(2)->fieldNC(), *g_extForceX = g_extForce_field->getField(0)->fieldNC(), *g_extForceY = g_extForce_field->getField(1)->fieldNC(), *g_extForceZ = g_extForce_field->getField(2)->fieldNC(), *g_col = g_col_field->getField()->fieldNC(), *g_active = g_active_field->getField()->fieldNC(); int point_count = gdp_out->getPointRange().getEntries(); std::vector<boost::array<freal,64> > p_w(point_count); std::vector<boost::array<vector3,64> > p_wgh(point_count); //Get world-to-grid conversion ratios //Particle's grid position can be found via (pos - grid_origin)/voxel_dims vector3 voxel_dims = g_mass_field->getVoxelSize(), grid_origin = g_mass_field->getOrig(), grid_divs = g_mass_field->getDivisions(); //Houdini uses voxel centers for grid nodes, rather than grid corners grid_origin += voxel_dims/2.0; freal voxelArea = voxel_dims[0]*voxel_dims[1]*voxel_dims[2]; /* //Reset grid for(int iX=0; iX < grid_divs[0]; iX++){ for(int iY=0; iY < grid_divs[1]; iY++){ for(int iZ=0; iZ < grid_divs[2]; iZ++){ g_mass->setValue(iX,iY,iZ,0); g_active->setValue(iX,iY,iZ,0); g_ovelX->setValue(iX,iY,iZ,0); g_ovelY->setValue(iX,iY,iZ,0); g_ovelZ->setValue(iX,iY,iZ,0); g_nvelX->setValue(iX,iY,iZ,0); g_nvelY->setValue(iX,iY,iZ,0); g_nvelZ->setValue(iX,iY,iZ,0); } } } */ /// STEP #1: Transfer mass to grid if (p_position.isValid()){ //Iterate through particles for (GA_Iterator it(gdp_out->getPointRange()); !it.atEnd(); it.advance()){ int pid = it.getOffset(); //Get grid position vector3 gpos = (p_position.get(pid) - grid_origin)/voxel_dims; int p_gridx = (int) gpos[0], p_gridy = (int) gpos[1], p_gridz = (int) gpos[2]; //g_mass_field->posToIndex(p_position.get(pid),p_gridx,p_gridy,p_gridz); freal particle_density = p_density.get(pid); //Compute weights and transfer mass for (int idx=0, z=p_gridz-1, z_end=z+3; z<=z_end; z++){ //Z-dimension interpolation freal z_pos = gpos[2]-z, wz = SIM_SnowSolver::bspline(z_pos), dz = SIM_SnowSolver::bsplineSlope(z_pos); for (int y=p_gridy-1, y_end=y+3; y<=y_end; y++){ //Y-dimension interpolation freal y_pos = gpos[1]-y, wy = SIM_SnowSolver::bspline(y_pos), dy = SIM_SnowSolver::bsplineSlope(y_pos); for (int x=p_gridx-1, x_end=x+3; x<=x_end; x++, idx++){ //X-dimension interpolation freal x_pos = gpos[0]-x, wx = SIM_SnowSolver::bspline(x_pos), dx = SIM_SnowSolver::bsplineSlope(x_pos); //Final weight is dyadic product of weights in each dimension freal weight = wx*wy*wz; p_w[pid-1][idx] = weight; //Weight gradient is a vector of partial derivatives p_wgh[pid-1][idx] = vector3(dx*wy*wz, wx*dy*wz, wx*wy*dz)/voxel_dims; //Interpolate mass freal node_mass = g_mass->getValue(x,y,z); node_mass += weight*particle_mass; g_mass->setValue(x,y,z,node_mass); } } } } } /// STEP #2: First timestep only - Estimate particle volumes using grid mass /* if (time == 0.0){ //Iterate through particles for (GA_Iterator it(gdp_out->getPointRange()); !it.atEnd(); it.advance()){ int pid = it.getOffset(); freal density = 0; //Get grid position int p_gridx = 0, p_gridy = 0, p_gridz = 0; vector3 gpos = (p_position.get(pid) - grid_origin)/voxel_dims; int p_gridx = (int) gpos[0], p_gridy = (int) gpos[1], p_gridz = (int) gpos[2]; //g_nvel_field->posToIndex(0,p_position.get(pid),p_gridx,p_gridy,p_gridz); //Transfer grid density (within radius) to particles for (int idx=0, z=p_gridz-1, z_end=z+3; z<=z_end; z++){ for (int y=p_gridy-1, y_end=y+3; y<=y_end; y++){ for (int x=p_gridx-1, x_end=x+3; x<=x_end; x++, idx++){ freal w = p_w[pid-1][idx]; if (w > EPSILON){ //Transfer density density += w * g_mass->getValue(x,y,z); } } } } density /= voxelArea; p_density.set(pid,density); p_volume.set(pid, particle_mass/density); } } //*/ /// STEP #3: Transfer velocity to grid //This must happen after transferring mass, to conserve momentum //Iterate through particles and transfer for (GA_Iterator it(gdp_in->getPointRange()); !it.atEnd(); it.advance()){ int pid = it.getOffset(); vector3 vel_fac = p_vel.get(pid)*particle_mass; //Get grid position vector3 gpos = (p_position.get(pid) - grid_origin)/voxel_dims; int p_gridx = (int) gpos[0], p_gridy = (int) gpos[1], p_gridz = (int) gpos[2]; //g_nvel_field->posToIndex(0,p_position.get(pid),p_gridx,p_gridy,p_gridz); //Transfer to grid nodes within radius for (int idx=0, z=p_gridz-1, z_end=z+3; z<=z_end; z++){ for (int y=p_gridy-1, y_end=y+3; y<=y_end; y++){ for (int x=p_gridx-1, x_end=x+3; x<=x_end; x++, idx++){ freal w = p_w[pid-1][idx]; if (w > EPSILON){ freal nodex_vel = g_ovelX->getValue(x,y,z) + vel_fac[0]*w; freal nodey_vel = g_ovelY->getValue(x,y,z) + vel_fac[1]*w; freal nodez_vel = g_ovelZ->getValue(x,y,z) + vel_fac[2]*w; g_ovelX->setValue(x,y,z,nodex_vel); g_ovelY->setValue(x,y,z,nodey_vel); g_ovelZ->setValue(x,y,z,nodez_vel); g_active->setValue(x,y,z,1.0); } } } } } //Division is slow (maybe?); we only want to do divide by mass once, for each active node for(int iX=0; iX < grid_divs[0]; iX++){ for(int iY=0; iY < grid_divs[1]; iY++){ for(int iZ=0; iZ < grid_divs[2]; iZ++){ //Only check nodes that have mass if (g_active->getValue(iX,iY,iZ)){ freal node_mass = 1/(g_mass->getValue(iX,iY,iZ)); g_ovelX->setValue(iX,iY,iZ,(g_ovelX->getValue(iX,iY,iZ)*node_mass)); g_ovelY->setValue(iX,iY,iZ,(g_ovelY->getValue(iX,iY,iZ)*node_mass)); g_ovelZ->setValue(iX,iY,iZ,(g_ovelZ->getValue(iX,iY,iZ)*node_mass)); } } } } /// STEP #4: Compute new grid velocities //Temporary variables for plasticity and force calculation //We need one set of variables for each thread that will be running eigen_matrix3 def_elastic, def_plastic, energy, svd_u, svd_v; Eigen::JacobiSVD<eigen_matrix3, Eigen::NoQRPreconditioner> svd; eigen_vector3 svd_e; matrix3 HDK_def_plastic, HDK_def_elastic, HDK_energy; freal* data_dp = HDK_def_plastic.data(); freal* data_de = HDK_def_elastic.data(); freal* data_energy = HDK_energy.data(); //Map Eigen matrices to HDK matrices Eigen::Map<eigen_matrix3> data_dp_map(data_dp); Eigen::Map<eigen_matrix3> data_de_map(data_de); Eigen::Map<eigen_matrix3> data_energy_map(data_energy); //Compute force at each particle and transfer to Eulerian grid //We use "nvel" to hold the grid force, since that variable is not in use for (GA_Iterator it(gdp_in->getPointRange()); !it.atEnd(); it.advance()){ int pid = it.getOffset(); //Apply plasticity to deformation gradient, before computing forces //We need to use the Eigen lib to do the SVD; transfer houdini matrices to Eigen matrices HDK_def_plastic = p_Fp.get(pid); HDK_def_elastic = p_Fe.get(pid); def_plastic = Eigen::Map<eigen_matrix3>(data_dp); def_elastic = Eigen::Map<eigen_matrix3>(data_de); //Compute singular value decomposition (uev*) svd.compute(def_elastic, Eigen::ComputeFullV | Eigen::ComputeFullU); svd_e = svd.singularValues(); svd_u = svd.matrixU(); svd_v = svd.matrixV(); //Clamp singular values for (int i=0; i<3; i++){ if (svd_e[i] < CRIT_COMPRESS) svd_e[i] = CRIT_COMPRESS; else if (svd_e[i] > CRIT_STRETCH) svd_e[i] = CRIT_STRETCH; } //Put SVD back together for new elastic and plastic gradients def_plastic = svd_v * svd_e.asDiagonal().inverse() * svd_u.transpose() * def_elastic * def_plastic; svd_v.transposeInPlace(); def_elastic = svd_u * svd_e.asDiagonal() * svd_v; //Now compute the energy partial derivative (which we use to get force at each grid node) energy = 2*mu*(def_elastic - svd_u*svd_v)*def_elastic.transpose(); //Je is the determinant of def_elastic (equivalent to svd_e.prod()) freal Je = svd_e.prod(), contour = lambda*Je*(Je-1), jp = def_plastic.determinant(), particle_vol = p_volume.get(pid); for (int i=0; i<3; i++) energy(i,i) += contour; energy *= particle_vol * exp(HARDENING*(1-jp)); //Transfer Eigen matrices back to HDK data_dp_map = def_plastic; data_de_map = def_elastic; data_energy_map = energy; p_Fp.set(pid,HDK_def_plastic); p_Fe.set(pid,HDK_def_elastic); //Transfer energy to surrounding grid nodes vector3 gpos = (p_position.get(pid) - grid_origin)/voxel_dims; int p_gridx = (int) gpos[0], p_gridy = (int) gpos[1], p_gridz = (int) gpos[2]; for (int idx=0, z=p_gridz-1, z_end=z+3; z<=z_end; z++){ for (int y=p_gridy-1, y_end=y+3; y<=y_end; y++){ for (int x=p_gridx-1, x_end=x+3; x<=x_end; x++, idx++){ freal w = p_w[pid-1][idx]; if (w > EPSILON){ vector3 ngrad = p_wgh[pid-1][idx]; g_nvelX->setValue(x,y,z,g_nvelX->getValue(x,y,z) + ngrad.dot(HDK_energy[0])); g_nvelY->setValue(x,y,z,g_nvelY->getValue(x,y,z) + ngrad.dot(HDK_energy[1])); g_nvelZ->setValue(x,y,z,g_nvelZ->getValue(x,y,z) + ngrad.dot(HDK_energy[2])); } } } } } //Use new forces to solve for new velocities for(int iX=0; iX < grid_divs[0]; iX++){ for(int iY=0; iY < grid_divs[1]; iY++){ for(int iZ=0; iZ < grid_divs[2]; iZ++){ //Only compute for active nodes if (g_active->getValue(iX,iY,iZ)){ freal nodex_ovel = g_ovelX->getValue(iX,iY,iZ); freal nodey_ovel = g_ovelY->getValue(iX,iY,iZ); freal nodez_ovel = g_ovelZ->getValue(iX,iY,iZ); freal forcex = g_nvelX->getValue(iX,iY,iZ); freal forcey = g_nvelY->getValue(iX,iY,iZ); freal forcez = g_nvelZ->getValue(iX,iY,iZ); freal node_mass = 1/(g_mass->getValue(iX,iY,iZ)); freal ext_forceX = GRAVITY[0] + g_extForceX->getValue(iX,iY,iZ); freal ext_forceY = GRAVITY[1] + g_extForceY->getValue(iX,iY,iZ); freal ext_forceZ = GRAVITY[2] + g_extForceZ->getValue(iX,iY,iZ); nodex_ovel += framerate*(ext_forceX - forcex*node_mass); nodey_ovel += framerate*(ext_forceY - forcey*node_mass); nodez_ovel += framerate*(ext_forceZ - forcez*node_mass); //Limit velocity to max_vel vector3 g_nvel(nodex_ovel, nodey_ovel, nodez_ovel); freal nvelNorm = g_nvel.length(); if(nvelNorm > max_vel){ freal velRatio = max_vel/nvelNorm; g_nvel*= velRatio; } g_nvelX->setValue(iX,iY,iZ,g_nvel[0]); g_nvelY->setValue(iX,iY,iZ,g_nvel[1]); g_nvelZ->setValue(iX,iY,iZ,g_nvel[2]); } } } } /// STEP #5: Grid collision resolution vector3 sdf_normal; //* for(int iX=1; iX < grid_divs[0]-1; iX++){ for(int iY=1; iY < grid_divs[1]-1; iY++){ for(int iZ=1; iZ < grid_divs[2]-1; iZ++){ if (g_active->getValue(iX,iY,iZ)){ if (!computeSDFNormal(g_col, iX, iY, iZ, sdf_normal)) continue; //Collider velocity vector3 vco( g_colVelX->getValue(iX,iY,iZ), g_colVelY->getValue(iX,iY,iZ), g_colVelZ->getValue(iX,iY,iZ) ); //Grid velocity vector3 v( g_nvelX->getValue(iX,iY,iZ), g_nvelY->getValue(iX,iY,iZ), g_nvelZ->getValue(iX,iY,iZ) ); //Skip if bodies are separating vector3 vrel = v - vco; freal vn = vrel.dot(sdf_normal); if (vn >= 0) continue; //Resolve collisions; also add velocity of collision object to snow velocity //Sticks to surface (too slow to overcome static friction) vector3 vt = vrel - (sdf_normal*vn); freal stick = vn*COF, vt_norm = vt.length(); if (vt_norm <= -stick) vt = vco; //Dynamic friction else vt += stick*vt/vt_norm + vco; g_nvelX->setValue(iX,iY,iZ,vt[0]); g_nvelY->setValue(iX,iY,iZ,vt[1]); g_nvelZ->setValue(iX,iY,iZ,vt[2]); } } } } //*/ /// STEP #6: Transfer grid velocities to particles and integrate /// STEP #7: Particle collision resolution vector3 pic, flip, col_vel; matrix3 vel_grad; //Iterate through particles for (GA_Iterator it(gdp_in->getPointRange()); !it.atEnd(); it.advance()){ int pid = it.getOffset(); //Particle position vector3 pos(p_position.get(pid)); //Reset velocity pic[0] = 0.0; pic[1] = 0.0; pic[2] = 0.0; flip = p_vel.get(pid); vel_grad.zero(); freal density = 0; //Get grid position vector3 gpos = (pos - grid_origin)/voxel_dims; int p_gridx = (int) gpos[0], p_gridy = (int) gpos[1], p_gridz = (int) gpos[2]; for (int idx=0, z=p_gridz-1, z_end=z+3; z<=z_end; z++){ for (int y=p_gridy-1, y_end=y+3; y<=y_end; y++){ for (int x=p_gridx-1, x_end=x+3; x<=x_end; x++, idx++){ freal w = p_w[pid-1][idx]; if (w > EPSILON){ const vector3 node_wg = p_wgh[pid-1][idx]; const vector3 node_nvel( g_nvelX->getValue(x,y,z), g_nvelY->getValue(x,y,z), g_nvelZ->getValue(x,y,z) ); //Transfer velocities pic += node_nvel*w; flip[0] += (node_nvel[0] - g_ovelX->getValue(x,y,z))*w; flip[1] += (node_nvel[1]- g_ovelY->getValue(x,y,z))*w; flip[2] += (node_nvel[2] - g_ovelZ->getValue(x,y,z))*w; //Transfer density density += w * g_mass->getValue(x,y,z); //Transfer veloctiy gradient vel_grad.outerproductUpdate(1.0, node_nvel, node_wg); } } } } //Finalize velocity update vector3 vel = flip*FLIP_PERCENT + pic*(1-FLIP_PERCENT); //Reset collision data freal col_sdf = 0; sdf_normal[0] = 0; sdf_normal[1] = 0; sdf_normal[2] = 0; col_vel[0] = 0; col_vel[1] = 0; col_vel[2] = 0; //Interpolate surrounding nodes' SDF info to the particle (trilinear interpolation) for (int idx=0, z=p_gridz, z_end=z+1; z<=z_end; z++){ freal w_z = gpos[2]-z; for (int y=p_gridy, y_end=y+1; y<=y_end; y++){ freal w_zy = w_z*(gpos[1]-y); for (int x=p_gridx, x_end=x+1; x<=x_end; x++, idx++){ freal weight = fabs(w_zy*(gpos[0]-x)); //cout << w_zy << "," << (gpos[0]-x) << "," << weight << endl; vector3 temp_normal; computeSDFNormal(g_col, x, y, z, temp_normal); //goto SKIP_PCOLLIDE; //cout << g_col->getValue(x, y, z) << endl; //Interpolate sdf_normal += temp_normal*weight; col_sdf += g_col->getValue(x, y, z)*weight; col_vel[0] += g_colVelX->getValue(x, y, z)*weight; col_vel[1] += g_colVelY->getValue(x, y, z)*weight; col_vel[2] += g_colVelZ->getValue(x, y, z)*weight; } } } //Resolve particle collisions //cout << col_sdf << endl; if (col_sdf > 0){ vector3 vrel = vel - col_vel; freal vn = vrel.dot(sdf_normal); //Skip if bodies are separating if (vn < 0){ //Resolve and add velocity of collision object to snow velocity //Sticks to surface (too slow to overcome static friction) vel = vrel - (sdf_normal*vn); freal stick = vn*COF, vel_norm = vel.length(); if (vel_norm <= -stick) vel = col_vel; //Dynamic friction else vel += stick*vel/vel_norm + col_vel; } } SKIP_PCOLLIDE: //Finalize density update density /= voxelArea; p_density.set(pid,density); //Update particle position pos += framerate*vel; //Limit particle position int mask = 0; /* for (int i=0; i<3; i++){ if (pos[i] > bbox_max_limit[i]){ pos[i] = bbox_max_limit[i]; vel[i] = 0.0; mask |= 1 << i; } else if (pos[i] < bbox_min_limit[i]){ pos[i] = bbox_min_limit[i]; vel[i] = 0.0; mask |= 1 << i; } } //Slow particle down at bounds (not really necessary...) if (mask){ for (int i=0; i<3; i++){ if (mask & 0x1) vel[i] *= .05; mask >>= 1; } }//*/ p_vel.set(pid,vel); p_position.set(pid,pos); //Update particle deformation gradient //Note: plasticity is computed on the next timestep... vel_grad *= framerate; vel_grad(0,0) += 1; vel_grad(1,1) += 1; vel_grad(2,2) += 1; p_Fe.set(pid, vel_grad*p_Fe.get(pid)); } gdh.unlock(gdp_out); gdh.unlock(gdp_in); return true; }