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()
{
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));
}
}
int main(int argc, char* argv[])
{
boost::timer time;
time.restart();
try
{
// ================= Creation of the model =======================
// User-defined main parameters
unsigned int nDofBall = 1; // degrees of freedom of ball 1
double Height = 0.05; // Distance between impactor balls and monodisperse balls
// Balls in the tapered chain
unsigned int NumberBalls = 10; // Number
double R_base_taper = 0.005; // Base radii of the tapered chain
double q_taper = 0.0; // Tapering factor
// Material properties of balls
double mass_density = 7780; // mass density
double Res_BallBall = 1.0; // Restitution coefficient at contacts ball-ball
double Res_BallWall = 1.0; // Restitution coefficient at contacts ball-wall
double PowCompLaw = 1.5; // Power of the compliance law: 1.0 for linear contact and 3/2 for the Hertzian contact
string TypeContactLaw = "BiStiffness"; // Type of compliance contact law
// Parameters for the global simulation
double t0 = 0; // initial computation time
double T = 0.5; // final computation time
double h = 0.001; // time step
unsigned int Npointsave = 500; // Number of data points to be saved
//---------------------------------------
// ---- Configuration of chaines
//--------------------------------------
//************* Balls ******************
double NumberContacts = NumberBalls ; // Number of contacts
//(1) Radius of balls
SP::SiconosVector RadiusBalls(new SiconosVector(NumberBalls));
for (unsigned int k = 0; k < NumberBalls; ++k)
{
(*RadiusBalls)(k) = (pow(double(1.0 - q_taper), int(k + 1))) * R_base_taper;
}
// (2) Mass of balls
SP::SiconosVector MassBalls(new SiconosVector(NumberBalls));
for (unsigned int id = 0; id < NumberBalls; ++id)
{
(*MassBalls)(id) = (4.0 / 3.0) * PI * pow((*RadiusBalls)(id), 3) * mass_density;
}
// (3) Initial position of balls
// For the impactor balls
SP::SiconosVector InitPosBalls(new SiconosVector(NumberBalls));
(*InitPosBalls)(0) = Height + (*RadiusBalls)(0);
for (unsigned int j = 1; j < NumberBalls; ++j)
{
(*InitPosBalls)(j) = (*InitPosBalls)(j - 1) + (*RadiusBalls)(j - 1) + (*RadiusBalls)(j);
}
// (4) Initial velocity of balls
SP::SiconosVector InitVelBalls(new SiconosVector(NumberBalls));
for (unsigned int i = 0; i < NumberBalls; ++i)
{
(*InitVelBalls)(i) = 0.0;
}
//****************** Contacts ******************
// (1) Restitution coefficient at contacts
SP::SiconosVector ResCofContacts(new SiconosVector(NumberContacts));
SP::SiconosVector ElasCofContacts(new SiconosVector(NumberContacts));
for (unsigned int id = 0; id < NumberContacts; ++id)
{
if (id == 0) // contact ball-wall
{
(*ResCofContacts)(id) = Res_BallWall;
}
else // contact ball-ball
{
(*ResCofContacts)(id) = Res_BallBall;
}
}
// // Display and save the configuration of the chain simulated
// cout << "Configuation of ball chains" << endl;
// cout.precision(15);
// cout << "Radius of balls: " << endl;
// RadiusBalls->display();
// cout << "Mass of balls: " << endl;
// MassBalls->display();
// cout << "Initial position of balls: " << endl;
// InitPosBalls->display();
// cout << "Initial velocity of balls: " << endl;
// InitVelBalls->display();
// cout << "Restitution coefficient at contacts:" << endl;
// ResCofContacts->display();
// -------------------------
// --- Dynamical systems ---
// -------------------------
cout << "====> Model loading ..." << endl << endl;
std::vector<SP::DynamicalSystem> VecOfallDS;
SP::SiconosMatrix MassBall;
SP::SiconosVector q0Ball;
SP::SiconosVector v0Ball;
SP::LagrangianLinearTIDS ball;
SP::SiconosVector FextBall;
double _Rball, _massBall, _Pos0Ball, _Vel0Ball ;
// -------------
// --- Model ---
// -------------
SP::Model BallChain(new Model(t0, T));
// ----------------
// --- Simulation ---
// ----------------
// -- (1) OneStepIntegrators --
SP::OneStepIntegrator OSI(new LsodarOSI());
for (unsigned int i = 0; i < NumberBalls; ++i)
{
_Rball = (*RadiusBalls)(i); // radius of the ball
_massBall = (*MassBalls)(i); // mass of the ball
_Pos0Ball = (*InitPosBalls)(i); // initial position of the ball
_Vel0Ball = (*InitVelBalls)(i); // initial velocity of the ball
// Declaration of the DS in Siconos
MassBall = SP::SiconosMatrix(new SimpleMatrix(nDofBall, nDofBall));
(*MassBall)(0, 0) = _massBall;
// -- Initial positions and velocities --
q0Ball = SP::SiconosVector(new SiconosVector(nDofBall));
v0Ball = SP::SiconosVector(new SiconosVector(nDofBall));
(*q0Ball)(0) = _Pos0Ball;
(*v0Ball)(0) = _Vel0Ball;
// -- The dynamical system --
ball = SP::LagrangianLinearTIDS(new LagrangianLinearTIDS(q0Ball, v0Ball, MassBall));
// -- Set external forces (weight1) --
FextBall = SP::SiconosVector(new SiconosVector(nDofBall));
(*FextBall)(0) = -_massBall * g;
ball->setFExtPtr(FextBall);
//
VecOfallDS.push_back(ball);
BallChain->nonSmoothDynamicalSystem()->insertDynamicalSystem(ball);
}
// --------------------
// --- Interactions ---
// --------------------
SP::SimpleMatrix H;
SP::SiconosVector E;
SP::NonSmoothLaw nslaw;
SP::Relation relation;
SP::Interaction interaction;
double ResCoef, Stiff, ElasPow;
for (unsigned int j = 0; j < NumberContacts; ++j)
{
ResCoef = (*ResCofContacts)(j) ;
if (j == 0) // for contact wall-ball
{
H.reset(new SimpleMatrix(1, nDofBall));
(*H)(0, 0) = 1.0;
E = SP::SiconosVector(new SiconosVector(1));
(*E)(0) = -(*RadiusBalls)(j);
}
else // For ball-ball contact
{
H.reset(new SimpleMatrix(1, (nDofBall + nDofBall)));
(*H)(0, 0) = -1.0;
(*H)(0, 1) = 1.0;
E = SP::SiconosVector(new SiconosVector(1));
(*E)(0) = -1.0 * ((*RadiusBalls)(j - 1) + (*RadiusBalls)(j));
}
//
nslaw = SP::NonSmoothLaw(new NewtonImpactNSL(ResCoef));
relation = SP::Relation(new LagrangianLinearTIR(H, E));
interaction = SP::Interaction(new Interaction(1, nslaw, relation));
if (j == 0) // for contact wall-ball
BallChain->nonSmoothDynamicalSystem()->link(interaction, VecOfallDS[j]);
else // For ball-ball contact
BallChain->nonSmoothDynamicalSystem()->link(interaction, VecOfallDS[j-1],VecOfallDS[j]);
}
// -- (2) Time discretisation --
SP::TimeDiscretisation t(new TimeDiscretisation(t0, h));
// -- (3) Non smooth problem --
SP::OneStepNSProblem impact(new LCP());
SP::OneStepNSProblem acceleration(new LCP());
// -- (4) Simulation setup with (1) (2) (3)
SP::EventDriven s(new EventDriven(t));
s->insertIntegrator(OSI);
s->insertNonSmoothProblem(impact, SICONOS_OSNSP_ED_IMPACT);
s->insertNonSmoothProblem(acceleration, SICONOS_OSNSP_ED_SMOOTH_ACC);
BallChain->setSimulation(s);
// =========================== End of model definition ===========================
//----------------------------------- Initialization-------------------------------
s->setPrintStat(true);
BallChain->initialize();
SP::DynamicalSystemsGraph DSG0 = BallChain->nonSmoothDynamicalSystem()->topology()->dSG(0);
SP::InteractionsGraph IndexSet0 = BallChain->nonSmoothDynamicalSystem()->topology()->indexSet(0);
SP::InteractionsGraph IndexSet1 = BallChain->nonSmoothDynamicalSystem()->topology()->indexSet(1);
SP::InteractionsGraph IndexSet2 = BallChain->nonSmoothDynamicalSystem()->topology()->indexSet(2);
// // Display topology of the system
// cout << "Number of vectices of IndexSet0: " << IndexSet0->size() << endl;
// cout << "Number of vectices of IndexSet1: " << IndexSet1->size() << endl;
// cout << "Number of vectices of IndexSet2: " << IndexSet2->size() << endl;
// cout << "Number of vectices of DSG0: " << DSG0->size() << endl;
//
SP::EventsManager eventsManager = s->eventsManager();
boost::progress_display show_progress(Npointsave);
// ================================= Computation =================================
// --- Get the values to be plotted ---
// -> saved in a matrix dataPlot
unsigned int outputSize = 2 * NumberBalls + 1;
SimpleMatrix dataPlot(Npointsave, outputSize);
// --- Time loop ---
cout << "====> Start computation ... " << endl << endl;
// ==== Simulation loop - Writing without explicit event handling =====
bool nonSmooth = false;
unsigned int NumberOfEvents = 0;
unsigned int NumberOfNSEvents = 0;
unsigned int k = 0;
DynamicalSystemsGraph::VIterator ui, uiend;
//====================================================================
while ((k < Npointsave) & (s->hasNextEvent()))
{
dataPlot(k, 0) = s->startingTime();
// Save state of the balls
unsigned int col_pos = 1;
unsigned int col_vel = NumberBalls + 1;
for (boost::tie(ui, uiend) = DSG0->vertices(); ui != uiend; ++ui)
{
SP::DynamicalSystem ds = DSG0->bundle(*ui);
SP::LagrangianDS lag_ds = std11::dynamic_pointer_cast<LagrangianDS>(ds);
SP::SiconosVector q = lag_ds->q();
SP::SiconosVector v = lag_ds->velocity();
dataPlot(k, col_pos) = (*q)(0);
dataPlot(k, col_vel) = (*v)(0);
col_pos++;
col_vel++;
}
++k;
s->advanceToEvent(); // run simulation from one event to the next
if (eventsManager->nextEvent()->getType() == 2)
{
nonSmooth = true;
};
//
s->processEvents(); // process events
if (nonSmooth)
{
dataPlot(k, 0) = s->startingTime();
// Save state of the balls
unsigned int col_pos = 1;
unsigned int col_vel = NumberBalls + 1;
for (boost::tie(ui, uiend) = DSG0->vertices(); ui != uiend; ++ui)
{
SP::DynamicalSystem ds = DSG0->bundle(*ui);
SP::LagrangianDS lag_ds = std11::dynamic_pointer_cast<LagrangianDS>(ds);
SP::SiconosVector q = lag_ds->qMemory()->getSiconosVector(1);
SP::SiconosVector v = lag_ds->velocityMemory()->getSiconosVector(1);
dataPlot(k, col_pos) = (*q)(0);
dataPlot(k, col_vel) = (*v)(0);
col_pos++;
col_vel++;
}
nonSmooth = false;
++NumberOfNSEvents;
++NumberOfEvents;
++show_progress;
++k;
}
// --- Get values to be plotted ---
++NumberOfEvents;
++show_progress;
}
cout << "Computation Time " << time.elapsed() << endl;
// --- Output files ---
cout << "====> Output file writing ..." << endl;
ioMatrix::write("result.dat", "ascii", dataPlot, "noDim");
// Comparison with a reference file
SimpleMatrix dataPlotRef(dataPlot);
dataPlotRef.zero();
ioMatrix::read("resultED_NewtonModel.ref", "ascii", dataPlotRef);
if ((dataPlot - dataPlotRef).normInf() > 1e-12)
{
std::cout << "Warning. The results is rather different from the reference file." << std::endl;
return 1;
}
}
catch (SiconosException e)
{
cout << e.report() << endl;
}
catch (...)
{
cout << "Exception caught." << endl;
}
cout << "Computation Time: " << time.elapsed() << endl;
}
void SchatzmanPaoliOSI::computeFreeState()
{
// This function computes "free" states of the DS belonging to this Integrator.
// "Free" means without taking non-smooth effects into account.
//double t = simulationLink->nextTime(); // End of the time step
//double told = simulationLink->startingTime(); // Beginning of the time step
//double h = t-told; // time step length
// Operators computed at told have index i, and (i+1) at t.
// Note: integration of r with a theta method has been removed
// SiconosVector *rold = static_cast<SiconosVector*>(d->rMemory()->getSiconosVector(0));
// Iteration through the set of Dynamical Systems.
//
DSIterator it; // Iterator through the set of DS.
SP::DynamicalSystem ds; // Current Dynamical System.
SP::SiconosMatrix W; // W SchatzmanPaoliOSI matrix of the current DS.
Type::Siconos dsType ; // Type of the current DS.
for (it = OSIDynamicalSystems->begin(); it != OSIDynamicalSystems->end(); ++it)
{
ds = *it; // the considered dynamical system
dsType = Type::value(*ds); // Its type
W = WMap[ds->number()]; // Its W SchatzmanPaoliOSI matrix of iteration.
//1 - Lagrangian Non Linear Systems
if (dsType == Type::LagrangianDS)
{
// // IN to be updated at current time: W, M, q, v, fL
// // IN at told: qi,vi, fLi
// // Note: indices i/i+1 corresponds to value at the beginning/end of the time step.
// // Index k stands for Newton iteration and thus corresponds to the last computed
// // value, ie the one saved in the DynamicalSystem.
// // "i" values are saved in memory vectors.
// // vFree = v_k,i+1 - W^{-1} ResiduFree
// // with
// // ResiduFree = M(q_k,i+1)(v_k,i+1 - v_i) - h*theta*forces(t,v_k,i+1, q_k,i+1) - h*(1-theta)*forces(ti,vi,qi)
// // -- Convert the DS into a Lagrangian one.
// SP::LagrangianDS d = std11::static_pointer_cast<LagrangianDS> (ds);
// // Get state i (previous time step) from Memories -> var. indexed with "Old"
// SP::SiconosVector qold =d->qMemory()->getSiconosVector(0);
// SP::SiconosVector vold = d->velocityMemory()->getSiconosVector(0);
// // --- ResiduFree computation ---
// // ResFree = M(v-vold) - h*[theta*forces(t) + (1-theta)*forces(told)]
// //
// // vFree pointer is used to compute and save ResiduFree in this first step.
// SP::SiconosVector vfree = d->workspace(DynamicalSystem::free);//workX[d];
// (*vfree)=*(d->workspace(DynamicalSystem::freeresidu));
// // -- Update W --
// // Note: during computeW, mass and jacobians of fL will be computed/
// computeW(t,d);
// SP::SiconosVector v = d->velocity(); // v = v_k,i+1
// // -- vfree = v - W^{-1} ResiduFree --
// // At this point vfree = residuFree
// // -> Solve WX = vfree and set vfree = X
// W->PLUForwardBackwardInPlace(*vfree);
// // -> compute real vfree
// *vfree *= -1.0;
// *vfree += *v;
RuntimeException::selfThrow("SchatzmanPaoliOSI::computeFreeState - not yet implemented for Dynamical system type: " + dsType);
}
// 2 - Lagrangian Linear Systems
else if (dsType == Type::LagrangianLinearTIDS)
{
// IN to be updated at current time: Fext
// IN at told: qi,vi, fext
// IN constants: K,C
// Note: indices i/i+1 corresponds to value at the beginning/end of the time step.
// "i" values are saved in memory vectors.
// vFree = v_i + W^{-1} ResiduFree // with
// ResiduFree = (-h*C -h^2*theta*K)*vi - h*K*qi + h*theta * Fext_i+1 + h*(1-theta)*Fext_i
// -- Convert the DS into a Lagrangian one.
SP::LagrangianLinearTIDS d = std11::static_pointer_cast<LagrangianLinearTIDS> (ds);
// Get state i (previous time step) from Memories -> var. indexed with "Old"
SP::SiconosVector qold = d->qMemory()->getSiconosVector(0); // q_k
// SP::SiconosVector vold = d->velocityMemory()->getSiconosVector(0); //v_k
// --- ResiduFree computation ---
// vFree pointer is used to compute and save ResiduFree in this first step.
// Velocity free and residu. vFree = RESfree (pointer equality !!).
SP::SiconosVector qfree = d->workspace(DynamicalSystem::free);//workX[d];
(*qfree) = *(d->workspace(DynamicalSystem::freeresidu));
W->PLUForwardBackwardInPlace(*qfree);
*qfree *= -1.0;
*qfree += *qold;
}
// 3 - Newton Euler Systems
else if (dsType == Type::NewtonEulerDS)
{
// // IN to be updated at current time: W, M, q, v, fL
// // IN at told: qi,vi, fLi
// // Note: indices i/i+1 corresponds to value at the beginning/end of the time step.
// // Index k stands for Newton iteration and thus corresponds to the last computed
// // value, ie the one saved in the DynamicalSystem.
// // "i" values are saved in memory vectors.
// // vFree = v_k,i+1 - W^{-1} ResiduFree
// // with
// // ResiduFree = M(q_k,i+1)(v_k,i+1 - v_i) - h*theta*forces(t,v_k,i+1, q_k,i+1) - h*(1-theta)*forces(ti,vi,qi)
// // -- Convert the DS into a Lagrangian one.
// SP::NewtonEulerDS d = std11::static_pointer_cast<NewtonEulerDS> (ds);
// computeW(t,d);
// // Get state i (previous time step) from Memories -> var. indexed with "Old"
// SP::SiconosVector qold =d->qMemory()->getSiconosVector(0);
// SP::SiconosVector vold = d->velocityMemory()->getSiconosVector(0);
// // --- ResiduFree computation ---
// // ResFree = M(v-vold) - h*[theta*forces(t) + (1-theta)*forces(told)]
// //
// // vFree pointer is used to compute and save ResiduFree in this first step.
// SP::SiconosVector vfree = d->workspace(DynamicalSystem::free);//workX[d];
// (*vfree)=*(d->workspace(DynamicalSystem::freeresidu));
// //*(d->vPredictor())=*(d->workspace(DynamicalSystem::freeresidu));
// // -- Update W --
// // Note: during computeW, mass and jacobians of fL will be computed/
// SP::SiconosVector v = d->velocity(); // v = v_k,i+1
// // -- vfree = v - W^{-1} ResiduFree --
// // At this point vfree = residuFree
// // -> Solve WX = vfree and set vfree = X
// // std::cout<<"SchatzmanPaoliOSI::computeFreeState residu free"<<endl;
// // vfree->display();
// d->luW()->PLUForwardBackwardInPlace(*vfree);
// // std::cout<<"SchatzmanPaoliOSI::computeFreeState -WRfree"<<endl;
// // vfree->display();
// // scal(h,*vfree,*vfree);
// // -> compute real vfree
// *vfree *= -1.0;
// *vfree += *v;
RuntimeException::selfThrow("SchatzmanPaoliOSI::computeFreeState - not yet implemented for Dynamical system type: " + dsType);
}
else
RuntimeException::selfThrow("SchatzmanPaoliOSI::computeFreeState - not yet implemented for Dynamical system type: " + dsType);
}
}
double SchatzmanPaoliOSI::computeResidu()
{
// This function is used to compute the residu for each "SchatzmanPaoliOSI-discretized" dynamical system.
// It then computes the norm of each of them and finally return the maximum
// value for those norms.
//
// The state values used are those saved in the DS, ie the last computed ones.
// $\mathcal R(x,r) = x - x_{k} -h\theta f( x , t_{k+1}) - h(1-\theta)f(x_k,t_k) - h r$
// $\mathcal R_{free}(x,r) = x - x_{k} -h\theta f( x , t_{k+1}) - h(1-\theta)f(x_k,t_k) $
double t = simulationLink->nextTime(); // End of the time step
double told = simulationLink->startingTime(); // Beginning of the time step
double h = t - told; // time step length
// Operators computed at told have index i, and (i+1) at t.
// Iteration through the set of Dynamical Systems.
//
DSIterator it;
SP::DynamicalSystem ds; // Current Dynamical System.
Type::Siconos dsType ; // Type of the current DS.
double maxResidu = 0;
double normResidu = maxResidu;
for (it = OSIDynamicalSystems->begin(); it != OSIDynamicalSystems->end(); ++it)
{
ds = *it; // the considered dynamical system
dsType = Type::value(*ds); // Its type
SP::SiconosVector residuFree = ds->workspace(DynamicalSystem::freeresidu);
// 1 - Lagrangian Non Linear Systems
if (dsType == Type::LagrangianDS)
{
// // residu = M(q*)(v_k,i+1 - v_i) - h*theta*forces(t,v_k,i+1, q_k,i+1) - h*(1-theta)*forces(ti,vi,qi) - pi+1
// // -- Convert the DS into a Lagrangian one.
// SP::LagrangianDS d = std11::static_pointer_cast<LagrangianDS> (ds);
// // Get state i (previous time step) from Memories -> var. indexed with "Old"
// SP::SiconosVector qold =d->qMemory()->getSiconosVector(0);
// SP::SiconosVector vold = d->velocityMemory()->getSiconosVector(0);
// SP::SiconosVector q =d->q();
// d->computeMass();
// SP::SiconosMatrix M = d->mass();
// SP::SiconosVector v = d->velocity(); // v = v_k,i+1
// //residuFree->zero();
// // std::cout << "(*v-*vold)->norm2()" << (*v-*vold).norm2() << std::endl;
// prod(*M, (*v-*vold), *residuFree); // residuFree = M(v - vold)
// if (d->forces()) // if fL exists
// {
// // computes forces(ti,vi,qi)
// d->computeForces(told,qold,vold);
// double coef = -h*(1-_theta);
// // residuFree += coef * fL_i
// scal(coef, *d->forces(), *residuFree, false);
// // computes forces(ti+1, v_k,i+1, q_k,i+1) = forces(t,v,q)
// //d->computeForces(t);
// // or forces(ti+1, v_k,i+1, q(v_k,i+1))
// //or
// SP::SiconosVector qbasedonv(new SiconosVector(*qold));
// *qbasedonv += h*( (1-_theta)* *vold + _theta * *v );
// d->computeForces(t,qbasedonv,v);
// coef = -h*_theta;
// // residuFree += coef * fL_k,i+1
// scal(coef, *d->forces(), *residuFree, false);
// }
// if (d->boundaryConditions())
// {
// d->boundaryConditions()->computePrescribedVelocity(t);
// unsigned int columnindex=0;
// SP::SimpleMatrix WBoundaryConditions = _WBoundaryConditionsMap[ds];
// SP::SiconosVector columntmp(new SiconosVector(ds->getDim()));
// for (vector<unsigned int>::iterator itindex = d->boundaryConditions()->velocityIndices()->begin() ;
// itindex != d->boundaryConditions()->velocityIndices()->end();
// ++itindex)
// {
// double DeltaPrescribedVelocity =
// d->boundaryConditions()->prescribedVelocity()->getValue(columnindex)
// - vold->getValue(columnindex);
// WBoundaryConditions->getCol(columnindex,*columntmp);
// *residuFree -= *columntmp * (DeltaPrescribedVelocity);
// residuFree->setValue(*itindex, columntmp->getValue(*itindex) *(DeltaPrescribedVelocity));
// columnindex ++;
// }
// }
// *(d->workspace(DynamicalSystem::free))=*residuFree; // copy residuFree in Workfree
// // std::cout << "SchatzmanPaoliOSI::ComputeResidu LagrangianDS residufree :" << std::endl;
// // residuFree->display();
// if (d->p(1))
// *(d->workspace(DynamicalSystem::free)) -= *d->p(1); // Compute Residu in Workfree Notation !!
// // std::cout << "SchatzmanPaoliOSI::ComputeResidu LagrangianDS residu :" << std::endl;
// // d->workspace(DynamicalSystem::free)->display();
// normResidu = d->workspace(DynamicalSystem::free)->norm2();
RuntimeException::selfThrow("SchatzmanPaoliOSI::computeResidu - not yet implemented for Dynamical system type: " + dsType);
}
// 2 - Lagrangian Linear Systems
else if (dsType == Type::LagrangianLinearTIDS)
{
// ResiduFree = M(-q_{k}+q_{k-1}) + h^2 (K q_k)+ h^2 C (\theta \Frac{q_k-q_{k-1}}{2h}+ (1-\theta) v_k)) (1)
// This formulae is only valid for the first computation of the residual for q = q_k
// otherwise the complete formulae must be applied, that is
// ResiduFree M(q-2q_{k}+q_{k-1}) + h^2 (K(\theta q+ (1-\theta) q_k)))+ h^2 C (\theta \Frac{q-q_{k-1}}{2h}+ (1-\theta) v_k)) (2)
// for q != q_k, the formulae (1) is wrong.
// in the sequel, only the equation (1) is implemented
// -- Convert the DS into a Lagrangian one.
SP::LagrangianLinearTIDS d = std11::static_pointer_cast<LagrangianLinearTIDS> (ds);
// Get state i (previous time step) from Memories -> var. indexed with "Old"
SP::SiconosVector q_k = d->qMemory()->getSiconosVector(0); // q_k
SP::SiconosVector q_k_1 = d->qMemory()->getSiconosVector(1); // q_{k-1}
SP::SiconosVector v_k = d->velocityMemory()->getSiconosVector(0); //v_k
// std::cout << "SchatzmanPaoliOSI::computeResidu - q_k_1 =" <<std::endl;
// q_k_1->display();
// std::cout << "SchatzmanPaoliOSI::computeResidu - q_k =" <<std::endl;
// q_k->display();
// std::cout << "SchatzmanPaoliOSI::computeResidu - v_k =" <<std::endl;
// v_k->display();
// --- ResiduFree computation Equation (1) ---
residuFree->zero();
double coeff;
// -- No need to update W --
//SP::SiconosVector v = d->velocity(); // v = v_k,i+1
SP::SiconosMatrix M = d->mass();
prod(*M, (*q_k_1 - *q_k), *residuFree); // residuFree = M(-q_{k}+q_{k-1})
SP::SiconosMatrix K = d->K();
if (K)
{
prod(h * h, *K, *q_k, *residuFree, false); // residuFree += h^2*K*qi
}
SP::SiconosMatrix C = d->C();
if (C)
prod(h * h, *C, (1.0 / (2.0 * h)*_theta * (*q_k - *q_k_1) + (1.0 - _theta)* *v_k) , *residuFree, false);
// residufree += h^2 C (\theta \Frac{q-q_{k-1}}{2h}+ (1-\theta) v_k))
SP::SiconosVector Fext = d->fExt();
if (Fext)
{
// computes Fext(ti)
d->computeFExt(told);
coeff = -h * h * (1 - _theta);
scal(coeff, *Fext, *residuFree, false); // residufree -= h^2*(1-_theta) * fext(ti)
// computes Fext(ti+1)
d->computeFExt(t);
coeff = -h * h * _theta;
scal(coeff, *Fext, *residuFree, false); // residufree -= h^2*_theta * fext(ti+1)
}
// if (d->boundaryConditions())
// {
// d->boundaryConditions()->computePrescribedVelocity(t);
// unsigned int columnindex=0;
// SP::SimpleMatrix WBoundaryConditions = _WBoundaryConditionsMap[ds];
// SP::SiconosVector columntmp(new SiconosVector(ds->getDim()));
// for (vector<unsigned int>::iterator itindex = d->boundaryConditions()->velocityIndices()->begin() ;
// itindex != d->boundaryConditions()->velocityIndices()->end();
// ++itindex)
// {
// double DeltaPrescribedVelocity =
// d->boundaryConditions()->prescribedVelocity()->getValue(columnindex)
// -vold->getValue(columnindex);
// WBoundaryConditions->getCol(columnindex,*columntmp);
// *residuFree += *columntmp * (DeltaPrescribedVelocity);
// residuFree->setValue(*itindex, - columntmp->getValue(*itindex) *(DeltaPrescribedVelocity));
// columnindex ++;
// }
// }
// std::cout << "SchatzmanPaoliOSI::ComputeResidu LagrangianLinearTIDS residufree :" << std::endl;
// residuFree->display();
(* d->workspace(DynamicalSystem::free)) = *residuFree; // copy residuFree in Workfree
if (d->p(0))
*(d->workspace(DynamicalSystem::free)) -= *d->p(0); // Compute Residu in Workfree Notation !!
// std::cout << "SchatzmanPaoliOSI::ComputeResidu LagrangianLinearTIDS p(0) :" << std::endl;
// if (d->p(0))
// d->p(0)->display();
// else
// std::cout << " p(0) :" << std::endl;
// std::cout << "SchatzmanPaoliOSI::ComputeResidu LagrangianLinearTIDS residu :" << std::endl;
// d->workspace(DynamicalSystem::free)->display();
// normResidu = d->workspace(DynamicalSystem::free)->norm2();
normResidu = 0.0; // we assume that v = vfree + W^(-1) p
// normResidu = realresiduFree->norm2();
}
else if (dsType == Type::NewtonEulerDS)
{
// // residu = M(q*)(v_k,i+1 - v_i) - h*_theta*forces(t,v_k,i+1, q_k,i+1) - h*(1-_theta)*forces(ti,vi,qi) - pi+1
// // -- Convert the DS into a Lagrangian one.
// SP::NewtonEulerDS d = std11::static_pointer_cast<NewtonEulerDS> (ds);
// // Get state i (previous time step) from Memories -> var. indexed with "Old"
// SP::SiconosVector qold =d->qMemory()->getSiconosVector(0);
// SP::SiconosVector vold = d->velocityMemory()->getSiconosVector(0);
// SP::SiconosVector q =d->q();
// SP::SiconosMatrix massMatrix = d->massMatrix();
// SP::SiconosVector v = d->velocity(); // v = v_k,i+1
// prod(*massMatrix, (*v-*vold), *residuFree); // residuFree = M(v - vold)
// if (d->forces()) // if fL exists
// {
// // computes forces(ti,vi,qi)
// SP::SiconosVector fLold=d->fLMemory()->getSiconosVector(0);
// double _thetaFL=0.5;
// double coef = -h*(1-_thetaFL);
// // residuFree += coef * fL_i
// scal(coef, *fLold, *residuFree, false);
// d->computeForces(t);
// // printf("cpmputeFreeState d->FL():\n");
// // d->forces()->display();
// coef = -h*_thetaFL;
// scal(coef, *d->forces(), *residuFree, false);
// }
// *(d->workspace(DynamicalSystem::free))=*residuFree;
// //cout<<"SchatzmanPaoliOSI::computeResidu :\n";
// // residuFree->display();
// if ( d->p(1) )
// *(d->workspace(DynamicalSystem::free)) -= *d->p(1);
// normResidu = d->workspace(DynamicalSystem::free)->norm2();
RuntimeException::selfThrow("SchatzmanPaoliOSI::computeResidu - not yet implemented for Dynamical system type: " + dsType);
}
else
RuntimeException::selfThrow("SchatzmanPaoliOSI::computeResidu - not yet implemented for Dynamical system type: " + dsType);
if (normResidu > maxResidu) maxResidu = normResidu;
}
return maxResidu;
}
void SchatzmanPaoliOSI::initW(double t, SP::DynamicalSystem ds)
{
// This function:
// - allocate memory for a matrix W
// - insert this matrix into WMap with ds as a key
if (!ds)
RuntimeException::selfThrow("SchatzmanPaoliOSI::initW(t,ds) - ds == NULL");
if (!OSIDynamicalSystems->isIn(ds))
RuntimeException::selfThrow("SchatzmanPaoliOSI::initW(t,ds) - ds does not belong to the OSI.");
unsigned int dsN = ds->number();
if (WMap.find(dsN) != WMap.end())
RuntimeException::selfThrow("SchatzmanPaoliOSI::initW(t,ds) - W(ds) is already in the map and has been initialized.");
//unsigned int sizeW = ds->getDim(); // n for first order systems, ndof for lagrangian.
// Memory allocation for W
// WMap[ds].reset(new SimpleMatrix(sizeW,sizeW));
// SP::SiconosMatrix W = WMap[ds];
double h = simulationLink->timeStep();
Type::Siconos dsType = Type::value(*ds);
// 1 - Lagrangian non linear systems
if (dsType == Type::LagrangianDS)
{
// SP::LagrangianDS d = std11::static_pointer_cast<LagrangianDS> (ds);
// SP::SiconosMatrix K = d->jacobianqForces(); // jacobian according to q
// SP::SiconosMatrix C = d->jacobianqDotForces(); // jacobian according to velocity
// WMap[ds].reset(new SimpleMatrix(*d->mass())); //*W = *d->mass();
// SP::SiconosMatrix W = WMap[ds];
// if (C)
// scal(-h*_theta, *C,*W,false); // W -= h*_theta*C
// if (K)
// scal(-h*h*_theta*_theta,*K,*W,false); //*W -= h*h*_theta*_theta**K;
// // WBoundaryConditions initialization
// if (d->boundaryConditions())
// initWBoundaryConditions(d);
RuntimeException::selfThrow("SchatzmanPaoliOSI::initW - not yet implemented for Dynamical system type :" + dsType);
}
// 4 - Lagrangian linear systems
else if (dsType == Type::LagrangianLinearTIDS)
{
SP::LagrangianLinearTIDS d = std11::static_pointer_cast<LagrangianLinearTIDS> (ds);
SP::SiconosMatrix K = d->K();
SP::SiconosMatrix C = d->C();
WMap[dsN].reset(new SimpleMatrix(*d->mass())); //*W = *d->mass();
SP::SiconosMatrix W = WMap[dsN];
if (C)
scal(1 / 2.0 * h * _theta, *C, *W, false); // W += 1/2.0*h*_theta *C
if (K)
scal(h * h * _theta * _theta, *K, *W, false); // W = h*h*_theta*_theta*K
// WBoundaryConditions initialization
if (d->boundaryConditions())
initWBoundaryConditions(d);
}
// === ===
else if (dsType == Type::NewtonEulerDS)
{
//WMap[ds].reset(new SimpleMatrix(3,3));
RuntimeException::selfThrow("SchatzmanPaoliOSI::initW - not yet implemented for Dynamical system type :" + dsType);
}
else RuntimeException::selfThrow("SchatzmanPaoliOSI::initW - not yet implemented for Dynamical system type :" + dsType);
// Remark: W is not LU-factorized nor inversed here.
// Function PLUForwardBackward will do that if required.
}
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));
}
}
void SchatzmanPaoliOSI::computeFreeState()
{
// This function computes "free" states of the DS belonging to this Integrator.
// "Free" means without taking non-smooth effects into account.
//double t = _simulation->nextTime(); // End of the time step
//double told = _simulation->startingTime(); // Beginning of the time step
//double h = t-told; // time step length
// Operators computed at told have index i, and (i+1) at t.
// Note: integration of r with a theta method has been removed
// SiconosVector *rold = static_cast<SiconosVector*>(d->rMemory()->getSiconosVector(0));
// Iteration through the set of Dynamical Systems.
//
SP::DynamicalSystem ds; // Current Dynamical System.
SP::SiconosMatrix W; // W SchatzmanPaoliOSI matrix of the current DS.
Type::Siconos dsType ; // Type of the current DS.
DynamicalSystemsGraph::VIterator dsi, dsend;
for (std11::tie(dsi, dsend) = _dynamicalSystemsGraph->vertices(); dsi != dsend; ++dsi)
{
if (!checkOSI(dsi)) continue;
ds = _dynamicalSystemsGraph->bundle(*dsi);
dsType = Type::value(*ds); // Its type
W = _dynamicalSystemsGraph->properties(*dsi).W; // Its W SchatzmanPaoliOSI matrix of iteration.
//1 - Lagrangian Non Linear Systems
if (dsType == Type::LagrangianDS)
{
RuntimeException::selfThrow("SchatzmanPaoliOSI::computeFreeState - not yet implemented for Dynamical system type: " + dsType);
}
// 2 - Lagrangian Linear Systems
else if (dsType == Type::LagrangianLinearTIDS)
{
// IN to be updated at current time: Fext
// IN at told: qi,vi, fext
// IN constants: K,C
// Note: indices i/i+1 corresponds to value at the beginning/end of the time step.
// "i" values are saved in memory vectors.
// vFree = v_i + W^{-1} ResiduFree // with
// ResiduFree = (-h*C -h^2*theta*K)*vi - h*K*qi + h*theta * Fext_i+1 + h*(1-theta)*Fext_i
// -- Convert the DS into a Lagrangian one.
SP::LagrangianLinearTIDS d = std11::static_pointer_cast<LagrangianLinearTIDS> (ds);
// Get state i (previous time step) from Memories -> var. indexed with "Old"
SP::SiconosVector qold = d->qMemory()->getSiconosVector(0); // q_k
// SP::SiconosVector vold = d->velocityMemory()->getSiconosVector(0); //v_k
// --- ResiduFree computation ---
// vFree pointer is used to compute and save ResiduFree in this first step.
// Velocity free and residu. vFree = RESfree (pointer equality !!).
SP::SiconosVector qfree = d->workspace(DynamicalSystem::free);//workX[d];
(*qfree) = *(d->workspace(DynamicalSystem::freeresidu));
W->PLUForwardBackwardInPlace(*qfree);
*qfree *= -1.0;
*qfree += *qold;
}
// 3 - Newton Euler Systems
else if (dsType == Type::NewtonEulerDS)
{
RuntimeException::selfThrow("SchatzmanPaoliOSI::computeFreeState - not yet implemented for Dynamical system type: " + dsType);
}
else
RuntimeException::selfThrow("SchatzmanPaoliOSI::computeFreeState - not yet implemented for Dynamical system type: " + dsType);
}
}
double SchatzmanPaoliOSI::computeResidu()
{
// This function is used to compute the residu for each "SchatzmanPaoliOSI-discretized" dynamical system.
// It then computes the norm of each of them and finally return the maximum
// value for those norms.
//
// The state values used are those saved in the DS, ie the last computed ones.
// $\mathcal R(x,r) = x - x_{k} -h\theta f( x , t_{k+1}) - h(1-\theta)f(x_k,t_k) - h r$
// $\mathcal R_{free}(x,r) = x - x_{k} -h\theta f( x , t_{k+1}) - h(1-\theta)f(x_k,t_k) $
double t = _simulation->nextTime(); // End of the time step
double told = _simulation->startingTime(); // Beginning of the time step
double h = t - told; // time step length
// Operators computed at told have index i, and (i+1) at t.
// Iteration through the set of Dynamical Systems.
//
SP::DynamicalSystem ds; // Current Dynamical System.
Type::Siconos dsType ; // Type of the current DS.
double maxResidu = 0;
double normResidu = maxResidu;
DynamicalSystemsGraph::VIterator dsi, dsend;
for (std11::tie(dsi, dsend) = _dynamicalSystemsGraph->vertices(); dsi != dsend; ++dsi)
{
if (!checkOSI(dsi)) continue;
SP::DynamicalSystem ds = _dynamicalSystemsGraph->bundle(*dsi);
dsType = Type::value(*ds); // Its type
SP::SiconosVector residuFree = ds->workspace(DynamicalSystem::freeresidu);
// 1 - Lagrangian Non Linear Systems
if (dsType == Type::LagrangianDS)
{
RuntimeException::selfThrow("SchatzmanPaoliOSI::computeResidu - not yet implemented for Dynamical system type: " + dsType);
}
// 2 - Lagrangian Linear Systems
else if (dsType == Type::LagrangianLinearTIDS)
{
// ResiduFree = M(-q_{k}+q_{k-1}) + h^2 (K q_k)+ h^2 C (\theta \Frac{q_k-q_{k-1}}{2h}+ (1-\theta) v_k)) (1)
// This formulae is only valid for the first computation of the residual for q = q_k
// otherwise the complete formulae must be applied, that is
// ResiduFree M(q-2q_{k}+q_{k-1}) + h^2 (K(\theta q+ (1-\theta) q_k)))+ h^2 C (\theta \Frac{q-q_{k-1}}{2h}+ (1-\theta) v_k)) (2)
// for q != q_k, the formulae (1) is wrong.
// in the sequel, only the equation (1) is implemented
// -- Convert the DS into a Lagrangian one.
SP::LagrangianLinearTIDS d = std11::static_pointer_cast<LagrangianLinearTIDS> (ds);
// Get state i (previous time step) from Memories -> var. indexed with "Old"
SP::SiconosVector q_k = d->qMemory()->getSiconosVector(0); // q_k
SP::SiconosVector q_k_1 = d->qMemory()->getSiconosVector(1); // q_{k-1}
SP::SiconosVector v_k = d->velocityMemory()->getSiconosVector(0); //v_k
// std::cout << "SchatzmanPaoliOSI::computeResidu - q_k_1 =" <<std::endl;
// q_k_1->display();
// std::cout << "SchatzmanPaoliOSI::computeResidu - q_k =" <<std::endl;
// q_k->display();
// std::cout << "SchatzmanPaoliOSI::computeResidu - v_k =" <<std::endl;
// v_k->display();
// --- ResiduFree computation Equation (1) ---
residuFree->zero();
double coeff;
// -- No need to update W --
//SP::SiconosVector v = d->velocity(); // v = v_k,i+1
SP::SiconosMatrix M = d->mass();
prod(*M, (*q_k_1 - *q_k), *residuFree); // residuFree = M(-q_{k}+q_{k-1})
SP::SiconosMatrix K = d->K();
if (K)
{
prod(h * h, *K, *q_k, *residuFree, false); // residuFree += h^2*K*qi
}
SP::SiconosMatrix C = d->C();
if (C)
prod(h * h, *C, (1.0 / (2.0 * h)*_theta * (*q_k - *q_k_1) + (1.0 - _theta)* *v_k) , *residuFree, false);
// residufree += h^2 C (\theta \Frac{q-q_{k-1}}{2h}+ (1-\theta) v_k))
SP::SiconosVector Fext = d->fExt();
if (Fext)
{
// computes Fext(ti)
d->computeFExt(told);
coeff = -h * h * (1 - _theta);
scal(coeff, *Fext, *residuFree, false); // residufree -= h^2*(1-_theta) * fext(ti)
// computes Fext(ti+1)
d->computeFExt(t);
coeff = -h * h * _theta;
scal(coeff, *Fext, *residuFree, false); // residufree -= h^2*_theta * fext(ti+1)
}
// std::cout << "SchatzmanPaoliOSI::ComputeResidu LagrangianLinearTIDS residufree :" << std::endl;
// residuFree->display();
(* d->workspace(DynamicalSystem::free)) = *residuFree; // copy residuFree in Workfree
if (d->p(0))
*(d->workspace(DynamicalSystem::free)) -= *d->p(0); // Compute Residu in Workfree Notation !!
// std::cout << "SchatzmanPaoliOSI::ComputeResidu LagrangianLinearTIDS p(0) :" << std::endl;
// if (d->p(0))
// d->p(0)->display();
// else
// std::cout << " p(0) :" << std::endl;
// std::cout << "SchatzmanPaoliOSI::ComputeResidu LagrangianLinearTIDS residu :" << std::endl;
// d->workspace(DynamicalSystem::free)->display();
// normResidu = d->workspace(DynamicalSystem::free)->norm2();
normResidu = 0.0; // we assume that v = vfree + W^(-1) p
// normResidu = realresiduFree->norm2();
}
else if (dsType == Type::NewtonEulerDS)
{
RuntimeException::selfThrow("SchatzmanPaoliOSI::computeResidu - not yet implemented for Dynamical system type: " + dsType);
}
else
RuntimeException::selfThrow("SchatzmanPaoliOSI::computeResidu - not yet implemented for Dynamical system type: " + dsType);
if (normResidu > maxResidu) maxResidu = normResidu;
}
return maxResidu;
}
void SchatzmanPaoliOSI::initW(double t, SP::DynamicalSystem ds, DynamicalSystemsGraph::VDescriptor& dsv)
{
// This function:
// - allocate memory for a matrix W
if (!ds)
RuntimeException::selfThrow("SchatzmanPaoliOSI::initW(t,ds) - ds == NULL");
if (!(checkOSI(_dynamicalSystemsGraph->descriptor(ds))))
RuntimeException::selfThrow("SchatzmanPaoliOSI::initW(t,ds) - ds does not belong to the OSI.");
if (_dynamicalSystemsGraph->properties(dsv).W)
RuntimeException::selfThrow("SchatzmanPaoliOSI::initW(t,ds) - W(ds) is already in the map and has been initialized.");
//unsigned int sizeW = ds->dimension(); // n for first order systems, ndof for lagrangian.
// Memory allocation for W
double h = _simulation->timeStep();
Type::Siconos dsType = Type::value(*ds);
// 1 - Lagrangian non linear systems
if (dsType == Type::LagrangianDS)
{
RuntimeException::selfThrow("SchatzmanPaoliOSI::initW - not yet implemented for Dynamical system type :" + dsType);
}
// 4 - Lagrangian linear systems
else if (dsType == Type::LagrangianLinearTIDS)
{
SP::LagrangianLinearTIDS d = std11::static_pointer_cast<LagrangianLinearTIDS> (ds);
SP::SiconosMatrix K = d->K();
SP::SiconosMatrix C = d->C();
_dynamicalSystemsGraph->properties(dsv).W.reset(new SimpleMatrix(*d->mass())); //*W = *d->mass();
SP::SiconosMatrix W = _dynamicalSystemsGraph->properties(dsv).W;
if (C)
scal(1 / 2.0 * h * _theta, *C, *W, false); // W += 1/2.0*h*_theta *C
if (K)
scal(h * h * _theta * _theta, *K, *W, false); // W = h*h*_theta*_theta*K
// WBoundaryConditions initialization
if (d->boundaryConditions())
initWBoundaryConditions(d,dsv);
}
// === ===
else if (dsType == Type::NewtonEulerDS)
{
RuntimeException::selfThrow("SchatzmanPaoliOSI::initW - not yet implemented for Dynamical system type :" + dsType);
}
else RuntimeException::selfThrow("SchatzmanPaoliOSI::initW - not yet implemented for Dynamical system type :" + dsType);
// Remark: W is not LU-factorized nor inversed here.
// Function PLUForwardBackward will do that if required.
}