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::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.
}