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 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");
}
}
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
}
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 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 MLCPProjectOnConstraints::computeInteractionBlock(const InteractionsGraph::EDescriptor& ed)
{
// Computes matrix _interactionBlocks[inter1][inter2] (and allocates memory if
// necessary) if inter1 and inter2 have commond DynamicalSystem. 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.
#ifdef MLCPPROJ_DEBUG
std::cout << "MLCPProjectOnConstraints::computeInteractionBlock currentInteractionBlock start " << std::endl;
#endif
// Get dimension of the NonSmoothLaw (ie dim of the interactionBlock)
SP::InteractionsGraph indexSet = simulation()->indexSet(indexSetLevel());
SP::DynamicalSystem ds = indexSet->bundle(ed);
SP::Interaction inter1 = indexSet->bundle(indexSet->source(ed));
SP::Interaction inter2 = indexSet->bundle(indexSet->target(ed));
// For the edge 'ds', we need to find relative position of this ds
// in inter1 and inter2 relation matrices (--> pos1 and pos2 below)
// - find if ds is source or target in inter_i
InteractionsGraph::VDescriptor vertex_inter;
// - get the corresponding position
unsigned int pos1, pos2;
// source of inter1 :
vertex_inter = indexSet->source(ed);
VectorOfSMatrices& workMInter1 = *indexSet->properties(vertex_inter).workMatrices;
SP::OneStepIntegrator Osi = indexSet->properties(vertex_inter).osi;
SP::DynamicalSystem tmpds = indexSet->properties(vertex_inter).source;
if (tmpds == ds)
pos1 = indexSet->properties(vertex_inter).source_pos;
else
{
tmpds = indexSet->properties(vertex_inter).target;
pos1 = indexSet->properties(vertex_inter).target_pos;
}
// now, inter2
vertex_inter = indexSet->target(ed);
VectorOfSMatrices& workMInter2 = *indexSet->properties(vertex_inter).workMatrices;
tmpds = indexSet->properties(vertex_inter).source;
if (tmpds == ds)
pos2 = indexSet->properties(vertex_inter).source_pos;
else
{
tmpds = indexSet->properties(vertex_inter).target;
pos2 = indexSet->properties(vertex_inter).target_pos;
}
unsigned int index1 = indexSet->index(indexSet->source(ed));
unsigned int index2 = indexSet->index(indexSet->target(ed));
unsigned int sizeY1 = 0;
sizeY1 = std11::static_pointer_cast<OSNSMatrixProjectOnConstraints>
(_M)->computeSizeForProjection(inter1);
unsigned int sizeY2 = 0;
sizeY2 = std11::static_pointer_cast<OSNSMatrixProjectOnConstraints>
(_M)->computeSizeForProjection(inter2);
SP::SiconosMatrix currentInteractionBlock;
assert(index1 != index2);
if (index2 > index1) // upper block
{
// if (! indexSet->properties(ed).upper_block)
// {
// indexSet->properties(ed).upper_block.reset(new SimpleMatrix(sizeY1, sizeY2));
// }
currentInteractionBlock = indexSet->upper_blockProj[ed];
#ifdef MLCPPROJ_DEBUG
std::cout << "MLCPProjectOnConstraints::computeInteractionBlock currentInteractionBlock " << std::endl;
// currentInteractionBlock->display();
std::cout << "sizeY1 " << sizeY1 << std::endl;
std::cout << "sizeY2 " << sizeY2 << std::endl;
std::cout << "upper_blockProj " << indexSet->upper_blockProj[ed].get() << " of edge " << ed << " of size " << currentInteractionBlock->size(0) << " x " << currentInteractionBlock->size(0) << " for interaction " << inter1->number() << " and interaction " << inter2->number() << std::endl;
// std::cout<<"inter1->display() "<< inter1->number()<< std::endl;
//inter1->display();
// std::cout<<"inter2->display() "<< inter2->number()<< std::endl;
//inter2->display();
#endif
assert(currentInteractionBlock->size(0) == sizeY1);
assert(currentInteractionBlock->size(1) == sizeY2);
}
else // lower block
{
// if (! indexSet->properties(ed).lower_block)
// {
// indexSet->properties(ed).lower_block.reset(new SimpleMatrix(sizeY1, sizeY2));
// }
assert(indexSet->lower_blockProj[ed]->size(0) == sizeY1);
assert(indexSet->lower_blockProj[ed]->size(1) == sizeY2);
currentInteractionBlock = indexSet->lower_blockProj[ed];
}
SP::SiconosMatrix leftInteractionBlock, rightInteractionBlock;
RELATION::TYPES relationType1, relationType2;
// 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.
relationType1 = inter1->relation()->getType();
relationType2 = inter2->relation()->getType();
if (relationType1 == NewtonEuler &&
relationType2 == NewtonEuler)
{
assert(inter1 != inter2);
currentInteractionBlock->zero();
#ifdef MLCPPROJ_WITH_CT
unsigned int sizeDS = (std11::static_pointer_cast<NewtonEulerDS>(ds))->getDim();
leftInteractionBlock.reset(new SimpleMatrix(sizeY1, sizeDS));
inter1->getLeftInteractionBlockForDS(pos1, leftInteractionBlock);
SP::NewtonEulerDS neds = (std11::static_pointer_cast<NewtonEulerDS>(ds));
SP::SimpleMatrix T = neds->T();
SP::SimpleMatrix workT(new SimpleMatrix(*T));
workT->trans();
SP::SimpleMatrix workT2(new SimpleMatrix(6, 6));
prod(*workT, *T, *workT2, true);
rightInteractionBlock.reset(new SimpleMatrix(sizeY2, sizeDS));
inter2->getLeftInteractionBlockForDS(pos2, rightInteractionBlock);
rightInteractionBlock->trans();
workT2->PLUForwardBackwardInPlace(*rightInteractionBlock);
prod(*leftInteractionBlock, *rightInteractionBlock, *currentInteractionBlock, false);
#else
unsigned int sizeDS = (std11::static_pointer_cast<NewtonEulerDS>(ds))->getqDim();
leftInteractionBlock.reset(new SimpleMatrix(sizeY1, sizeDS));
inter1->getLeftInteractionBlockForDSProjectOnConstraints(pos1, leftInteractionBlock);
SP::NewtonEulerDS neds = (std11::static_pointer_cast<NewtonEulerDS>(ds));
rightInteractionBlock.reset(new SimpleMatrix(sizeY2, sizeDS));
inter2->getLeftInteractionBlockForDSProjectOnConstraints(pos2, rightInteractionBlock);
rightInteractionBlock->trans();
prod(*leftInteractionBlock, *rightInteractionBlock, *currentInteractionBlock, false);
}
#endif
else if (relationType1 == Lagrangian &&
relationType2 == Lagrangian)
{
unsigned int sizeDS = ds->getDim();
leftInteractionBlock.reset(new SimpleMatrix(sizeY1, sizeDS));
inter1->getLeftInteractionBlockForDS(pos1, leftInteractionBlock, workMInter1);
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()) // V.A. Should we do that ?
{
for (std::vector<unsigned int>::iterator itindex =
d->boundaryConditions()->velocityIndices()->begin() ;
itindex != d->boundaryConditions()->velocityIndices()->end();
++itindex)
{
// (sizeY1,sizeDS));
SP::SiconosVector coltmp(new SiconosVector(sizeY1));
coltmp->zero();
leftInteractionBlock->setCol(*itindex, *coltmp);
}
}
}
#ifdef MLCPPROJ_DEBUG
std::cout << "MLCPProjectOnConstraints::computeInteractionBlock : leftInteractionBlock" << std::endl;
leftInteractionBlock->display();
#endif
// inter1 != inter2
rightInteractionBlock.reset(new SimpleMatrix(sizeY2, sizeDS));
inter2->getLeftInteractionBlockForDS(pos2, rightInteractionBlock, workMInter2);
#ifdef MLCPPROJ_DEBUG
std::cout << "MLCPProjectOnConstraints::computeInteractionBlock : rightInteractionBlock" << std::endl;
rightInteractionBlock->display();
#endif
// Warning: we use getLeft for Right interactionBlock
// because right = transpose(left) and because of
// size checking inside the getBlock function, a
// getRight call will fail.
SP::SiconosMatrix centralInteractionBlock = getOSIMatrix(Osi, ds);
#ifdef MLCPPROJ_DEBUG
std::cout << "MLCPProjectOnConstraints::computeInteractionBlock : centralInteractionBlocks " << std::endl;
centralInteractionBlock->display();
#endif
rightInteractionBlock->trans();
if (_useMassNormalization)
{
centralInteractionBlock->PLUForwardBackwardInPlace(*rightInteractionBlock);
//*currentInteractionBlock += *leftInteractionBlock ** work;
prod(*leftInteractionBlock, *rightInteractionBlock, *currentInteractionBlock, false);
}
else
{
prod(*leftInteractionBlock, *rightInteractionBlock, *currentInteractionBlock, false);
}
#ifdef MLCPPROJ_DEBUG
std::cout << "MLCPProjectOnConstraints::computeInteractionBlock : currentInteractionBlock" << std::endl;
currentInteractionBlock->display();
#endif
}
else
RuntimeException::selfThrow("MLCPProjectOnConstraints::computeInteractionBlock not yet implemented for relation of type " + relationType1);
}