void EventDriven::computeJacobianfx(OneStepIntegrator& osi,
integer *sizeOfX,
doublereal *time,
doublereal *x,
doublereal *jacob)
{
assert((osi.getType() == OSI::LSODAROSI) &&
"EventDriven::computeJacobianfx(osi, ...), not yet implemented for a one step integrator of type " + osi.getType());
LsodarOSI& lsodar = static_cast<LsodarOSI&>(osi);
// Remark A: according to DLSODAR doc, each call to jacobian is
// preceded by a call to f with the same arguments NEQ, T, and Y.
// Thus to gain some efficiency, intermediate quantities shared by
// both calculations may be saved in class members? fill in xWork
// vector (ie all the x of the ds of this osi) with x fillXWork(x);
// -> copy
// Maybe this step is not necessary? because of
// remark A above
// Compute the jacobian of the vector field according to x for the
// current ds
double t = *time;
lsodar.computeJacobianRhs(t, *_DSG0);
// Save jacobianX values from dynamical system into current jacob
// (in-out parameter)
unsigned int i = 0;
unsigned pos = 0;
DynamicalSystemsGraph::VIterator dsi, dsend;
SP::DynamicalSystemsGraph osiDSGraph = lsodar.dynamicalSystemsGraph();
for (std11::tie(dsi, dsend) = osiDSGraph->vertices(); dsi != dsend; ++dsi)
{
if (!(lsodar.checkOSI(dsi))) continue;
DynamicalSystem& ds = *(osiDSGraph->bundle(*dsi));
Type::Siconos dsType = Type::value(ds);
if (dsType == Type::LagrangianDS || dsType == Type::LagrangianLinearTIDS)
{
LagrangianDS& lds = static_cast<LagrangianDS&>(ds);
BlockMatrix& jacotmp = *lds.jacobianRhsx();
for (unsigned int j = 0; j < lds.n(); ++j)
{
for (unsigned int k = 0; k < lds.dimension(); ++k)
jacob[i++] = jacotmp(k, j);
}
}
else if(dsType == Type::FirstOrderNonLinearDS || dsType == Type::FirstOrderLinearDS
|| dsType == Type::FirstOrderLinearTIDS)
{
SimpleMatrix& jacotmp = static_cast<SimpleMatrix&>(*(ds.jacobianRhsx())); // Pointer link !
pos += jacotmp.copyData(&jacob[pos]);
}
else
{
RuntimeException::selfThrow("EventDriven::computeJacobianfx, type of DynamicalSystem not yet supported.");
}
}
}
void EventDriven::initOSIs()
{
for (OSIIterator itosi = _allOSI->begin(); itosi != _allOSI->end(); ++itosi)
{
// Initialize the acceleration like for NewMarkAlphaScheme
if ((*itosi)->getType() == OSI::NEWMARKALPHAOSI)
{
SP::NewMarkAlphaOSI osi_NewMark = std11::static_pointer_cast<NewMarkAlphaOSI>(*itosi);
DynamicalSystemsGraph::VIterator dsi, dsend;
SP::DynamicalSystemsGraph osiDSGraph = (*itosi)->dynamicalSystemsGraph();
for (std11::tie(dsi, dsend) = osiDSGraph->vertices(); dsi != dsend; ++dsi)
{
if (!(*itosi)->checkOSI(dsi)) continue;
SP::DynamicalSystem ds = osiDSGraph->bundle(*dsi);
if ((Type::value(*ds) == Type::LagrangianDS) || (Type::value(*ds) == Type::LagrangianLinearTIDS))
{
SP::LagrangianDS d = std11::static_pointer_cast<LagrangianDS>(ds);
*(d->workspace(DynamicalSystem::acce_like)) = *(d->acceleration()); // set a0 = ddotq0
// Allocate the memory to stock coefficients of the polynomial for the dense output
d->allocateWorkMatrix(LagrangianDS::coeffs_denseoutput, ds->dimension(), (osi_NewMark->getOrderDenseOutput() + 1));
}
}
}
}
}
std::vector<SP::DynamicalSystem> dynamicalSystems(SP::DynamicalSystemsGraph dsg)
{
std::vector<SP::DynamicalSystem> r = std::vector<SP::DynamicalSystem>();
DynamicalSystemsGraph::VIterator vi, viend;
for (boost::tie(vi, viend) = dsg->vertices(); vi != viend; ++vi)
{
r.push_back(dsg->bundle(*vi));
};
return r;
};
void TimeStepping::initializeNewtonLoop()
{
DEBUG_BEGIN("TimeStepping::initializeNewtonLoop()\n");
double tkp1 = getTkp1();
assert(!isnan(tkp1));
for (OSIIterator it = _allOSI->begin(); it != _allOSI->end() ; ++it)
{
(*it)->computeInitialNewtonState();
(*it)->computeResidu();
}
// Predictive contact -- update initial contacts after updating DS positions
updateWorldFromDS();
updateInteractions();
// Changes in updateInteractions may require initialization
initializeNSDSChangelog();
SP::InteractionsGraph indexSet0 = _nsds->topology()->indexSet0();
if (indexSet0->size()>0)
{
for (OSIIterator itOSI = _allOSI->begin(); itOSI != _allOSI->end() ; ++itOSI)
{
(*itOSI)->updateOutput(nextTime());
(*itOSI)->updateInput(nextTime());
}
}
SP::DynamicalSystemsGraph dsGraph = _nsds->dynamicalSystems();
for (DynamicalSystemsGraph::VIterator vi = dsGraph->begin(); vi != dsGraph->end(); ++vi)
{
dsGraph->bundle(*vi)->updatePlugins(tkp1);
}
for (OSIIterator it = _allOSI->begin(); it != _allOSI->end() ; ++it)
(*it)->computeResidu();
if (_computeResiduY)
{
for (OSIIterator itOSI = _allOSI->begin(); itOSI != _allOSI->end() ; ++itOSI)
{
(*itOSI)->computeResiduOutput(tkp1, indexSet0);
}
}
DEBUG_END("TimeStepping::initializeNewtonLoop()\n");
}
std::vector<std::pair<unsigned int, unsigned int> >
graphLayoutInt(SP::DynamicalSystemsGraph dsg)
{
std::vector<std::pair<unsigned int, unsigned int> > r =
std::vector<std::pair<unsigned int, unsigned int> >();
DynamicalSystemsGraph::EIterator ei, eiend;
for (boost::tie(ei, eiend) = dsg->edges(); ei != eiend; ++ei)
{
std::pair<unsigned int, unsigned int>
p(dsg->bundle(dsg->source(*ei))->number(),
dsg->bundle(dsg->target(*ei))->number());
r.push_back(p);
};
return r;
};
void Simulation::processEvents()
{
_eventsManager->processEvents(*this);
if (_eventsManager->hasNextEvent())
{
// For TimeStepping Scheme, need to update IndexSets, but not for EventDriven scheme
if (Type::value(*this) != Type::EventDriven)
{
updateIndexSets();
}
}
/* should be evaluated only if needed */
SP::DynamicalSystemsGraph dsGraph = _nsds->dynamicalSystems();
for (DynamicalSystemsGraph::VIterator vi = dsGraph->begin(); vi != dsGraph->end(); ++vi)
{
dsGraph->bundle(*vi)->endStep();
}
}
void EventDriven::initOSIRhs()
{
// === initialization for OneStepIntegrators ===
OSI::TYPES osiType = (*_allOSI->begin())->getType();
for (OSIIterator itosi = _allOSI->begin(); itosi != _allOSI->end(); ++itosi)
{
//Check whether OSIs used are of the same type
if ((*itosi)->getType() != osiType)
RuntimeException::selfThrow("OSIs used must be of the same type");
// perform the initialization
DynamicalSystemsGraph::VIterator dsi, dsend;
SP::DynamicalSystemsGraph osiDSGraph = (*itosi)->dynamicalSystemsGraph();
for (std11::tie(dsi, dsend) = osiDSGraph->vertices(); dsi != dsend; ++dsi)
{
if (!(*itosi)->checkOSI(dsi)) continue;
SP::DynamicalSystem ds = osiDSGraph->bundle(*dsi);
// Initialize right-hand side
ds->initRhs(startingTime());
}
}
}
void Simulation::initialize(SP::Model m, bool withOSI)
{
// === Connection with the model ===
assert(m && "Simulation::initialize(model) - model = NULL.");
_model = std11::weak_ptr<Model>(m);
_T = m->finalT();
// === Events manager initialization ===
_eventsManager->initialize(_T);
_tinit = _eventsManager->startingTime();
//===
if (withOSI)
{
if (numberOfOSI() == 0)
RuntimeException::selfThrow("Simulation::initialize No OSI !");
// === OneStepIntegrators initialization ===
for (OSIIterator itosi = _allOSI->begin();
itosi != _allOSI->end(); ++itosi)
{
for (DSIterator itds = (*itosi)->dynamicalSystems()->begin();
itds != (*itosi)->dynamicalSystems()->end();
++itds)
{
(*itds)->initialize(model()->t0(),
(*itosi)->getSizeMem());
addInOSIMap(*itds, *itosi);
}
(*itosi)->setSimulationPtr(shared_from_this());
(*itosi)->initialize();
}
}
// This is the default
_levelMinForInput = LEVELMAX;
_levelMaxForInput = 0;
_levelMinForOutput = LEVELMAX;
_levelMaxForOutput = 0;
computeLevelsForInputAndOutput();
// Loop over all DS in the graph, to reset NS part of each DS.
// Note FP : this was formerly done in inter->initialize call with local levels values
// but I think it's ok (better?) to do it with the simulation levels values.
DynamicalSystemsGraph::VIterator dsi, dsend;
SP::DynamicalSystemsGraph DSG = model()->nonSmoothDynamicalSystem()->topology()->dSG(0);
for (std11::tie(dsi, dsend) = DSG->vertices(); dsi != dsend; ++dsi)
{
//assert(_levelMinForInput <= _levelMaxForInput);
for (unsigned int k = _levelMinForInput ; k < _levelMaxForInput + 1; k++)
{
DSG->bundle(*dsi)->initializeNonSmoothInput(k);
}
}
InteractionsGraph::VIterator ui, uiend;
SP::InteractionsGraph indexSet0 = model()->nonSmoothDynamicalSystem()->topology()->indexSet0();
for (std11::tie(ui, uiend) = indexSet0->vertices(); ui != uiend; ++ui)
{
Interaction& inter = *indexSet0->bundle(*ui);
inter.initialize(_tinit, indexSet0->properties(*ui));
}
// Initialize OneStepNSProblem(s). Depends on the type of simulation.
// Warning FP : must be done in any case, even if the interactions set
// is empty.
initOSNS();
// Process events at time _tinit. Useful to save values in memories
// for example. Warning: can not be called during
// eventsManager->initialize, because it needs the initialization of
// OSI, OSNS ...
_eventsManager->preUpdate(*this);
_tend = _eventsManager->nextTime();
// Set Model current time (warning: current time of the model
// corresponds to the time of the next event to be treated).
model()->setCurrentTime(nextTime());
// End of initialize:
// - all OSI and OSNS (ie DS and Interactions) states are computed
// - for time _tinit and saved into memories.
// - Sensors or related objects are updated for t=_tinit.
// - current time of the model is equal to t1, time of the first
// - event after _tinit.
// - currentEvent of the simu. corresponds to _tinit and nextEvent
// - to _tend.
// If _printStat is true, open output file.
if (_printStat)
{
statOut.open("simulationStat.dat", std::ios::out | std::ios::trunc);
if (!statOut.is_open())
SiconosVectorException::selfThrow("writing error : Fail to open file simulationStat.dat ");
statOut << "============================================" <<std::endl;
statOut << " Siconos Simulation of type " << Type::name(*this) << "." <<std::endl;
statOut <<std::endl;
statOut << "The tolerance parameter is equal to: " << _tolerance <<std::endl;
statOut <<std::endl <<std::endl;
}
}
void EventDriven::computef(OneStepIntegrator& osi, integer * sizeOfX, doublereal * time, doublereal * x, doublereal * xdot)
{
// computeF is supposed to fill xdot in, using the definition of the
// dynamical systems belonging to the osi
// Check osi type: only lsodar is allowed.
assert((osi.getType() == OSI::LSODAROSI) && "EventDriven::computef(osi, ...), not yet implemented for a one step integrator of type " + osi.getType());
LsodarOSI& lsodar = static_cast<LsodarOSI&>(osi);
// fill in xWork vector (ie all the x of the ds of this osi) with x
lsodar.fillXWork(sizeOfX, x);
double t = *time;
// Update Jacobian matrices at all interactions
InteractionsGraph::VIterator ui, uiend;
for (std11::tie(ui, uiend) = _indexSet0->vertices(); ui != uiend; ++ui)
{
Interaction& inter = *_indexSet0->bundle(*ui);
inter.relation()->computeJach(t, inter, _indexSet0->properties(*ui));
}
// solve a LCP at "acceleration" level if required
if (!_allNSProblems->empty())
{
if (((*_allNSProblems)[SICONOS_OSNSP_ED_SMOOTH_ACC]->hasInteractions()))
{
// Update the state of the DS
(*_allNSProblems)[SICONOS_OSNSP_ED_SMOOTH_ACC]->compute(t);
_nsds->updateInput(t,2); // Necessary to compute DS state below
}
// Compute the right-hand side ( xdot = f + r in DS) for all the
//ds, with the new value of input. lsodar->computeRhs(t);
}
// update the DS of the OSI.
lsodar.computeRhs(t, *_DSG0);
// for the DS state, ie the ones computed by lsodar (x above)
// Update Index sets? No !!
// Get the required value, ie xdot for output.
unsigned pos = 0;
DynamicalSystemsGraph::VIterator dsi, dsend;
SP::DynamicalSystemsGraph osiDSGraph = lsodar.dynamicalSystemsGraph();
for (std11::tie(dsi, dsend) = osiDSGraph->vertices(); dsi != dsend; ++dsi)
{
if (!(lsodar.checkOSI(dsi))) continue;
DynamicalSystem& ds = *(osiDSGraph->bundle(*dsi));
Type::Siconos dsType = Type::value(ds);
if (dsType == Type::LagrangianDS || dsType == Type::LagrangianLinearTIDS)
{
LagrangianDS& LDS = static_cast<LagrangianDS&>(ds);
SiconosVector& qDotTmp = *LDS.velocity();
SiconosVector& qDotDotTmp = *LDS.acceleration();
pos += qDotTmp.copyData(&xdot[pos]);
pos += qDotDotTmp.copyData(&xdot[pos]);
}
else
{
SiconosVector& xtmp2 = ds.getRhs(); // Pointer link !
pos += xtmp2.copyData(&xdot[pos]);
}
}
}
void Hem5OSI::fprob(integer* IFCN,
integer* NQ,
integer* NV,
integer* NU,
integer* NL,
integer* LDG, integer* LDF, integer* LDA,
integer* NBLK, integer* NMRC,
integer* NPGP, integer* NPFL,
integer* INDGR, integer* INDGC, integer * INDFLR, integer * INDFLC,
doublereal* time,
doublereal* q, doublereal* v, doublereal* u, doublereal* xl,
doublereal* G, doublereal* GQ, doublereal * F,
doublereal* GQQ, doublereal* GT, doublereal * FL,
doublereal* QDOT, doublereal* UDOT, doublereal * AM)
{
DEBUG_PRINTF("Hem5OSI::fprob(integer* IFCN,...) with IFCN = %i \n", (int)*IFCN);
DEBUG_PRINTF("NQ = %i\t NV = %i \t NU = %i, NL = %i \n", (int)*NQ, (int)*NV, (int)*NU, (int)*NL);
DEBUG_PRINTF("LDG = %i\t LDF = %i \t LDA = %i \n", (int)*LDG, (int)*LDF, (int)*LDA);
// fill in xWork vector (ie all the x of the ds of this osi) with x
fillqWork(NQ, q);
fillvWork(NV, v);
double t = *time;
simulationLink->model()->setCurrentTime(t);
SP::DynamicalSystemsGraph dsGraph = simulationLink->model()->nonSmoothDynamicalSystem()->dynamicalSystems();
int ifcn = (int)(*IFCN);
if ((ifcn == 1) || (ifcn >= 7)) // compute Mass AM
{
unsigned int pos=0;
for (DynamicalSystemsGraph::VIterator vi = dsGraph->begin(); vi != dsGraph->end(); ++vi)
{
SP::DynamicalSystem ds = dsGraph->bundle(*vi);
if (Type::value(*ds) == Type::LagrangianDS ||
Type::value(*ds) == Type::LagrangianLinearTIDS)
{
LagrangianDS& lds = *std11::static_pointer_cast<LagrangianDS>(ds);
lds.computeMass();
for (unsigned int ii =pos ; ii < ((unsigned int)(*NV)+pos); ii ++)
{
for (unsigned int jj =pos ; jj < ((unsigned int)(*NV)+pos); jj ++)
{
AM[ii + jj*(int)(*NV)] = lds.mass()->getValue(ii,jj) ;
}
}
pos += lds.getDim();
}
else
{
RuntimeException::selfThrow("Hem5OSI::fprob(), Only integration of Lagrangian DS is allowed");
}
DEBUG_EXPR(
for (int kk =0 ; kk < (int)(*NV)* (int)(*NV); kk ++)
{
std::cout << AM[kk] << std::endl;
}
);
}
void KernelTest::t6()
{
SP::Model bouncingBall = Siconos::load("BouncingBall1.xml");
try
{
double T = bouncingBall->finalT();
double t0 = bouncingBall->t0();
double h = bouncingBall->simulation()->timeStep();
int N = (int)((T - t0) / h); // Number of time steps
SP::DynamicalSystemsGraph dsg =
bouncingBall->nonSmoothDynamicalSystem()->topology()->dSG(0);
SP::LagrangianDS ball = std11::static_pointer_cast<LagrangianDS>
(dsg->bundle(*(dsg->begin())));
SP::TimeStepping s = std11::static_pointer_cast<TimeStepping>(bouncingBall->simulation());
SP::Interaction inter;
InteractionsGraph::VIterator ui, uiend;
SP::InteractionsGraph indexSet0 = bouncingBall->nonSmoothDynamicalSystem()->topology()->indexSet(0);
for (std11::tie(ui, uiend) = indexSet0->vertices(); ui != uiend; ++ui)
inter = indexSet0->bundle(*ui);
// --- Get the values to be plotted ---
// -> saved in a matrix dataPlot
unsigned int outputSize = 5;
SimpleMatrix dataPlot(N + 1, outputSize);
SP::SiconosVector q = ball->q();
SP::SiconosVector v = ball->velocity();
SP::SiconosVector p = ball->p(1);
SP::SiconosVector lambda = inter->lambda(1);
dataPlot(0, 0) = bouncingBall->t0();
dataPlot(0, 1) = (*q)(0);
dataPlot(0, 2) = (*v)(0);
dataPlot(0, 3) = (*p)(0);
dataPlot(0, 4) = (*lambda)(0);
// --- Time loop ---
cout << "====> Start computation ... " << endl << endl;
// ==== Simulation loop - Writing without explicit event handling =====
int k = 1;
boost::progress_display show_progress(N);
boost::timer time;
time.restart();
while (s->hasNextEvent())
{
s->computeOneStep();
// --- Get values to be plotted ---
dataPlot(k, 0) = s->nextTime();
dataPlot(k, 1) = (*q)(0);
dataPlot(k, 2) = (*v)(0);
dataPlot(k, 3) = (*p)(0);
dataPlot(k, 4) = (*lambda)(0);
s->nextStep();
++show_progress;
k++;
}
cout << endl << "End of computation - Number of iterations done: " << k - 1 << endl;
cout << "Computation Time " << time.elapsed() << endl;
// --- Output files ---
cout << "====> Output file writing ..." << endl;
dataPlot.resize(k, outputSize);
ioMatrix::write("result.dat", "ascii", dataPlot, "noDim");
// Comparison with a reference file
SimpleMatrix dataPlotRef(dataPlot);
dataPlotRef.zero();
ioMatrix::read("result.ref", "ascii", dataPlotRef);
if ((dataPlot - dataPlotRef).normInf() > 1e-12)
{
std::cout <<
"Warning. The results is rather different from the reference file :"
<<
(dataPlot - dataPlotRef).normInf()
<<
std::endl;
CPPUNIT_ASSERT(false);
}
}
catch (SiconosException e)
{
cout << e.report() << endl;
CPPUNIT_ASSERT(false);
}
catch (...)
{
cout << "Exception caught in BouncingBallTS.cpp" << endl;
CPPUNIT_ASSERT(false);
}
}
