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); } }