/** Constructor with the plugin name * \param PO a PluggedObject * \param n the number of rows of the matrix * \param p the number of column of the matrix * \param indx the column index (optional) */ SubPluggedObject(const PluggedObject& PO, const unsigned int n, const unsigned int p, const unsigned int indx = 0): _indx(indx), _p(p) { _pluginName = "Sub" + PO.getPluginName(); _tmpMat.reset(new SimpleMatrix(n, p)); #if (__GNUG__ && !( __clang__ || __INTEL_COMPILER || __APPLE__ )) fPtr = (void *)&SubPluggedObject::computeAndExtract; _parentfPtr = PO.fPtr; #else RuntimeException::selfThrow("SubPluggedObject must be compiled with GCC !"); #endif };
/** Constructor with the plugin name * \param PO a PluggedObject * \param n the number of rows of the matrix * \param p the number of column of the matrix * \param indx the column index (optional) */ SubPluggedObject(const PluggedObject& PO, const unsigned int n, const unsigned int p, const unsigned int indx = 0): _indx(indx), _p(p) { _pluginName = "Sub" + PO.pluginName(); _tmpMat.reset(new SimpleMatrix(n, p)); #if (__GNUG__ && !( __clang__ || __INTEL_COMPILER || __APPLE__ ) && (((__GNUC__ > 5) && (__GNUC_MINOR__ > 0)))) #pragma GCC diagnostic ignored "-Wpmf-conversions" fPtr = (void *)&SubPluggedObject::computeAndExtract; _parentfPtr = PO.fPtr; #else RuntimeException::selfThrow("SubPluggedObject must be compiled with GCC !"); #endif };
/** Copy constructor * \param SPO a PluggedObject we are going to copy */ SubPluggedObject(const SubPluggedObject& SPO): PluggedObject(SPO), _indx(SPO.getIndex()), _p(SPO.getp()) { _parentfPtr = SPO.getParentfPtr(); _tmpMat.reset(new SimpleMatrix(SPO.getTmpMat())); }
// ================= Creation of the model ======================= void Disks::init() { SP::TimeDiscretisation timedisc_; SP::TimeStepping simulation_; SP::FrictionContact osnspb_; // User-defined main parameters double t0 = 0; // initial computation time double T = std::numeric_limits<double>::infinity(); double h = 0.01; // time step double g = 9.81; double theta = 0.5; // theta for MoreauJeanOSI integrator std::string solverName = "NSGS"; // ----------------------------------------- // --- Dynamical systems && interactions --- // ----------------------------------------- double R; double m; try { // ------------ // --- Init --- // ------------ std::cout << "====> Model loading ..." << std::endl << std::endl; _plans.reset(new SimpleMatrix("plans.dat", true)); if (_plans->size(0) == 0) { /* default plans */ double A1 = P1A; double B1 = P1B; double C1 = P1C; double A2 = P2A; double B2 = P2B; double C2 = P2C; _plans.reset(new SimpleMatrix(6, 6)); _plans->zero(); (*_plans)(0, 0) = 0; (*_plans)(0, 1) = 1; (*_plans)(0, 2) = -GROUND; (*_plans)(1, 0) = 1; (*_plans)(1, 1) = 0; (*_plans)(1, 2) = WALL; (*_plans)(2, 0) = 1; (*_plans)(2, 1) = 0; (*_plans)(2, 2) = -WALL; (*_plans)(3, 0) = 0; (*_plans)(3, 1) = 1; (*_plans)(3, 2) = -TOP; (*_plans)(4, 0) = A1; (*_plans)(4, 1) = B1; (*_plans)(4, 2) = C1; (*_plans)(5, 0) = A2; (*_plans)(5, 1) = B2; (*_plans)(5, 2) = C2; } /* set center positions */ for (unsigned int i = 0 ; i < _plans->size(0); ++i) { SP::DiskPlanR tmpr; tmpr.reset(new DiskPlanR(1, (*_plans)(i, 0), (*_plans)(i, 1), (*_plans)(i, 2), (*_plans)(i, 3), (*_plans)(i, 4), (*_plans)(i, 5))); (*_plans)(i, 3) = tmpr->getXCenter(); (*_plans)(i, 4) = tmpr->getYCenter(); } /* _moving_plans.reset(new FMatrix(1,6)); (*_moving_plans)(0,0) = &A; (*_moving_plans)(0,1) = &B; (*_moving_plans)(0,2) = &C; (*_moving_plans)(0,3) = &DA; (*_moving_plans)(0,4) = &DB; (*_moving_plans)(0,5) = &DC;*/ SP::SiconosMatrix Disks; Disks.reset(new SimpleMatrix("disks.dat", true)); // -- OneStepIntegrators -- SP::OneStepIntegrator osi; osi.reset(new MoreauJeanOSI(theta)); // -- Model -- _model.reset(new Model(t0, T)); for (unsigned int i = 0; i < Disks->size(0); i++) { R = Disks->getValue(i, 2); m = Disks->getValue(i, 3); SP::SiconosVector qTmp; SP::SiconosVector vTmp; qTmp.reset(new SiconosVector(NDOF)); vTmp.reset(new SiconosVector(NDOF)); vTmp->zero(); (*qTmp)(0) = (*Disks)(i, 0); (*qTmp)(1) = (*Disks)(i, 1); SP::LagrangianDS body; if (R > 0) body.reset(new Disk(R, m, qTmp, vTmp)); else body.reset(new Circle(-R, m, qTmp, vTmp)); // -- Set external forces (weight) -- SP::SiconosVector FExt; FExt.reset(new SiconosVector(NDOF)); FExt->zero(); FExt->setValue(1, -m * g); body->setFExtPtr(FExt); // add the dynamical system to the one step integrator osi->insertDynamicalSystem(body); // add the dynamical system in the non smooth dynamical system _model->nonSmoothDynamicalSystem()->insertDynamicalSystem(body); } _model->nonSmoothDynamicalSystem()->setSymmetric(true); // ------------------ // --- Simulation --- // ------------------ // -- Time discretisation -- timedisc_.reset(new TimeDiscretisation(t0, h)); // -- OneStepNsProblem -- osnspb_.reset(new FrictionContact(2)); osnspb_->numericsSolverOptions()->iparam[0] = 100; // Max number of // iterations osnspb_->numericsSolverOptions()->iparam[1] = 20; // compute error // iterations osnspb_->numericsSolverOptions()->dparam[0] = 1e-3; // Tolerance osnspb_->setMaxSize(6 * ((3 * Ll * Ll + 3 * Ll) / 2 - Ll)); osnspb_->setMStorageType(1); // Sparse storage osnspb_->setNumericsVerboseMode(0); osnspb_->setKeepLambdaAndYState(true); // inject previous solution // -- Simulation -- simulation_.reset(new TimeStepping(timedisc_)); std11::static_pointer_cast<TimeStepping>(simulation_)->setNewtonMaxIteration(3); simulation_->insertIntegrator(osi); simulation_->insertNonSmoothProblem(osnspb_); simulation_->setCheckSolverFunction(localCheckSolverOuput); // --- Simulation initialization --- std::cout << "====> Simulation initialisation ..." << std::endl << std::endl; SP::NonSmoothLaw nslaw(new NewtonImpactFrictionNSL(0, 0, 0.3, 2)); _playground.reset(new SpaceFilter(3, 6, _model, _plans, _moving_plans)); _playground->insert(nslaw, 0, 0); _model->initialize(simulation_); } catch (SiconosException e) { std::cout << e.report() << std::endl; exit(1); } catch (...) { std::cout << "Exception caught in Disks::init()" << std::endl; exit(1); } }
// ================= Creation of the model ======================= void Spheres::init() { SP::TimeDiscretisation timedisc_; SP::Simulation simulation_; SP::FrictionContact osnspb_; // User-defined main parameters double t0 = 0; // initial computation time double T = std::numeric_limits<double>::infinity(); double h = 0.01; // time step double g = 9.81; double theta = 0.5; // theta for MoreauJeanOSI integrator std::string solverName = "NSGS"; // ----------------------------------------- // --- Dynamical systems && interactions --- // ----------------------------------------- double R; double m; try { // ------------ // --- Init --- // ------------ std::cout << "====> Model loading ..." << std::endl << std::endl; _plans.reset(new SimpleMatrix("plans.dat", true)); SP::SiconosMatrix Spheres; Spheres.reset(new SimpleMatrix("spheres.dat", true)); // -- OneStepIntegrators -- SP::OneStepIntegrator osi; osi.reset(new MoreauJeanOSI(theta)); // -- Model -- _model.reset(new Model(t0, T)); for (unsigned int i = 0; i < Spheres->size(0); i++) { R = Spheres->getValue(i, 3); m = Spheres->getValue(i, 4); SP::SiconosVector qTmp; SP::SiconosVector vTmp; qTmp.reset(new SiconosVector(NDOF)); vTmp.reset(new SiconosVector(NDOF)); vTmp->zero(); (*qTmp)(0) = (*Spheres)(i, 0); (*qTmp)(1) = (*Spheres)(i, 1); (*qTmp)(2) = (*Spheres)(i, 2); (*qTmp)(3) = M_PI / 2; (*qTmp)(4) = M_PI / 4; (*qTmp)(5) = M_PI / 2; (*vTmp)(0) = 0; (*vTmp)(1) = 0; (*vTmp)(2) = 0; (*vTmp)(3) = 0; (*vTmp)(4) = 0; (*vTmp)(5) = 0; SP::LagrangianDS body; body.reset(new SphereLDS(R, m, std11::shared_ptr<SiconosVector>(qTmp), std11::shared_ptr<SiconosVector>(vTmp))); // -- Set external forces (weight) -- SP::SiconosVector FExt; FExt.reset(new SiconosVector(NDOF)); FExt->zero(); FExt->setValue(2, -m * g); body->setFExtPtr(FExt); // add the dynamical system to the one step integrator osi->insertDynamicalSystem(body); // add the dynamical system in the non smooth dynamical system _model->nonSmoothDynamicalSystem()->insertDynamicalSystem(body); } // ------------------ // --- Simulation --- // ------------------ // -- Time discretisation -- timedisc_.reset(new TimeDiscretisation(t0, h)); // -- OneStepNsProblem -- osnspb_.reset(new FrictionContact(3)); osnspb_->numericsSolverOptions()->iparam[0] = 100; // Max number of // iterations osnspb_->numericsSolverOptions()->iparam[1] = 20; // compute error // iterations osnspb_->numericsSolverOptions()->iparam[4] = 2; // projection osnspb_->numericsSolverOptions()->dparam[0] = 1e-6; // Tolerance osnspb_->numericsSolverOptions()->dparam[2] = 1e-8; // Local tolerance osnspb_->setMaxSize(16384); // max number of interactions osnspb_->setMStorageType(1); // Sparse storage osnspb_->setNumericsVerboseMode(0); // 0 silent, 1 verbose osnspb_->setKeepLambdaAndYState(true); // inject previous solution simulation_.reset(new TimeStepping(timedisc_)); simulation_->insertIntegrator(osi); simulation_->insertNonSmoothProblem(osnspb_); // simulation_->setCheckSolverFunction(localCheckSolverOuput); // --- Simulation initialization --- std::cout << "====> Simulation initialisation ..." << std::endl << std::endl; SP::NonSmoothLaw nslaw(new NewtonImpactFrictionNSL(0, 0, 0.8, 3)); _playground.reset(new SpaceFilter(3, 6, _model, _plans, _moving_plans)); _playground->insert(nslaw, 0, 0); _model->initialize(simulation_); } catch (SiconosException e) { std::cout << e.report() << std::endl; exit(1); } catch (...) { std::cout << "Exception caught in Spheres::init()" << std::endl; exit(1); } }
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); }