void MoreauJeanDirectProjectionOSI::initializeWorkVectorsForDS( double t, SP::DynamicalSystem ds)
{
DEBUG_BEGIN("MoreauJeanDirectProjectionOSI::initializeWorkVectorsForDS( double t, SP::DynamicalSystem ds) \n");
MoreauJeanOSI::initializeWorkVectorsForDS(t, ds);
const DynamicalSystemsGraph::VDescriptor& dsv = _dynamicalSystemsGraph->descriptor(ds);
VectorOfVectors& workVectors = *_dynamicalSystemsGraph->properties(dsv).workVectors;
Type::Siconos dsType = Type::value(*ds);
if(dsType == Type::LagrangianDS || dsType == Type::LagrangianLinearTIDS)
{
SP::LagrangianDS d = std11::static_pointer_cast<LagrangianDS> (ds);
workVectors[MoreauJeanOSI::QTMP].reset(new SiconosVector(d->dimension()));
}
else if(dsType == Type::NewtonEulerDS)
{
SP::NewtonEulerDS d = std11::static_pointer_cast<NewtonEulerDS>(ds);
workVectors[MoreauJeanOSI::QTMP].reset(new SiconosVector(d->getqDim()));
}
else
{
RuntimeException::selfThrow("MoreauJeanDirectProjectionOSI::initialize() - DS not of the right type");
}
for (unsigned int k = _levelMinForInput ; k < _levelMaxForInput + 1; k++)
{
DEBUG_PRINTF("ds->initializeNonSmoothInput(%i)\n", k);
ds->initializeNonSmoothInput(k);
DEBUG_EXPR_WE(
SP::LagrangianDS d = std11::static_pointer_cast<LagrangianDS> (ds);
if (d->p(k))
std::cout << "d->p(" << k <<" ) exists" << std::endl;
);
}
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 D1MinusLinearOSI::initializeWorkVectorsForDS(double t, SP::DynamicalSystem ds)
{
// 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 || dsType == Type::LagrangianDS || dsType == Type::NewtonEulerDS);
if(dsType == Type::LagrangianDS || dsType == Type::LagrangianLinearTIDS)
{
SP::LagrangianDS lds = std11::static_pointer_cast<LagrangianDS> (ds);
lds->init_generalized_coordinates(2); // acceleration is required for the ds
lds->init_inverse_mass(); // invMass required to update post-impact velocity
ds_work_vectors.resize(D1MinusLinearOSI::WORK_LENGTH);
ds_work_vectors[D1MinusLinearOSI::RESIDU_FREE].reset(new SiconosVector(lds->dimension()));
ds_work_vectors[D1MinusLinearOSI::FREE].reset(new SiconosVector(lds->dimension()));
ds_work_vectors[D1MinusLinearOSI::FREE_TDG].reset(new SiconosVector(lds->dimension()));
// Update dynamical system components (for memory swap).
lds->computeForces(t, lds->q(), lds->velocity());
lds->swapInMemory();
}
else if(dsType == Type::NewtonEulerDS)
{
SP::NewtonEulerDS neds = std11::static_pointer_cast<NewtonEulerDS> (ds);
neds->init_inverse_mass(); // invMass required to update post-impact velocity
ds_work_vectors.resize(D1MinusLinearOSI::WORK_LENGTH);
ds_work_vectors[D1MinusLinearOSI::RESIDU_FREE].reset(new SiconosVector(neds->dimension()));
ds_work_vectors[D1MinusLinearOSI::FREE].reset(new SiconosVector(neds->dimension()));
ds_work_vectors[D1MinusLinearOSI::FREE_TDG].reset(new SiconosVector(neds->dimension()));
//Compute a first value of the forces to store it in _forcesMemory
neds->computeForces(t, neds->q(), neds->twist());
neds->swapInMemory();
}
else
RuntimeException::selfThrow("D1MinusLinearOSI::initialize - not implemented for Dynamical system type: " + dsType);
for (unsigned int k = _levelMinForInput ; k < _levelMaxForInput + 1; k++)
{
ds->initializeNonSmoothInput(k);
}
}
