void MLCPProjectOnConstraints::postCompute() { _hasBeenUpdated = true; // This function is used to set y/lambda values using output from // lcp_driver (w,z). Only Interactions (ie Interactions) of // indexSet(leveMin) are concerned. // === Get index set from Topology === SP::InteractionsGraph indexSet = simulation()->indexSet(indexSetLevel()); // y and lambda vectors SP::SiconosVector lambda; SP::SiconosVector y; // === Loop through "active" Interactions (ie present in // indexSets[1]) === /** We chose to do a small step _alpha in view of stabilized the algorithm.*/ #ifdef MLCPPROJ_DEBUG printf("MLCPProjectOnConstraints::postCompute damping value = %f\n", _alpha); #endif (*_z) *= _alpha; unsigned int pos = 0; #ifdef MLCPPROJ_DEBUG printf("MLCPProjectOnConstraints::postCompute _z\n"); _z->display(); display(); #endif InteractionsGraph::VIterator ui, uiend; for (std11::tie(ui, uiend) = indexSet->vertices(); ui != uiend; ++ui) { SP::Interaction inter = indexSet->bundle(*ui); // Get the relative position of inter-interactionBlock in the vector w // or z pos = _M->getPositionOfInteractionBlock(*inter); RELATION::TYPES relationType = inter->relation()->getType(); if (relationType == NewtonEuler) { postComputeNewtonEulerR(inter, pos); } else if (relationType == Lagrangian) { postComputeLagrangianR(inter, pos); } else { RuntimeException::selfThrow("MLCPProjectOnConstraints::computeInteractionBlock - relation type is not from Lagrangian type neither NewtonEuler."); } } }
void SchatzmanPaoliOSI::computeFreeOutput(InteractionsGraph::VDescriptor& vertex_inter, OneStepNSProblem* osnsp) { DEBUG_BEGIN("SchatzmanPaoliOSI::computeFreeOutput(InteractionsGraph::VDescriptor& vertex_inter, OneStepNSProblem* osnsp)\n"); /** \warning: ensures that it can also work with two different osi for two different ds ? */ SP::InteractionsGraph indexSet = osnsp->simulation()->indexSet(osnsp->indexSetLevel()); SP::Interaction inter = indexSet->bundle(vertex_inter); SP::OneStepNSProblems allOSNS = _simulation->oneStepNSProblems(); VectorOfBlockVectors& inter_work_block = *indexSet->properties(vertex_inter).workBlockVectors; // Get relation and non smooth law types RELATION::TYPES relationType = inter->relation()->getType(); RELATION::SUBTYPES relationSubType = inter->relation()->getSubType(); unsigned int sizeY = inter->nonSmoothLaw()->size(); unsigned int relativePosition = 0; Index coord(8); coord[0] = relativePosition; coord[1] = relativePosition + sizeY; coord[2] = 0; coord[4] = 0; coord[6] = 0; coord[7] = sizeY; SP::SiconosMatrix C; SP::SiconosMatrix D; SP::SiconosMatrix F; SP::BlockVector deltax; SiconosVector& osnsp_rhs = *(*indexSet->properties(vertex_inter).workVectors)[SchatzmanPaoliOSI::OSNSP_RHS]; SP::SiconosVector e; SP::BlockVector Xfree = inter_work_block[SchatzmanPaoliOSI::xfree];; assert(Xfree); SP::Interaction mainInteraction = inter; assert(mainInteraction); assert(mainInteraction->relation()); DEBUG_EXPR(inter->display(););
void MLCPProjectOnConstraints::postComputeNewtonEulerR(SP::Interaction inter, unsigned int pos) { SP::NewtonEulerR ner = (std11::static_pointer_cast<NewtonEulerR>(inter->relation())); SP::SiconosVector lambda = inter->lambda(0); SP::SiconosVector y = inter->y(0); unsigned int sizeY = std11::static_pointer_cast<OSNSMatrixProjectOnConstraints> (_M)->computeSizeForProjection(inter); // Copy _w/_z values, starting from index pos into y/lambda. //setBlock(*_w, y, sizeY, pos, 0); setBlock(*_z, lambda, sizeY, pos, 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(););
void EulerMoreauOSI::computeW(double t, DynamicalSystem& ds, DynamicalSystemsGraph::VDescriptor& dsgVD) { // 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. unsigned int dsN = ds.number(); assert((WMap.find(dsN) != WMap.end()) && "EulerMoreauOSI::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); SiconosMatrix& W = *WMap[dsN]; // 1 - First order non linear systems if (dsType == Type::FirstOrderNonLinearDS) { // W = M - h*_theta* [jacobian_x f(t,x,z)] FirstOrderNonLinearDS& d = static_cast<FirstOrderNonLinearDS&> (ds); // Copy M or I if M is Null into W if (d.M()) W = *d.M(); else W.eye(); d.computeJacobianfx(t); // Add -h*_theta*jacobianfx to W scal(-h * _theta, *d.jacobianfx(), W, false); } // 2 - First order linear systems else if (dsType == Type::FirstOrderLinearDS || dsType == Type::FirstOrderLinearTIDS) { FirstOrderLinearDS& d = static_cast<FirstOrderLinearDS&> (ds); if (dsType == Type::FirstOrderLinearDS) d.computeA(t); if (d.M()) W = *d.M(); else W.eye(); scal(-h * _theta, *d.A(), W, false); } else RuntimeException::selfThrow("EulerMoreauOSI::computeW - not yet implemented for Dynamical system type :" + dsType); // if (_useGamma) { Topology& topo = *simulationLink->model()->nonSmoothDynamicalSystem()->topology(); DynamicalSystemsGraph& DSG0 = *topo.dSG(0); InteractionsGraph& indexSet = *topo.indexSet(0); DynamicalSystemsGraph::OEIterator oei, oeiend; InteractionsGraph::VDescriptor ivd; SP::SiconosMatrix K; SP::Interaction inter; for (std11::tie(oei, oeiend) = DSG0.out_edges(dsgVD); oei != oeiend; ++oei) { inter = DSG0.bundle(*oei); ivd = indexSet.descriptor(inter); FirstOrderR& rel = static_cast<FirstOrderR&>(*inter->relation()); K = rel.K(); if (!K) K = (*indexSet.properties(ivd).workMatrices)[FirstOrderR::mat_K]; if (K) { scal(-h * _gamma, *K, W, false); } } } // Remark: W is not LU-factorized here. // Function PLUForwardBackward will do that if required. }
void LsodarOSI::computeFreeOutput(InteractionsGraph::VDescriptor& vertex_inter, OneStepNSProblem* osnsp) { SP::OneStepNSProblems allOSNS = simulationLink->oneStepNSProblems(); SP::InteractionsGraph indexSet = osnsp->simulation()->indexSet(osnsp->indexSetLevel()); SP::Interaction inter = indexSet->bundle(vertex_inter); VectorOfBlockVectors& DSlink = *indexSet->properties(vertex_inter).DSlink; // Get relation and non smooth law types RELATION::TYPES relationType = inter->relation()->getType(); RELATION::SUBTYPES relationSubType = inter->relation()->getSubType(); unsigned int sizeY = inter->nonSmoothLaw()->size(); unsigned int relativePosition = 0; SP::Interaction mainInteraction = inter; Index coord(8); coord[0] = relativePosition; coord[1] = relativePosition + sizeY; coord[2] = 0; coord[4] = 0; coord[6] = 0; coord[7] = sizeY; SP::SiconosMatrix C; // SP::SiconosMatrix D; // SP::SiconosMatrix F; SiconosVector& yForNSsolver = *inter->yForNSsolver(); SP::BlockVector Xfree; // All of these values should be stored in the node corrseponding to the Interactionwhen a MoreauJeanOSI scheme is used. /* V.A. 10/10/2010 * Following the type of OSNS we need to retrieve the velocity or the acceleration * This tricks is not very nice but for the moment the OSNS do not known if * it is in accelaration of not */ //SP::OneStepNSProblems allOSNS = _simulation->oneStepNSProblems(); if (((*allOSNS)[SICONOS_OSNSP_ED_SMOOTH_ACC]).get() == osnsp) { if (relationType == Lagrangian) { Xfree = DSlink[LagrangianR::xfree]; } // else if (relationType == NewtonEuler) // { // Xfree = inter->data(NewtonEulerR::free); // } assert(Xfree); // std::cout << "Computeqblock Xfree (Gamma)========" << std::endl; // Xfree->display(); } else if (((*allOSNS)[SICONOS_OSNSP_ED_IMPACT]).get() == osnsp) { Xfree = DSlink[LagrangianR::q1]; // std::cout << "Computeqblock Xfree (Velocity)========" << std::endl; // Xfree->display(); } else RuntimeException::selfThrow(" computeqBlock for Event Event-driven is wrong "); if (relationType == Lagrangian) { C = mainInteraction->relation()->C(); if (C) { assert(Xfree); coord[3] = C->size(1); coord[5] = C->size(1); subprod(*C, *Xfree, yForNSsolver, coord, true); } SP::SiconosMatrix ID(new SimpleMatrix(sizeY, sizeY)); ID->eye(); Index xcoord(8); xcoord[0] = 0; xcoord[1] = sizeY; xcoord[2] = 0; xcoord[3] = sizeY; xcoord[4] = 0; xcoord[5] = sizeY; xcoord[6] = 0; xcoord[7] = sizeY; // For the relation of type LagrangianRheonomousR if (relationSubType == RheonomousR) { if (((*allOSNS)[SICONOS_OSNSP_ED_SMOOTH_ACC]).get() == osnsp) { RuntimeException::selfThrow("LsodarOSI::computeFreeOutput not yet implemented for LCP at acceleration level with LagrangianRheonomousR"); } else if (((*allOSNS)[SICONOS_OSNSP_TS_VELOCITY]).get() == osnsp) { SiconosVector q = *DSlink[LagrangianR::q0]; SiconosVector z = *DSlink[LagrangianR::z]; std11::static_pointer_cast<LagrangianRheonomousR>(inter->relation())->computehDot(simulation()->getTkp1(), q, z); *DSlink[LagrangianR::z] = z; subprod(*ID, *(std11::static_pointer_cast<LagrangianRheonomousR>(inter->relation())->hDot()), yForNSsolver, xcoord, false); // y += hDot } else RuntimeException::selfThrow("LsodarOSI::computeFreeOutput not implemented for SICONOS_OSNSP "); } // For the relation of type LagrangianScleronomousR if (relationSubType == ScleronomousR) { if (((*allOSNS)[SICONOS_OSNSP_ED_SMOOTH_ACC]).get() == osnsp) { std11::static_pointer_cast<LagrangianScleronomousR>(inter->relation())->computedotjacqhXqdot(simulation()->getTkp1(), *inter, DSlink); subprod(*ID, *(std11::static_pointer_cast<LagrangianScleronomousR>(inter->relation())->dotjacqhXqdot()), yForNSsolver, xcoord, false); // y += NonLinearPart } } } else RuntimeException::selfThrow("LsodarOSI::computeFreeOutput not yet implemented for Relation of type " + relationType); if (((*allOSNS)[SICONOS_OSNSP_ED_IMPACT]).get() == osnsp) { if (inter->relation()->getType() == Lagrangian || inter->relation()->getType() == NewtonEuler) { SP::SiconosVisitor nslEffectOnFreeOutput(new _NSLEffectOnFreeOutput(osnsp, inter)); inter->nonSmoothLaw()->accept(*nslEffectOnFreeOutput); } } }
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 SchatzmanPaoliOSI::computeFreeOutput(InteractionsGraph::VDescriptor& vertex_inter, OneStepNSProblem* osnsp) { /** \warning: ensures that it can also work with two different osi for two different ds ? */ SP::InteractionsGraph indexSet = osnsp->simulation()->indexSet(osnsp->indexSetLevel()); SP::Interaction inter = indexSet->bundle(vertex_inter); SP::OneStepNSProblems allOSNS = simulationLink->oneStepNSProblems(); VectorOfBlockVectors& DSlink = *indexSet->properties(vertex_inter).DSlink; // Get relation and non smooth law types RELATION::TYPES relationType = inter->relation()->getType(); RELATION::SUBTYPES relationSubType = inter->relation()->getSubType(); unsigned int sizeY = inter->nonSmoothLaw()->size(); unsigned int relativePosition = 0; Index coord(8); coord[0] = relativePosition; coord[1] = relativePosition + sizeY; coord[2] = 0; coord[4] = 0; coord[6] = 0; coord[7] = sizeY; SP::SiconosMatrix C; SP::SiconosMatrix D; SP::SiconosMatrix F; SP::BlockVector deltax; SiconosVector& yForNSsolver = *inter->yForNSsolver(); SP::SiconosVector e; SP::BlockVector Xfree; if (relationType == NewtonEuler) { Xfree = DSlink[NewtonEulerR::xfree]; } else if (relationType == Lagrangian) { Xfree = DSlink[LagrangianR::xfree]; } assert(Xfree); assert(Xfree); SP::Interaction mainInteraction = inter; assert(mainInteraction); assert(mainInteraction->relation()); if (relationSubType == LinearTIR) { if (((*allOSNS)[SICONOS_OSNSP_TS_VELOCITY]).get() != osnsp) RuntimeException::selfThrow("SchatzmanPaoliOSI::computeFreeOutput not yet implemented for SICONOS_OSNSP "); C = mainInteraction->relation()->C(); if (C) { assert(Xfree); coord[3] = C->size(1); coord[5] = C->size(1); // creates a POINTER link between workX[ds] (xfree) and the // corresponding interactionBlock in each Interactionfor each ds of the // current Interaction. if (_useGammaForRelation) { assert(deltax); subprod(*C, *deltax, yForNSsolver, coord, true); } else { subprod(*C, *Xfree, yForNSsolver, coord, true); // subprod(*C,*(*(mainInteraction->dynamicalSystemsBegin()))->workspace(DynamicalSystem::free),*Yp,coord,true); // if (mainInteraction->dynamicalSystems()->size() == 2) // { // subprod(*C,*(*++(mainInteraction->dynamicalSystemsBegin()))->workspace(DynamicalSystem::free),*Yp,coord,false); // } } } SP::LagrangianLinearTIR ltir = std11::static_pointer_cast<LagrangianLinearTIR> (mainInteraction->relation()); e = ltir->e(); if (e) { yForNSsolver += *e; } } else RuntimeException::selfThrow("SchatzmanPaoliOSI::ComputeFreeOutput not yet implemented for relation of Type : " + relationType); if (inter->relation()->getSubType() == LinearTIR) { SP::SiconosVisitor nslEffectOnFreeOutput(new _NSLEffectOnFreeOutput(osnsp, inter)); inter->nonSmoothLaw()->accept(*nslEffectOnFreeOutput); } }
void MLCPProjectOnConstraints::computeDiagonalInteractionBlock(const InteractionsGraph::VDescriptor& vd) { SP::InteractionsGraph indexSet = simulation()->indexSet(indexSetLevel()); SP::DynamicalSystem DS1 = indexSet->properties(vd).source; SP::DynamicalSystem DS2 = indexSet->properties(vd).target; SP::Interaction inter = indexSet->bundle(vd); SP::OneStepIntegrator Osi = indexSet->properties(vd).osi; unsigned int pos1, pos2; pos1 = indexSet->properties(vd).source_pos; pos2 = indexSet->properties(vd).target_pos; unsigned int sizeY = 0; sizeY = std11::static_pointer_cast<OSNSMatrixProjectOnConstraints> (_M)->computeSizeForProjection(inter); #ifdef MLCPPROJ_DEBUG std::cout << "\nMLCPProjectOnConstraints::computeDiagonalInteractionBlock" <<std::endl; std::cout << "indexSetLevel()" << indexSetLevel() << std::endl; // std::cout << "indexSet :"<< indexSet << std::endl; // std::cout << "vd :"<< vd << std::endl; // indexSet->display(); // std::cout << "DS1 :" << std::endl; // DS1->display(); // std::cout << "DS2 :" << std::endl; // DS2->display(); #endif assert(indexSet->blockProj[vd]); SP::SiconosMatrix currentInteractionBlock = indexSet->blockProj[vd]; #ifdef MLCPPROJ_DEBUG // std::cout<<"MLCPProjectOnConstraints::computeDiagonalInteractionBlock "<<std::endl; // currentInteractionBlock->display(); std::cout << "sizeY " << sizeY << std::endl; std::cout << "blockProj " << indexSet->blockProj[vd].get() << " of edge " << vd << " of size " << currentInteractionBlock->size(0) << " x " << currentInteractionBlock->size(0) << " for interaction " << inter->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) == sizeY); assert(currentInteractionBlock->size(1) == sizeY); if (!_hasBeenUpdated) computeOptions(inter, inter); // 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. // Get the W and Theta maps of one of the Interaction - // Warning: in the current version, if OSI!=MoreauJeanOSI, this fails. // If OSI = MOREAU, centralInteractionBlocks = W if OSI = LSODAR, // centralInteractionBlocks = M (mass matrices) SP::SiconosMatrix leftInteractionBlock, rightInteractionBlock, leftInteractionBlock1; // 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. VectorOfSMatrices& workMInter = *indexSet->properties(vd).workMatrices; currentInteractionBlock->zero(); // loop over the common DS bool endl = false; unsigned int pos = pos1; for (SP::DynamicalSystem ds = DS1; !endl; ds = DS2) { assert(ds == DS1 || ds == DS2); endl = (ds == DS2); if (Type::value(*ds) == Type::LagrangianLinearTIDS || Type::value(*ds) == Type::LagrangianDS) { if (inter->relation()->getType() != Lagrangian) { RuntimeException::selfThrow( "MLCPProjectOnConstraints::computeDiagonalInteractionBlock - relation is not of type Lagrangian with a LagrangianDS."); } SP::LagrangianDS lds = (std11::static_pointer_cast<LagrangianDS>(ds)); unsigned int sizeDS = lds->getDim(); leftInteractionBlock.reset(new SimpleMatrix(sizeY, sizeDS)); inter->getLeftInteractionBlockForDS(pos, leftInteractionBlock, workMInter); if (lds->boundaryConditions()) // V.A. Should we do that ? { for (std::vector<unsigned int>::iterator itindex = lds->boundaryConditions()->velocityIndices()->begin() ; itindex != lds->boundaryConditions()->velocityIndices()->end(); ++itindex) { // (sizeY,sizeDS)); SP::SiconosVector coltmp(new SiconosVector(sizeY)); coltmp->zero(); leftInteractionBlock->setCol(*itindex, *coltmp); } } // (inter1 == inter2) SP::SiconosMatrix work(new SimpleMatrix(*leftInteractionBlock)); // // std::cout<<"LinearOSNS : leftUBlock\n"; // work->display(); work->trans(); // std::cout<<"LinearOSNS::computeInteractionBlock leftInteractionBlock"<<endl; // leftInteractionBlock->display(); if (_useMassNormalization) { SP::SiconosMatrix centralInteractionBlock = getOSIMatrix(Osi, ds); centralInteractionBlock->PLUForwardBackwardInPlace(*work); prod(*leftInteractionBlock, *work, *currentInteractionBlock, false); // gemm(CblasNoTrans,CblasNoTrans,1.0,*leftInteractionBlock,*work,1.0,*currentInteractionBlock); } else { prod(*leftInteractionBlock, *work, *currentInteractionBlock, false); } //*currentInteractionBlock *=h; } else if (Type::value(*ds) == Type::NewtonEulerDS) { if (inter->relation()->getType() != NewtonEuler) { RuntimeException::selfThrow("MLCPProjectOnConstraints::computeDiagonalInteractionBlock - relation is not from NewtonEulerR."); } SP::NewtonEulerDS neds = (std11::static_pointer_cast<NewtonEulerDS>(ds)); #ifdef MLCPPROJ_WITH_CT unsigned int sizeDS = neds->getDim(); 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); leftInteractionBlock.reset(new SimpleMatrix(sizeY, sizeDS)); inter->getLeftInteractionBlockForDS(pos, leftInteractionBlock); SP::SiconosMatrix work(new SimpleMatrix(*leftInteractionBlock)); std::cout << "LinearOSNS : leftUBlock\n"; work->display(); work->trans(); std::cout << "LinearOSNS::computeInteractionBlock workT2" <<std::endl; workT2->display(); workT2->PLUForwardBackwardInPlace(*work); prod(*leftInteractionBlock, *work, *currentInteractionBlock, false); #else if (0) //(std11::static_pointer_cast<NewtonEulerR> inter->relation())->_isConstact){ { // unsigned int sizeDS = neds->getDim(); // 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); // leftInteractionBlock1.reset(new SimpleMatrix(sizeY, sizeDS)); // inter->getLeftInteractionBlockForDS(pos, leftInteractionBlock); // leftInteractionBlock.reset(new SimpleMatrix(1, sizeDS)); // for (unsigned int ii = 0; ii < sizeDS; ii++) // leftInteractionBlock->setValue(1, ii, leftInteractionBlock1->getValue(1, ii)); // // SP::SiconosMatrix work(new SimpleMatrix(*leftInteractionBlock)); // //cout<<"LinearOSNS : leftUBlock\n"; // //work->display(); // work->trans(); // //cout<<"LinearOSNS::computeInteractionBlock workT2"<<endl; // //workT2->display(); // workT2->PLUForwardBackwardInPlace(*work); // prod(*leftInteractionBlock, *work, *currentInteractionBlock, false); } else { unsigned int sizeDS = (std11::static_pointer_cast<NewtonEulerDS>(ds))->getqDim(); leftInteractionBlock.reset(new SimpleMatrix(sizeY, sizeDS)); inter->getLeftInteractionBlockForDSProjectOnConstraints(pos, leftInteractionBlock); // #ifdef MLCPPROJ_DEBUG // std::cout << "MLCPProjectOnConstraints::computeDiagonalInteractionBlock - NewtonEuler case leftInteractionBlock : " << std::endl; // leftInteractionBlock->display(); // #endif SP::SiconosMatrix work(new SimpleMatrix(*leftInteractionBlock)); //cout<<"LinearOSNS sizeY="<<sizeY<<": leftUBlock\n"; //work->display(); work->trans(); prod(*leftInteractionBlock, *work, *currentInteractionBlock, false); // #ifdef MLCPPROJ_DEBUG // std::cout << "MLCPProjectOnConstraints::computeDiagonalInteractionBlock - NewtonEuler case currentInteractionBlock : "<< std::endl; // currentInteractionBlock->display(); // #endif } } else RuntimeException::selfThrow("MLCPProjectOnConstraints::computeDiagonalInteractionBlock - ds is not from NewtonEulerDS neither a LagrangianDS."); #endif #ifdef MLCPPROJ_DEBUG std::cout << "MLCPProjectOnConstraints::computeDiagonalInteractionBlock DiaginteractionBlock " << std::endl; currentInteractionBlock->display(); #endif // Set pos for next loop. pos = pos2; } }
void MLCPProjectOnConstraints::computeOptions(SP::Interaction inter1, SP::Interaction inter2) { // printf("MLCPProjectOnConstraints::computeOptions\n"); // Get dimension of the NonSmoothLaw (ie dim of the interactionBlock) RELATION::TYPES relationType1; relationType1 = inter1->relation()->getType(); // Retrieve size of Y (projected variable) unsigned int sizeY1; sizeY1 = std11::static_pointer_cast<OSNSMatrixProjectOnConstraints> (_M)->computeSizeForProjection(inter1); // Compute the number of equalities unsigned int equalitySize1 = sizeY1; //default behavior if (Type::value(*(inter1->nonSmoothLaw())) == Type::NewtonImpactFrictionNSL || Type::value(*(inter1->nonSmoothLaw())) == Type::NewtonImpactNSL) { if (_doProjOnEquality) { equalitySize1 = sizeY1; } else { equalitySize1 = 0; } } else if (Type::value(*(inter1->nonSmoothLaw())) == Type::MixedComplementarityConditionNSL) { equalitySize1 = std11::static_pointer_cast<MixedComplementarityConditionNSL>(inter1->nonSmoothLaw())->getEqualitySize(); } // Compute the number of inequalities unsigned int inequalitySize1 = sizeY1 - equalitySize1; if (inter1 == inter2) { //inter1->getExtraInteractionBlock(currentInteractionBlock); _m += inequalitySize1; _n += equalitySize1; // _m=0; //_n=6; if (_curBlock > MLCP_NB_BLOCKS - 2) printf("MLCP.cpp : number of block to small, memory crach below!!!\n"); /*add an equality block.*/ // #ifdef MLCPPROJ_DEBUG // printf("MLCPProjectOnConstraints::computeOptions()\n"); // #endif if (equalitySize1 > 0) { _numerics_problem.blocksRows[_curBlock + 1] = _numerics_problem.blocksRows[_curBlock] + equalitySize1; _numerics_problem.blocksIsComp[_curBlock] = 0; // #ifdef MLCPPROJ_DEBUG // std::cout << "_curBlock : " << _curBlock <<std::endl; // std::cout << "_numerics_problem.blocksRows["<<_curBlock+1 <<" ] : " << _numerics_problem.blocksRows[_curBlock+1] <<std::endl; // std::cout << "_numerics_problem.blocksIsComp["<<_curBlock <<" ] : " << _numerics_problem.blocksIsComp[_curBlock] <<std::endl; // #endif _curBlock++; } /*add a complementarity block.*/ if (inequalitySize1 > 0) { _numerics_problem.blocksRows[_curBlock + 1] = _numerics_problem.blocksRows[_curBlock] + inequalitySize1; _numerics_problem.blocksIsComp[_curBlock] = 1; // #ifdef MLCPPROJ_DEBUG // std::cout << "_curBlock : " << _curBlock <<std::endl; // std::cout << "_numerics_problem.blocksRows["<<_curBlock+1<< "] : " << _numerics_problem.blocksRows[_curBlock+1] <<std::endl; // std::cout << "_numerics_problem.blocksIsComp["<<_curBlock<< "] : " << _numerics_problem.blocksIsComp[_curBlock] <<std::endl; // #endif _curBlock++; } } // #ifdef MLCPPROJ_DEBUG // std::cout << "_m : " << _m <<std::endl; // std::cout << "_n : " << _n <<std::endl; // #endif }
void MLCPProjectOnConstraints::postComputeLagrangianR(SP::Interaction inter, unsigned int pos) { SP::LagrangianR lr = std11::static_pointer_cast<LagrangianR>(inter->relation()); #ifdef MLCPPROJ_DEBUG printf("MLCPProjectOnConstraints::postComputeLagrangian inter->y(0)\n"); inter->y(0)->display(); printf("MLCPProjectOnConstraints::postComputeLagrangian lr->jachq \n"); lr->jachq()->display(); printf("MLCPProjectOnConstraints::postComputeLagrangianR q before update\n"); SP::InteractionsGraph indexSet = simulation()->indexSet(indexSetLevel()); InteractionsGraph::VDescriptor ui = indexSet->descriptor(inter); InteractionsGraph::OEIterator oei, oeiend; for(std11::tie(oei, oeiend) = indexSet->out_edges(ui); oei != oeiend; ++oei) { SP::LagrangianDS lds = std11::static_pointer_cast<LagrangianDS>(indexSet->bundle(*oei)); lds->q()->display(); } #endif //unsigned int sizeY = inter->nonSmoothLaw()->size(); // y and lambda vectors SP::SiconosVector lambda = inter->lambda(0); SP::SiconosVector y = inter->y(0); unsigned int sizeY = std11::static_pointer_cast<OSNSMatrixProjectOnConstraints> (_M)->computeSizeForProjection(inter); // Copy _w/_z values, starting from index pos into y/lambda. //setBlock(*_w, y, sizeY, pos, 0); setBlock(*_z, lambda, sizeY, pos, 0); #ifdef MLCPPROJ_DEBUG printf("MLCPP lambda of Interaction is pos =%i :\n", pos); // aBuff->display(); lambda->display(); unsigned int nslawsize = inter->nonSmoothLaw()->size(); SP::SiconosVector aBuff(new SiconosVector(nslawsize)); setBlock(*_z, aBuff, sizeY, pos, 0); SP::SiconosMatrix J = lr->jachq(); SP::SimpleMatrix aux(new SimpleMatrix(*J)); aux->trans(); // SP::SiconosVector tmp(new SiconosVector(*(lr->q()))); // prod(*aux, *aBuff, *(tmp), false); // //prod(*aux,*lambda,*(lr->q()),false); // std:: std::cout << " tmp = tmp + J^T * lambda" << std::endl; // tmp->display(); #endif // // WARNING : Must not be done here. and should be called with the correct time. // // compute p(0) // inter->computeInput(0.0 ,0); // // \warning aBuff should normally be in lambda[0] // // The update of the position in DS should be made // // in MoreauJeanOSI::upateState or ProjectedMoreauJeanOSI::updateState // SP::SiconosMatrix J=lr->jachq(); // SP::SimpleMatrix aux(new SimpleMatrix(*J)); // aux->trans(); // SP::SiconosVector tmp (new SiconosVector(*(lr->q()))); // std:: std::cout << " tmp ="<<std::endl; // tmp->display(); // std:: std::cout << " lr->q() ="<<std::endl; // lr->q()->display(); // //prod(*aux,*lambda,*(lr->q()),false); // prod(*aux,*aBuff,*(tmp),false); // std:: std::cout << " tmp = tmp + J * lambda"<<std::endl; // tmp->display(); // // The following step should be done on MoreauJeanOSI::upateState or ProjectedMoreauJeanOSI::updateState // DSIterator itDS = inter->dynamicalSystemsBegin(); // while(itDS!=inter->dynamicalSystemsEnd()) // { // Type::Siconos dsType = Type::value(**itDS); // if((dsType !=Type::LagrangianDS) and // (dsType !=Type::LagrangianLinearTIDS) ) // { // RuntimeException::selfThrow("MLCPProjectOnConstraint::postCompute- ds is not of Lagrangian DS type."); // } // SP::LagrangianDS d = std11::static_pointer_cast<LagrangianDS> (*itDS); // SP::SiconosVector q = d->q(); // *q += *d->p(0); // std::cout << " q=" << std::endl; // q->display(); // itDS++; // } // if ((*lr->q() - *tmp).normInf() > 1e-12) // { // RuntimeException::selfThrow("youyou"); // } #ifdef MLCPPROJ_DEBUG printf("MLCPProjectOnConstraints::postComputeLagrangianR _z\n"); _z->display(); printf("MLCPProjectOnConstraints::postComputeLagrangianR updated\n"); VectorOfBlockVectors& DSlink = *(indexSet->properties(ui)).DSlink; // (*DSlink[LagrangianR::q0]).display(); // (lr->q())->display(); #endif //RuntimeException::selfThrow("MLCPProjectOnConstraints::postComputeLagrangianR() - not yet implemented"); }
void DisksViewer::draw() { int i; char qs[6]; float lbd, w; float lbdmax = 0.; DSIterator itDS; SP::DynamicalSystemsSet involvedDS; SP::InteractionsGraph I1; SP::Interaction interaction; SP::Relation relation; if (Siconos_->model()->nonSmoothDynamicalSystem()->topology()->numberOfIndexSet() > 1) { I1 = Siconos_->model()->simulation()->indexSet(1); // calibration InteractionsGraph::VIterator ui, uiend; for (boost::tie(ui, uiend) = I1->vertices(); ui != uiend; ++ui) { lbdmax = fmax(I1->bundle(*ui)->lambdaOld(1)->getValue(0), lbdmax); } for (boost::tie(ui, uiend) = I1->vertices(); ui != uiend; ++ui) { interaction = I1->bundle(*ui); relation = interaction->relation(); lbd = interaction->lambdaOld(1)->getValue(0); // screen width of interaction w = lbd / (2 * fmax(lbdmax, 1.)) + .03; // disk/disk SP::DynamicalSystem d1 = I1->properties(*ui).source; SP::DynamicalSystem d2 = I1->properties(*ui).target; SP::SiconosVector q1 = ask<ForPosition>(*d1); float x1 = (*q1)(0); float y1 = (*q1)(1); float r1 = ask<ForRadius>(*d1); if (d1 != d2) { SP::SiconosVector q2 = ask<ForPosition>(*d2); float x2 = (*q2)(0); float y2 = (*q2)(1); float r2 = ask<ForRadius>(*d2); float d = hypotf(x1 - x2, y1 - y2); glPushMatrix(); glColor3f(.0f, .0f, .0f); drawRec(x1, y1, x1 + (x2 - x1)*r1 / d, y1 + (y2 - y1)*r1 / d, w); drawRec(x2, y2, x2 + (x1 - x2)*r2 / d, y2 + (y1 - y2)*r2 / d, w); glPopMatrix(); } else { SP::SiconosMatrix jachq = ask<ForJachq>(*relation); double jx = jachq->getValue(0, 0); double jy = jachq->getValue(0, 1); double dj = hypot(jx, jy); glPushMatrix(); glColor3f(.0f, .0f, .0f); drawRec(x1, y1, x1 - r1 * jx / dj, y1 - r1 * jy / dj, w); glPopMatrix(); } } } for (unsigned int i = 0; i < GETNDS(Siconos_); i++) { if (shapes_[i]->selected()) { drawSelectedQGLShape(*shapes_[i]); } else { drawQGLShape(*shapes_[i]); } } glColor3f(.45, .45, .45); glLineWidth(1.); drawGrid(100, 200); setGridIsDrawn(); glColor3f(.1, .1, .3); drawVec(-100, 0, 100, 0); drawVec(0, -100, 0, 100); glColor3f(0, 0, 1); glLineWidth(4.); if (Siconos_->plans()) { for (unsigned int i = 0 ; i < Siconos_->plans()->size(0) ; ++i) { double A = (*Siconos_->plans())(i, 0); double B = (*Siconos_->plans())(i, 1); //double C = (*Siconos_->plans())(i,2); double xc = (*Siconos_->plans())(i, 3); double yc = (*Siconos_->plans())(i, 4); double w = fmin(1e10, (*Siconos_->plans())(i, 5)); double H = hypot(A, B); if (w == 0) w = 1e10; // assert ( fabs(A*xc + B*yc + C) <= std::numeric_limits<double>::epsilon() ); drawVec(xc, yc, xc - 0.5 * w * B / H, yc + 0.5 * w * A / H); drawVec(xc, yc, xc + 0.5 * w * B / H, yc - 0.5 * w * A / H); } } if (Siconos_->movingPlans()) { double time = Siconos_->model()->currentTime(); for (unsigned int i = 0 ; i < Siconos_->movingPlans()->size1() ; ++i) { double A = (*Siconos_->movingPlans())(i, 0)(time); double B = (*Siconos_->movingPlans())(i, 1)(time); double C = (*Siconos_->movingPlans())(i, 2)(time); double w = 1e10; double H = hypot(A, B); double xc = 0.; double yc = 0.; if (fabs(C) > std::numeric_limits<double>::epsilon()) { if (A == 0) // By+C=0 { yc = -C / B; } else if (B == 0) // Ax+C=0 { xc = -C / A; } else // Ax+By+C=0 { if (xc != 0) yc = - (A * xc + C) / B; else xc = - (B * yc + C) / A; } } drawVec(xc, yc, xc - 0.5 * w * B / H, yc + 0.5 * w * A / H); drawVec(xc, yc, xc + 0.5 * w * B / H, yc - 0.5 * w * A / H); } } glColor3f(.1, .1, .1); glLineWidth(4.); QGLViewer::drawArrow(qglviewer::Vec(0, 0, .1), qglviewer::Vec(1, 0, .1), .01, 3); QGLViewer::drawArrow(qglviewer::Vec(0, 0, .1), qglviewer::Vec(0, 1, .1), .01, 3); glLineWidth(1.); for (i = -100; i <= 100; i += 5) { sprintf(qs, "%d", i); // print((float)i,-.8,qs,small_text); //print(-.8,(float)i,qs,small_text); drawVec((float)i, -.2, (float)i, .2); drawVec(-.2, (float)i, .2, (float)i); } for (i = -100; i <= 100; i++) { drawVec((float)i, -.1, (float)i, .1); drawVec(-.1, (float)i, .1, (float)i); } }