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