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