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::updateState(const unsigned int level) { double h = simulationLink->timeStep(); double RelativeTol = simulationLink->relativeConvergenceTol(); bool useRCC = simulationLink->useRelativeConvergenceCriteron(); if (useRCC) simulationLink->setRelativeConvergenceCriterionHeld(true); DSIterator it; SP::SiconosMatrix W; for (it = OSIDynamicalSystems->begin(); it != OSIDynamicalSystems->end(); ++it) { SP::DynamicalSystem ds = *it; W = WMap[ds->number()]; // Get the DS type Type::Siconos dsType = Type::value(*ds); // 1 - Lagrangian Systems if (dsType == Type::LagrangianDS || dsType == Type::LagrangianLinearTIDS) { // get dynamical system SP::LagrangianDS d = std11::static_pointer_cast<LagrangianDS> (ds); // SiconosVector *vfree = d->velocityFree(); SP::SiconosVector q = d->q(); bool baux = dsType == Type::LagrangianDS && useRCC && simulationLink->relativeConvergenceCriterionHeld(); if (level != LEVELMAX) { // To compute q, we solve W(q - qfree) = p if (d->p(level)) { *q = *d->p(level); // q = p W->PLUForwardBackwardInPlace(*q); } // if (d->boundaryConditions()) // for (vector<unsigned int>::iterator // itindex = d->boundaryConditions()->velocityIndices()->begin() ; // itindex != d->boundaryConditions()->velocityIndices()->end(); // ++itindex) // v->setValue(*itindex, 0.0); *q += * ds->workspace(DynamicalSystem::free); } else *q = * ds->workspace(DynamicalSystem::free); // Computation of the velocity SP::SiconosVector v = d->velocity(); SP::SiconosVector q_k_1 = d->qMemory()->getSiconosVector(1); // q_{k-1} // std::cout << "SchatzmanPaoliOSI::updateState - q_k_1 =" <<std::endl; // q_k_1->display(); // std::cout << "SchatzmanPaoliOSI::updateState - q =" <<std::endl; // q->display(); *v = 1.0 / (2.0 * h) * (*q - *q_k_1); // std::cout << "SchatzmanPaoliOSI::updateState - v =" <<std::endl; // v->display(); // int bc=0; // SP::SiconosVector columntmp(new SiconosVector(ds->getDim())); // if (d->boundaryConditions()) // { // for (vector<unsigned int>::iterator itindex = d->boundaryConditions()->velocityIndices()->begin() ; // itindex != d->boundaryConditions()->velocityIndices()->end(); // ++itindex) // { // _WBoundaryConditionsMap[ds]->getCol(bc,*columntmp); // /*\warning we assume that W is symmetric in the Lagrangian case*/ // double value = - inner_prod(*columntmp, *v); // value += (d->p(level))->getValue(*itindex); // /* \warning the computation of reactionToBoundaryConditions take into // account the contact impulse but not the external and internal forces. // A complete computation of the residue should be better */ // d->reactionToBoundaryConditions()->setValue(bc,value) ; // bc++; // } if (baux) { ds->subWorkVector(q, DynamicalSystem::local_buffer); double aux = ((ds->workspace(DynamicalSystem::local_buffer))->norm2()) / (ds->normRef()); if (aux > RelativeTol) simulationLink->setRelativeConvergenceCriterionHeld(false); } } //2 - Newton Euler Systems else if (dsType == Type::NewtonEulerDS) { // // get dynamical system // SP::NewtonEulerDS d = std11::static_pointer_cast<NewtonEulerDS> (ds); // SP::SiconosVector v = d->velocity(); // #ifdef SCHATZMANPAOLI_NE_DEBUG // std::cout<<"SchatzmanPaoliOSI::updatestate prev v"<<endl; // v->display(); // #endif // /*d->p has been fill by the Relation->computeInput, it contains // B \lambda _{k+1}*/ // *v = *d->p(level); // v = p // d->luW()->PLUForwardBackwardInPlace(*v); // #ifdef SCHATZMANPAOLI_NE_DEBUG // std::cout<<"SchatzmanPaoliOSI::updatestate hWB lambda"<<endl; // v->display(); // #endif // *v += * ds->workspace(DynamicalSystem::free); // #ifdef SCHATZMANPAOLI_NE_DEBUG // std::cout<<"SchatzmanPaoliOSI::updatestate work free"<<endl; // ds->workspace(DynamicalSystem::free)->display(); // std::cout<<"SchatzmanPaoliOSI::updatestate new v"<<endl; // v->display(); // #endif // //compute q // //first step consists in computing \dot q. // //second step consists in updating q. // // // SP::SiconosMatrix T = d->T(); // SP::SiconosVector dotq = d->dotq(); // prod(*T,*v,*dotq,true); // // std::cout<<"SchatzmanPaoliOSI::updateState v"<<endl; // // v->display(); // // std::cout<<"SchatzmanPaoliOSI::updateState dotq"<<endl; // // dotq->display(); // SP::SiconosVector q = d->q(); // // -> get previous time step state // SP::SiconosVector dotqold = d->dotqMemory()->getSiconosVector(0); // SP::SiconosVector qold = d->qMemory()->getSiconosVector(0); // // *q = *qold + h*(theta * *v +(1.0 - theta)* *vold) // double coeff = h*_theta; // scal(coeff, *dotq, *q) ; // q = h*theta*v // coeff = h*(1-_theta); // scal(coeff,*dotqold,*q,false); // q += h(1-theta)*vold // *q += *qold; // #ifdef SCHATZMANPAOLI_NE_DEBUG // std::cout<<"new q before normalizing"<<endl; // q->display(); // #endif // //q[3:6] must be normalized // d->normalizeq(); // dotq->setValue(3,(q->getValue(3)-qold->getValue(3))/h); // dotq->setValue(4,(q->getValue(4)-qold->getValue(4))/h); // dotq->setValue(5,(q->getValue(5)-qold->getValue(5))/h); // dotq->setValue(6,(q->getValue(6)-qold->getValue(6))/h); // d->updateT(); RuntimeException::selfThrow("SchatzmanPaoliOSI::updateState - not yet implemented for Dynamical system type: " + dsType); } else RuntimeException::selfThrow("SchatzmanPaoliOSI::updateState - not yet implemented for Dynamical system type: " + dsType); } }