double get_time() { CTime * timer; return (double) timer->getTime(); }
void SVDLinearSolver<TMatrix,TVector>::solve(Matrix& M, Vector& x, Vector& b) { #ifdef SOFA_DUMP_VISITOR_INFO simulation::Visitor::printComment("SVD"); #endif #ifdef DISPLAY_TIME CTime timer; double time1 = (double) timer.getTime(); #endif const bool verbose = f_verbose.getValue(); /// Convert the matrix and the right-hand vector to Eigen objects Eigen::MatrixXd m(M.rowSize(),M.colSize()); Eigen::VectorXd rhs(M.rowSize()); for(unsigned i=0; i<(unsigned)M.rowSize(); i++ ) { for( unsigned j=0; j<(unsigned)M.colSize(); j++ ) m(i,j) = M[i][j]; rhs(i) = b[i]; } msg_info_when(verbose) << "solve, Here is the matrix m: " << m ; /// Compute the SVD decomposition and the condition number Eigen::JacobiSVD<Eigen::MatrixXd> svd(m, Eigen::ComputeThinU | Eigen::ComputeThinV); f_conditionNumber.setValue( (Real)(svd.singularValues()(0) / svd.singularValues()(M.rowSize()-1)) ); if(verbose) { msg_info() << "solve, the singular values are:" << sendl << svd.singularValues() << msgendl << "Its left singular vectors are the columns of the thin U matrix: " << msgendl << svd.matrixU() << msgendl << "Its right singular vectors are the columns of the thin V matrix:" msgendl << svd.matrixV() ; }else{ msg_info() << "solve, the singular values are:" << sendl << svd.singularValues() << msgendl; } /// Solve the equation system and copy the solution to the SOFA vector // Eigen::VectorXd solution = svd.solve(rhs); // for(unsigned i=0; i<M.rowSize(); i++ ){ // x[i] = solution(i); // } Eigen::VectorXd Ut_b = svd.matrixU().transpose() * rhs; Eigen::VectorXd S_Ut_b(M.colSize()); for( unsigned i=0; i<(unsigned)M.colSize(); i++ ) /// product with the diagonal matrix, using the threshold for near-null values { if( svd.singularValues()[i] > f_minSingularValue.getValue() ) S_Ut_b[i] = Ut_b[i]/svd.singularValues()[i]; else S_Ut_b[i] = (Real)0.0 ; } Eigen::VectorXd solution = svd.matrixV() * S_Ut_b; for(unsigned i=0; i<(unsigned)M.rowSize(); i++ ) { x[i] = (Real) solution(i); } #ifdef DISPLAY_TIME time1 = (double)(((double) timer.getTime() - time1) * timeStamp / (nb_iter-1)); dmsg_info() << " solve, SVD = "<<time1; #endif dmsg_info() << "solve, rhs vector = " << msgendl << rhs.transpose() << msgendl << " solution = \n" << msgendl << x << msgendl << " verification, mx - b = " << msgendl << (m * solution - rhs ).transpose() << msgendl; }
void CudaMasterContactSolver<real>::step(const core::ExecParams* params /* PARAMS FIRST */, double dt) { sofa::helper::AdvancedTimer::stepBegin("AnimationStep"); context = dynamic_cast<simulation::Node *>(this->getContext()); // access to current node #ifdef DISPLAY_TIME CTime *timer; double timeScale = 1.0 / (double)CTime::getRefTicksPerSec(); timer = new CTime(); double time_Free_Motion = (double) timer->getTime(); #endif // Update the BehaviorModels // Required to allow the RayPickInteractor interaction simulation::BehaviorUpdatePositionVisitor updatePos(params /* PARAMS FIRST */, dt); context->execute(&updatePos); simulation::MechanicalBeginIntegrationVisitor beginVisitor(params /* PARAMS FIRST */, dt); context->execute(&beginVisitor); // Free Motion simulation::SolveVisitor freeMotion(params /* PARAMS FIRST */, dt, true); context->execute(&freeMotion); //simulation::MechanicalPropagateFreePositionVisitor().execute(context); { sofa::core::MechanicalParams mparams(*params); sofa::core::MultiVecCoordId xfree = sofa::core::VecCoordId::freePosition(); mparams.x() = xfree; simulation::MechanicalPropagatePositionVisitor(&mparams /* PARAMS FIRST */, 0, xfree, true).execute(context); } //core::VecId dx_id = (VecId)core::VecDerivId::dx(); //simulation::MechanicalVOpVisitor(params /* PARAMS FIRST */, dx_id).execute( context); //simulation::MechanicalPropagateDxVisitor(params /* PARAMS FIRST */, dx_id, true).execute( context); //ignore the masks (is it necessary?) //simulation::MechanicalVOpVisitor(params /* PARAMS FIRST */, dx_id).execute( context); #ifdef DISPLAY_TIME time_Free_Motion = ((double) timer->getTime() - time_Free_Motion)*timeScale; double time_computeCollision = (double) timer->getTime(); #endif computeCollision(); #ifdef DISPLAY_TIME time_computeCollision = ((double) timer->getTime() - time_computeCollision)*timeScale; double time_build_LCP = (double) timer->getTime(); #endif //MechanicalResetContactForceVisitor().execute(context); for (unsigned int i=0;i<constraintCorrections.size();i++) { core::behavior::BaseConstraintCorrection* cc = constraintCorrections[i]; cc->resetContactForce(); } build_LCP(); #ifdef DISPLAY_TIME time_build_LCP = ((double) timer->getTime() - time_build_LCP)*timeScale; double time_solve_LCP = (double) timer->getTime(); #endif double _tol = tol.getValue(); int _maxIt = maxIt.getValue(); if (! initial_guess.getValue()) _f.clear(); double error = 0.0; #ifdef CHECK real t1,t2; if (_mu > 0.0) { f_check.resize(_numConstraints,MBSIZE); for (unsigned i=0;i<_numConstraints;i++) f_check[i] = _f[i]; real toln = ((int) (_realNumConstraints/3) + 1) * (real)_tol; t2 = sofa::gpu::cuda::CudaLCP<real>::CudaNlcp_gaussseidel(useGPU_d.getValue(),_numConstraints, _dFree.getCudaVector(), _W.getCudaMatrix(), f_check.getCudaVector(), _mu,_toln, _maxIt); t1 = sofa::gpu::cuda::CudaLCP<real>::CudaNlcp_gaussseidel(0,_numConstraints, _dFree.getCudaVector(), _W.getCudaMatrix(), _f.getCudaVector(), _mu,_toln, _maxIt); } else { f_check.resize(_numConstraints); for (unsigned i=0;i<_numConstraints;i++) f_check[i] = _f[i]; t2 = sofa::gpu::cuda::CudaLCP<real>::CudaGaussSeidelLCP1(useGPU_d.getValue(),_numConstraints, _dFree.getCudaVector(), _W.getCudaMatrix(), f_check.getCudaVector(), _tol, _maxIt); t1 = sofa::gpu::cuda::CudaLCP<real>::CudaGaussSeidelLCP1(0,_numConstraints, _dFree.getCudaVector(), _W.getCudaMatrix(), _f.getCudaVector(), _tol, _maxIt); } for (unsigned i=0;i<f_check.size();i++) { if ((f_check.element(i)-_f.element(i)>CHECK) || (f_check.element(i)-_f.element(i)<-CHECK)) { std::cerr << "Error(" << useGPU_d.getValue() << ") dim(" << _numConstraints << ") realDim(" << _realNumConstraints << ") elmt(" << i << ") : (cpu," << f_check.element(i) << ") (gpu,(" << _f.element(i) << ")" << std::endl; } } #else sout << "numcontacts = " << _realNumConstraints << " RealNumContacts =" << _numConstraints << sendl; if (_mu > 0.0) { real toln = ((int) (_realNumConstraints/3) + 1) * (real)_tol; error = sofa::gpu::cuda::CudaLCP<real>::CudaNlcp_gaussseidel(useGPU_d.getValue(),_realNumConstraints, _dFree.getCudaVector(), _W.getCudaMatrix(), _f.getCudaVector(), _mu,toln, _maxIt); // printf("\nFE = ["); // for (unsigned j=0;j<_numConstraints;j++) { // printf("%f ",_f.element(j)); // } // printf("]\n"); // real toln = ((int) (_numConstraints/3) + 1) * (real)_tol; // error = sofa::gpu::cuda::CudaLCP<real>::CudaNlcp_gaussseidel(useGPU_d.getValue(),_realNumConstraints, _dFree.getCudaVector(), _W.getCudaMatrix(), _f.getCudaVector(), _mu,toln, _maxIt); if (this->f_printLog.getValue()) { printf("M = [\n"); for (unsigned j=0;j<_numConstraints;j++) { for (unsigned i=0;i<_numConstraints;i++) { printf("%f\t",_W.element(i,j)); } printf("\n"); } printf("]\n"); printf("q = ["); for (unsigned j=0;j<_numConstraints;j++) { printf("%f\t",_dFree.element(j)); } printf("]\n"); printf("FS = ["); for (unsigned j=0;j<_numConstraints;j++) { printf("%f ",_f.element(j)); } printf("]\n"); } } else { error = sofa::gpu::cuda::CudaLCP<real>::CudaGaussSeidelLCP1(useGPU_d.getValue(),_realNumConstraints, _dFree.getCudaVector(), _W.getCudaMatrix(), _f.getCudaVector(), (real)_tol, _maxIt); } #endif if (error > _tol) sout << "No convergence in gaussSeidelLCP1 : error = " << error << sendl; #ifdef DISPLAY_TIME time_solve_LCP = ((double) timer->getTime() - time_solve_LCP)*timeScale; double time_contactCorrections = (double) timer->getTime(); #endif if (initial_guess.getValue()) keepContactForcesValue(); // MechanicalApplyContactForceVisitor(_result).execute(context); for (unsigned int i=0;i<constraintCorrections.size();i++) { core::behavior::BaseConstraintCorrection* cc = constraintCorrections[i]; cc->applyContactForce(&_f); } core::MechanicalParams mparams(*params); simulation::MechanicalPropagateAndAddDxVisitor(&mparams).execute(context); //simulation::MechanicalPropagatePositionAndVelocityVisitor().execute(context); //simulation::MechanicalPropagateAndAddDxVisitor().execute( context); //MechanicalResetContactForceVisitor().execute(context); for (unsigned int i=0;i<constraintCorrections.size();i++) { core::behavior::BaseConstraintCorrection* cc = constraintCorrections[i]; cc->resetContactForce(); } #ifdef DISPLAY_TIME time_contactCorrections = ((double) timer->getTime() - time_contactCorrections)*timeScale; #endif #ifdef DISPLAY_TIME double total = time_Free_Motion + time_computeCollision + time_build_LCP + time_solve_LCP + time_contactCorrections; if (this->print_info.getValue()) { sout<<"********* Start Iteration : " << _numConstraints << " contacts *********" <<sendl; sout<<"Free Motion\t" << time_Free_Motion <<" s \t| " << time_Free_Motion*100.0/total << "%" <<sendl; sout<<"ComputeCollision\t" << time_computeCollision <<" s \t| " << time_computeCollision*100.0/total << "%" <<sendl; sout<<"Build_LCP\t" << time_build_LCP <<" s \t| " << time_build_LCP*100.0/total << "%" <<sendl; sout<<"Solve_LCP\t" << time_solve_LCP <<" s \t| " << time_solve_LCP*100.0/total << "%" <<sendl; sout<<"ContactCorrections\t" << time_contactCorrections <<" s \t| " << time_contactCorrections*100.0/total << "%" <<sendl; unsigned nbNnul = 0; unsigned sz = _realNumConstraints * _realNumConstraints; for (unsigned j=0;j<sz;j++) if (_W.getCudaMatrix().hostRead()[j]==0.0) nbNnul++; sout<<"Sparsity = " << ((double) (((double) nbNnul * 100.0) / ((double)sz))) <<"%" <<sendl; } #endif simulation::MechanicalEndIntegrationVisitor endVisitor(params /* PARAMS FIRST */, dt); context->execute(&endVisitor); sofa::helper::AdvancedTimer::stepEnd("AnimationStep"); }