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);
}
}
double SchatzmanPaoliOSI::computeResidu()
{
// This function is used to compute the residu for each "SchatzmanPaoliOSI-discretized" dynamical system.
// It then computes the norm of each of them and finally return the maximum
// value for those norms.
//
// The state values used are those saved in the DS, ie the last computed ones.
// $\mathcal R(x,r) = x - x_{k} -h\theta f( x , t_{k+1}) - h(1-\theta)f(x_k,t_k) - h r$
// $\mathcal R_{free}(x,r) = x - x_{k} -h\theta f( x , t_{k+1}) - h(1-\theta)f(x_k,t_k) $
double t = simulationLink->nextTime(); // End of the time step
double told = simulationLink->startingTime(); // Beginning of the time step
double h = t - told; // time step length
// Operators computed at told have index i, and (i+1) at t.
// Iteration through the set of Dynamical Systems.
//
DSIterator it;
SP::DynamicalSystem ds; // Current Dynamical System.
Type::Siconos dsType ; // Type of the current DS.
double maxResidu = 0;
double normResidu = maxResidu;
for (it = OSIDynamicalSystems->begin(); it != OSIDynamicalSystems->end(); ++it)
{
ds = *it; // the considered dynamical system
dsType = Type::value(*ds); // Its type
SP::SiconosVector residuFree = ds->workspace(DynamicalSystem::freeresidu);
// 1 - Lagrangian Non Linear Systems
if (dsType == Type::LagrangianDS)
{
// // residu = M(q*)(v_k,i+1 - v_i) - h*theta*forces(t,v_k,i+1, q_k,i+1) - h*(1-theta)*forces(ti,vi,qi) - pi+1
// // -- Convert the DS into a Lagrangian one.
// SP::LagrangianDS d = std11::static_pointer_cast<LagrangianDS> (ds);
// // Get state i (previous time step) from Memories -> var. indexed with "Old"
// SP::SiconosVector qold =d->qMemory()->getSiconosVector(0);
// SP::SiconosVector vold = d->velocityMemory()->getSiconosVector(0);
// SP::SiconosVector q =d->q();
// d->computeMass();
// SP::SiconosMatrix M = d->mass();
// SP::SiconosVector v = d->velocity(); // v = v_k,i+1
// //residuFree->zero();
// // std::cout << "(*v-*vold)->norm2()" << (*v-*vold).norm2() << std::endl;
// prod(*M, (*v-*vold), *residuFree); // residuFree = M(v - vold)
// if (d->forces()) // if fL exists
// {
// // computes forces(ti,vi,qi)
// d->computeForces(told,qold,vold);
// double coef = -h*(1-_theta);
// // residuFree += coef * fL_i
// scal(coef, *d->forces(), *residuFree, false);
// // computes forces(ti+1, v_k,i+1, q_k,i+1) = forces(t,v,q)
// //d->computeForces(t);
// // or forces(ti+1, v_k,i+1, q(v_k,i+1))
// //or
// SP::SiconosVector qbasedonv(new SiconosVector(*qold));
// *qbasedonv += h*( (1-_theta)* *vold + _theta * *v );
// d->computeForces(t,qbasedonv,v);
// coef = -h*_theta;
// // residuFree += coef * fL_k,i+1
// scal(coef, *d->forces(), *residuFree, false);
// }
// if (d->boundaryConditions())
// {
// d->boundaryConditions()->computePrescribedVelocity(t);
// unsigned int columnindex=0;
// SP::SimpleMatrix WBoundaryConditions = _WBoundaryConditionsMap[ds];
// SP::SiconosVector columntmp(new SiconosVector(ds->getDim()));
// for (vector<unsigned int>::iterator itindex = d->boundaryConditions()->velocityIndices()->begin() ;
// itindex != d->boundaryConditions()->velocityIndices()->end();
// ++itindex)
// {
// double DeltaPrescribedVelocity =
// d->boundaryConditions()->prescribedVelocity()->getValue(columnindex)
// - vold->getValue(columnindex);
// WBoundaryConditions->getCol(columnindex,*columntmp);
// *residuFree -= *columntmp * (DeltaPrescribedVelocity);
// residuFree->setValue(*itindex, columntmp->getValue(*itindex) *(DeltaPrescribedVelocity));
// columnindex ++;
// }
// }
// *(d->workspace(DynamicalSystem::free))=*residuFree; // copy residuFree in Workfree
// // std::cout << "SchatzmanPaoliOSI::ComputeResidu LagrangianDS residufree :" << std::endl;
// // residuFree->display();
// if (d->p(1))
// *(d->workspace(DynamicalSystem::free)) -= *d->p(1); // Compute Residu in Workfree Notation !!
// // std::cout << "SchatzmanPaoliOSI::ComputeResidu LagrangianDS residu :" << std::endl;
// // d->workspace(DynamicalSystem::free)->display();
// normResidu = d->workspace(DynamicalSystem::free)->norm2();
RuntimeException::selfThrow("SchatzmanPaoliOSI::computeResidu - not yet implemented for Dynamical system type: " + dsType);
}
// 2 - Lagrangian Linear Systems
else if (dsType == Type::LagrangianLinearTIDS)
{
// ResiduFree = M(-q_{k}+q_{k-1}) + h^2 (K q_k)+ h^2 C (\theta \Frac{q_k-q_{k-1}}{2h}+ (1-\theta) v_k)) (1)
// This formulae is only valid for the first computation of the residual for q = q_k
// otherwise the complete formulae must be applied, that is
// ResiduFree M(q-2q_{k}+q_{k-1}) + h^2 (K(\theta q+ (1-\theta) q_k)))+ h^2 C (\theta \Frac{q-q_{k-1}}{2h}+ (1-\theta) v_k)) (2)
// for q != q_k, the formulae (1) is wrong.
// in the sequel, only the equation (1) is implemented
// -- Convert the DS into a Lagrangian one.
SP::LagrangianLinearTIDS d = std11::static_pointer_cast<LagrangianLinearTIDS> (ds);
// Get state i (previous time step) from Memories -> var. indexed with "Old"
SP::SiconosVector q_k = d->qMemory()->getSiconosVector(0); // q_k
SP::SiconosVector q_k_1 = d->qMemory()->getSiconosVector(1); // q_{k-1}
SP::SiconosVector v_k = d->velocityMemory()->getSiconosVector(0); //v_k
// std::cout << "SchatzmanPaoliOSI::computeResidu - q_k_1 =" <<std::endl;
// q_k_1->display();
// std::cout << "SchatzmanPaoliOSI::computeResidu - q_k =" <<std::endl;
// q_k->display();
// std::cout << "SchatzmanPaoliOSI::computeResidu - v_k =" <<std::endl;
// v_k->display();
// --- ResiduFree computation Equation (1) ---
residuFree->zero();
double coeff;
// -- No need to update W --
//SP::SiconosVector v = d->velocity(); // v = v_k,i+1
SP::SiconosMatrix M = d->mass();
prod(*M, (*q_k_1 - *q_k), *residuFree); // residuFree = M(-q_{k}+q_{k-1})
SP::SiconosMatrix K = d->K();
if (K)
{
prod(h * h, *K, *q_k, *residuFree, false); // residuFree += h^2*K*qi
}
SP::SiconosMatrix C = d->C();
if (C)
prod(h * h, *C, (1.0 / (2.0 * h)*_theta * (*q_k - *q_k_1) + (1.0 - _theta)* *v_k) , *residuFree, false);
// residufree += h^2 C (\theta \Frac{q-q_{k-1}}{2h}+ (1-\theta) v_k))
SP::SiconosVector Fext = d->fExt();
if (Fext)
{
// computes Fext(ti)
d->computeFExt(told);
coeff = -h * h * (1 - _theta);
scal(coeff, *Fext, *residuFree, false); // residufree -= h^2*(1-_theta) * fext(ti)
// computes Fext(ti+1)
d->computeFExt(t);
coeff = -h * h * _theta;
scal(coeff, *Fext, *residuFree, false); // residufree -= h^2*_theta * fext(ti+1)
}
// if (d->boundaryConditions())
// {
// d->boundaryConditions()->computePrescribedVelocity(t);
// unsigned int columnindex=0;
// SP::SimpleMatrix WBoundaryConditions = _WBoundaryConditionsMap[ds];
// SP::SiconosVector columntmp(new SiconosVector(ds->getDim()));
// for (vector<unsigned int>::iterator itindex = d->boundaryConditions()->velocityIndices()->begin() ;
// itindex != d->boundaryConditions()->velocityIndices()->end();
// ++itindex)
// {
// double DeltaPrescribedVelocity =
// d->boundaryConditions()->prescribedVelocity()->getValue(columnindex)
// -vold->getValue(columnindex);
// WBoundaryConditions->getCol(columnindex,*columntmp);
// *residuFree += *columntmp * (DeltaPrescribedVelocity);
// residuFree->setValue(*itindex, - columntmp->getValue(*itindex) *(DeltaPrescribedVelocity));
// columnindex ++;
// }
// }
// std::cout << "SchatzmanPaoliOSI::ComputeResidu LagrangianLinearTIDS residufree :" << std::endl;
// residuFree->display();
(* d->workspace(DynamicalSystem::free)) = *residuFree; // copy residuFree in Workfree
if (d->p(0))
*(d->workspace(DynamicalSystem::free)) -= *d->p(0); // Compute Residu in Workfree Notation !!
// std::cout << "SchatzmanPaoliOSI::ComputeResidu LagrangianLinearTIDS p(0) :" << std::endl;
// if (d->p(0))
// d->p(0)->display();
// else
// std::cout << " p(0) :" << std::endl;
// std::cout << "SchatzmanPaoliOSI::ComputeResidu LagrangianLinearTIDS residu :" << std::endl;
// d->workspace(DynamicalSystem::free)->display();
// normResidu = d->workspace(DynamicalSystem::free)->norm2();
normResidu = 0.0; // we assume that v = vfree + W^(-1) p
// normResidu = realresiduFree->norm2();
}
else if (dsType == Type::NewtonEulerDS)
{
// // residu = M(q*)(v_k,i+1 - v_i) - h*_theta*forces(t,v_k,i+1, q_k,i+1) - h*(1-_theta)*forces(ti,vi,qi) - pi+1
// // -- Convert the DS into a Lagrangian one.
// SP::NewtonEulerDS d = std11::static_pointer_cast<NewtonEulerDS> (ds);
// // Get state i (previous time step) from Memories -> var. indexed with "Old"
// SP::SiconosVector qold =d->qMemory()->getSiconosVector(0);
// SP::SiconosVector vold = d->velocityMemory()->getSiconosVector(0);
// SP::SiconosVector q =d->q();
// SP::SiconosMatrix massMatrix = d->massMatrix();
// SP::SiconosVector v = d->velocity(); // v = v_k,i+1
// prod(*massMatrix, (*v-*vold), *residuFree); // residuFree = M(v - vold)
// if (d->forces()) // if fL exists
// {
// // computes forces(ti,vi,qi)
// SP::SiconosVector fLold=d->fLMemory()->getSiconosVector(0);
// double _thetaFL=0.5;
// double coef = -h*(1-_thetaFL);
// // residuFree += coef * fL_i
// scal(coef, *fLold, *residuFree, false);
// d->computeForces(t);
// // printf("cpmputeFreeState d->FL():\n");
// // d->forces()->display();
// coef = -h*_thetaFL;
// scal(coef, *d->forces(), *residuFree, false);
// }
// *(d->workspace(DynamicalSystem::free))=*residuFree;
// //cout<<"SchatzmanPaoliOSI::computeResidu :\n";
// // residuFree->display();
// if ( d->p(1) )
// *(d->workspace(DynamicalSystem::free)) -= *d->p(1);
// normResidu = d->workspace(DynamicalSystem::free)->norm2();
RuntimeException::selfThrow("SchatzmanPaoliOSI::computeResidu - not yet implemented for Dynamical system type: " + dsType);
}
else
RuntimeException::selfThrow("SchatzmanPaoliOSI::computeResidu - not yet implemented for Dynamical system type: " + dsType);
if (normResidu > maxResidu) maxResidu = normResidu;
}
return maxResidu;
}
double SchatzmanPaoliOSI::computeResidu()
{
// This function is used to compute the residu for each "SchatzmanPaoliOSI-discretized" dynamical system.
// It then computes the norm of each of them and finally return the maximum
// value for those norms.
//
// The state values used are those saved in the DS, ie the last computed ones.
// $\mathcal R(x,r) = x - x_{k} -h\theta f( x , t_{k+1}) - h(1-\theta)f(x_k,t_k) - h r$
// $\mathcal R_{free}(x,r) = x - x_{k} -h\theta f( x , t_{k+1}) - h(1-\theta)f(x_k,t_k) $
double t = _simulation->nextTime(); // End of the time step
double told = _simulation->startingTime(); // Beginning of the time step
double h = t - told; // time step length
// Operators computed at told have index i, and (i+1) at t.
// Iteration through the set of Dynamical Systems.
//
SP::DynamicalSystem ds; // Current Dynamical System.
Type::Siconos dsType ; // Type of the current DS.
double maxResidu = 0;
double normResidu = maxResidu;
DynamicalSystemsGraph::VIterator dsi, dsend;
for (std11::tie(dsi, dsend) = _dynamicalSystemsGraph->vertices(); dsi != dsend; ++dsi)
{
if (!checkOSI(dsi)) continue;
SP::DynamicalSystem ds = _dynamicalSystemsGraph->bundle(*dsi);
dsType = Type::value(*ds); // Its type
SP::SiconosVector residuFree = ds->workspace(DynamicalSystem::freeresidu);
// 1 - Lagrangian Non Linear Systems
if (dsType == Type::LagrangianDS)
{
RuntimeException::selfThrow("SchatzmanPaoliOSI::computeResidu - not yet implemented for Dynamical system type: " + dsType);
}
// 2 - Lagrangian Linear Systems
else if (dsType == Type::LagrangianLinearTIDS)
{
// ResiduFree = M(-q_{k}+q_{k-1}) + h^2 (K q_k)+ h^2 C (\theta \Frac{q_k-q_{k-1}}{2h}+ (1-\theta) v_k)) (1)
// This formulae is only valid for the first computation of the residual for q = q_k
// otherwise the complete formulae must be applied, that is
// ResiduFree M(q-2q_{k}+q_{k-1}) + h^2 (K(\theta q+ (1-\theta) q_k)))+ h^2 C (\theta \Frac{q-q_{k-1}}{2h}+ (1-\theta) v_k)) (2)
// for q != q_k, the formulae (1) is wrong.
// in the sequel, only the equation (1) is implemented
// -- Convert the DS into a Lagrangian one.
SP::LagrangianLinearTIDS d = std11::static_pointer_cast<LagrangianLinearTIDS> (ds);
// Get state i (previous time step) from Memories -> var. indexed with "Old"
SP::SiconosVector q_k = d->qMemory()->getSiconosVector(0); // q_k
SP::SiconosVector q_k_1 = d->qMemory()->getSiconosVector(1); // q_{k-1}
SP::SiconosVector v_k = d->velocityMemory()->getSiconosVector(0); //v_k
// std::cout << "SchatzmanPaoliOSI::computeResidu - q_k_1 =" <<std::endl;
// q_k_1->display();
// std::cout << "SchatzmanPaoliOSI::computeResidu - q_k =" <<std::endl;
// q_k->display();
// std::cout << "SchatzmanPaoliOSI::computeResidu - v_k =" <<std::endl;
// v_k->display();
// --- ResiduFree computation Equation (1) ---
residuFree->zero();
double coeff;
// -- No need to update W --
//SP::SiconosVector v = d->velocity(); // v = v_k,i+1
SP::SiconosMatrix M = d->mass();
prod(*M, (*q_k_1 - *q_k), *residuFree); // residuFree = M(-q_{k}+q_{k-1})
SP::SiconosMatrix K = d->K();
if (K)
{
prod(h * h, *K, *q_k, *residuFree, false); // residuFree += h^2*K*qi
}
SP::SiconosMatrix C = d->C();
if (C)
prod(h * h, *C, (1.0 / (2.0 * h)*_theta * (*q_k - *q_k_1) + (1.0 - _theta)* *v_k) , *residuFree, false);
// residufree += h^2 C (\theta \Frac{q-q_{k-1}}{2h}+ (1-\theta) v_k))
SP::SiconosVector Fext = d->fExt();
if (Fext)
{
// computes Fext(ti)
d->computeFExt(told);
coeff = -h * h * (1 - _theta);
scal(coeff, *Fext, *residuFree, false); // residufree -= h^2*(1-_theta) * fext(ti)
// computes Fext(ti+1)
d->computeFExt(t);
coeff = -h * h * _theta;
scal(coeff, *Fext, *residuFree, false); // residufree -= h^2*_theta * fext(ti+1)
}
// std::cout << "SchatzmanPaoliOSI::ComputeResidu LagrangianLinearTIDS residufree :" << std::endl;
// residuFree->display();
(* d->workspace(DynamicalSystem::free)) = *residuFree; // copy residuFree in Workfree
if (d->p(0))
*(d->workspace(DynamicalSystem::free)) -= *d->p(0); // Compute Residu in Workfree Notation !!
// std::cout << "SchatzmanPaoliOSI::ComputeResidu LagrangianLinearTIDS p(0) :" << std::endl;
// if (d->p(0))
// d->p(0)->display();
// else
// std::cout << " p(0) :" << std::endl;
// std::cout << "SchatzmanPaoliOSI::ComputeResidu LagrangianLinearTIDS residu :" << std::endl;
// d->workspace(DynamicalSystem::free)->display();
// normResidu = d->workspace(DynamicalSystem::free)->norm2();
normResidu = 0.0; // we assume that v = vfree + W^(-1) p
// normResidu = realresiduFree->norm2();
}
else if (dsType == Type::NewtonEulerDS)
{
RuntimeException::selfThrow("SchatzmanPaoliOSI::computeResidu - not yet implemented for Dynamical system type: " + dsType);
}
else
RuntimeException::selfThrow("SchatzmanPaoliOSI::computeResidu - not yet implemented for Dynamical system type: " + dsType);
if (normResidu > maxResidu) maxResidu = normResidu;
}
return maxResidu;
}
