void mainLoop()
{
while(1)
{
updateTime();
handleEvents();
if(network != NULL)
network->timepass();
tracker.timepass();
if (activeFrame)
activeFrame->timepass();
if(engine != NULL)
{
if(engine->controller)
engine->controller->timepass(getDt());
if(engine->view)
engine->view->timepass(getDt());
for(unsigned ii=0; ii < engine->aiPlayers.size(); ii++)
engine->aiPlayers[ii]->timepass(getDt());
}
drawScreen();
if(shouldDestroyEngine) {
if(engine) {
delete engine;
engine = NULL;
}
if(network) {
delete network;
network = NULL;
}
shouldDestroyEngine = false;
}
}
}
void ParticlePool::missileTrail(const Position &pos, float radius)
{
radius *= 0.5;
float entspeed = pos.getVelocityMagnitude();
for (int i = 0; i < 6; i++)
{
float randAngle = randFloat(M_PI-M_PI/60, M_PI+M_PI/60);
float speed = randFloat(1000.0, 1700.0);
float offset_time = randFloat(0, getDt()) * (entspeed-speed);
float spawnX = pos.getX() - (radius + offset_time) * -cos(pos.getR() + M_PI);
float spawnY = pos.getY() - (radius + offset_time) * sin(pos.getR() + M_PI);
float spawnVelX = speed * ( cos(pos.getR() + randAngle)) + pos.getX_vel();
float spawnVelY = speed * (-sin(pos.getR() + randAngle)) + pos.getY_vel();
Position spawnPos = Position(spawnX, spawnY);
spawnPos.setX_vel(spawnVelX);
spawnPos.setY_vel(spawnVelY);
Particle p = Particle(
255,
randInt(64, 215),
0,
randInt(128,255),
randFloat(0.16, 0.24),
spawnPos,
900);
add(p);
}
}
CompartmentLoader::CompartmentLoader( const URIHandler& params )
: EventSource( params )
, _impl( new CompartmentLoader::Impl( *this, params ))
{
if( getDt() < 0.f )
setDt( _impl->_report.getTimestep( ));
}
void Fireball::timepass()
{
Shot::timepass();
travelDistance -= position.getVelocityMagnitude() * getDt();
if(travelDistance < 0)
detonate();
}
void Healthmeter::draw()
{
/*
Color bgcol = Color(100,100,100,200);
glEnable(GL_POINT_SMOOTH);
graphics.drawPoint(bgcol, x, y, 64);
glDisable(GL_POINT_SMOOTH);
*/
if (tempLength > length) {
tempLength -= getDt()*0.5;
updateColor();
}
else if(tempLength != length) {
tempLength = length;
updateColor();
}
glColor4ub(100, 100, 100, 200);
graphics.drawColoredImage(img, x, y, 0, 1);
// update: by scaling .4 by health amount,
// the circle gets smaller as you die.
glColor3ub(r,g,b);
if(length>0)
graphics.drawColoredImage(img, x, y, 0, tempLength);
else { tempLength = 1.0;}
// old thing
//glColor3f(0,0,0);
//graphics.drawColoredImage(img, x, y, 0, 0.4 - 0.4*(currlength/maxlength));
}
void Godunov::MultiPhase(const double & CFL, unsigned phase) {
double t = 0;
Cell leftCell;
Cell rightCell;
bool phaseInteraction;
while (t < getMax()) {
BoundaryConditions();
for (unsigned i = 0; i < getCell().size(); ++i) {
phaseInteraction = false;
leftCell = getCell()[i % getCell().size()];
rightCell = getCell()[(i + 1) % getCell().size()];
if (i == getCell().size() - 2)
continue;
for (unsigned j = 0; j < phase; ++j)
if (leftCell.getPhase()[j].getPhi()
!= rightCell.getPhase()[j].getPhi()) {
phaseInteraction = true;
break;
}
if (!phaseInteraction) {
for (unsigned j = 0; j < phase; ++j)
detachedRiemann(leftCell, rightCell, i, j);
}
}
computeDT(CFL);
timeIntegration();
for (unsigned i = 0; i < getCell().size(); ++i) {
setCell()[i].computeScalFromCons();
}
t += getDt();
}
}
void AISteering::turnTo(const Position &destination)
{
float uncorrectedAngleDifference = getAngleDifference(destination);
float correctedAngleDifference = uncorrectedAngleDifference;
//angle corrections
while(correctedAngleDifference <= -M_PI)
correctedAngleDifference += 2*M_PI;
while(correctedAngleDifference >= M_PI)
correctedAngleDifference -= 2*M_PI;
//check the uncorrected angle to see if you're facing the target
//if not, then steer accordingly
if( abs(correctedAngleDifference) < getDt()*bot->type->getTurnSpeed() )
{
botPosition->setR(getHeading(*botPosition, destination));
bot->turnLeft(false);
bot->turnRight(false);
}
/* if(isFacing(destination))
{
bot->turnLeft(false);
bot->turnRight(false);
}*/
else if(correctedAngleDifference > 0)
{
bot->turnLeft(false);
bot->turnRight(true);
}
else
{
bot->turnLeft(true);
bot->turnRight(false);
}
}
VSDLoader::VSDLoader( const URIHandler& params )
: EventSource( params )
, _impl( new VSDLoader::Impl( *this, params ))
{
if( getDt() < 0.f )
setDt( _impl->_voltageReport.getTimestep( ));
}
SpikeLoader::SpikeLoader( const URIHandler& params )
: EventSource( params )
, _impl( new Impl( *this, params ))
{
if( getDt() < 0.f )
setDt( params.getConfig().getTimestep( ));
}
float BlockLowPass::update(float input)
{
float b = 2 * float(M_PI) * getFCut() * getDt();
float a = b / (1 + b);
setState(a * input + (1 - a)*getState());
return getState();
}
void HomingMissile::timepass(void)
{
BaseMissile::timepass();
if(target==-1)
return;
if(server->entities.find(target) == server->entities.end()) {
target=-1;
position.setR_vel(0.0);
return;
}
Entity *ent = server->entities[target];
Vector2D targetPosition = ent->getPosition().positionVector();
Vector2D relPosition = targetPosition - position.positionVector();
float targetHeading = atan2(-relPosition.y, relPosition.x);
float angleDifference = position.getR() - targetHeading;
while(angleDifference < -M_PI) angleDifference += 2*M_PI;
while(angleDifference > M_PI) angleDifference -= 2*M_PI;
if(abs(angleDifference) < missileTurnSpeed*getDt()) {
position.setR(targetHeading);
if(position.getR_vel() != 0.0)
position.setR_vel(0.0);
} else if(angleDifference > 0) {
position.setR_vel(-missileTurnSpeed);
} else {
position.setR_vel(missileTurnSpeed);
}
position.setX_vel( missileSpeed * cos(position.getR()) );
position.setY_vel( missileSpeed * -sin(position.getR()) );
}
void AnimateVisitor::processBehaviorModel(simulation::Node*, core::BehaviorModel* obj)
{
sofa::helper::AdvancedTimer::stepBegin("BehaviorModel",obj);
obj->updatePosition(getDt());
sofa::helper::AdvancedTimer::stepEnd("BehaviorModel",obj);
}
TestLoader::TestLoader( const URIHandler& params )
: EventSource( params )
, _impl( new TestLoader::Impl( *this ))
{
if( getDt() < 0.f )
setDt( 1.f );
}
float BlockIntegralTrap::update(float input)
{
// trapezoidal integration
setY(_limit.update(getY() +
(getU() + input) / 2.0f * getDt()));
setU(input);
return getY();
}
void Mine::timepass() {
if (isArmed) {
detonateTime -= getDt();
if (detonateTime < 0) detonate();
}
Entity::timepass();
}
float BlockHighPass::update(float input)
{
float b = 2 * float(M_PI) * getFCut() * getDt();
float a = 1 / (1 + b);
setY(a * (getY() + input - getU()));
setU(input);
return getY();
}
void BlockLocalPositionEstimator::predict()
{
// if can't update anything, don't propagate
// state or covariance
if (!_validXY && !_validZ) { return; }
if (integrate) {
matrix::Quaternion<float> q(&_sub_att.get().q[0]);
_eul = matrix::Euler<float>(q);
_R_att = matrix::Dcm<float>(q);
Vector3f a(_sub_sensor.get().accelerometer_m_s2);
// note, bias is removed in dynamics function
_u = _R_att * a;
_u(U_az) += 9.81f; // add g
} else {
_u = Vector3f(0, 0, 0);
}
// update state space based on new states
updateSSStates();
// continuous time kalman filter prediction
// integrate runge kutta 4th order
// TODO move rk4 algorithm to matrixlib
// https://en.wikipedia.org/wiki/Runge%E2%80%93Kutta_methods
float h = getDt();
Vector<float, n_x> k1, k2, k3, k4;
k1 = dynamics(0, _x, _u);
k2 = dynamics(h / 2, _x + k1 * h / 2, _u);
k3 = dynamics(h / 2, _x + k2 * h / 2, _u);
k4 = dynamics(h, _x + k3 * h, _u);
Vector<float, n_x> dx = (k1 + k2 * 2 + k3 * 2 + k4) * (h / 6);
// propagate
correctionLogic(dx);
_x += dx;
Matrix<float, n_x, n_x> dP = (_A * _P + _P * _A.transpose() +
_B * _R * _B.transpose() + _Q) * getDt();
covPropagationLogic(dP);
_P += dP;
_xLowPass.update(_x);
_aglLowPass.update(agl());
}
void AnimateVisitor::processOdeSolver(simulation::Node* node, core::behavior::OdeSolver* solver)
{
sofa::helper::AdvancedTimer::stepBegin("Mechanical",node);
/* MechanicalIntegrationVisitor act(getDt());
node->execute(&act);*/
// cerr<<"AnimateVisitor::processOdeSolver "<<solver->getName()<<endl;
solver->solve(params, getDt());
sofa::helper::AdvancedTimer::stepEnd("Mechanical",node);
}
//position class takes care of movement
//have to manually update the colors
void ParticlePool::timepass()
{
for(ParticleList::iterator ii = particles.begin(); ii != particles.end(); ++ii)
{
ii->alpha -= ii->fadeSpeed * getDt();
if (ii->alpha <= 0.0f)
{
ii = particles.erase(ii);
--ii;
}
}
}
matrix::Vector<Type, M> update(const matrix::Matrix<Type, M, 1> &input)
{
for (size_t i = 0; i < M; i++) {
if (!PX4_ISFINITE(getState()(i))) {
setState(input);
}
}
float b = 2 * float(M_PI) * getFCut() * getDt();
float a = b / (1 + b);
setState(input * a + getState() * (1 - a));
return getState();
}
void Receiver1D::writeToRsfFile(const std::string& rsf_filename) const {
TRACE() << "write receiver data to rsf file: " << rsf_filename;
RsfDesc rsfDesc;
rsfDesc.setD1(getDt());
rsfDesc.setD2(getGeometryDelta()[0]);
rsfDesc.setO1(0);
rsfDesc.setO2(0);
rsfDesc.setN1(getNt());
rsfDesc.setN2(getGeometryDim()[0]);
rsfDesc.setLabel1("Time");
rsfDesc.setUnit1("s");
ReadWrite::writeToRsfFile(rsf_filename, rsfDesc, mData);
}
void Shot::timepass(void)
{
Entity::timepass();
age += getDt();
//kill the shot before it gets to the edge
const int borderPadding = 10;
if(position.getX()>rightBorder - borderPadding ||
position.getX()<leftBorder + borderPadding ||
position.getY()>bottomBorder - borderPadding ||
position.getY()<topBorder + borderPadding)
{
deleteMe();
}
}
float BlockDerivative::update(float input)
{
float output;
if (_initialized) {
output = _lowPass.update((input - getU()) / getDt());
} else {
// if this is the first call to update
// we have no valid derivative
// and so we use the assumption the
// input value is not changing much,
// which is the best we can do here.
output = 0.0f;
_initialized = true;
}
setU(input);
return output;
}
void Godunov::SinglePhase(const double & CFL) {
double t = 0;
while (t < getMax()) {
BoundaryConditions();
for (unsigned i = 0; i < getCell().size(); ++i) {
if (i == getCell().size() - 2)
continue;
RiemannSolver riemann(getCell()[i % getCell().size()],
getCell()[(i + 1) % getCell().size()], 1);
riemann.solve();
riemann.phase_star_[0].Sample(0, setCell()[i].setPhase()[0].setU_half(),
setCell()[i].setPhase()[0].setP_half(),
setCell()[i].setPhase()[0].setD_half(), riemann.phase_star_[0].pstar[0]
, riemann.phase_star_[0].ustar[0]);
setCell()[i].setPhase()[0].InterCellFlux();
}
computeDT(CFL);
timeIntegration();
for (unsigned i = 0; i < getCell().size(); ++i) {
setCell()[i].setPhase()[0].computeScalFromCons();
}
t += getDt();
}
}
bool
StaggeredStokesProjectionPreconditioner::solveSystem(SAMRAIVectorReal<NDIM, double>& x,
SAMRAIVectorReal<NDIM, double>& b)
{
IBAMR_TIMER_START(t_solve_system);
// Initialize the solver (if necessary).
const bool deallocate_at_completion = !d_is_initialized;
if (!d_is_initialized) initializeSolverState(x, b);
// Determine whether we are solving a steady-state problem.
const bool steady_state =
d_U_problem_coefs.cIsZero() ||
(d_U_problem_coefs.cIsConstant() && MathUtilities<double>::equalEps(d_U_problem_coefs.getCConstant(), 0.0));
// Get the vector components.
const int F_U_idx = b.getComponentDescriptorIndex(0);
const int F_P_idx = b.getComponentDescriptorIndex(1);
const Pointer<Variable<NDIM> >& F_U_var = b.getComponentVariable(0);
const Pointer<Variable<NDIM> >& F_P_var = b.getComponentVariable(1);
Pointer<SideVariable<NDIM, double> > F_U_sc_var = F_U_var;
Pointer<CellVariable<NDIM, double> > F_P_cc_var = F_P_var;
const int U_idx = x.getComponentDescriptorIndex(0);
const int P_idx = x.getComponentDescriptorIndex(1);
const Pointer<Variable<NDIM> >& U_var = x.getComponentVariable(0);
const Pointer<Variable<NDIM> >& P_var = x.getComponentVariable(1);
Pointer<SideVariable<NDIM, double> > U_sc_var = U_var;
Pointer<CellVariable<NDIM, double> > P_cc_var = P_var;
// Setup the component solver vectors.
Pointer<SAMRAIVectorReal<NDIM, double> > F_U_vec;
F_U_vec = new SAMRAIVectorReal<NDIM, double>(d_object_name + "::F_U", d_hierarchy, d_coarsest_ln, d_finest_ln);
F_U_vec->addComponent(F_U_sc_var, F_U_idx, d_velocity_wgt_idx, d_velocity_data_ops);
Pointer<SAMRAIVectorReal<NDIM, double> > U_vec;
U_vec = new SAMRAIVectorReal<NDIM, double>(d_object_name + "::U", d_hierarchy, d_coarsest_ln, d_finest_ln);
U_vec->addComponent(U_sc_var, U_idx, d_velocity_wgt_idx, d_velocity_data_ops);
Pointer<SAMRAIVectorReal<NDIM, double> > Phi_scratch_vec;
Phi_scratch_vec =
new SAMRAIVectorReal<NDIM, double>(d_object_name + "::Phi_scratch", d_hierarchy, d_coarsest_ln, d_finest_ln);
Phi_scratch_vec->addComponent(d_Phi_var, d_Phi_scratch_idx, d_pressure_wgt_idx, d_pressure_data_ops);
Pointer<SAMRAIVectorReal<NDIM, double> > F_Phi_vec;
F_Phi_vec = new SAMRAIVectorReal<NDIM, double>(d_object_name + "::F_Phi", d_hierarchy, d_coarsest_ln, d_finest_ln);
F_Phi_vec->addComponent(d_F_Phi_var, d_F_Phi_idx, d_pressure_wgt_idx, d_pressure_data_ops);
Pointer<SAMRAIVectorReal<NDIM, double> > P_vec;
P_vec = new SAMRAIVectorReal<NDIM, double>(d_object_name + "::P", d_hierarchy, d_coarsest_ln, d_finest_ln);
P_vec->addComponent(P_cc_var, P_idx, d_pressure_wgt_idx, d_pressure_data_ops);
// Allocate scratch data.
for (int ln = d_coarsest_ln; ln <= d_finest_ln; ++ln)
{
Pointer<PatchLevel<NDIM> > level = d_hierarchy->getPatchLevel(ln);
level->allocatePatchData(d_Phi_scratch_idx);
level->allocatePatchData(d_F_Phi_idx);
}
// (1) Solve the velocity sub-problem for an initial approximation to U.
//
// U^* := inv(rho/dt - K*mu*L) F_U
//
// An approximate Helmholtz solver is used.
d_velocity_solver->setHomogeneousBc(true);
LinearSolver* p_velocity_solver = dynamic_cast<LinearSolver*>(d_velocity_solver.getPointer());
if (p_velocity_solver) p_velocity_solver->setInitialGuessNonzero(false);
d_velocity_solver->solveSystem(*U_vec, *F_U_vec);
// (2) Solve the pressure sub-problem.
//
// We treat two cases:
//
// (i) rho/dt = 0.
//
// In this case,
//
// U - U^* + G Phi = 0
// -D U = F_P
//
// so that
//
// Phi := inv(-L_p) * F_Phi = inv(-L_p) * (-F_P - D U^*)
// P := -K*mu*F_Phi
//
// in which L_p = D*G.
//
// (ii) rho/dt != 0.
//
// In this case,
//
// rho (U - U^*) + G Phi = 0
// -D U = F_P
//
// so that
//
// Phi := inv(-L_rho) * F_phi = inv(-L_rho) * (-F_P - D U^*)
// P := (1/dt - K*mu*L_rho)*Phi = (1/dt) Phi - K*mu*F_phi
//
// in which L_rho = D*(1/rho)*G.
//
// Approximate Poisson solvers are used in both cases.
d_hier_math_ops->div(d_F_Phi_idx,
d_F_Phi_var,
-1.0,
U_idx,
U_sc_var,
d_no_fill_op,
d_new_time,
/*cf_bdry_synch*/ true,
-1.0,
F_P_idx,
F_P_cc_var);
d_pressure_solver->setHomogeneousBc(true);
LinearSolver* p_pressure_solver = dynamic_cast<LinearSolver*>(d_pressure_solver.getPointer());
p_pressure_solver->setInitialGuessNonzero(false);
d_pressure_solver->solveSystem(*Phi_scratch_vec, *F_Phi_vec);
if (steady_state)
{
d_pressure_data_ops->scale(P_idx, -d_U_problem_coefs.getDConstant(), d_F_Phi_idx);
}
else
{
d_pressure_data_ops->linearSum(
P_idx, 1.0 / getDt(), d_Phi_scratch_idx, -d_U_problem_coefs.getDConstant(), d_F_Phi_idx);
}
// (3) Evaluate U in terms of U^* and Phi.
//
// We treat two cases:
//
// (i) rho = 0. In this case,
//
// U = U^* - G Phi
//
// (ii) rho != 0. In this case,
//
// U = U^* - (1.0/rho) G Phi
double coef;
if (steady_state)
{
coef = -1.0;
}
else
{
coef = d_P_problem_coefs.getDConstant();
}
d_hier_math_ops->grad(U_idx,
U_sc_var,
/*cf_bdry_synch*/ true,
coef,
d_Phi_scratch_idx,
d_Phi_var,
d_Phi_bdry_fill_op,
d_pressure_solver->getSolutionTime(),
1.0,
U_idx,
U_sc_var);
// Account for nullspace vectors.
correctNullspace(U_vec, P_vec);
// Deallocate scratch data.
for (int ln = d_coarsest_ln; ln <= d_finest_ln; ++ln)
{
Pointer<PatchLevel<NDIM> > level = d_hierarchy->getPatchLevel(ln);
level->deallocatePatchData(d_Phi_scratch_idx);
level->deallocatePatchData(d_F_Phi_idx);
}
// Deallocate the solver (if necessary).
if (deallocate_at_completion) deallocateSolverState();
IBAMR_TIMER_STOP(t_solve_system);
return true;
} // solveSystem
void BlockLocalPositionEstimator::predict()
{
// get acceleration
matrix::Quatf q(&_sub_att.get().q[0]);
_eul = matrix::Euler<float>(q);
_R_att = matrix::Dcm<float>(q);
Vector3f a(_sub_sensor.get().accelerometer_m_s2);
// note, bias is removed in dynamics function
_u = _R_att * a;
_u(U_az) += 9.81f; // add g
// update state space based on new states
updateSSStates();
// continuous time kalman filter prediction
// integrate runge kutta 4th order
// TODO move rk4 algorithm to matrixlib
// https://en.wikipedia.org/wiki/Runge%E2%80%93Kutta_methods
float h = getDt();
Vector<float, n_x> k1, k2, k3, k4;
k1 = dynamics(0, _x, _u);
k2 = dynamics(h / 2, _x + k1 * h / 2, _u);
k3 = dynamics(h / 2, _x + k2 * h / 2, _u);
k4 = dynamics(h, _x + k3 * h, _u);
Vector<float, n_x> dx = (k1 + k2 * 2 + k3 * 2 + k4) * (h / 6);
// don't integrate position if no valid xy data
if (!(_estimatorInitialized & EST_XY)) {
dx(X_x) = 0;
dx(X_vx) = 0;
dx(X_y) = 0;
dx(X_vy) = 0;
}
// don't integrate z if no valid z data
if (!(_estimatorInitialized & EST_Z)) {
dx(X_z) = 0;
}
// don't integrate tz if no valid tz data
if (!(_estimatorInitialized & EST_TZ)) {
dx(X_tz) = 0;
}
// saturate bias
float bx = dx(X_bx) + _x(X_bx);
float by = dx(X_by) + _x(X_by);
float bz = dx(X_bz) + _x(X_bz);
if (std::abs(bx) > BIAS_MAX) {
bx = BIAS_MAX * bx / std::abs(bx);
dx(X_bx) = bx - _x(X_bx);
}
if (std::abs(by) > BIAS_MAX) {
by = BIAS_MAX * by / std::abs(by);
dx(X_by) = by - _x(X_by);
}
if (std::abs(bz) > BIAS_MAX) {
bz = BIAS_MAX * bz / std::abs(bz);
dx(X_bz) = bz - _x(X_bz);
}
// propagate
_x += dx;
Matrix<float, n_x, n_x> dP = (_A * _P + _P * _A.transpose() +
_B * _R * _B.transpose() + _Q) * getDt();
// covariance propagation logic
for (int i = 0; i < n_x; i++) {
if (_P(i, i) > P_MAX) {
// if diagonal element greater than max, stop propagating
dP(i, i) = 0;
for (int j = 0; j < n_x; j++) {
dP(i, j) = 0;
dP(j, i) = 0;
}
}
}
_P += dP;
_xLowPass.update(_x);
_aglLowPass.update(agl());
}
float BlockIntegral::update(float input)
{
// trapezoidal integration
setY(_limit.update(getY() + input * getDt()));
return getY();
}
float BlockDerivative::update(float input)
{
float output = _lowPass.update((input - getU()) / getDt());
setU(input);
return output;
}
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;
}
}
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;
}
}
