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));
}
}
}
}
}
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");
}
}
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 Hem5OSI::computeJacobianRhs(double t)
{
DynamicalSystemsGraph::VIterator dsi, dsend;
for (std11::tie(dsi, dsend) = _dynamicalSystemsGraph->vertices(); dsi != dsend; ++dsi)
{
if (!checkOSI(dsi)) continue;
SP::DynamicalSystem ds = _dynamicalSystemsGraph->bundle(*dsi);
ds->computeJacobianRhsx(t);
}
}
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 Topology::insertDynamicalSystem(SP::DynamicalSystem ds)
{
DynamicalSystemsGraph::VDescriptor dsgv = _DSG[0]->add_vertex(ds);
_DSG[0]->properties(dsgv).workVectors.reset(new VectorOfVectors());
_DSG[0]->properties(dsgv).workMatrices.reset(new VectorOfMatrices());
ds->initWorkSpace(*_DSG[0]->properties(dsgv).workVectors, *_DSG[0]->properties(dsgv).workMatrices);
}
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 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::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()];
}
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::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;
}
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 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;
}
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 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 !!
}
std::pair<DynamicalSystemsGraph::EDescriptor, InteractionsGraph::VDescriptor>
Topology::link(SP::Interaction inter, SP::DynamicalSystem ds, SP::DynamicalSystem ds2)
{
DEBUG_PRINTF("Topology::link : inter %p, ds1 %p, ds2 %p\n", &*inter, &*ds,
&*ds2);
if (indexSet0()->is_vertex(inter))
{
removeInteractionFromIndexSet(inter);
}
unsigned int sumOfDSSizes = 0, sumOfZSizes = 0;
sumOfDSSizes += ds->getDim();
if(ds->z())
sumOfZSizes += ds->z()->size();
if(ds2)
{
sumOfDSSizes += ds2->getDim();
if(ds->z())
sumOfZSizes += ds2->z()->size();
inter->setHas2Bodies(true);
}
DEBUG_PRINTF("sumOfDSSizes = %i\t, sumOfZSizes = %i\n ", sumOfDSSizes, sumOfZSizes);
inter->setDSSizes(sumOfDSSizes, sumOfZSizes);
return addInteractionInIndexSet0(inter, ds, ds2);
}
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 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);
}
}
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 LinearOSNS::computeDiagonalInteractionBlock(const InteractionsGraph::VDescriptor& vd)
{
DEBUG_BEGIN("LinearOSNS::computeDiagonalInteractionBlock(const InteractionsGraph::VDescriptor& vd)\n");
// Computes matrix _interactionBlocks[inter1][inter1] (and allocates memory if
// necessary) one or two DS are concerned by inter1 . How
// _interactionBlocks are computed depends explicitely on the type of
// Relation of each Interaction.
// Warning: we suppose that at this point, all non linear
// operators (G for lagrangian relation for example) have been
// computed through plug-in mechanism.
// Get dimension of the NonSmoothLaw (ie dim of the interactionBlock)
SP::InteractionsGraph indexSet = simulation()->indexSet(indexSetLevel());
SP::Interaction inter = indexSet->bundle(vd);
// Get osi property from interaction
// We assume that all ds in vertex_inter have the same osi.
SP::OneStepIntegrator Osi = indexSet->properties(vd).osi;
//SP::OneStepIntegrator Osi = simulation()->integratorOfDS(ds);
OSI::TYPES osiType = Osi->getType();
// At most 2 DS are linked by an Interaction
SP::DynamicalSystem DS1;
SP::DynamicalSystem DS2;
unsigned int pos1, pos2;
// --- Get the dynamical system(s) (edge(s)) connected to the current interaction (vertex) ---
if (indexSet->properties(vd).source != indexSet->properties(vd).target)
{
DEBUG_PRINT("a two DS Interaction\n");
DS1 = indexSet->properties(vd).source;
DS2 = indexSet->properties(vd).target;
}
else
{
DEBUG_PRINT("a single DS Interaction\n");
DS1 = indexSet->properties(vd).source;
DS2 = DS1;
// \warning this looks like some debug code, but it gets executed even with NDEBUG.
// may be compiler does something smarter, but still it should be rewritten. --xhub
InteractionsGraph::OEIterator oei, oeiend;
for (std11::tie(oei, oeiend) = indexSet->out_edges(vd);
oei != oeiend; ++oei)
{
// note : at most 4 edges
DS2 = indexSet->bundle(*oei);
if (DS2 != DS1)
{
assert(false);
break;
}
}
}
assert(DS1);
assert(DS2);
pos1 = indexSet->properties(vd).source_pos;
pos2 = indexSet->properties(vd).target_pos;
// --- Check block size ---
assert(indexSet->properties(vd).block->size(0) == inter->nonSmoothLaw()->size());
assert(indexSet->properties(vd).block->size(1) == inter->nonSmoothLaw()->size());
// --- Compute diagonal block ---
// Block to be set in OSNS Matrix, corresponding to
// the current interaction
SP::SiconosMatrix currentInteractionBlock = indexSet->properties(vd).block;
SP::SiconosMatrix leftInteractionBlock, rightInteractionBlock;
RELATION::TYPES relationType;
double h = simulation()->currentTimeStep();
// General form of the interactionBlock is : interactionBlock =
// a*extraInteractionBlock + b * leftInteractionBlock * centralInteractionBlocks
// * rightInteractionBlock a and b are scalars, centralInteractionBlocks a
// matrix depending on the integrator (and on the DS), the
// simulation type ... left, right and extra depend on the relation
// type and the non smooth law.
relationType = inter->relation()->getType();
VectorOfSMatrices& workMInter = *indexSet->properties(vd).workMatrices;
inter->getExtraInteractionBlock(currentInteractionBlock, workMInter);
unsigned int nslawSize = inter->nonSmoothLaw()->size();
// loop over the DS connected to the interaction.
bool endl = false;
unsigned int pos = pos1;
for (SP::DynamicalSystem ds = DS1; !endl; ds = DS2)
{
assert(ds == DS1 || ds == DS2);
endl = (ds == DS2);
unsigned int sizeDS = ds->dimension();
// get _interactionBlocks corresponding to the current DS
// These _interactionBlocks depends on the relation type.
leftInteractionBlock.reset(new SimpleMatrix(nslawSize, sizeDS));
inter->getLeftInteractionBlockForDS(pos, leftInteractionBlock, workMInter);
DEBUG_EXPR(leftInteractionBlock->display(););
// Computing depends on relation type -> move this in Interaction method?
if (relationType == FirstOrder)
{
rightInteractionBlock.reset(new SimpleMatrix(sizeDS, nslawSize));
inter->getRightInteractionBlockForDS(pos, rightInteractionBlock, workMInter);
if (osiType == OSI::EULERMOREAUOSI)
{
if ((std11::static_pointer_cast<EulerMoreauOSI> (Osi))->useGamma() || (std11::static_pointer_cast<EulerMoreauOSI> (Osi))->useGammaForRelation())
{
*rightInteractionBlock *= (std11::static_pointer_cast<EulerMoreauOSI> (Osi))->gamma();
}
}
// for ZOH, we have a different formula ...
if (osiType == OSI::ZOHOSI && indexSet->properties(vd).forControl)
{
*rightInteractionBlock = std11::static_pointer_cast<ZeroOrderHoldOSI>(Osi)->Bd(ds);
prod(*leftInteractionBlock, *rightInteractionBlock, *currentInteractionBlock, false);
}
else
{
// centralInteractionBlock contains a lu-factorized matrix and we solve
// centralInteractionBlock * X = rightInteractionBlock with PLU
SP::SiconosMatrix centralInteractionBlock = getOSIMatrix(Osi, ds);
centralInteractionBlock->PLUForwardBackwardInPlace(*rightInteractionBlock);
inter->computeKhat(*rightInteractionBlock, workMInter, h); // if K is non 0
// integration of r with theta method removed
// *currentInteractionBlock += h *Theta[*itDS]* *leftInteractionBlock * (*rightInteractionBlock); //left = C, right = W.B
//gemm(h,*leftInteractionBlock,*rightInteractionBlock,1.0,*currentInteractionBlock);
*leftInteractionBlock *= h;
prod(*leftInteractionBlock, *rightInteractionBlock, *currentInteractionBlock, false);
//left = C, right = inv(W).B
}
}
else if (relationType == Lagrangian ||
relationType == NewtonEuler)
{
SP::BoundaryCondition bc;
Type::Siconos dsType = Type::value(*ds);
if (dsType == Type::LagrangianLinearTIDS || dsType == Type::LagrangianDS)
{
SP::LagrangianDS d = std11::static_pointer_cast<LagrangianDS> (ds);
if (d->boundaryConditions()) bc = d->boundaryConditions();
}
else if (dsType == Type::NewtonEulerDS)
{
SP::NewtonEulerDS d = std11::static_pointer_cast<NewtonEulerDS> (ds);
if (d->boundaryConditions()) bc = d->boundaryConditions();
}
if (bc)
{
for (std::vector<unsigned int>::iterator itindex = bc->velocityIndices()->begin() ;
itindex != bc->velocityIndices()->end();
++itindex)
{
// (nslawSize,sizeDS));
SP::SiconosVector coltmp(new SiconosVector(nslawSize));
coltmp->zero();
leftInteractionBlock->setCol(*itindex, *coltmp);
}
}
DEBUG_PRINT("leftInteractionBlock after application of boundary conditions\n");
DEBUG_EXPR(leftInteractionBlock->display(););
std::pair<DynamicalSystemsGraph::EDescriptor, InteractionsGraph::VDescriptor>
Topology::addInteractionInIndexSet0(SP::Interaction inter, SP::DynamicalSystem ds1, SP::DynamicalSystem ds2)
{
// !! Private function !!
//
// Add inter and ds into IG/DSG
// Compute number of constraints
unsigned int nsLawSize = inter->nonSmoothLaw()->size();
unsigned int m = inter->getSizeOfY() / nsLawSize;
if (m > 1)
RuntimeException::selfThrow("Topology::addInteractionInIndexSet0 - m > 1. Obsolete !");
_numberOfConstraints += nsLawSize;
SP::DynamicalSystem ds2_ = ds2;
// _DSG is the hyper forest : (vertices : dynamical systems, edges :
// Interactions)
//
// _IG is the hyper graph : (vertices : Interactions, edges :
// dynamical systems)
assert(_DSG[0]->edges_number() == _IG[0]->size());
// _IG = L(_DSG), L is the line graph transformation
// vector of the Interaction
DynamicalSystemsGraph::VDescriptor dsgv1, dsgv2;
dsgv1 = _DSG[0]->add_vertex(ds1);
SP::VectorOfVectors workVds1 = _DSG[0]->properties(dsgv1).workVectors;
SP::VectorOfVectors workVds2;
if (!workVds1)
{
workVds1.reset(new VectorOfVectors());
_DSG[0]->properties(dsgv1).workMatrices.reset(new VectorOfMatrices());
ds1->initWorkSpace(*workVds1, *_DSG[0]->properties(dsgv1).workMatrices);
}
if(ds2)
{
dsgv2 = _DSG[0]->add_vertex(ds2);
workVds2 = _DSG[0]->properties(dsgv2).workVectors;
if (!workVds2)
{
workVds2.reset(new VectorOfVectors());
_DSG[0]->properties(dsgv2).workMatrices.reset(new VectorOfMatrices());
ds2->initWorkSpace(*workVds2, *_DSG[0]->properties(dsgv2).workMatrices);
}
}
else
{
dsgv2 = dsgv1;
ds2_ = ds1;
workVds2 = workVds1;
}
// this may be a multi edges graph
assert(!_DSG[0]->is_edge(dsgv1, dsgv2, inter));
assert(!_IG[0]->is_vertex(inter));
InteractionsGraph::VDescriptor ig_new_ve;
DynamicalSystemsGraph::EDescriptor new_ed;
std11::tie(new_ed, ig_new_ve) = _DSG[0]->add_edge(dsgv1, dsgv2, inter, *_IG[0]);
InteractionProperties& interProp = _IG[0]->properties(ig_new_ve);
interProp.DSlink.reset(new VectorOfBlockVectors);
interProp.workVectors.reset(new VectorOfVectors);
interProp.workMatrices.reset(new VectorOfSMatrices);
unsigned int nslawSize = inter->nonSmoothLaw()->size();
interProp.block.reset(new SimpleMatrix(nslawSize, nslawSize));
inter->setDSLinkAndWorkspace(interProp, *ds1, *workVds1, *ds2_, *workVds2);
// add self branches in vertex properties
// note : boost graph SEGFAULT on self branch removal
// see https://svn.boost.org/trac/boost/ticket/4622
_IG[0]->properties(ig_new_ve).source = ds1;
_IG[0]->properties(ig_new_ve).source_pos = 0;
if(!ds2)
{
_IG[0]->properties(ig_new_ve).target = ds1;
_IG[0]->properties(ig_new_ve).target_pos = 0;
}
else
{
_IG[0]->properties(ig_new_ve).target = ds2;
_IG[0]->properties(ig_new_ve).target_pos = ds1->getDim();
}
assert(_IG[0]->bundle(ig_new_ve) == inter);
assert(_IG[0]->is_vertex(inter));
assert(_DSG[0]->is_edge(dsgv1, dsgv2, inter));
assert(_DSG[0]->edges_number() == _IG[0]->size());
return std::pair<DynamicalSystemsGraph::EDescriptor, InteractionsGraph::VDescriptor>(new_ed, ig_new_ve);
}
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);
}
}
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::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.
}
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 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);
}
}
double D1MinusLinearOSI::computeResiduHalfExplicitAccelerationLevel()
{
DEBUG_BEGIN("\n D1MinusLinearOSI::computeResiduHalfExplicitAccelerationLevel()\n");
double t = _simulation->nextTime(); // end of the time step
double told = _simulation->startingTime(); // beginning of the time step
double h = _simulation->timeStep(); // time step length
SP::OneStepNSProblems allOSNS = _simulation->oneStepNSProblems(); // all OSNSP
SP::Topology topo = _simulation->nonSmoothDynamicalSystem()->topology();
SP::InteractionsGraph indexSet2 = topo->indexSet(2);
/**************************************************************************************************************
* Step 1- solve a LCP at acceleration level for lambda^+_{k} for the last set indices
* if index2 is empty we should skip this step
**************************************************************************************************************/
DEBUG_PRINT("\nEVALUATE LEFT HAND SIDE\n");
DEBUG_EXPR(std::cout<< "allOSNS->empty() " << std::boolalpha << allOSNS->empty() << std::endl << std::endl);
DEBUG_EXPR(std::cout<< "allOSNS->size() " << allOSNS->size() << std::endl << std::endl);
// -- LEFT SIDE --
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);
SP::SiconosVector accFree;
SP::SiconosVector work_tdg;
SP::SiconosMatrix Mold;
DEBUG_EXPR((*it)->display());
if ((dsType == Type::LagrangianDS) || (dsType == Type::LagrangianLinearTIDS))
{
SP::LagrangianDS d = std11::static_pointer_cast<LagrangianDS> (ds);
accFree = d->workspace(DynamicalSystem::free); /* POINTER CONSTRUCTOR : will contain
* the acceleration without contact force */
accFree->zero();
// get left state from memory
SP::SiconosVector qold = d->qMemory()->getSiconosVector(0);
SP::SiconosVector vold = d->velocityMemory()->getSiconosVector(0); // right limit
Mold = d->mass();
DEBUG_EXPR(accFree->display());
DEBUG_EXPR(qold->display());
DEBUG_EXPR(vold->display());
DEBUG_EXPR(Mold->display());
if (! d->workspace(DynamicalSystem::free_tdg))
{
d->allocateWorkVector(DynamicalSystem::free_tdg, d->dimension()) ;
}
work_tdg = d->workspace(DynamicalSystem::free_tdg);
work_tdg->zero();
DEBUG_EXPR(work_tdg->display());
if (d->forces())
{
d->computeForces(told, qold, vold);
DEBUG_EXPR(d->forces()->display());
*accFree += *(d->forces());
}
Mold->PLUForwardBackwardInPlace(*accFree); // contains left (right limit) acceleration without contact force
d->addWorkVector(accFree,DynamicalSystem::free_tdg); // store the value in WorkFreeFree
}
else if(dsType == Type::NewtonEulerDS)
{
SP::NewtonEulerDS d = std11::static_pointer_cast<NewtonEulerDS> (ds);
accFree = d->workspace(DynamicalSystem::free); // POINTER CONSTRUCTOR : contains acceleration without contact force
accFree->zero();
// get left state from memory
SP::SiconosVector qold = d->qMemory()->getSiconosVector(0);
SP::SiconosVector vold = d->velocityMemory()->getSiconosVector(0); // right limit
//Mold = d->mass();
assert(!d->mass()->isPLUInversed());
Mold.reset(new SimpleMatrix(*(d->mass()))); // we copy the mass matrix to avoid its factorization
DEBUG_EXPR(accFree->display());
DEBUG_EXPR(qold->display());
DEBUG_EXPR(vold->display());
DEBUG_EXPR(Mold->display());
if (! d->workspace(DynamicalSystem::free_tdg))
{
d->allocateWorkVector(DynamicalSystem::free_tdg, d->dimension()) ;
}
work_tdg = d->workspace(DynamicalSystem::free_tdg);
work_tdg->zero();
DEBUG_EXPR(work_tdg->display());
if (d->forces())
{
d->computeForces(told, qold, vold);
DEBUG_EXPR(d->forces()->display());
*accFree += *(d->forces());
}
Mold->PLUForwardBackwardInPlace(*accFree); // contains left (right limit) acceleration without contact force
d->addWorkVector(accFree,DynamicalSystem::free_tdg); // store the value in WorkFreeFree
}
else
{
RuntimeException::selfThrow("D1MinusLinearOSI::computeResidu - not yet implemented for Dynamical system type: " + dsType);
}
DEBUG_PRINT("accFree contains right limit acceleration at t^+_k with contact force :\n");
DEBUG_EXPR(accFree->display());
DEBUG_PRINT("work_tdg contains right limit acceleration at t^+_k without contact force :\n");
DEBUG_EXPR(work_tdg->display());
}
if (!allOSNS->empty())
{
if (indexSet2->size() >0)
{
InteractionsGraph::VIterator ui, uiend;
SP::Interaction inter;
for (std11::tie(ui, uiend) = indexSet2->vertices(); ui != uiend; ++ui)
{
inter = indexSet2->bundle(*ui);
inter->relation()->computeJach(t, *inter, indexSet2->properties(*ui));
inter->relation()->computeJacg(told, *inter, indexSet2->properties(*ui));
}
if (_simulation->nonSmoothDynamicalSystem()->topology()->hasChanged())
{
for (OSNSIterator itOsns = allOSNS->begin(); itOsns != allOSNS->end(); ++itOsns)
{
(*itOsns)->setHasBeenUpdated(false);
}
}
assert((*allOSNS)[SICONOS_OSNSP_TS_VELOCITY + 1]);
if (((*allOSNS)[SICONOS_OSNSP_TS_VELOCITY + 1]->hasInteractions())) // it should be equivalent to indexSet2
{
DEBUG_PRINT("We compute lambda^+_{k} \n");
(*allOSNS)[SICONOS_OSNSP_TS_VELOCITY + 1]->compute(told);
DEBUG_EXPR((*allOSNS)[SICONOS_OSNSP_TS_VELOCITY + 1]->display());
}
// Note Franck : at the time this results in a call to swapInMem of all Interactions of the NSDS
// So let the simu do this.
//(*allOSNS)[SICONOS_OSNSP_TS_VELOCITY + 1]->saveInMemory(); // we push y and lambda in Memories
_simulation->nonSmoothDynamicalSystem()->pushInteractionsInMemory();
_simulation->nonSmoothDynamicalSystem()->updateInput(_simulation->nextTime(),2);
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::LagrangianDS) || (dsType == Type::LagrangianLinearTIDS))
{
SP::LagrangianDS d = std11::static_pointer_cast<LagrangianDS> (ds);
SP::SiconosVector accFree = d->workspace(DynamicalSystem::free); // POINTER CONSTRUCTOR : contains acceleration without contact force
SP::SiconosVector dummy(new SiconosVector(*(d->p(2)))); // value = contact force
SP::SiconosMatrix Mold = d->mass();
Mold->PLUForwardBackwardInPlace(*dummy);
*accFree += *(dummy);
DEBUG_EXPR(d->p(2)->display());
}
else if (dsType == Type::NewtonEulerDS)
{
SP::NewtonEulerDS d = std11::static_pointer_cast<NewtonEulerDS> (ds);
SP::SiconosVector accFree = d->workspace(DynamicalSystem::free); // POINTER CONSTRUCTOR : contains acceleration without contact force
SP::SiconosVector dummy(new SiconosVector(*(d->p(2)))); // value = contact force
SP::SiconosMatrix Mold(new SimpleMatrix(*(d->mass()))); // we copy the mass matrix to avoid its factorization
DEBUG_EXPR(Mold->display());
Mold->PLUForwardBackwardInPlace(*dummy);
*accFree += *(dummy);
DEBUG_EXPR(d->p(2)->display());
}
else
RuntimeException::selfThrow("D1MinusLinearOSI::computeResidu - not yet implemented for Dynamical system type: " + dsType);
}
}
}
/**************************************************************************************************************
* Step 2 - compute v_{k,1}
**************************************************************************************************************/
DEBUG_PRINT("\n PREDICT RIGHT HAND SIDE\n");
for (std11::tie(dsi, dsend) = _dynamicalSystemsGraph->vertices(); dsi != dsend; ++dsi)
{
if (!checkOSI(dsi)) continue;
SP::DynamicalSystem ds = _dynamicalSystemsGraph->bundle(*dsi);
// type of the current DS
Type::Siconos dsType = Type::value(*ds);
/* \warning the following conditional statement should be removed with a MechanicalDS class */
if ((dsType == Type::LagrangianDS) || (dsType == Type::LagrangianLinearTIDS))
{
SP::LagrangianDS d = std11::static_pointer_cast<LagrangianDS> (ds);
SP::SiconosVector accFree = d->workspace(DynamicalSystem::free); // contains acceleration without contact force
// get left state from memory
SP::SiconosVector qold = d->qMemory()->getSiconosVector(0);
SP::SiconosVector vold = d->velocityMemory()->getSiconosVector(0);
// initialize *it->residuFree and predicted right velocity (left limit)
SP::SiconosVector residuFree = ds->workspace(DynamicalSystem::freeresidu); // contains residu without nonsmooth effect
SP::SiconosVector v = d->velocity(); //contains velocity v_{k+1}^- and not free velocity
residuFree->zero();
v->zero();
DEBUG_EXPR(accFree->display());
DEBUG_EXPR(qold->display());
DEBUG_EXPR(vold->display());
*residuFree -= 0.5 * h**accFree;
*v += h**accFree;
*v += *vold;
DEBUG_EXPR(residuFree->display());
DEBUG_EXPR(v->display());
SP::SiconosVector q = d->q(); // POINTER CONSTRUCTOR : contains position q_{k+1}
*q = *qold;
scal(0.5 * h, *vold + *v, *q, false);
DEBUG_EXPR(q->display());
}
else if (dsType == Type::NewtonEulerDS)
{
SP::NewtonEulerDS d = std11::static_pointer_cast<NewtonEulerDS> (ds);
SP::SiconosVector accFree = d->workspace(DynamicalSystem::free);
// get left state from memory
SP::SiconosVector qold = d->qMemory()->getSiconosVector(0);
SP::SiconosVector vold = d->velocityMemory()->getSiconosVector(0);
// initialize *it->residuFree and predicted right velocity (left limit)
SP::SiconosVector residuFree = ds->workspace(DynamicalSystem::freeresidu); // contains residu without nonsmooth effect
SP::SiconosVector v = d->velocity(); //contains velocity v_{k+1}^- and not free velocity
residuFree->zero();
v->zero();
DEBUG_EXPR(accFree->display());
DEBUG_EXPR(qold->display());
DEBUG_EXPR(vold->display());
*residuFree -= 0.5 * h**accFree;
*v += h**accFree;
*v += *vold;
DEBUG_EXPR(residuFree->display());
DEBUG_EXPR(v->display());
//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);
SP::SiconosVector dotqold = d->dotqMemory()->getSiconosVector(0);
SP::SiconosVector q = d->q(); // POINTER CONSTRUCTOR : contains position q_{k+1}
*q = *qold;
scal(0.5 * h, *dotqold + *dotq, *q, false);
DEBUG_PRINT("new q before normalizing\n");
DEBUG_EXPR(q->display());
//q[3:6] must be normalized
d->normalizeq();
d->computeT();
DEBUG_PRINT("new q after normalizing\n");
DEBUG_EXPR(q->display());
}
else
RuntimeException::selfThrow("D1MinusLinearOSI::computeResidu - not yet implemented for Dynamical system type: " + dsType);
/** At this step, we obtain
* \f[
* \begin{cases}
* v_{k,0} = \mbox{\tt vold} \\
* q_{k,0} = qold \\
* F_{k,+} = F(told,qold,vold) \\
* Work_{freefree} = M^{-1}_k (F^+_{k}) \mbox{stored in work_tdg} \\
* Work_{free} = M^{-1}_k (P^+_{2,k}+F^+_{k}) \mbox{stored in accFree} \\
* R_{free} = -h/2 * M^{-1}_k (P^+_{2,k}+F^+_{k}) \mbox{stored in ResiduFree} \\
* v_{k,1} = v_{k,0} + h * M^{-1}_k (P^+_{2,k}+F^+_{k}) \mbox{stored in v} \\
* q_{k,1} = q_{k,0} + \frac{h}{2} (v_{k,0} + v_{k,1}) \mbox{stored in q} \\
* \end{cases}
* \f]
**/
}
DEBUG_PRINT("\n DECIDE STRATEGY\n");
/** Decide of the strategy impact or smooth multiplier.
* Compute _isThereImpactInTheTimeStep
*/
_isThereImpactInTheTimeStep = false;
if (!allOSNS->empty())
{
for (unsigned int level = _simulation->levelMinForOutput();
level < _simulation->levelMaxForOutput(); level++)
{
_simulation->nonSmoothDynamicalSystem()->updateOutput(_simulation->nextTime(),level);
}
_simulation->updateIndexSets();
SP::Topology topo = _simulation->nonSmoothDynamicalSystem()->topology();
SP::InteractionsGraph indexSet3 = topo->indexSet(3);
if (indexSet3->size() > 0)
{
_isThereImpactInTheTimeStep = true;
DEBUG_PRINT("There is an impact in the step. indexSet3->size() > 0. _isThereImpactInTheTimeStep = true;\n");
}
else
{
_isThereImpactInTheTimeStep = false;
DEBUG_PRINT("There is no impact in the step. indexSet3->size() = 0. _isThereImpactInTheTimeStep = false;\n");
}
}
/* If _isThereImpactInTheTimeStep = true;
* we recompute residuFree by removing the contribution of the nonimpulsive contact forces.
* We add the contribution of the external forces at the end
* of the time--step
* If _isThereImpactInTheTimeStep = false;
* we recompute residuFree by adding the contribution of the external forces at the end
* and the contribution of the nonimpulsive contact forces that are computed by solving the osnsp.
*/
if (_isThereImpactInTheTimeStep)
{
DEBUG_PRINT("There is an impact in the step. indexSet3->size() > 0. _isThereImpactInTheTimeStep = true\n");
for (std11::tie(dsi, dsend) = _dynamicalSystemsGraph->vertices(); dsi != dsend; ++dsi)
{
if (!checkOSI(dsi)) continue;
SP::DynamicalSystem ds = _dynamicalSystemsGraph->bundle(*dsi);
// type of the current DS
Type::Siconos dsType = Type::value(*ds);
/* \warning the following conditional statement should be removed with a MechanicalDS class */
if ((dsType == Type::LagrangianDS) || (dsType == Type::LagrangianLinearTIDS))
{
SP::LagrangianDS d = std11::static_pointer_cast<LagrangianDS> (ds);
SP::SiconosVector residuFree = d->workspace(DynamicalSystem::freeresidu);
SP::SiconosVector v = d->velocity();
SP::SiconosVector q = d->q();
SP::SiconosVector qold = d->qMemory()->getSiconosVector(0);
SP::SiconosVector vold = d->velocityMemory()->getSiconosVector(0); // right limit
SP::SiconosMatrix M = d->mass(); // POINTER CONSTRUCTOR : contains mass matrix
//residuFree->zero();
//v->zero();
SP::SiconosVector work_tdg = d->workspace(DynamicalSystem::free_tdg);
assert(work_tdg);
*residuFree = - 0.5 * h**work_tdg;
d->computeMass();
DEBUG_EXPR(M->display());
if (d->forces())
{
d->computeForces(t, q, v);
*work_tdg = *(d->forces());
DEBUG_EXPR(d->forces()->display());
}
M->PLUForwardBackwardInPlace(*work_tdg); // contains right (left limit) acceleration without contact force
*residuFree -= 0.5 * h**work_tdg;
DEBUG_EXPR(residuFree->display());
}
else if (dsType == Type::NewtonEulerDS)
{
SP::NewtonEulerDS d = std11::static_pointer_cast<NewtonEulerDS> (ds);
SP::SiconosVector residuFree = d->workspace(DynamicalSystem::freeresidu);
SP::SiconosVector v = d->velocity();
SP::SiconosVector q = d->q();
SP::SiconosVector qold = d->qMemory()->getSiconosVector(0);
SP::SiconosVector vold = d->velocityMemory()->getSiconosVector(0); // right limit
SP::SiconosMatrix M(new SimpleMatrix(*(d->mass()))); // we copy the mass matrix to avoid its factorization;
DEBUG_EXPR(M->display());
//residuFree->zero();
v->zero();
SP::SiconosVector work_tdg = d->workspace(DynamicalSystem::free_tdg);
assert(work_tdg);
*residuFree = 0.5 * h**work_tdg;
work_tdg->zero();
if (d->forces())
{
d->computeForces(t, q, v);
*work_tdg += *(d->forces());
}
M->PLUForwardBackwardInPlace(*work_tdg); // contains right (left limit) acceleration without contact force
*residuFree -= 0.5 * h**work_tdg;
DEBUG_EXPR(residuFree->display());
}
else
RuntimeException::selfThrow("D1MinusLinearOSI::computeResidu - not yet implemented for Dynamical system type: " + dsType);
}
}
else
{
DEBUG_PRINT("There is no impact in the step. indexSet3->size() = 0. _isThereImpactInTheTimeStep = false;\n");
// -- RIGHT SIDE --
// calculate acceleration without contact force
for (std11::tie(dsi, dsend) = _dynamicalSystemsGraph->vertices(); dsi != dsend; ++dsi)
{
if (!checkOSI(dsi)) continue;
SP::DynamicalSystem ds = _dynamicalSystemsGraph->bundle(*dsi);
// type of the current DS
Type::Siconos dsType = Type::value(*ds);
/* \warning the following conditional statement should be removed with a MechanicalDS class */
if ((dsType == Type::LagrangianDS) || (dsType == Type::LagrangianLinearTIDS))
{
SP::LagrangianDS d = std11::static_pointer_cast<LagrangianDS> (ds);
SP::SiconosVector accFree = d->workspace(DynamicalSystem::free); // POINTER CONSTRUCTOR : contains acceleration without contact force
accFree->zero();
// get right state from memory
SP::SiconosVector q = d->q(); // contains position q_{k+1}
SP::SiconosVector v = d->velocity(); // contains velocity v_{k+1}^- and not free velocity
SP::SiconosMatrix M = d->mass(); // POINTER CONSTRUCTOR : contains mass matrix
DEBUG_EXPR(accFree->display());
DEBUG_EXPR(q->display());
DEBUG_EXPR(v->display());
// Lagrangian Nonlinear Systems
if (dsType == Type::LagrangianDS || dsType == Type::LagrangianLinearTIDS)
{
d->computeMass();
DEBUG_EXPR(M->display());
if (d->forces())
{
d->computeForces(t, q, v);
*accFree += *(d->forces());
}
}
else
RuntimeException::selfThrow
("D1MinusLinearOSI::computeResidu - not yet implemented for Dynamical system type: " + dsType);
M->PLUForwardBackwardInPlace(*accFree); // contains right (left limit) acceleration without contact force
DEBUG_PRINT("accFree contains left limit acceleration at t^-_{k+1} without contact force :\n");
DEBUG_EXPR(accFree->display());
}
else if (dsType == Type::NewtonEulerDS)
{
SP::NewtonEulerDS d = std11::static_pointer_cast<NewtonEulerDS> (ds);
SP::SiconosVector accFree = d->workspace(DynamicalSystem::free); // POINTER CONSTRUCTOR : contains acceleration without contact force
accFree->zero();
// get right state from memory
SP::SiconosVector q = d->q(); // contains position q_{k+1}
SP::SiconosVector v = d->velocity(); // contains velocity v_{k+1}^- and not free velocity
SP::SiconosMatrix M(new SimpleMatrix(*(d->mass()))); // we copy the mass matrix to avoid its factorization;
DEBUG_EXPR(accFree->display());
DEBUG_EXPR(q->display());
DEBUG_EXPR(v->display());
if (d->forces())
{
d->computeForces(t, q, v);
*accFree += *(d->forces());
}
M->PLUForwardBackwardInPlace(*accFree); // contains right (left limit) acceleration without contact force
DEBUG_PRINT("accFree contains left limit acceleration at t^-_{k+1} without contact force :\n");
DEBUG_EXPR(accFree->display());
}
else
RuntimeException::selfThrow("D1MinusLinearOSI::computeResidu - not yet implemented for Dynamical system type: " + dsType);
}
// solve a LCP at acceleration level only for contacts which have been active at the beginning of the time-step
if (!allOSNS->empty())
{
// for (unsigned int level = _simulation->levelMinForOutput(); level < _simulation->levelMaxForOutput(); level++)
// {
// _simulation->updateOutput(level);
// }
// _simulation->updateIndexSets();
DEBUG_PRINT("We compute lambda^-_{k+1} \n");
InteractionsGraph::VIterator ui, uiend;
SP::Interaction inter;
for (std11::tie(ui, uiend) = indexSet2->vertices(); ui != uiend; ++ui)
{
inter = indexSet2->bundle(*ui);
inter->relation()->computeJach(t, *inter, indexSet2->properties(*ui));
inter->relation()->computeJacg(t, *inter, indexSet2->properties(*ui));
}
if (_simulation->nonSmoothDynamicalSystem()->topology()->hasChanged())
{
for (OSNSIterator itOsns = allOSNS->begin(); itOsns != allOSNS->end(); ++itOsns)
{
(*itOsns)->setHasBeenUpdated(false);
}
}
if (((*allOSNS)[SICONOS_OSNSP_TS_VELOCITY + 1]->hasInteractions()))
{
(*allOSNS)[SICONOS_OSNSP_TS_VELOCITY + 1]->compute(t);
DEBUG_EXPR((*allOSNS)[SICONOS_OSNSP_TS_VELOCITY + 1]->display(););
_simulation->nonSmoothDynamicalSystem()->updateInput(_simulation->nextTime(),2);
}
void Simulation::initialize(SP::Model m, bool withOSI)
{
// === Connection with the model ===
assert(m && "Simulation::initialize(model) - model = NULL.");
_T = m->finalT();
_nsds = m->nonSmoothDynamicalSystem();
// === Events manager initialization ===
_eventsManager->initialize(_T);
_tinit = _eventsManager->startingTime();
//===
if (withOSI)
{
if (numberOfOSI() == 0)
RuntimeException::selfThrow("Simulation::initialize No OSI !");
DynamicalSystemsGraph::VIterator dsi, dsend;
SP::DynamicalSystemsGraph DSG = _nsds->topology()->dSG(0);
for (std11::tie(dsi, dsend) = DSG->vertices(); dsi != dsend; ++dsi)
{
SP::OneStepIntegrator osi = DSG->properties(*dsi).osi;
SP::DynamicalSystem ds = DSG->bundle(*dsi);
if (!osi)
{
// By default, if the user has not set the OSI, we assign the first OSI to all DS
_nsds->topology()->setOSI(ds,*_allOSI->begin());
//std::cout << "By default, if the user has not set the OSI, we assign the first OSI to all DS"<<std::endl;
}
osi = DSG->properties(*dsi).osi;
ds->initialize(m->t0(), osi->getSizeMem());
}
// === OneStepIntegrators initialization ===
for (OSIIterator itosi = _allOSI->begin();
itosi != _allOSI->end(); ++itosi)
{
// for (DSIterator itds = (*itosi)->dynamicalSystems()->begin();
// itds != (*itosi)->dynamicalSystems()->end();
// ++itds)
// {
// (*itds)->initialize(startingTime(),
// (*itosi)->getSizeMem());
// addInOSIMap(*itds, *itosi);
// }
(*itosi)->setSimulationPtr(shared_from_this());
(*itosi)->initialize(*m);
}
}
// 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 = _nsds->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 = _nsds->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();
// 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;
}
}
