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");
}
void MLCPProjectOnConstraints::computeInteractionBlock(const InteractionsGraph::EDescriptor& ed)
{
// Computes matrix _interactionBlocks[inter1][inter2] (and allocates memory if
// necessary) if inter1 and inter2 have commond DynamicalSystem. How
// _interactionBlocks are computed depends explicitely on the type of
// Relation of each Interaction.
// Warning: we suppose that at this point, all non linear
// operators (G for lagrangian relation for example) have been
// computed through plug-in mechanism.
#ifdef MLCPPROJ_DEBUG
std::cout << "MLCPProjectOnConstraints::computeInteractionBlock currentInteractionBlock start " << std::endl;
#endif
// Get dimension of the NonSmoothLaw (ie dim of the interactionBlock)
SP::InteractionsGraph indexSet = simulation()->indexSet(indexSetLevel());
SP::DynamicalSystem ds = indexSet->bundle(ed);
SP::Interaction inter1 = indexSet->bundle(indexSet->source(ed));
SP::Interaction inter2 = indexSet->bundle(indexSet->target(ed));
// For the edge 'ds', we need to find relative position of this ds
// in inter1 and inter2 relation matrices (--> pos1 and pos2 below)
// - find if ds is source or target in inter_i
InteractionsGraph::VDescriptor vertex_inter;
// - get the corresponding position
unsigned int pos1, pos2;
// source of inter1 :
vertex_inter = indexSet->source(ed);
VectorOfSMatrices& workMInter1 = *indexSet->properties(vertex_inter).workMatrices;
SP::OneStepIntegrator Osi = indexSet->properties(vertex_inter).osi;
SP::DynamicalSystem tmpds = indexSet->properties(vertex_inter).source;
if (tmpds == ds)
pos1 = indexSet->properties(vertex_inter).source_pos;
else
{
tmpds = indexSet->properties(vertex_inter).target;
pos1 = indexSet->properties(vertex_inter).target_pos;
}
// now, inter2
vertex_inter = indexSet->target(ed);
VectorOfSMatrices& workMInter2 = *indexSet->properties(vertex_inter).workMatrices;
tmpds = indexSet->properties(vertex_inter).source;
if (tmpds == ds)
pos2 = indexSet->properties(vertex_inter).source_pos;
else
{
tmpds = indexSet->properties(vertex_inter).target;
pos2 = indexSet->properties(vertex_inter).target_pos;
}
unsigned int index1 = indexSet->index(indexSet->source(ed));
unsigned int index2 = indexSet->index(indexSet->target(ed));
unsigned int sizeY1 = 0;
sizeY1 = std11::static_pointer_cast<OSNSMatrixProjectOnConstraints>
(_M)->computeSizeForProjection(inter1);
unsigned int sizeY2 = 0;
sizeY2 = std11::static_pointer_cast<OSNSMatrixProjectOnConstraints>
(_M)->computeSizeForProjection(inter2);
SP::SiconosMatrix currentInteractionBlock;
assert(index1 != index2);
if (index2 > index1) // upper block
{
// if (! indexSet->properties(ed).upper_block)
// {
// indexSet->properties(ed).upper_block.reset(new SimpleMatrix(sizeY1, sizeY2));
// }
currentInteractionBlock = indexSet->upper_blockProj[ed];
#ifdef MLCPPROJ_DEBUG
std::cout << "MLCPProjectOnConstraints::computeInteractionBlock currentInteractionBlock " << std::endl;
// currentInteractionBlock->display();
std::cout << "sizeY1 " << sizeY1 << std::endl;
std::cout << "sizeY2 " << sizeY2 << std::endl;
std::cout << "upper_blockProj " << indexSet->upper_blockProj[ed].get() << " of edge " << ed << " of size " << currentInteractionBlock->size(0) << " x " << currentInteractionBlock->size(0) << " for interaction " << inter1->number() << " and interaction " << inter2->number() << std::endl;
// std::cout<<"inter1->display() "<< inter1->number()<< std::endl;
//inter1->display();
// std::cout<<"inter2->display() "<< inter2->number()<< std::endl;
//inter2->display();
#endif
assert(currentInteractionBlock->size(0) == sizeY1);
assert(currentInteractionBlock->size(1) == sizeY2);
}
else // lower block
{
// if (! indexSet->properties(ed).lower_block)
// {
// indexSet->properties(ed).lower_block.reset(new SimpleMatrix(sizeY1, sizeY2));
// }
assert(indexSet->lower_blockProj[ed]->size(0) == sizeY1);
assert(indexSet->lower_blockProj[ed]->size(1) == sizeY2);
currentInteractionBlock = indexSet->lower_blockProj[ed];
}
SP::SiconosMatrix leftInteractionBlock, rightInteractionBlock;
RELATION::TYPES relationType1, relationType2;
// General form of the interactionBlock is : interactionBlock =
// a*extraInteractionBlock + b * leftInteractionBlock * centralInteractionBlocks
// * rightInteractionBlock a and b are scalars, centralInteractionBlocks a
// matrix depending on the integrator (and on the DS), the
// simulation type ... left, right and extra depend on the relation
// type and the non smooth law.
relationType1 = inter1->relation()->getType();
relationType2 = inter2->relation()->getType();
if (relationType1 == NewtonEuler &&
relationType2 == NewtonEuler)
{
assert(inter1 != inter2);
currentInteractionBlock->zero();
#ifdef MLCPPROJ_WITH_CT
unsigned int sizeDS = (std11::static_pointer_cast<NewtonEulerDS>(ds))->getDim();
leftInteractionBlock.reset(new SimpleMatrix(sizeY1, sizeDS));
inter1->getLeftInteractionBlockForDS(pos1, leftInteractionBlock);
SP::NewtonEulerDS neds = (std11::static_pointer_cast<NewtonEulerDS>(ds));
SP::SimpleMatrix T = neds->T();
SP::SimpleMatrix workT(new SimpleMatrix(*T));
workT->trans();
SP::SimpleMatrix workT2(new SimpleMatrix(6, 6));
prod(*workT, *T, *workT2, true);
rightInteractionBlock.reset(new SimpleMatrix(sizeY2, sizeDS));
inter2->getLeftInteractionBlockForDS(pos2, rightInteractionBlock);
rightInteractionBlock->trans();
workT2->PLUForwardBackwardInPlace(*rightInteractionBlock);
prod(*leftInteractionBlock, *rightInteractionBlock, *currentInteractionBlock, false);
#else
unsigned int sizeDS = (std11::static_pointer_cast<NewtonEulerDS>(ds))->getqDim();
leftInteractionBlock.reset(new SimpleMatrix(sizeY1, sizeDS));
inter1->getLeftInteractionBlockForDSProjectOnConstraints(pos1, leftInteractionBlock);
SP::NewtonEulerDS neds = (std11::static_pointer_cast<NewtonEulerDS>(ds));
rightInteractionBlock.reset(new SimpleMatrix(sizeY2, sizeDS));
inter2->getLeftInteractionBlockForDSProjectOnConstraints(pos2, rightInteractionBlock);
rightInteractionBlock->trans();
prod(*leftInteractionBlock, *rightInteractionBlock, *currentInteractionBlock, false);
}
#endif
else if (relationType1 == Lagrangian &&
relationType2 == Lagrangian)
{
unsigned int sizeDS = ds->getDim();
leftInteractionBlock.reset(new SimpleMatrix(sizeY1, sizeDS));
inter1->getLeftInteractionBlockForDS(pos1, leftInteractionBlock, workMInter1);
Type::Siconos dsType = Type::value(*ds);
if (dsType == Type::LagrangianLinearTIDS || dsType == Type::LagrangianDS)
{
SP::LagrangianDS d = std11::static_pointer_cast<LagrangianDS> (ds);
if (d->boundaryConditions()) // V.A. Should we do that ?
{
for (std::vector<unsigned int>::iterator itindex =
d->boundaryConditions()->velocityIndices()->begin() ;
itindex != d->boundaryConditions()->velocityIndices()->end();
++itindex)
{
// (sizeY1,sizeDS));
SP::SiconosVector coltmp(new SiconosVector(sizeY1));
coltmp->zero();
leftInteractionBlock->setCol(*itindex, *coltmp);
}
}
}
#ifdef MLCPPROJ_DEBUG
std::cout << "MLCPProjectOnConstraints::computeInteractionBlock : leftInteractionBlock" << std::endl;
leftInteractionBlock->display();
#endif
// inter1 != inter2
rightInteractionBlock.reset(new SimpleMatrix(sizeY2, sizeDS));
inter2->getLeftInteractionBlockForDS(pos2, rightInteractionBlock, workMInter2);
#ifdef MLCPPROJ_DEBUG
std::cout << "MLCPProjectOnConstraints::computeInteractionBlock : rightInteractionBlock" << std::endl;
rightInteractionBlock->display();
#endif
// Warning: we use getLeft for Right interactionBlock
// because right = transpose(left) and because of
// size checking inside the getBlock function, a
// getRight call will fail.
SP::SiconosMatrix centralInteractionBlock = getOSIMatrix(Osi, ds);
#ifdef MLCPPROJ_DEBUG
std::cout << "MLCPProjectOnConstraints::computeInteractionBlock : centralInteractionBlocks " << std::endl;
centralInteractionBlock->display();
#endif
rightInteractionBlock->trans();
if (_useMassNormalization)
{
centralInteractionBlock->PLUForwardBackwardInPlace(*rightInteractionBlock);
//*currentInteractionBlock += *leftInteractionBlock ** work;
prod(*leftInteractionBlock, *rightInteractionBlock, *currentInteractionBlock, false);
}
else
{
prod(*leftInteractionBlock, *rightInteractionBlock, *currentInteractionBlock, false);
}
#ifdef MLCPPROJ_DEBUG
std::cout << "MLCPProjectOnConstraints::computeInteractionBlock : currentInteractionBlock" << std::endl;
currentInteractionBlock->display();
#endif
}
else
RuntimeException::selfThrow("MLCPProjectOnConstraints::computeInteractionBlock not yet implemented for relation of type " + relationType1);
}
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 MLCPProjectOnConstraints::computeDiagonalInteractionBlock(const InteractionsGraph::VDescriptor& vd)
{
SP::InteractionsGraph indexSet = simulation()->indexSet(indexSetLevel());
SP::DynamicalSystem DS1 = indexSet->properties(vd).source;
SP::DynamicalSystem DS2 = indexSet->properties(vd).target;
SP::Interaction inter = indexSet->bundle(vd);
SP::OneStepIntegrator Osi = indexSet->properties(vd).osi;
unsigned int pos1, pos2;
pos1 = indexSet->properties(vd).source_pos;
pos2 = indexSet->properties(vd).target_pos;
unsigned int sizeY = 0;
sizeY = std11::static_pointer_cast<OSNSMatrixProjectOnConstraints>
(_M)->computeSizeForProjection(inter);
#ifdef MLCPPROJ_DEBUG
std::cout << "\nMLCPProjectOnConstraints::computeDiagonalInteractionBlock" <<std::endl;
std::cout << "indexSetLevel()" << indexSetLevel() << std::endl;
// std::cout << "indexSet :"<< indexSet << std::endl;
// std::cout << "vd :"<< vd << std::endl;
// indexSet->display();
// std::cout << "DS1 :" << std::endl;
// DS1->display();
// std::cout << "DS2 :" << std::endl;
// DS2->display();
#endif
assert(indexSet->blockProj[vd]);
SP::SiconosMatrix currentInteractionBlock = indexSet->blockProj[vd];
#ifdef MLCPPROJ_DEBUG
// std::cout<<"MLCPProjectOnConstraints::computeDiagonalInteractionBlock "<<std::endl;
// currentInteractionBlock->display();
std::cout << "sizeY " << sizeY << std::endl;
std::cout << "blockProj " << indexSet->blockProj[vd].get() << " of edge " << vd << " of size " << currentInteractionBlock->size(0) << " x " << currentInteractionBlock->size(0) << " for interaction " << inter->number() << std::endl;
// std::cout<<"inter1->display() "<< inter1->number()<< std::endl;
//inter1->display();
// std::cout<<"inter2->display() "<< inter2->number()<< std::endl;
//inter2->display();
#endif
assert(currentInteractionBlock->size(0) == sizeY);
assert(currentInteractionBlock->size(1) == sizeY);
if (!_hasBeenUpdated)
computeOptions(inter, inter);
// Computes matrix _interactionBlocks[inter1][inter2] (and allocates memory if
// necessary) if inter1 and inter2 have commond DynamicalSystem. How
// _interactionBlocks are computed depends explicitely on the type of
// Relation of each Interaction.
// Warning: we suppose that at this point, all non linear
// operators (G for lagrangian relation for example) have been
// computed through plug-in mechanism.
// Get the W and Theta maps of one of the Interaction -
// Warning: in the current version, if OSI!=MoreauJeanOSI, this fails.
// If OSI = MOREAU, centralInteractionBlocks = W if OSI = LSODAR,
// centralInteractionBlocks = M (mass matrices)
SP::SiconosMatrix leftInteractionBlock, rightInteractionBlock, leftInteractionBlock1;
// General form of the interactionBlock is : interactionBlock =
// a*extraInteractionBlock + b * leftInteractionBlock * centralInteractionBlocks
// * rightInteractionBlock a and b are scalars, centralInteractionBlocks a
// matrix depending on the integrator (and on the DS), the
// simulation type ... left, right and extra depend on the relation
// type and the non smooth law.
VectorOfSMatrices& workMInter = *indexSet->properties(vd).workMatrices;
currentInteractionBlock->zero();
// loop over the common DS
bool endl = false;
unsigned int pos = pos1;
for (SP::DynamicalSystem ds = DS1; !endl; ds = DS2)
{
assert(ds == DS1 || ds == DS2);
endl = (ds == DS2);
if (Type::value(*ds) == Type::LagrangianLinearTIDS ||
Type::value(*ds) == Type::LagrangianDS)
{
if (inter->relation()->getType() != Lagrangian)
{
RuntimeException::selfThrow(
"MLCPProjectOnConstraints::computeDiagonalInteractionBlock - relation is not of type Lagrangian with a LagrangianDS.");
}
SP::LagrangianDS lds = (std11::static_pointer_cast<LagrangianDS>(ds));
unsigned int sizeDS = lds->getDim();
leftInteractionBlock.reset(new SimpleMatrix(sizeY, sizeDS));
inter->getLeftInteractionBlockForDS(pos, leftInteractionBlock, workMInter);
if (lds->boundaryConditions()) // V.A. Should we do that ?
{
for (std::vector<unsigned int>::iterator itindex =
lds->boundaryConditions()->velocityIndices()->begin() ;
itindex != lds->boundaryConditions()->velocityIndices()->end();
++itindex)
{
// (sizeY,sizeDS));
SP::SiconosVector coltmp(new SiconosVector(sizeY));
coltmp->zero();
leftInteractionBlock->setCol(*itindex, *coltmp);
}
}
// (inter1 == inter2)
SP::SiconosMatrix work(new SimpleMatrix(*leftInteractionBlock));
//
// std::cout<<"LinearOSNS : leftUBlock\n";
// work->display();
work->trans();
// std::cout<<"LinearOSNS::computeInteractionBlock leftInteractionBlock"<<endl;
// leftInteractionBlock->display();
if (_useMassNormalization)
{
SP::SiconosMatrix centralInteractionBlock = getOSIMatrix(Osi, ds);
centralInteractionBlock->PLUForwardBackwardInPlace(*work);
prod(*leftInteractionBlock, *work, *currentInteractionBlock, false);
// gemm(CblasNoTrans,CblasNoTrans,1.0,*leftInteractionBlock,*work,1.0,*currentInteractionBlock);
}
else
{
prod(*leftInteractionBlock, *work, *currentInteractionBlock, false);
}
//*currentInteractionBlock *=h;
}
else if (Type::value(*ds) == Type::NewtonEulerDS)
{
if (inter->relation()->getType() != NewtonEuler)
{
RuntimeException::selfThrow("MLCPProjectOnConstraints::computeDiagonalInteractionBlock - relation is not from NewtonEulerR.");
}
SP::NewtonEulerDS neds = (std11::static_pointer_cast<NewtonEulerDS>(ds));
#ifdef MLCPPROJ_WITH_CT
unsigned int sizeDS = neds->getDim();
SP::SimpleMatrix T = neds->T();
SP::SimpleMatrix workT(new SimpleMatrix(*T));
workT->trans();
SP::SimpleMatrix workT2(new SimpleMatrix(6, 6));
prod(*workT, *T, *workT2, true);
leftInteractionBlock.reset(new SimpleMatrix(sizeY, sizeDS));
inter->getLeftInteractionBlockForDS(pos, leftInteractionBlock);
SP::SiconosMatrix work(new SimpleMatrix(*leftInteractionBlock));
std::cout << "LinearOSNS : leftUBlock\n";
work->display();
work->trans();
std::cout << "LinearOSNS::computeInteractionBlock workT2" <<std::endl;
workT2->display();
workT2->PLUForwardBackwardInPlace(*work);
prod(*leftInteractionBlock, *work, *currentInteractionBlock, false);
#else
if (0) //(std11::static_pointer_cast<NewtonEulerR> inter->relation())->_isConstact){
{
// unsigned int sizeDS = neds->getDim();
// SP::SimpleMatrix T = neds->T();
// SP::SimpleMatrix workT(new SimpleMatrix(*T));
// workT->trans();
// SP::SimpleMatrix workT2(new SimpleMatrix(6, 6));
// prod(*workT, *T, *workT2, true);
// leftInteractionBlock1.reset(new SimpleMatrix(sizeY, sizeDS));
// inter->getLeftInteractionBlockForDS(pos, leftInteractionBlock);
// leftInteractionBlock.reset(new SimpleMatrix(1, sizeDS));
// for (unsigned int ii = 0; ii < sizeDS; ii++)
// leftInteractionBlock->setValue(1, ii, leftInteractionBlock1->getValue(1, ii));
//
// SP::SiconosMatrix work(new SimpleMatrix(*leftInteractionBlock));
// //cout<<"LinearOSNS : leftUBlock\n";
// //work->display();
// work->trans();
// //cout<<"LinearOSNS::computeInteractionBlock workT2"<<endl;
// //workT2->display();
// workT2->PLUForwardBackwardInPlace(*work);
// prod(*leftInteractionBlock, *work, *currentInteractionBlock, false);
}
else
{
unsigned int sizeDS = (std11::static_pointer_cast<NewtonEulerDS>(ds))->getqDim();
leftInteractionBlock.reset(new SimpleMatrix(sizeY, sizeDS));
inter->getLeftInteractionBlockForDSProjectOnConstraints(pos, leftInteractionBlock);
// #ifdef MLCPPROJ_DEBUG
// std::cout << "MLCPProjectOnConstraints::computeDiagonalInteractionBlock - NewtonEuler case leftInteractionBlock : " << std::endl;
// leftInteractionBlock->display();
// #endif
SP::SiconosMatrix work(new SimpleMatrix(*leftInteractionBlock));
//cout<<"LinearOSNS sizeY="<<sizeY<<": leftUBlock\n";
//work->display();
work->trans();
prod(*leftInteractionBlock, *work, *currentInteractionBlock, false);
// #ifdef MLCPPROJ_DEBUG
// std::cout << "MLCPProjectOnConstraints::computeDiagonalInteractionBlock - NewtonEuler case currentInteractionBlock : "<< std::endl;
// currentInteractionBlock->display();
// #endif
}
}
else
RuntimeException::selfThrow("MLCPProjectOnConstraints::computeDiagonalInteractionBlock - ds is not from NewtonEulerDS neither a LagrangianDS.");
#endif
#ifdef MLCPPROJ_DEBUG
std::cout << "MLCPProjectOnConstraints::computeDiagonalInteractionBlock DiaginteractionBlock " << std::endl;
currentInteractionBlock->display();
#endif
// Set pos for next loop.
pos = pos2;
}
}