void D1MinusLinearOSI::initializeWorkVectorsForDS(double t, SP::DynamicalSystem ds)
{
// Get work buffers from the graph
VectorOfVectors& ds_work_vectors = *_initializeDSWorkVectors(ds);
// Check dynamical system type
Type::Siconos dsType = Type::value(*ds);
assert(dsType == Type::LagrangianLinearTIDS || dsType == Type::LagrangianDS || dsType == Type::NewtonEulerDS);
if(dsType == Type::LagrangianDS || dsType == Type::LagrangianLinearTIDS)
{
SP::LagrangianDS lds = std11::static_pointer_cast<LagrangianDS> (ds);
lds->init_generalized_coordinates(2); // acceleration is required for the ds
lds->init_inverse_mass(); // invMass required to update post-impact velocity
ds_work_vectors.resize(D1MinusLinearOSI::WORK_LENGTH);
ds_work_vectors[D1MinusLinearOSI::RESIDU_FREE].reset(new SiconosVector(lds->dimension()));
ds_work_vectors[D1MinusLinearOSI::FREE].reset(new SiconosVector(lds->dimension()));
ds_work_vectors[D1MinusLinearOSI::FREE_TDG].reset(new SiconosVector(lds->dimension()));
// Update dynamical system components (for memory swap).
lds->computeForces(t, lds->q(), lds->velocity());
lds->swapInMemory();
}
else if(dsType == Type::NewtonEulerDS)
{
SP::NewtonEulerDS neds = std11::static_pointer_cast<NewtonEulerDS> (ds);
neds->init_inverse_mass(); // invMass required to update post-impact velocity
ds_work_vectors.resize(D1MinusLinearOSI::WORK_LENGTH);
ds_work_vectors[D1MinusLinearOSI::RESIDU_FREE].reset(new SiconosVector(neds->dimension()));
ds_work_vectors[D1MinusLinearOSI::FREE].reset(new SiconosVector(neds->dimension()));
ds_work_vectors[D1MinusLinearOSI::FREE_TDG].reset(new SiconosVector(neds->dimension()));
//Compute a first value of the forces to store it in _forcesMemory
neds->computeForces(t, neds->q(), neds->twist());
neds->swapInMemory();
}
else
RuntimeException::selfThrow("D1MinusLinearOSI::initialize - not implemented for Dynamical system type: " + dsType);
for (unsigned int k = _levelMinForInput ; k < _levelMaxForInput + 1; k++)
{
ds->initializeNonSmoothInput(k);
}
}
void D1MinusLinearOSI::updateState(const unsigned int level)
{
DEBUG_PRINTF("\n D1MinusLinearOSI::updateState(const unsigned int level) start for level = %i\n",level);
for (DSIterator it = OSIDynamicalSystems->begin(); it != OSIDynamicalSystems->end(); ++it)
{
// type of the current DS
Type::Siconos dsType = Type::value(**it);
/* \warning the following conditional statement should be removed with a MechanicalDS class */
/* Lagrangian DS*/
if ((dsType == Type::LagrangianDS) || (dsType == Type::LagrangianLinearTIDS))
{
SP::LagrangianDS d = std11::static_pointer_cast<LagrangianDS> (*it);
SP::SiconosMatrix M = d->mass();
SP::SiconosVector v = d->velocity();
DEBUG_PRINT("Position and velocity before update\n");
DEBUG_EXPR(d->q()->display());
DEBUG_EXPR(d->velocity()->display());
/* Add the contribution of the impulse if any */
if (d->p(1))
{
DEBUG_EXPR(d->p(1)->display());
/* copy the value of the impulse */
SP::SiconosVector dummy(new SiconosVector(*(d->p(1))));
/* Compute the velocity jump due to the impulse */
M->PLUForwardBackwardInPlace(*dummy);
/* Add the velocity jump to the free velocity */
*v += *dummy;
}
DEBUG_PRINT("Position and velocity after update\n");
DEBUG_EXPR(d->q()->display());
DEBUG_EXPR(d->velocity()->display());
}
/* NewtonEuler Systems */
else if (dsType == Type::NewtonEulerDS)
{
SP::NewtonEulerDS d = std11::static_pointer_cast<NewtonEulerDS> (*it);
SP::SiconosMatrix M(new SimpleMatrix(*(d->mass()))); // we copy the mass matrix to avoid its factorization;
SP::SiconosVector v = d->velocity(); // POINTER CONSTRUCTOR : contains new velocity
if (d->p(1))
{
// Update the velocity
SP::SiconosVector dummy(new SiconosVector(*(d->p(1)))); // value = nonsmooth impulse
M->PLUForwardBackwardInPlace(*dummy); // solution for its velocity equivalent
*v += *dummy; // add free velocity
// update \f$ \dot q \f$
SP::SiconosMatrix T = d->T();
SP::SiconosVector dotq = d->dotq();
prod(*T, *v, *dotq, true);
DEBUG_PRINT("\nRIGHT IMPULSE\n");
DEBUG_EXPR(d->p(1)->display());
}
DEBUG_EXPR(d->q()->display());
DEBUG_EXPR(d->velocity()->display());
}
else
RuntimeException::selfThrow("D1MinusLinearOSI::computeResidu - not yet implemented for Dynamical system type: " + dsType);
}
DEBUG_PRINT("\n D1MinusLinearOSI::updateState(const unsigned int level) end\n");
}
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 KernelTest::t6()
{
SP::Model bouncingBall = Siconos::load("BouncingBall1.xml");
try
{
double T = bouncingBall->finalT();
double t0 = bouncingBall->t0();
double h = bouncingBall->simulation()->timeStep();
int N = (int)((T - t0) / h); // Number of time steps
SP::DynamicalSystemsGraph dsg =
bouncingBall->nonSmoothDynamicalSystem()->topology()->dSG(0);
SP::LagrangianDS ball = std11::static_pointer_cast<LagrangianDS>
(dsg->bundle(*(dsg->begin())));
SP::TimeStepping s = std11::static_pointer_cast<TimeStepping>(bouncingBall->simulation());
SP::Interaction inter;
InteractionsGraph::VIterator ui, uiend;
SP::InteractionsGraph indexSet0 = bouncingBall->nonSmoothDynamicalSystem()->topology()->indexSet(0);
for (std11::tie(ui, uiend) = indexSet0->vertices(); ui != uiend; ++ui)
inter = indexSet0->bundle(*ui);
// --- Get the values to be plotted ---
// -> saved in a matrix dataPlot
unsigned int outputSize = 5;
SimpleMatrix dataPlot(N + 1, outputSize);
SP::SiconosVector q = ball->q();
SP::SiconosVector v = ball->velocity();
SP::SiconosVector p = ball->p(1);
SP::SiconosVector lambda = inter->lambda(1);
dataPlot(0, 0) = bouncingBall->t0();
dataPlot(0, 1) = (*q)(0);
dataPlot(0, 2) = (*v)(0);
dataPlot(0, 3) = (*p)(0);
dataPlot(0, 4) = (*lambda)(0);
// --- Time loop ---
cout << "====> Start computation ... " << endl << endl;
// ==== Simulation loop - Writing without explicit event handling =====
int k = 1;
boost::progress_display show_progress(N);
boost::timer time;
time.restart();
while (s->hasNextEvent())
{
s->computeOneStep();
// --- Get values to be plotted ---
dataPlot(k, 0) = s->nextTime();
dataPlot(k, 1) = (*q)(0);
dataPlot(k, 2) = (*v)(0);
dataPlot(k, 3) = (*p)(0);
dataPlot(k, 4) = (*lambda)(0);
s->nextStep();
++show_progress;
k++;
}
cout << endl << "End of computation - Number of iterations done: " << k - 1 << endl;
cout << "Computation Time " << time.elapsed() << endl;
// --- Output files ---
cout << "====> Output file writing ..." << endl;
dataPlot.resize(k, outputSize);
ioMatrix::write("result.dat", "ascii", dataPlot, "noDim");
// Comparison with a reference file
SimpleMatrix dataPlotRef(dataPlot);
dataPlotRef.zero();
ioMatrix::read("result.ref", "ascii", dataPlotRef);
if ((dataPlot - dataPlotRef).normInf() > 1e-12)
{
std::cout <<
"Warning. The results is rather different from the reference file :"
<<
(dataPlot - dataPlotRef).normInf()
<<
std::endl;
CPPUNIT_ASSERT(false);
}
}
catch (SiconosException e)
{
cout << e.report() << endl;
CPPUNIT_ASSERT(false);
}
catch (...)
{
cout << "Exception caught in BouncingBallTS.cpp" << endl;
CPPUNIT_ASSERT(false);
}
}
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);
}
}
void MLCPProjectOnConstraints::postComputeLagrangianR(SP::Interaction inter, unsigned int pos)
{
SP::LagrangianR lr = std11::static_pointer_cast<LagrangianR>(inter->relation());
#ifdef MLCPPROJ_DEBUG
printf("MLCPProjectOnConstraints::postComputeLagrangian inter->y(0)\n");
inter->y(0)->display();
printf("MLCPProjectOnConstraints::postComputeLagrangian lr->jachq \n");
lr->jachq()->display();
printf("MLCPProjectOnConstraints::postComputeLagrangianR q before update\n");
SP::InteractionsGraph indexSet = simulation()->indexSet(indexSetLevel());
InteractionsGraph::VDescriptor ui = indexSet->descriptor(inter);
InteractionsGraph::OEIterator oei, oeiend;
for(std11::tie(oei, oeiend) = indexSet->out_edges(ui);
oei != oeiend; ++oei)
{
SP::LagrangianDS lds = std11::static_pointer_cast<LagrangianDS>(indexSet->bundle(*oei));
lds->q()->display();
}
#endif
//unsigned int sizeY = inter->nonSmoothLaw()->size();
// y and lambda vectors
SP::SiconosVector lambda = inter->lambda(0);
SP::SiconosVector y = inter->y(0);
unsigned int sizeY = std11::static_pointer_cast<OSNSMatrixProjectOnConstraints>
(_M)->computeSizeForProjection(inter);
// Copy _w/_z values, starting from index pos into y/lambda.
//setBlock(*_w, y, sizeY, pos, 0);
setBlock(*_z, lambda, sizeY, pos, 0);
#ifdef MLCPPROJ_DEBUG
printf("MLCPP lambda of Interaction is pos =%i :\n", pos);
// aBuff->display();
lambda->display();
unsigned int nslawsize = inter->nonSmoothLaw()->size();
SP::SiconosVector aBuff(new SiconosVector(nslawsize));
setBlock(*_z, aBuff, sizeY, pos, 0);
SP::SiconosMatrix J = lr->jachq();
SP::SimpleMatrix aux(new SimpleMatrix(*J));
aux->trans();
// SP::SiconosVector tmp(new SiconosVector(*(lr->q())));
// prod(*aux, *aBuff, *(tmp), false);
// //prod(*aux,*lambda,*(lr->q()),false);
// std:: std::cout << " tmp = tmp + J^T * lambda" << std::endl;
// tmp->display();
#endif
// // WARNING : Must not be done here. and should be called with the correct time.
// // compute p(0)
// inter->computeInput(0.0 ,0);
// // \warning aBuff should normally be in lambda[0]
// // The update of the position in DS should be made
// // in MoreauJeanOSI::upateState or ProjectedMoreauJeanOSI::updateState
// SP::SiconosMatrix J=lr->jachq();
// SP::SimpleMatrix aux(new SimpleMatrix(*J));
// aux->trans();
// SP::SiconosVector tmp (new SiconosVector(*(lr->q())));
// std:: std::cout << " tmp ="<<std::endl;
// tmp->display();
// std:: std::cout << " lr->q() ="<<std::endl;
// lr->q()->display();
// //prod(*aux,*lambda,*(lr->q()),false);
// prod(*aux,*aBuff,*(tmp),false);
// std:: std::cout << " tmp = tmp + J * lambda"<<std::endl;
// tmp->display();
// // The following step should be done on MoreauJeanOSI::upateState or ProjectedMoreauJeanOSI::updateState
// DSIterator itDS = inter->dynamicalSystemsBegin();
// while(itDS!=inter->dynamicalSystemsEnd())
// {
// Type::Siconos dsType = Type::value(**itDS);
// if((dsType !=Type::LagrangianDS) and
// (dsType !=Type::LagrangianLinearTIDS) )
// {
// RuntimeException::selfThrow("MLCPProjectOnConstraint::postCompute- ds is not of Lagrangian DS type.");
// }
// SP::LagrangianDS d = std11::static_pointer_cast<LagrangianDS> (*itDS);
// SP::SiconosVector q = d->q();
// *q += *d->p(0);
// std::cout << " q=" << std::endl;
// q->display();
// itDS++;
// }
// if ((*lr->q() - *tmp).normInf() > 1e-12)
// {
// RuntimeException::selfThrow("youyou");
// }
#ifdef MLCPPROJ_DEBUG
printf("MLCPProjectOnConstraints::postComputeLagrangianR _z\n");
_z->display();
printf("MLCPProjectOnConstraints::postComputeLagrangianR updated\n");
VectorOfBlockVectors& DSlink = *(indexSet->properties(ui)).DSlink;
// (*DSlink[LagrangianR::q0]).display();
// (lr->q())->display();
#endif
//RuntimeException::selfThrow("MLCPProjectOnConstraints::postComputeLagrangianR() - not yet implemented");
}
