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 SchatzmanPaoliOSI::initializeIterationMatrixW(double t, SP::DynamicalSystem ds) { // This function: // - allocate memory for the matrix W // - update its content for the current (initial) state of the dynamical system, depending on its type. if(!ds) RuntimeException::selfThrow("SchatzmanPaoliOSI::initializeIterationMatrixW(t,ds) - ds == NULL"); if(!(checkOSI(_dynamicalSystemsGraph->descriptor(ds)))) RuntimeException::selfThrow("SchatzmanPaoliOSI::initializeIterationMatrixW(t,ds) - ds does not belong to the OSI."); const DynamicalSystemsGraph::VDescriptor& dsv = _dynamicalSystemsGraph->descriptor(ds); if(_dynamicalSystemsGraph->properties(dsv).W) RuntimeException::selfThrow("SchatzmanPaoliOSI::initializeIterationMatrixW(t,ds) - W(ds) is already in the map and has been initialized."); // Memory allocation for W double h = _simulation->timeStep(); Type::Siconos dsType = Type::value(*ds); unsigned int sizeW = ds->dimension(); // 1 - Lagrangian non linear systems if(dsType == Type::LagrangianDS) { RuntimeException::selfThrow("SchatzmanPaoliOSI::initializeIterationMatrixW - 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); if(d->mass()) { _dynamicalSystemsGraph->properties(dsv).W.reset(new SimpleMatrix(*d->mass())); //*W = *d->mass(); } else { _dynamicalSystemsGraph->properties(dsv).W.reset(new SimpleMatrix(sizeW, sizeW)); _dynamicalSystemsGraph->properties(dsv).W->eye(); } SP::SiconosMatrix K = d->K(); SP::SiconosMatrix C = d->C(); SP::SiconosMatrix W = _dynamicalSystemsGraph->properties(dsv).W; if(C) scal(1 / 2.0 * h * _theta, *C, *W, false); // W += 1/2.0*h*_theta *C if(K) scal(h * h * _theta * _theta, *K, *W, false); // W = h*h*_theta*_theta*K // WBoundaryConditions initialization if(d->boundaryConditions()) initializeIterationMatrixWBoundaryConditions(d,dsv); } // === === else if(dsType == Type::NewtonEulerDS) { RuntimeException::selfThrow("SchatzmanPaoliOSI::initializeIterationMatrixW - not yet implemented for Dynamical system type :" + dsType); } else RuntimeException::selfThrow("SchatzmanPaoliOSI::initializeIterationMatrixW - 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 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(););
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))->dimension(); 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->dimension(); 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); }