const SimpleMatrix SchatzmanPaoliOSI::getWBoundaryConditions(SP::DynamicalSystem ds)
{
assert(ds &&
"SchatzmanPaoliOSI::getWBoundaryConditions(ds): ds == NULL.");
// return *(WBoundaryConditionsMap[0]);
assert(_WBoundaryConditionsMap[ds->number()] &&
"SchatzmanPaoliOSI::getWBoundaryConditions(ds): WBoundaryConditions[ds] == NULL.");
return *(_WBoundaryConditionsMap[ds->number()]); // Copy !!
}
void EulerMoreauOSI::setW(const SiconosMatrix& newValue, SP::DynamicalSystem ds)
{
// Check if ds is in the OSI
if (!OSIDynamicalSystems->isIn(ds))
RuntimeException::selfThrow("EulerMoreauOSI::setW(newVal,ds) - ds does not belong to this Integrator ...");
// Check dimensions consistency
unsigned int line = newValue.size(0);
unsigned int col = newValue.size(1);
if (line != col) // Check that newValue is square
RuntimeException::selfThrow("EulerMoreauOSI::setW(newVal,ds) - newVal is not square! ");
if (!ds)
RuntimeException::selfThrow("EulerMoreauOSI::setW(newVal,ds) - ds == NULL.");
unsigned int sizeW = ds->getDim(); // n for first order systems, ndof for lagrangian.
unsigned int dsN = ds->number();
if (line != sizeW) // check consistency between newValue and dynamical system size
RuntimeException::selfThrow("EulerMoreauOSI::setW(newVal,ds) - unconsistent dimension between newVal and dynamical system to be integrated ");
// Memory allocation for W, if required
if (!WMap[dsN]) // allocate a new W if required
{
WMap[dsN].reset(new SimpleMatrix(newValue));
}
else // or fill-in an existing one if dimensions are consistent.
{
if (line == WMap[dsN]->size(0) && col == WMap[dsN]->size(1))
*(WMap[dsN]) = newValue;
else
RuntimeException::selfThrow("EulerMoreauOSI - setW: inconsistent dimensions with problem size for given input matrix W");
}
}
SP::SimpleMatrix EulerMoreauOSI::W(SP::DynamicalSystem ds)
{
assert(ds && "EulerMoreauOSI::W(ds): ds == NULL.");
// return WMap[0];
// if(WMap[ds]==NULL)
// RuntimeException::selfThrow("EulerMoreauOSI::W(ds): W[ds] == NULL.");
return WMap[ds->number()];
}
SP::SiconosMatrix EulerMoreauOSI::WBoundaryConditions(SP::DynamicalSystem ds)
{
assert(ds && "EulerMoreauOSI::WBoundaryConditions(ds): ds == NULL.");
// return WBoundaryConditionsMap[0];
// if(WBoundaryConditionsMap[ds]==NULL)
// RuntimeException::selfThrow("EulerMoreauOSI::WBoundaryConditions(ds): W[ds] == NULL.");
return _WBoundaryConditionsMap[ds->number()];
}
const SimpleMatrix EulerMoreauOSI::getW(SP::DynamicalSystem ds)
{
int dsN = ds->number();
assert(ds &&
"EulerMoreauOSI::getW(ds): ds == NULL.");
// return *(WMap[0]);
assert(WMap[dsN] &&
"EulerMoreauOSI::getW(ds): W[ds] == NULL.");
return *(WMap[dsN]); // Copy !!
}
const SimpleMatrix EulerMoreauOSI::getWBoundaryConditions(SP::DynamicalSystem ds)
{
assert(ds &&
"EulerMoreauOSI::getWBoundaryConditions(ds): ds == NULL.");
// return *(WBoundaryConditionsMap[0]);
unsigned int dsN = ds->number();
assert(_WBoundaryConditionsMap[dsN] &&
"EulerMoreauOSI::getWBoundaryConditions(ds): WBoundaryConditions[ds] == NULL.");
return *(_WBoundaryConditionsMap[dsN]); // Copy !!
}
const SimpleMatrix SchatzmanPaoliOSI::getW(SP::DynamicalSystem ds)
{
assert(ds &&
"SchatzmanPaoliOSI::getW(ds): ds == NULL.");
// return *(WMap[0]);
unsigned int dsN = ds->number();
assert(WMap[dsN] &&
"SchatzmanPaoliOSI::getW(ds): W[ds] == NULL.");
return *(WMap[dsN]); // Copy !!
}
void EulerMoreauOSI::setWPtr(SP::SimpleMatrix newPtr, SP::DynamicalSystem ds)
{
unsigned int line = newPtr->size(0);
unsigned int col = newPtr->size(1);
if (line != col) // Check that newPtr is square
RuntimeException::selfThrow("EulerMoreauOSI::setWPtr(newVal) - newVal is not square! ");
if (!ds)
RuntimeException::selfThrow("EulerMoreauOSI::setWPtr(newVal,ds) - ds == NULL.");
unsigned int sizeW = ds->getDim(); // n for first order systems, ndof for lagrangian.
if (line != sizeW) // check consistency between newValue and dynamical system size
RuntimeException::selfThrow("EulerMoreauOSI::setW(newVal) - unconsistent dimension between newVal and dynamical system to be integrated ");
WMap[ds->number()] = newPtr; // link with new pointer
}
SP::DynamicalSystem Topology::getDynamicalSystem(unsigned int requiredNumber)
{
DynamicalSystemsGraph::VIterator vi, vdend;
SP::DynamicalSystem ds;
unsigned int currentNumber;
for (std11::tie(vi, vdend) = _DSG[0]->vertices(); vi != vdend; ++vi)
{
ds = _DSG[0]->bundle(*vi);
currentNumber = ds->number();
if (currentNumber == requiredNumber)
return ds;
}
RuntimeException::selfThrow("Topology::getDynamicalSystem(n) ds not found.");
return ds;
}
void SchatzmanPaoliOSI::display()
{
OneStepIntegrator::display();
std::cout << "====== SchatzmanPaoliOSI OSI display ======" <<std::endl;
DynamicalSystemsGraph::VIterator dsi, dsend;
for (std11::tie(dsi, dsend) = _dynamicalSystemsGraph->vertices(); dsi != dsend; ++dsi)
{
if (!checkOSI(dsi)) continue;
SP::DynamicalSystem ds = _dynamicalSystemsGraph->bundle(*dsi);
std::cout << "--------------------------------" <<std::endl;
std::cout << "--> W of dynamical system number " << ds->number() << ": " <<std::endl;
if (_dynamicalSystemsGraph->properties(*dsi).W) _dynamicalSystemsGraph->properties(*dsi).W->display();
else std::cout << "-> NULL" <<std::endl;
std::cout << "--> and corresponding theta is: " << _theta <<std::endl;
}
std::cout << "================================" <<std::endl;
}
void EulerMoreauOSI::computeFreeState()
{
// This function computes "free" states of the DS belonging to this Integrator.
// "Free" means without taking non-smooth effects into account.
DEBUG_PRINT("EulerMoreauOSI::computeFreeState() starts\n");
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.
Type::Siconos dsType ; // Type of the current DS.
// XXX to be removed -- xhub
Topology& topo = *simulationLink->model()->nonSmoothDynamicalSystem()->topology();
DynamicalSystemsGraph& DSG0 = *topo.dSG(0);
for (it = OSIDynamicalSystems->begin(); it != OSIDynamicalSystems->end(); ++it)
{
ds = *it; // the considered dynamical system
// XXX TMP hack -- xhub
// we have to iterate over the edges of the DSG0 -> the following won't be necessary anymore
// Maurice will do that with subgraph :)
DynamicalSystemsGraph::VDescriptor dsgVD = DSG0.descriptor(ds);
VectorOfVectors& workVectors = *DSG0.properties(dsgVD).workVectors;
dsType = Type::value(*ds); // Its type
SiconosMatrix& W = *WMap[ds->number()]; // Its W EulerMoreauOSI matrix of iteration.
// 1 - First Order Non Linear Systems
if (dsType == Type::FirstOrderNonLinearDS || dsType == Type::FirstOrderLinearDS || dsType == Type::FirstOrderLinearTIDS)
{
// xfree = x - W^{-1} (ResiduFree - h(1-gamma)*rold)
// with ResiduFree = = M(x - x_k) - h*theta*f(t_{k+1}, x) - h*(1-theta)*f(t_k, x_k)
// to be updated at current time: W, f
// fold is f at t_k
// not time dependant: M
FirstOrderNonLinearDS& d = *std11::static_pointer_cast<FirstOrderNonLinearDS>(ds);
// Get state i (previous time step) from Memories -> var. indexed with "Old"
// SP::SiconosVector xold = d->xMemory()->getSiconosVector(0); // xi
SiconosVector& x = *d.x(); // x = x_k or x = x_{k+1}^{\alpha}
// xfree gets ResiduFree at first
SiconosVector& xfree = *workVectors[FirstOrderDS::xfree];
xfree = *workVectors[FirstOrderDS::residuFree];
DEBUG_PRINT("EulerMoreauOSI::computeFreeState xfree <- residuFree\n");
DEBUG_EXPR(xfree.display());
if (_useGamma)
{
SiconosVector& rold = *d.rMemory()->getSiconosVector(0);
double coeff = -h * (1 - _gamma);
scal(coeff, rold, xfree, false); // xfree += -h(1-gamma)*rold
}
// At this point xfree = (ResiduFree - h(1-gamma)*rold)
// -> Solve WX = xfree and set xfree = X
W.PLUForwardBackwardInPlace(xfree);
// at this point, xfree = W^{-1} (ResiduFree - h(1-gamma)*rold)
// -> compute real xfree = x - W^{-1} (ResiduFree - h(1-gamma)*rold)
xfree *= -1.0;
xfree += x;
DEBUG_EXPR(xfree.display());
// now the crazy intermediate variables
// xPartialNS was updated before this fonction call
// It constains either 0 (first Newton iterate)
// or g(x, \lambda, t_{k+1}) - B_{k+1}^{\alpha} \lambda - K_{k+1}^{\alpha} x
SiconosVector& xPartialNS = *workVectors[FirstOrderDS::xPartialNS];
DEBUG_PRINT("EulerMoreauOSI::computeFreeState xPartialNS from Interaction\n");
DEBUG_EXPR(xPartialNS.display());
// -> Solve WX = g(x, \lambda, t_{k+1}) - B_{k+1}^{\alpha} \lambda - K_{k+1}^{\alpha} x
// and set xPartialNS = X
W.PLUForwardBackwardInPlace(xPartialNS);
scal(h, xPartialNS, xPartialNS);
// compute real xPartialNS = xfree + ...
xPartialNS += xfree;
DEBUG_PRINT("EulerMoreauOSI::computeFreeState xPartialNS real value\n");
DEBUG_EXPR(xPartialNS.display());
// deltaxForRelation = (\widetilde{K}_{k+1}^{\alpha})^{-1} xPartialNS - x
SiconosVector& deltaxForRelation = *workVectors[FirstOrderDS::deltaxForRelation];
deltaxForRelation = xPartialNS;
deltaxForRelation -= x;
DEBUG_EXPR(deltaxForRelation.display());
// have a look at the end of the DevNotes for this part
if (_useGammaForRelation)
{
if (!(dsType == Type::FirstOrderLinearDS || dsType == Type::FirstOrderLinearTIDS))
RuntimeException::selfThrow("EulerMoreauOSI::computeFreeState - _useGammaForRelation == true is only implemented for FirstOrderLinearDS or FirstOrderLinearTIDS");
deltaxForRelation = xfree;
scal(_gamma, deltaxForRelation, deltaxForRelation);
SiconosVector& xold = *d.xMemory()->getSiconosVector(0);
scal(1.0 - _gamma, xold, deltaxForRelation, false);
}
// some output
DEBUG_EXPR(xfree.display(););
DEBUG_EXPR(xPartialNS.display(););
void EulerMoreauOSI::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("EulerMoreauOSI::initW(t,ds) - ds == NULL");
if (!OSIDynamicalSystems->isIn(ds))
RuntimeException::selfThrow("EulerMoreauOSI::initW(t,ds) - ds does not belong to the OSI.");
unsigned int dsN = ds->number();
if (WMap.find(dsN) != WMap.end())
RuntimeException::selfThrow("EulerMoreauOSI::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 - First order non linear systems
if (dsType == Type::FirstOrderNonLinearDS || dsType == Type::FirstOrderLinearDS || dsType == Type::FirstOrderLinearTIDS)
{
// // Memory allocation for W
// WMap[ds].reset(new SimpleMatrix(sizeW,sizeW));
// SP::SiconosMatrix W = WMap[ds];
// W = M - h*_theta* [jacobian_x f(t,x,z)]
SP::FirstOrderNonLinearDS d = std11::static_pointer_cast<FirstOrderNonLinearDS> (ds);
// Copy M or I if M is Null into W
// SP::SiconosMatrix W = WMap[ds];
if (d->M())
// *W = *d->M();
WMap[dsN].reset(new SimpleMatrix(*d->M()));
else
{
//W->eye();
// WMap[ds].reset(new SimpleMatrix(sizeW,sizeW,Siconos::IDENTITY));
WMap[dsN].reset(new SimpleMatrix(sizeW, sizeW)); // Warning if the Jacobian is a sparse matrix
WMap[dsN]->eye();
}
SP::SiconosMatrix W = WMap[dsN];
// d->computeJacobianfx(t); // Computation of JacxF is not required here
// since it must have been done in OSI->initialize, before a call to this function.
// Add -h*_theta*jacobian_XF to W
scal(-h * _theta, *d->jacobianfx(), *W, false);
}
else RuntimeException::selfThrow("EulerMoreauOSI::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::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);
}
}
void SchatzmanPaoliOSI::computeW(double t, SP::DynamicalSystem ds)
{
// Compute W matrix of the Dynamical System ds, at time t and for the current ds state.
// When this function is called, WMap[ds] is supposed to exist and not to be null
// Memory allocation has been done during initW.
assert(ds &&
"SchatzmanPaoliOSI::computeW(t,ds) - ds == NULL");
unsigned int dsN = ds->number();
assert((WMap.find(dsN) != WMap.end()) &&
"SchatzmanPaoliOSI::computeW(t,ds) - W(ds) does not exists. Maybe you forget to initialize the osi?");
//double h = simulationLink->timeStep();
Type::Siconos dsType = Type::value(*ds);
SP::SiconosMatrix W = WMap[dsN];
// 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
// d->computeMass();
// *W = *d->mass();
// if (C)
// {
// d->computeJacobianqDotForces(t);
// scal(-h*_theta, *C,*W,false); // W -= h*_theta*C
// }
// if (K)
// {
// d->computeJacobianqForces(t);
// scal(-h*h*_theta*_theta,*K,*W,false); //*W -= h*h*_theta*_theta**K;
// }
RuntimeException::selfThrow("SchatzmanPaoliOSI::computeW - not yet implemented for Dynamical system type :" + dsType);
}
// 4 - Lagrangian linear systems
else if (dsType == Type::LagrangianLinearTIDS)
{
// Nothing: W does not depend on time.
}
// === ===
else if (dsType == Type::NewtonEulerDS)
{
// SP::NewtonEulerDS d = std11::static_pointer_cast<NewtonEulerDS> (ds);
// d->computeJacobianvFL(t);
// double thetaFL=_theta;
// *(d->luW())=*(d->jacobianvFL());
// scal(h*thetaFL,*(d->jacobianvFL()),*(d->luW()),true);
// *(d->luW())+=*(d->massMatrix());
// //cout<<"SchatzmanPaoliOSI::computeW luW before LUFact\n";
// //d->luW()->display();
// d->luW()->PLUFactorizationInPlace();
RuntimeException::selfThrow("SchatzmanPaoliOSI::computeW - not yet implemented for Dynamical system type :" + dsType);
}
else RuntimeException::selfThrow("SchatzmanPaoliOSI::computeW - not yet implemented for Dynamical system type :" + dsType);
// Remark: W is not LU-factorized here.
// Function PLUForwardBackward will do that if required.
}
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::updateState(const unsigned int level)
{
double h = simulationLink->timeStep();
double RelativeTol = simulationLink->relativeConvergenceTol();
bool useRCC = simulationLink->useRelativeConvergenceCriteron();
if (useRCC)
simulationLink->setRelativeConvergenceCriterionHeld(true);
DSIterator it;
SP::SiconosMatrix W;
for (it = OSIDynamicalSystems->begin(); it != OSIDynamicalSystems->end(); ++it)
{
SP::DynamicalSystem ds = *it;
W = WMap[ds->number()];
// Get the DS type
Type::Siconos dsType = Type::value(*ds);
// 1 - Lagrangian Systems
if (dsType == Type::LagrangianDS || dsType == Type::LagrangianLinearTIDS)
{
// get dynamical system
SP::LagrangianDS d = std11::static_pointer_cast<LagrangianDS> (ds);
// SiconosVector *vfree = d->velocityFree();
SP::SiconosVector q = d->q();
bool baux = dsType == Type::LagrangianDS && useRCC && simulationLink->relativeConvergenceCriterionHeld();
if (level != LEVELMAX)
{
// To compute q, we solve W(q - qfree) = p
if (d->p(level))
{
*q = *d->p(level); // q = p
W->PLUForwardBackwardInPlace(*q);
}
// if (d->boundaryConditions())
// for (vector<unsigned int>::iterator
// itindex = d->boundaryConditions()->velocityIndices()->begin() ;
// itindex != d->boundaryConditions()->velocityIndices()->end();
// ++itindex)
// v->setValue(*itindex, 0.0);
*q += * ds->workspace(DynamicalSystem::free);
}
else
*q = * ds->workspace(DynamicalSystem::free);
// Computation of the velocity
SP::SiconosVector v = d->velocity();
SP::SiconosVector q_k_1 = d->qMemory()->getSiconosVector(1); // q_{k-1}
// std::cout << "SchatzmanPaoliOSI::updateState - q_k_1 =" <<std::endl;
// q_k_1->display();
// std::cout << "SchatzmanPaoliOSI::updateState - q =" <<std::endl;
// q->display();
*v = 1.0 / (2.0 * h) * (*q - *q_k_1);
// std::cout << "SchatzmanPaoliOSI::updateState - v =" <<std::endl;
// v->display();
// int bc=0;
// SP::SiconosVector columntmp(new SiconosVector(ds->getDim()));
// if (d->boundaryConditions())
// {
// for (vector<unsigned int>::iterator itindex = d->boundaryConditions()->velocityIndices()->begin() ;
// itindex != d->boundaryConditions()->velocityIndices()->end();
// ++itindex)
// {
// _WBoundaryConditionsMap[ds]->getCol(bc,*columntmp);
// /*\warning we assume that W is symmetric in the Lagrangian case*/
// double value = - inner_prod(*columntmp, *v);
// value += (d->p(level))->getValue(*itindex);
// /* \warning the computation of reactionToBoundaryConditions take into
// account the contact impulse but not the external and internal forces.
// A complete computation of the residue should be better */
// d->reactionToBoundaryConditions()->setValue(bc,value) ;
// bc++;
// }
if (baux)
{
ds->subWorkVector(q, DynamicalSystem::local_buffer);
double aux = ((ds->workspace(DynamicalSystem::local_buffer))->norm2()) / (ds->normRef());
if (aux > RelativeTol)
simulationLink->setRelativeConvergenceCriterionHeld(false);
}
}
//2 - Newton Euler Systems
else if (dsType == Type::NewtonEulerDS)
{
// // get dynamical system
// SP::NewtonEulerDS d = std11::static_pointer_cast<NewtonEulerDS> (ds);
// SP::SiconosVector v = d->velocity();
// #ifdef SCHATZMANPAOLI_NE_DEBUG
// std::cout<<"SchatzmanPaoliOSI::updatestate prev v"<<endl;
// v->display();
// #endif
// /*d->p has been fill by the Relation->computeInput, it contains
// B \lambda _{k+1}*/
// *v = *d->p(level); // v = p
// d->luW()->PLUForwardBackwardInPlace(*v);
// #ifdef SCHATZMANPAOLI_NE_DEBUG
// std::cout<<"SchatzmanPaoliOSI::updatestate hWB lambda"<<endl;
// v->display();
// #endif
// *v += * ds->workspace(DynamicalSystem::free);
// #ifdef SCHATZMANPAOLI_NE_DEBUG
// std::cout<<"SchatzmanPaoliOSI::updatestate work free"<<endl;
// ds->workspace(DynamicalSystem::free)->display();
// std::cout<<"SchatzmanPaoliOSI::updatestate new v"<<endl;
// v->display();
// #endif
// //compute q
// //first step consists in computing \dot q.
// //second step consists in updating q.
// //
// SP::SiconosMatrix T = d->T();
// SP::SiconosVector dotq = d->dotq();
// prod(*T,*v,*dotq,true);
// // std::cout<<"SchatzmanPaoliOSI::updateState v"<<endl;
// // v->display();
// // std::cout<<"SchatzmanPaoliOSI::updateState dotq"<<endl;
// // dotq->display();
// SP::SiconosVector q = d->q();
// // -> get previous time step state
// SP::SiconosVector dotqold = d->dotqMemory()->getSiconosVector(0);
// SP::SiconosVector qold = d->qMemory()->getSiconosVector(0);
// // *q = *qold + h*(theta * *v +(1.0 - theta)* *vold)
// double coeff = h*_theta;
// scal(coeff, *dotq, *q) ; // q = h*theta*v
// coeff = h*(1-_theta);
// scal(coeff,*dotqold,*q,false); // q += h(1-theta)*vold
// *q += *qold;
// #ifdef SCHATZMANPAOLI_NE_DEBUG
// std::cout<<"new q before normalizing"<<endl;
// q->display();
// #endif
// //q[3:6] must be normalized
// d->normalizeq();
// dotq->setValue(3,(q->getValue(3)-qold->getValue(3))/h);
// dotq->setValue(4,(q->getValue(4)-qold->getValue(4))/h);
// dotq->setValue(5,(q->getValue(5)-qold->getValue(5))/h);
// dotq->setValue(6,(q->getValue(6)-qold->getValue(6))/h);
// d->updateT();
RuntimeException::selfThrow("SchatzmanPaoliOSI::updateState - not yet implemented for Dynamical system type: " + dsType);
}
else RuntimeException::selfThrow("SchatzmanPaoliOSI::updateState - not yet implemented for Dynamical system type: " + dsType);
}
}
