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"); }
Visitor::Result AnimateVisitor::processNodeTopDown(simulation::Node* node) { if (!node->isActive()) return Visitor::RESULT_PRUNE; if (node->isSleeping()) return Visitor::RESULT_PRUNE; if (!firstNodeVisited) { firstNodeVisited=true; sofa::core::ConstraintParams cparams(*this->params); MechanicalResetConstraintVisitor resetConstraint(&cparams); node->execute(&resetConstraint); } if (dt == 0) setDt(node->getDt()); else node->setDt(dt); if (node->collisionPipeline != NULL) { processCollisionPipeline(node, node->collisionPipeline); } if (!node->solver.empty() ) { sofa::helper::AdvancedTimer::StepVar timer("Mechanical",node); SReal nextTime = node->getTime() + dt; { IntegrateBeginEvent evBegin; PropagateEventVisitor eventPropagation( this->params, &evBegin); eventPropagation.execute(node); } MechanicalBeginIntegrationVisitor beginVisitor(this->params, dt); node->execute(&beginVisitor); sofa::core::MechanicalParams m_mparams(*this->params); m_mparams.setDt(dt); { unsigned int constraintId=0; core::ConstraintParams cparams; simulation::MechanicalAccumulateConstraint(&cparams, core::MatrixDerivId::constraintJacobian(),constraintId).execute(node); } for( unsigned i=0; i<node->solver.size(); i++ ) { ctime_t t0 = begin(node, node->solver[i]); node->solver[i]->solve(params, getDt()); end(node, node->solver[i], t0); } MechanicalProjectPositionAndVelocityVisitor(&m_mparams, nextTime, sofa::core::VecCoordId::position(), sofa::core::VecDerivId::velocity() ).execute( node ); MechanicalPropagateOnlyPositionAndVelocityVisitor(&m_mparams, nextTime,VecCoordId::position(),VecDerivId::velocity(), #ifdef SOFA_SUPPORT_MAPPED_MASS VecDerivId::dx(), #endif true).execute( node ); MechanicalEndIntegrationVisitor endVisitor(this->params, dt); node->execute(&endVisitor); { IntegrateEndEvent evBegin; PropagateEventVisitor eventPropagation(this->params, &evBegin); eventPropagation.execute(node); } return RESULT_PRUNE; } { // process InteractionForceFields for_each(this, node, node->interactionForceField, &AnimateVisitor::fwdInteractionForceField); return RESULT_CONTINUE; } }
Visitor::Result AnimateVisitor::processNodeTopDown(simulation::Node* node) { //cerr<<"AnimateVisitor::process Node "<<node->getName()<<endl; if (!node->isActive()) return Visitor::RESULT_PRUNE; #ifdef SOFA_HAVE_EIGEN2 if (!firstNodeVisited) { firstNodeVisited=true; // core::behavior::BaseAnimationLoop* presenceAnimationManager; // node->get(presenceAnimationManager, core::objectmodel::BaseContext::SearchDown); // if (!presenceAnimationManager) // { // std::cerr << "AnimateVisitor::processNodeTopDown, ERROR: no BaseAnimationLoop found while searching down from node: " << node->getName() << std::endl; // } sofa::core::MechanicalParams mparams(*this->params); mparams.setDt(dt); MechanicalResetConstraintVisitor resetConstraint(&mparams); node->execute(&resetConstraint); } #endif if (dt == 0) setDt(node->getDt()); else node->setDt(dt); if (node->collisionPipeline != NULL) { //ctime_t t0 = begin(node, node->collisionPipeline); #ifndef SOFA_SMP { CollisionBeginEvent evBegin; PropagateEventVisitor eventPropagation(this->params /* PARAMS FIRST */, &evBegin); eventPropagation.execute(node); } processCollisionPipeline(node, node->collisionPipeline); { CollisionEndEvent evEnd; PropagateEventVisitor eventPropagation(this->params /* PARAMS FIRST */, &evEnd); eventPropagation.execute(node); } #endif //end(node, node->collisionPipeline, t0); } /* if (node->solver != NULL) { ctime_t t0 = begin(node, node->solver); processOdeSolver(node, node->solver); end(node, node->solver, t0); return RESULT_PRUNE; }*/ if (!node->solver.empty() ) { sofa::helper::AdvancedTimer::StepVar timer("Mechanical",node); double nextTime = node->getTime() + dt; { IntegrateBeginEvent evBegin; PropagateEventVisitor eventPropagation( this->params /* PARAMS FIRST */, &evBegin); eventPropagation.execute(node); } MechanicalBeginIntegrationVisitor beginVisitor(this->params /* PARAMS FIRST */, dt); node->execute(&beginVisitor); sofa::core::MechanicalParams m_mparams(*this->params); m_mparams.setDt(dt); #ifdef SOFA_HAVE_EIGEN2 { unsigned int constraintId=0; core::ConstraintParams cparams; //MechanicalAccumulateConstraint(&m_mparams /* PARAMS FIRST */, constraintId, VecCoordId::position()).execute(node); simulation::MechanicalAccumulateConstraint(&cparams /* PARAMS FIRST */, core::MatrixDerivId::holonomicC(),constraintId).execute(node); } #endif for( unsigned i=0; i<node->solver.size(); i++ ) { ctime_t t0 = begin(node, node->solver[i]); //cerr<<"AnimateVisitor::processNodeTpDown solver "<<node->solver[i]->getName()<<endl; node->solver[i]->solve(params /* PARAMS FIRST */, getDt()); end(node, node->solver[i], t0); } MechanicalPropagatePositionAndVelocityVisitor(&m_mparams /* PARAMS FIRST */, nextTime,VecCoordId::position(),VecDerivId::velocity(), #ifdef SOFA_SUPPORT_MAPPED_MASS VecDerivId::dx(), #endif true).execute( node ); MechanicalEndIntegrationVisitor endVisitor(this->params /* PARAMS FIRST */, dt); node->execute(&endVisitor); { IntegrateEndEvent evBegin; PropagateEventVisitor eventPropagation(this->params /* PARAMS FIRST */, &evBegin); eventPropagation.execute(node); } return RESULT_PRUNE; } /* if (node->mechanicalModel != NULL) { std::cerr << "Graph Error: MechanicalState without solver." << std::endl; return RESULT_PRUNE; } */ { // process InteractionForceFields for_each(this, node, node->interactionForceField, &AnimateVisitor::fwdInteractionForceField); return RESULT_CONTINUE; } }