void AnimateVisitor::processCollisionPipeline(simulation::Node* node, core::collision::Pipeline* obj)
{
sofa::helper::AdvancedTimer::stepBegin("Collision",obj);
sofa::helper::AdvancedTimer::stepBegin("begin collision",obj);
{
CollisionBeginEvent evBegin;
PropagateEventVisitor eventPropagation( params, &evBegin);
eventPropagation.execute(node->getContext());
}
sofa::helper::AdvancedTimer::stepEnd("begin collision",obj);
CollisionVisitor act(this->params);
node->execute(&act);
sofa::helper::AdvancedTimer::stepBegin("end collision",obj);
{
CollisionEndEvent evEnd;
PropagateEventVisitor eventPropagation( params, &evEnd);
eventPropagation.execute(node->getContext());
}
sofa::helper::AdvancedTimer::stepEnd("end collision",obj);
sofa::helper::AdvancedTimer::stepEnd("Collision",obj);
}
void CollisionAnimationLoop::integrate(const core::ExecParams* params, SReal dt)
{
{
IntegrateBeginEvent evBegin;
PropagateEventVisitor eventPropagation( params, &evBegin);
eventPropagation.execute(getContext());
}
MechanicalIntegrationVisitor act( params, dt );
act.setTags(this->getTags());
act.execute( getContext() );
{
IntegrateEndEvent evBegin;
PropagateEventVisitor eventPropagation( params, &evBegin);
eventPropagation.execute(getContext());
}
}
void CollisionAnimationLoop::computeCollision(const core::ExecParams* params)
{
dmsg_info() <<"CollisionAnimationLoop::computeCollision()" ;
{
CollisionBeginEvent evBegin;
PropagateEventVisitor eventPropagation( params, &evBegin);
eventPropagation.execute(getContext());
}
CollisionVisitor act(params);
act.setTags(this->getTags());
act.execute( getContext() );
{
CollisionEndEvent evEnd;
PropagateEventVisitor eventPropagation( params, &evEnd);
eventPropagation.execute(getContext());
}
}
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;
}
}
