void SchatzmanPaoliOSI::initialize()
{
OneStepIntegrator::initialize();
// Get initial time
double t0 = simulationLink->model()->t0();
// Compute W(t0) for all ds
ConstDSIterator itDS;
for (itDS = OSIDynamicalSystems->begin(); itDS != OSIDynamicalSystems->end(); ++itDS)
{
Type::Siconos dsType = Type::value(*(*itDS));
if (dsType == Type::LagrangianLinearTIDS)
{
// Computation of the first step for starting
SP::LagrangianLinearTIDS d = std11::static_pointer_cast<LagrangianLinearTIDS> (*itDS);
SP::SiconosVector q0 = d->q0();
SP::SiconosVector q = d->q();
SP::SiconosVector v0 = d->velocity0();
SP::SiconosVector velocity = d->velocity();
// std::cout << " q0 = " << std::endl;
// q0->display();
// std::cout << " v0 = " << std::endl;
// v0->display();
// We first swap the initial value contained in q and v after initialization.
d->qMemory()->swap(*q);
d->velocityMemory()->swap(*velocity);
// we compute the new state values
double h = simulationLink->timeStep();
*q = *q0 + h* * v0;
//*velocity=*velocity; we do nothing for the velocity
// This value will swapped when OneStepIntegrator::saveInMemory will be called
// by the rest of Simulation::initialize (_eventsManager->preUpdate();)
// SP::SiconosVector qprev = d->qMemory()->getSiconosVector(0);
// SP::SiconosVector qprev2 = d->qMemory()->getSiconosVector(1);
// SP::SiconosVector vprev = d->velocityMemory()->getSiconosVector(0);
// std::cout << " qprev = " << std::endl;
// qprev->display();
// std::cout << " qprev2 = " << std::endl;
// qprev2->display();
// std::cout << " vprev = " << std::endl;
// vprev->display();
}
// Memory allocation for workX. workX[ds*] corresponds to xfree (or vfree in lagrangian case).
// workX[*itDS].reset(new SiconosVector((*itDS)->getDim()));
// W initialization
initW(t0, *itDS);
// if ((*itDS)->getType() == Type::LagrangianDS || (*itDS)->getType() == Type::FirstOrderNonLinearDS)
(*itDS)->allocateWorkVector(DynamicalSystem::local_buffer, WMap[(*itDS)->number()]->size(0));
}
}
void SchatzmanPaoliOSI::initializeWorkVectorsForDS( double t, SP::DynamicalSystem ds)
{
DEBUG_BEGIN("SchatzmanPaoliOSI::initializeWorkVectorsForDS( double t, SP::DynamicalSystem ds)\n");
// Get work buffers from the graph
VectorOfVectors& ds_work_vectors = *_initializeDSWorkVectors(ds);
// Check dynamical system type
Type::Siconos dsType = Type::value(*ds);
assert(dsType == Type::LagrangianLinearTIDS);
if(dsType == Type::LagrangianLinearTIDS)
{
SP::LagrangianLinearTIDS lltids = std11::static_pointer_cast<LagrangianLinearTIDS> (ds);
// buffers allocation (inside the graph)
ds_work_vectors.resize(SchatzmanPaoliOSI::WORK_LENGTH);
ds_work_vectors[SchatzmanPaoliOSI::RESIDU_FREE].reset(new SiconosVector(lltids->dimension()));
ds_work_vectors[SchatzmanPaoliOSI::FREE].reset(new SiconosVector(lltids->dimension()));
ds_work_vectors[SchatzmanPaoliOSI::LOCAL_BUFFER].reset(new SiconosVector(lltids->dimension()));
SP::SiconosVector q0 = lltids->q0();
SP::SiconosVector q = lltids->q();
SP::SiconosVector v0 = lltids->velocity0();
SP::SiconosVector velocity = lltids->velocity();
// We first swap the initial value contained in q and v after initialization.
lltids->swapInMemory();
// we compute the new state values
double h = _simulation->timeStep();
*q = *q0 + h* * v0;
//*velocity=*velocity; we do nothing for the velocity
lltids->swapInMemory();
}
// W initialization
initializeIterationMatrixW(t, ds);
for (unsigned int k = _levelMinForInput ; k < _levelMaxForInput + 1; k++)
{
ds->initializeNonSmoothInput(k);
}
// if ((*itDS)->getType() == Type::LagrangianDS || (*itDS)->getType() == Type::FirstOrderNonLinearDS)
DEBUG_EXPR(ds->display());
DEBUG_END("SchatzmanPaoliOSI::initializeWorkVectorsForDS( double t, SP::DynamicalSystem ds)\n");
}
void SchatzmanPaoliOSI::initialize(Model& m)
{
OneStepIntegrator::initialize(m);
// Get initial time
double t0 = _simulation->startingTime();
// Compute W(t0) for all ds
DynamicalSystemsGraph::VIterator dsi, dsend;
for (std11::tie(dsi, dsend) = _dynamicalSystemsGraph->vertices(); dsi != dsend; ++dsi)
{
if (!checkOSI(dsi)) continue;
SP::DynamicalSystem ds = _dynamicalSystemsGraph->bundle(*dsi);
Type::Siconos dsType = Type::value(*ds);
if (dsType == Type::LagrangianLinearTIDS)
{
// Computation of the first step for starting
SP::LagrangianLinearTIDS d = std11::static_pointer_cast<LagrangianLinearTIDS> (ds);
SP::SiconosVector q0 = d->q0();
SP::SiconosVector q = d->q();
SP::SiconosVector v0 = d->velocity0();
SP::SiconosVector velocity = d->velocity();
// std::cout << " q0 = " << std::endl;
// q0->display();
// std::cout << " v0 = " << std::endl;
// v0->display();
// We first swap the initial value contained in q and v after initialization.
d->qMemory()->swap(*q);
d->velocityMemory()->swap(*velocity);
// we compute the new state values
double h = _simulation->timeStep();
*q = *q0 + h* * v0;
//*velocity=*velocity; we do nothing for the velocity
// This value will swapped when OneStepIntegrator::saveInMemory will be called
// by the rest of Simulation::initialize (_eventsManager->preUpdate();)
// SP::SiconosVector qprev = d->qMemory()->getSiconosVector(0);
// SP::SiconosVector qprev2 = d->qMemory()->getSiconosVector(1);
// SP::SiconosVector vprev = d->velocityMemory()->getSiconosVector(0);
// std::cout << " qprev = " << std::endl;
// qprev->display();
// std::cout << " qprev2 = " << std::endl;
// qprev2->display();
// std::cout << " vprev = " << std::endl;
// vprev->display();
}
// Memory allocation for workX. workX[ds*] corresponds to xfree (or vfree in lagrangian case).
// workX[*itDS].reset(new SiconosVector((*itDS)->dimension()));
// W initialization
initW(t0, ds, *dsi);
// if ((*itDS)->getType() == Type::LagrangianDS || (*itDS)->getType() == Type::FirstOrderNonLinearDS)
ds->allocateWorkVector(DynamicalSystem::local_buffer,_dynamicalSystemsGraph->properties(*dsi).W->size(0));
}
}