void LagrangianDSTest::testcomputeDS()
{
std::cout << "-->Test: computeDS." <<std::endl;
DynamicalSystem * ds(new LagrangianDS(tmpxml2));
SP::LagrangianDS copy = std11::static_pointer_cast<LagrangianDS>(ds);
double time = 1.5;
ds->initialize("EventDriven", time);
ds->computeRhs(time);
std::cout << "-->Test: computeDS." <<std::endl;
ds->computeJacobianRhsx(time);
std::cout << "-->Test: computeDS." <<std::endl;
SimpleMatrix M(3, 3);
M(0, 0) = 1;
M(1, 1) = 2;
M(2, 2) = 3;
SP::SiconosMatrix jx = ds->jacobianRhsx();
SP::SiconosVector vf = ds->rhs();
CPPUNIT_ASSERT_EQUAL_MESSAGE("testComputeDSI : ", *(vf->vector(0)) == *velocity0, true);
CPPUNIT_ASSERT_EQUAL_MESSAGE("testComputeDSJ : ", prod(M, *(vf->vector(1))) == (copy->getFExt() - copy->getFInt() - copy->getFGyr()) , true);
CPPUNIT_ASSERT_EQUAL_MESSAGE("testComputeDSL : ", prod(M, *(jx->block(1, 0))) == (copy->getJacobianFL(0)) , true);
CPPUNIT_ASSERT_EQUAL_MESSAGE("testComputeDSL : ", prod(M, *(jx->block(1, 1))) == (copy->getJacobianFL(1)) , true);
std::cout << "--> computeDS test ended with success." <<std::endl;
}
void DynamicalSystem::setJacobianRhsxPtr(SP::SiconosMatrix newPtr)
{
// check dimensions ...
if (newPtr->size(0) != _n || newPtr->size(1) != _n)
RuntimeException::selfThrow("DynamicalSystem::setJacobianRhsxPtr - inconsistent sizes between _jacxRhs input and n - Maybe you forget to set n?");
_jacxRhs = newPtr;
}
void adjointInput::JacobianXbeta(double t, SiconosVector& xvalue, SP::SiconosMatrix JacXbeta)
{
JacXbeta->setValue(0, 0, 0.0) ;
JacXbeta->setValue(0, 1, -1.0 / 2.0) ;
JacXbeta->setValue(1, 0, 1.0 / 2.0) ;
JacXbeta->setValue(1, 1, 0.0) ;
#ifdef SICONOS_DEBUG
std::cout << "JacXbeta\n" << std::endl;;
JacXbeta->display();
#endif
}
/** Constructor with the plugin name
* \param PO a PluggedObject
* \param n the number of rows of the matrix
* \param p the number of column of the matrix
* \param indx the column index (optional)
*/
SubPluggedObject(const PluggedObject& PO, const unsigned int n, const unsigned int p, const unsigned int indx = 0): _indx(indx), _p(p)
{
_pluginName = "Sub" + PO.getPluginName();
_tmpMat.reset(new SimpleMatrix(n, p));
#if (__GNUG__ && !( __clang__ || __INTEL_COMPILER || __APPLE__ ))
fPtr = (void *)&SubPluggedObject::computeAndExtract;
_parentfPtr = PO.fPtr;
#else
RuntimeException::selfThrow("SubPluggedObject must be compiled with GCC !");
#endif
};
/** Constructor with the plugin name
* \param PO a PluggedObject
* \param n the number of rows of the matrix
* \param p the number of column of the matrix
* \param indx the column index (optional)
*/
SubPluggedObject(const PluggedObject& PO, const unsigned int n, const unsigned int p, const unsigned int indx = 0): _indx(indx), _p(p)
{
_pluginName = "Sub" + PO.pluginName();
_tmpMat.reset(new SimpleMatrix(n, p));
#if (__GNUG__ && !( __clang__ || __INTEL_COMPILER || __APPLE__ ) && (((__GNUC__ > 5) && (__GNUC_MINOR__ > 0))))
#pragma GCC diagnostic ignored "-Wpmf-conversions"
fPtr = (void *)&SubPluggedObject::computeAndExtract;
_parentfPtr = PO.fPtr;
#else
RuntimeException::selfThrow("SubPluggedObject must be compiled with GCC !");
#endif
};
// ================= Creation of the model =======================
void Spheres::init()
{
SP::TimeDiscretisation timedisc_;
SP::Simulation simulation_;
SP::FrictionContact osnspb_;
// User-defined main parameters
double t0 = 0; // initial computation time
double T = std::numeric_limits<double>::infinity();
double h = 0.01; // time step
double g = 9.81;
double theta = 0.5; // theta for MoreauJeanOSI integrator
std::string solverName = "NSGS";
// -----------------------------------------
// --- Dynamical systems && interactions ---
// -----------------------------------------
double R;
double m;
try
{
// ------------
// --- Init ---
// ------------
std::cout << "====> Model loading ..." << std::endl << std::endl;
_plans.reset(new SimpleMatrix("plans.dat", true));
SP::SiconosMatrix Spheres;
Spheres.reset(new SimpleMatrix("spheres.dat", true));
// -- OneStepIntegrators --
SP::OneStepIntegrator osi;
osi.reset(new MoreauJeanOSI(theta));
// -- Model --
_model.reset(new Model(t0, T));
for (unsigned int i = 0; i < Spheres->size(0); i++)
{
R = Spheres->getValue(i, 3);
m = Spheres->getValue(i, 4);
SP::SiconosVector qTmp;
SP::SiconosVector vTmp;
qTmp.reset(new SiconosVector(NDOF));
vTmp.reset(new SiconosVector(NDOF));
vTmp->zero();
(*qTmp)(0) = (*Spheres)(i, 0);
(*qTmp)(1) = (*Spheres)(i, 1);
(*qTmp)(2) = (*Spheres)(i, 2);
(*qTmp)(3) = M_PI / 2;
(*qTmp)(4) = M_PI / 4;
(*qTmp)(5) = M_PI / 2;
(*vTmp)(0) = 0;
(*vTmp)(1) = 0;
(*vTmp)(2) = 0;
(*vTmp)(3) = 0;
(*vTmp)(4) = 0;
(*vTmp)(5) = 0;
SP::LagrangianDS body;
body.reset(new SphereLDS(R, m, std11::shared_ptr<SiconosVector>(qTmp), std11::shared_ptr<SiconosVector>(vTmp)));
// -- Set external forces (weight) --
SP::SiconosVector FExt;
FExt.reset(new SiconosVector(NDOF));
FExt->zero();
FExt->setValue(2, -m * g);
body->setFExtPtr(FExt);
// add the dynamical system to the one step integrator
osi->insertDynamicalSystem(body);
// add the dynamical system in the non smooth dynamical system
_model->nonSmoothDynamicalSystem()->insertDynamicalSystem(body);
}
// ------------------
// --- Simulation ---
// ------------------
// -- Time discretisation --
timedisc_.reset(new TimeDiscretisation(t0, h));
// -- OneStepNsProblem --
osnspb_.reset(new FrictionContact(3));
osnspb_->numericsSolverOptions()->iparam[0] = 100; // Max number of
// iterations
osnspb_->numericsSolverOptions()->iparam[1] = 20; // compute error
// iterations
osnspb_->numericsSolverOptions()->iparam[4] = 2; // projection
osnspb_->numericsSolverOptions()->dparam[0] = 1e-6; // Tolerance
osnspb_->numericsSolverOptions()->dparam[2] = 1e-8; // Local tolerance
osnspb_->setMaxSize(16384); // max number of interactions
osnspb_->setMStorageType(1); // Sparse storage
osnspb_->setNumericsVerboseMode(0); // 0 silent, 1 verbose
osnspb_->setKeepLambdaAndYState(true); // inject previous solution
simulation_.reset(new TimeStepping(timedisc_));
simulation_->insertIntegrator(osi);
simulation_->insertNonSmoothProblem(osnspb_);
// simulation_->setCheckSolverFunction(localCheckSolverOuput);
// --- Simulation initialization ---
std::cout << "====> Simulation initialisation ..." << std::endl << std::endl;
SP::NonSmoothLaw nslaw(new NewtonImpactFrictionNSL(0, 0, 0.8, 3));
_playground.reset(new SpaceFilter(3, 6, _model, _plans, _moving_plans));
_playground->insert(nslaw, 0, 0);
_model->initialize(simulation_);
}
catch (SiconosException e)
{
std::cout << e.report() << std::endl;
exit(1);
}
catch (...)
{
std::cout << "Exception caught in Spheres::init()" << std::endl;
exit(1);
}
}
void LsodarOSI::computeFreeOutput(InteractionsGraph::VDescriptor& vertex_inter, OneStepNSProblem* osnsp)
{
SP::OneStepNSProblems allOSNS = simulationLink->oneStepNSProblems();
SP::InteractionsGraph indexSet = osnsp->simulation()->indexSet(osnsp->indexSetLevel());
SP::Interaction inter = indexSet->bundle(vertex_inter);
VectorOfBlockVectors& DSlink = *indexSet->properties(vertex_inter).DSlink;
// Get relation and non smooth law types
RELATION::TYPES relationType = inter->relation()->getType();
RELATION::SUBTYPES relationSubType = inter->relation()->getSubType();
unsigned int sizeY = inter->nonSmoothLaw()->size();
unsigned int relativePosition = 0;
SP::Interaction mainInteraction = inter;
Index coord(8);
coord[0] = relativePosition;
coord[1] = relativePosition + sizeY;
coord[2] = 0;
coord[4] = 0;
coord[6] = 0;
coord[7] = sizeY;
SP::SiconosMatrix C;
// SP::SiconosMatrix D;
// SP::SiconosMatrix F;
SiconosVector& yForNSsolver = *inter->yForNSsolver();
SP::BlockVector Xfree;
// All of these values should be stored in the node corrseponding to the Interactionwhen a MoreauJeanOSI scheme is used.
/* V.A. 10/10/2010
* Following the type of OSNS we need to retrieve the velocity or the acceleration
* This tricks is not very nice but for the moment the OSNS do not known if
* it is in accelaration of not
*/
//SP::OneStepNSProblems allOSNS = _simulation->oneStepNSProblems();
if (((*allOSNS)[SICONOS_OSNSP_ED_SMOOTH_ACC]).get() == osnsp)
{
if (relationType == Lagrangian)
{
Xfree = DSlink[LagrangianR::xfree];
}
// else if (relationType == NewtonEuler)
// {
// Xfree = inter->data(NewtonEulerR::free);
// }
assert(Xfree);
// std::cout << "Computeqblock Xfree (Gamma)========" << std::endl;
// Xfree->display();
}
else if (((*allOSNS)[SICONOS_OSNSP_ED_IMPACT]).get() == osnsp)
{
Xfree = DSlink[LagrangianR::q1];
// std::cout << "Computeqblock Xfree (Velocity)========" << std::endl;
// Xfree->display();
}
else
RuntimeException::selfThrow(" computeqBlock for Event Event-driven is wrong ");
if (relationType == Lagrangian)
{
C = mainInteraction->relation()->C();
if (C)
{
assert(Xfree);
coord[3] = C->size(1);
coord[5] = C->size(1);
subprod(*C, *Xfree, yForNSsolver, coord, true);
}
SP::SiconosMatrix ID(new SimpleMatrix(sizeY, sizeY));
ID->eye();
Index xcoord(8);
xcoord[0] = 0;
xcoord[1] = sizeY;
xcoord[2] = 0;
xcoord[3] = sizeY;
xcoord[4] = 0;
xcoord[5] = sizeY;
xcoord[6] = 0;
xcoord[7] = sizeY;
// For the relation of type LagrangianRheonomousR
if (relationSubType == RheonomousR)
{
if (((*allOSNS)[SICONOS_OSNSP_ED_SMOOTH_ACC]).get() == osnsp)
{
RuntimeException::selfThrow("LsodarOSI::computeFreeOutput not yet implemented for LCP at acceleration level with LagrangianRheonomousR");
}
else if (((*allOSNS)[SICONOS_OSNSP_TS_VELOCITY]).get() == osnsp)
{
SiconosVector q = *DSlink[LagrangianR::q0];
SiconosVector z = *DSlink[LagrangianR::z];
std11::static_pointer_cast<LagrangianRheonomousR>(inter->relation())->computehDot(simulation()->getTkp1(), q, z);
*DSlink[LagrangianR::z] = z;
subprod(*ID, *(std11::static_pointer_cast<LagrangianRheonomousR>(inter->relation())->hDot()), yForNSsolver, xcoord, false); // y += hDot
}
else
RuntimeException::selfThrow("LsodarOSI::computeFreeOutput not implemented for SICONOS_OSNSP ");
}
// For the relation of type LagrangianScleronomousR
if (relationSubType == ScleronomousR)
{
if (((*allOSNS)[SICONOS_OSNSP_ED_SMOOTH_ACC]).get() == osnsp)
{
std11::static_pointer_cast<LagrangianScleronomousR>(inter->relation())->computedotjacqhXqdot(simulation()->getTkp1(), *inter, DSlink);
subprod(*ID, *(std11::static_pointer_cast<LagrangianScleronomousR>(inter->relation())->dotjacqhXqdot()), yForNSsolver, xcoord, false); // y += NonLinearPart
}
}
}
else
RuntimeException::selfThrow("LsodarOSI::computeFreeOutput not yet implemented for Relation of type " + relationType);
if (((*allOSNS)[SICONOS_OSNSP_ED_IMPACT]).get() == osnsp)
{
if (inter->relation()->getType() == Lagrangian || inter->relation()->getType() == NewtonEuler)
{
SP::SiconosVisitor nslEffectOnFreeOutput(new _NSLEffectOnFreeOutput(osnsp, inter));
inter->nonSmoothLaw()->accept(*nslEffectOnFreeOutput);
}
}
}
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::computeFreeOutput(InteractionsGraph::VDescriptor& vertex_inter, OneStepNSProblem* osnsp)
{
/** \warning: ensures that it can also work with two different osi for two different ds ?
*/
SP::InteractionsGraph indexSet = osnsp->simulation()->indexSet(osnsp->indexSetLevel());
SP::Interaction inter = indexSet->bundle(vertex_inter);
SP::OneStepNSProblems allOSNS = simulationLink->oneStepNSProblems();
VectorOfBlockVectors& DSlink = *indexSet->properties(vertex_inter).DSlink;
// Get relation and non smooth law types
RELATION::TYPES relationType = inter->relation()->getType();
RELATION::SUBTYPES relationSubType = inter->relation()->getSubType();
unsigned int sizeY = inter->nonSmoothLaw()->size();
unsigned int relativePosition = 0;
Index coord(8);
coord[0] = relativePosition;
coord[1] = relativePosition + sizeY;
coord[2] = 0;
coord[4] = 0;
coord[6] = 0;
coord[7] = sizeY;
SP::SiconosMatrix C;
SP::SiconosMatrix D;
SP::SiconosMatrix F;
SP::BlockVector deltax;
SiconosVector& yForNSsolver = *inter->yForNSsolver();
SP::SiconosVector e;
SP::BlockVector Xfree;
if (relationType == NewtonEuler)
{
Xfree = DSlink[NewtonEulerR::xfree];
}
else if (relationType == Lagrangian)
{
Xfree = DSlink[LagrangianR::xfree];
}
assert(Xfree);
assert(Xfree);
SP::Interaction mainInteraction = inter;
assert(mainInteraction);
assert(mainInteraction->relation());
if (relationSubType == LinearTIR)
{
if (((*allOSNS)[SICONOS_OSNSP_TS_VELOCITY]).get() != osnsp)
RuntimeException::selfThrow("SchatzmanPaoliOSI::computeFreeOutput not yet implemented for SICONOS_OSNSP ");
C = mainInteraction->relation()->C();
if (C)
{
assert(Xfree);
coord[3] = C->size(1);
coord[5] = C->size(1);
// creates a POINTER link between workX[ds] (xfree) and the
// corresponding interactionBlock in each Interactionfor each ds of the
// current Interaction.
if (_useGammaForRelation)
{
assert(deltax);
subprod(*C, *deltax, yForNSsolver, coord, true);
}
else
{
subprod(*C, *Xfree, yForNSsolver, coord, true);
// subprod(*C,*(*(mainInteraction->dynamicalSystemsBegin()))->workspace(DynamicalSystem::free),*Yp,coord,true);
// if (mainInteraction->dynamicalSystems()->size() == 2)
// {
// subprod(*C,*(*++(mainInteraction->dynamicalSystemsBegin()))->workspace(DynamicalSystem::free),*Yp,coord,false);
// }
}
}
SP::LagrangianLinearTIR ltir = std11::static_pointer_cast<LagrangianLinearTIR> (mainInteraction->relation());
e = ltir->e();
if (e)
{
yForNSsolver += *e;
}
}
else
RuntimeException::selfThrow("SchatzmanPaoliOSI::ComputeFreeOutput not yet implemented for relation of Type : " + relationType);
if (inter->relation()->getSubType() == LinearTIR)
{
SP::SiconosVisitor nslEffectOnFreeOutput(new _NSLEffectOnFreeOutput(osnsp, inter));
inter->nonSmoothLaw()->accept(*nslEffectOnFreeOutput);
}
}
void LinearOSNS::computeDiagonalInteractionBlock(const InteractionsGraph::VDescriptor& vd)
{
DEBUG_BEGIN("LinearOSNS::computeDiagonalInteractionBlock(const InteractionsGraph::VDescriptor& vd)\n");
// Computes matrix _interactionBlocks[inter1][inter1] (and allocates memory if
// necessary) one or two DS are concerned by inter1 . 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 dimension of the NonSmoothLaw (ie dim of the interactionBlock)
SP::InteractionsGraph indexSet = simulation()->indexSet(indexSetLevel());
SP::Interaction inter = indexSet->bundle(vd);
// Get osi property from interaction
// We assume that all ds in vertex_inter have the same osi.
SP::OneStepIntegrator Osi = indexSet->properties(vd).osi;
//SP::OneStepIntegrator Osi = simulation()->integratorOfDS(ds);
OSI::TYPES osiType = Osi->getType();
// At most 2 DS are linked by an Interaction
SP::DynamicalSystem DS1;
SP::DynamicalSystem DS2;
unsigned int pos1, pos2;
// --- Get the dynamical system(s) (edge(s)) connected to the current interaction (vertex) ---
if (indexSet->properties(vd).source != indexSet->properties(vd).target)
{
DEBUG_PRINT("a two DS Interaction\n");
DS1 = indexSet->properties(vd).source;
DS2 = indexSet->properties(vd).target;
}
else
{
DEBUG_PRINT("a single DS Interaction\n");
DS1 = indexSet->properties(vd).source;
DS2 = DS1;
// \warning this looks like some debug code, but it gets executed even with NDEBUG.
// may be compiler does something smarter, but still it should be rewritten. --xhub
InteractionsGraph::OEIterator oei, oeiend;
for (std11::tie(oei, oeiend) = indexSet->out_edges(vd);
oei != oeiend; ++oei)
{
// note : at most 4 edges
DS2 = indexSet->bundle(*oei);
if (DS2 != DS1)
{
assert(false);
break;
}
}
}
assert(DS1);
assert(DS2);
pos1 = indexSet->properties(vd).source_pos;
pos2 = indexSet->properties(vd).target_pos;
// --- Check block size ---
assert(indexSet->properties(vd).block->size(0) == inter->nonSmoothLaw()->size());
assert(indexSet->properties(vd).block->size(1) == inter->nonSmoothLaw()->size());
// --- Compute diagonal block ---
// Block to be set in OSNS Matrix, corresponding to
// the current interaction
SP::SiconosMatrix currentInteractionBlock = indexSet->properties(vd).block;
SP::SiconosMatrix leftInteractionBlock, rightInteractionBlock;
RELATION::TYPES relationType;
double h = simulation()->currentTimeStep();
// 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.
relationType = inter->relation()->getType();
VectorOfSMatrices& workMInter = *indexSet->properties(vd).workMatrices;
inter->getExtraInteractionBlock(currentInteractionBlock, workMInter);
unsigned int nslawSize = inter->nonSmoothLaw()->size();
// loop over the DS connected to the interaction.
bool endl = false;
unsigned int pos = pos1;
for (SP::DynamicalSystem ds = DS1; !endl; ds = DS2)
{
assert(ds == DS1 || ds == DS2);
endl = (ds == DS2);
unsigned int sizeDS = ds->dimension();
// get _interactionBlocks corresponding to the current DS
// These _interactionBlocks depends on the relation type.
leftInteractionBlock.reset(new SimpleMatrix(nslawSize, sizeDS));
inter->getLeftInteractionBlockForDS(pos, leftInteractionBlock, workMInter);
DEBUG_EXPR(leftInteractionBlock->display(););
// Computing depends on relation type -> move this in Interaction method?
if (relationType == FirstOrder)
{
rightInteractionBlock.reset(new SimpleMatrix(sizeDS, nslawSize));
inter->getRightInteractionBlockForDS(pos, rightInteractionBlock, workMInter);
if (osiType == OSI::EULERMOREAUOSI)
{
if ((std11::static_pointer_cast<EulerMoreauOSI> (Osi))->useGamma() || (std11::static_pointer_cast<EulerMoreauOSI> (Osi))->useGammaForRelation())
{
*rightInteractionBlock *= (std11::static_pointer_cast<EulerMoreauOSI> (Osi))->gamma();
}
}
// for ZOH, we have a different formula ...
if (osiType == OSI::ZOHOSI && indexSet->properties(vd).forControl)
{
*rightInteractionBlock = std11::static_pointer_cast<ZeroOrderHoldOSI>(Osi)->Bd(ds);
prod(*leftInteractionBlock, *rightInteractionBlock, *currentInteractionBlock, false);
}
else
{
// centralInteractionBlock contains a lu-factorized matrix and we solve
// centralInteractionBlock * X = rightInteractionBlock with PLU
SP::SiconosMatrix centralInteractionBlock = getOSIMatrix(Osi, ds);
centralInteractionBlock->PLUForwardBackwardInPlace(*rightInteractionBlock);
inter->computeKhat(*rightInteractionBlock, workMInter, h); // if K is non 0
// integration of r with theta method removed
// *currentInteractionBlock += h *Theta[*itDS]* *leftInteractionBlock * (*rightInteractionBlock); //left = C, right = W.B
//gemm(h,*leftInteractionBlock,*rightInteractionBlock,1.0,*currentInteractionBlock);
*leftInteractionBlock *= h;
prod(*leftInteractionBlock, *rightInteractionBlock, *currentInteractionBlock, false);
//left = C, right = inv(W).B
}
}
else if (relationType == Lagrangian ||
relationType == NewtonEuler)
{
SP::BoundaryCondition bc;
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()) bc = d->boundaryConditions();
}
else if (dsType == Type::NewtonEulerDS)
{
SP::NewtonEulerDS d = std11::static_pointer_cast<NewtonEulerDS> (ds);
if (d->boundaryConditions()) bc = d->boundaryConditions();
}
if (bc)
{
for (std::vector<unsigned int>::iterator itindex = bc->velocityIndices()->begin() ;
itindex != bc->velocityIndices()->end();
++itindex)
{
// (nslawSize,sizeDS));
SP::SiconosVector coltmp(new SiconosVector(nslawSize));
coltmp->zero();
leftInteractionBlock->setCol(*itindex, *coltmp);
}
}
DEBUG_PRINT("leftInteractionBlock after application of boundary conditions\n");
DEBUG_EXPR(leftInteractionBlock->display(););
void OneStepNSProblem::updateInteractionBlocks()
{
DEBUG_PRINT("OneStepNSProblem::updateInteractionBlocks() starts\n");
// The present functions checks various conditions and possibly
// compute interactionBlocks matrices.
//
// Let interi and interj be two Interactions.
//
// Things to be checked are:
// 1 - is the topology time invariant?
// 2 - does interactionBlocks[interi][interj] already exists (ie has been
// computed in a previous time step)?
// 3 - do we need to compute this interactionBlock? A interactionBlock is
// to be computed if interi and interj are in IndexSet1 AND if interi and
// interj have common DynamicalSystems.
//
// The possible cases are:
//
// - If 1 and 2 are true then it does nothing. 3 is not checked.
// - If 1 == true, 2 == false, 3 == false, it does nothing.
// - If 1 == true, 2 == false, 3 == true, it computes the
// interactionBlock.
// - If 1==false, 2 is not checked, and the interactionBlock is
// computed if 3==true.
//
// Get index set from Simulation
SP::InteractionsGraph indexSet = simulation()->indexSet(indexSetLevel());
bool isLinear = simulation()->model()->nonSmoothDynamicalSystem()->isLinear();
// we put diagonal informations on vertices
// self loops with bgl are a *nightmare* at the moment
// (patch 65198 on standard boost install)
if (indexSet->properties().symmetric)
{
DEBUG_PRINT("OneStepNSProblem::updateInteractionBlocks(). Symmetric case");
InteractionsGraph::VIterator vi, viend;
for (std11::tie(vi, viend) = indexSet->vertices();
vi != viend; ++vi)
{
SP::Interaction inter = indexSet->bundle(*vi);
unsigned int nslawSize = inter->nonSmoothLaw()->size();
if (! indexSet->properties(*vi).block)
{
indexSet->properties(*vi).block.reset(new SimpleMatrix(nslawSize, nslawSize));
}
if (!isLinear || !_hasBeenUpdated)
{
computeDiagonalInteractionBlock(*vi);
}
}
/* interactionBlock must be zeroed at init */
std::vector<bool> initialized;
initialized.resize(indexSet->edges_number());
std::fill(initialized.begin(), initialized.end(), false);
InteractionsGraph::EIterator ei, eiend;
for (std11::tie(ei, eiend) = indexSet->edges();
ei != eiend; ++ei)
{
SP::Interaction inter1 = indexSet->bundle(indexSet->source(*ei));
SP::Interaction inter2 = indexSet->bundle(indexSet->target(*ei));
/* on adjoint graph there is at most 2 edges between source and target */
InteractionsGraph::EDescriptor ed1, ed2;
std11::tie(ed1, ed2) = indexSet->edges(indexSet->source(*ei), indexSet->target(*ei));
assert(*ei == ed1 || *ei == ed2);
/* the first edge has the lower index */
assert(indexSet->index(ed1) <= indexSet->index(ed2));
// Memory allocation if needed
unsigned int nslawSize1 = inter1->nonSmoothLaw()->size();
unsigned int nslawSize2 = inter2->nonSmoothLaw()->size();
unsigned int isrc = indexSet->index(indexSet->source(*ei));
unsigned int itar = indexSet->index(indexSet->target(*ei));
SP::SiconosMatrix currentInteractionBlock;
if (itar > isrc) // upper block
{
if (! indexSet->properties(ed1).upper_block)
{
indexSet->properties(ed1).upper_block.reset(new SimpleMatrix(nslawSize1, nslawSize2));
if (ed2 != ed1)
indexSet->properties(ed2).upper_block = indexSet->properties(ed1).upper_block;
}
currentInteractionBlock = indexSet->properties(ed1).upper_block;
}
else // lower block
{
if (! indexSet->properties(ed1).lower_block)
{
indexSet->properties(ed1).lower_block.reset(new SimpleMatrix(nslawSize1, nslawSize2));
if (ed2 != ed1)
indexSet->properties(ed2).lower_block = indexSet->properties(ed1).lower_block;
}
currentInteractionBlock = indexSet->properties(ed1).lower_block;
}
if (!initialized[indexSet->index(ed1)])
{
initialized[indexSet->index(ed1)] = true;
currentInteractionBlock->zero();
}
if (!isLinear || !_hasBeenUpdated)
{
{
computeInteractionBlock(*ei);
}
// allocation for transposed block
// should be avoided
if (itar > isrc) // upper block has been computed
{
if (!indexSet->properties(ed1).lower_block)
{
indexSet->properties(ed1).lower_block.
reset(new SimpleMatrix(indexSet->properties(ed1).upper_block->size(1),
indexSet->properties(ed1).upper_block->size(0)));
}
indexSet->properties(ed1).lower_block->trans(*indexSet->properties(ed1).upper_block);
indexSet->properties(ed2).lower_block = indexSet->properties(ed1).lower_block;
}
else
{
assert(itar < isrc); // lower block has been computed
if (!indexSet->properties(ed1).upper_block)
{
indexSet->properties(ed1).upper_block.
reset(new SimpleMatrix(indexSet->properties(ed1).lower_block->size(1),
indexSet->properties(ed1).lower_block->size(0)));
}
indexSet->properties(ed1).upper_block->trans(*indexSet->properties(ed1).lower_block);
indexSet->properties(ed2).upper_block = indexSet->properties(ed1).upper_block;
}
}
}
}
else // not symmetric => follow out_edges for each vertices
{
DEBUG_PRINT("OneStepNSProblem::updateInteractionBlocks(). Non symmetric case\n");
InteractionsGraph::VIterator vi, viend;
for (std11::tie(vi, viend) = indexSet->vertices();
vi != viend; ++vi)
{
DEBUG_PRINT("OneStepNSProblem::updateInteractionBlocks(). Computation of diaganal block\n");
SP::Interaction inter = indexSet->bundle(*vi);
unsigned int nslawSize = inter->nonSmoothLaw()->size();
if (! indexSet->properties(*vi).block)
{
indexSet->properties(*vi).block.reset(new SimpleMatrix(nslawSize, nslawSize));
}
if (!isLinear || !_hasBeenUpdated)
{
computeDiagonalInteractionBlock(*vi);
}
/* on a undirected graph, out_edges gives all incident edges */
InteractionsGraph::OEIterator oei, oeiend;
/* interactionBlock must be zeroed at init */
std::map<SP::SiconosMatrix, bool> initialized;
for (std11::tie(oei, oeiend) = indexSet->out_edges(*vi);
oei != oeiend; ++oei)
{
/* on adjoint graph there is at most 2 edges between source and target */
InteractionsGraph::EDescriptor ed1, ed2;
std11::tie(ed1, ed2) = indexSet->edges(indexSet->source(*oei), indexSet->target(*oei));
if (indexSet->properties(ed1).upper_block)
{
initialized[indexSet->properties(ed1).upper_block] = false;
}
// if(indexSet->properties(ed2).upper_block)
// {
// initialized[indexSet->properties(ed2).upper_block] = false;
// }
if (indexSet->properties(ed1).lower_block)
{
initialized[indexSet->properties(ed1).lower_block] = false;
}
// if(indexSet->properties(ed2).lower_block)
// {
// initialized[indexSet->properties(ed2).lower_block] = false;
// }
}
for (std11::tie(oei, oeiend) = indexSet->out_edges(*vi);
oei != oeiend; ++oei)
{
DEBUG_PRINT("OneStepNSProblem::updateInteractionBlocks(). Computation of extra-diaganal block\n");
/* on adjoint graph there is at most 2 edges between source and target */
InteractionsGraph::EDescriptor ed1, ed2;
std11::tie(ed1, ed2) = indexSet->edges(indexSet->source(*oei), indexSet->target(*oei));
assert(*oei == ed1 || *oei == ed2);
/* the first edge as the lower index */
assert(indexSet->index(ed1) <= indexSet->index(ed2));
SP::Interaction inter1 = indexSet->bundle(indexSet->source(*oei));
SP::Interaction inter2 = indexSet->bundle(indexSet->target(*oei));
// Memory allocation if needed
unsigned int nslawSize1 = inter1->nonSmoothLaw()->size();
unsigned int nslawSize2 = inter2->nonSmoothLaw()->size();
unsigned int isrc = indexSet->index(indexSet->source(*oei));
unsigned int itar = indexSet->index(indexSet->target(*oei));
SP::SiconosMatrix currentInteractionBlock;
if (itar > isrc) // upper block
{
if (! indexSet->properties(ed1).upper_block)
{
indexSet->properties(ed1).upper_block.reset(new SimpleMatrix(nslawSize1, nslawSize2));
initialized[indexSet->properties(ed1).upper_block] = false;
if (ed2 != ed1)
indexSet->properties(ed2).upper_block = indexSet->properties(ed1).upper_block;
}
currentInteractionBlock = indexSet->properties(ed1).upper_block;
}
else // lower block
{
if (! indexSet->properties(ed1).lower_block)
{
indexSet->properties(ed1).lower_block.reset(new SimpleMatrix(nslawSize1, nslawSize2));
initialized[indexSet->properties(ed1).lower_block] = false;
if (ed2 != ed1)
indexSet->properties(ed2).lower_block = indexSet->properties(ed1).lower_block;
}
currentInteractionBlock = indexSet->properties(ed1).lower_block;
}
if (!initialized[currentInteractionBlock])
{
initialized[currentInteractionBlock] = true;
currentInteractionBlock->zero();
}
if (!isLinear || !_hasBeenUpdated)
{
if (isrc != itar)
computeInteractionBlock(*oei);
}
}
}
}
DEBUG_EXPR(displayBlocks(indexSet););
void SchatzmanPaoliOSI::computeFreeState()
{
// This function computes "free" states of the DS belonging to this Integrator.
// "Free" means without taking non-smooth effects into account.
//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.
// Note: integration of r with a theta method has been removed
// SiconosVector *rold = static_cast<SiconosVector*>(d->rMemory()->getSiconosVector(0));
// Iteration through the set of Dynamical Systems.
//
SP::DynamicalSystem ds; // Current Dynamical System.
SP::SiconosMatrix W; // W SchatzmanPaoliOSI matrix of the current DS.
Type::Siconos dsType ; // Type of the current DS.
DynamicalSystemsGraph::VIterator dsi, dsend;
for (std11::tie(dsi, dsend) = _dynamicalSystemsGraph->vertices(); dsi != dsend; ++dsi)
{
if (!checkOSI(dsi)) continue;
ds = _dynamicalSystemsGraph->bundle(*dsi);
dsType = Type::value(*ds); // Its type
W = _dynamicalSystemsGraph->properties(*dsi).W; // Its W SchatzmanPaoliOSI matrix of iteration.
//1 - Lagrangian Non Linear Systems
if (dsType == Type::LagrangianDS)
{
RuntimeException::selfThrow("SchatzmanPaoliOSI::computeFreeState - not yet implemented for Dynamical system type: " + dsType);
}
// 2 - Lagrangian Linear Systems
else if (dsType == Type::LagrangianLinearTIDS)
{
// IN to be updated at current time: Fext
// IN at told: qi,vi, fext
// IN constants: K,C
// Note: indices i/i+1 corresponds to value at the beginning/end of the time step.
// "i" values are saved in memory vectors.
// vFree = v_i + W^{-1} ResiduFree // with
// ResiduFree = (-h*C -h^2*theta*K)*vi - h*K*qi + h*theta * Fext_i+1 + h*(1-theta)*Fext_i
// -- 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 qold = d->qMemory()->getSiconosVector(0); // q_k
// SP::SiconosVector vold = d->velocityMemory()->getSiconosVector(0); //v_k
// --- ResiduFree computation ---
// vFree pointer is used to compute and save ResiduFree in this first step.
// Velocity free and residu. vFree = RESfree (pointer equality !!).
SP::SiconosVector qfree = d->workspace(DynamicalSystem::free);//workX[d];
(*qfree) = *(d->workspace(DynamicalSystem::freeresidu));
W->PLUForwardBackwardInPlace(*qfree);
*qfree *= -1.0;
*qfree += *qold;
}
// 3 - Newton Euler Systems
else if (dsType == Type::NewtonEulerDS)
{
RuntimeException::selfThrow("SchatzmanPaoliOSI::computeFreeState - not yet implemented for Dynamical system type: " + dsType);
}
else
RuntimeException::selfThrow("SchatzmanPaoliOSI::computeFreeState - not yet implemented for Dynamical system type: " + dsType);
}
}
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);
}
void MLCPProjectOnConstraints::updateInteractionBlocks()
{
// The present functions checks various conditions and possibly
// compute interactionBlocks matrices.
//
// Let interi and interj be two Interactions.
//
// Things to be checked are:
// 1 - is the topology time invariant?
// 2 - does interactionBlocks[interi][interj] already exists (ie has been
// computed in a previous time step)?
// 3 - do we need to compute this interactionBlock? A interactionBlock is
// to be computed if interi and interj are in IndexSet1 AND if interi and
// interj have common DynamicalSystems.
//
// The possible cases are:
//
// - If 1 and 2 are true then it does nothing. 3 is not checked.
// - If 1 == true, 2 == false, 3 == false, it does nothing.
// - If 1 == true, 2 == false, 3 == true, it computes the
// interactionBlock.
// - If 1==false, 2 is not checked, and the interactionBlock is
// computed if 3==true.
//
#ifdef MLCPPROJ_DEBUG
std::cout << " " << std::endl;
std::cout << "===================================================" << std::endl;
std::cout << "MLCPProjectOnConstraints::updateInteractionBlocks()" << std::endl;
#endif
// Get index set from Simulation
SP::InteractionsGraph indexSet = simulation()->indexSet(indexSetLevel());
// It seems that index() in not update in Index(0)
// see comment in void Simulation::updateIndexSets()
if (indexSetLevel() == 0)
{
indexSet->update_vertices_indices();
indexSet->update_edges_indices();
}
bool isLinear = simulation()->model()->nonSmoothDynamicalSystem()->isLinear();
// we put diagonal informations on vertices
// self loops with bgl are a *nightmare* at the moment
// (patch 65198 on standard boost install)
if (indexSet->properties().symmetric)
{
RuntimeException::selfThrow
(" MLCPProjectOnConstraints::updateInteractionBlocks() - not yet implemented for symmetric case");
}
else // not symmetric => follow out_edges for each vertices
{
if (!_hasBeenUpdated)
{
// printf("MLCPProjectOnConstraints::updateInteractionBlocks must be updated.\n");
_n = 0;
_m = 0;
_curBlock = 0;
}
InteractionsGraph::VIterator vi, viend;
for (std11::tie(vi, viend) = indexSet->vertices();
vi != viend; ++vi)
{
SP::Interaction inter = indexSet->bundle(*vi);
unsigned int nslawSize = std11::static_pointer_cast<OSNSMatrixProjectOnConstraints>
(_M)->computeSizeForProjection(inter);
#ifdef MLCPPROJ_DEBUG
std::cout << " " << std::endl;
std::cout << "Start to work on Interaction " << inter->number() << "of vertex" << *vi << std::endl;
#endif
if (! indexSet->blockProj[*vi])
{
#ifdef MLCPPROJ_DEBUG
std::cout << "Allocation of blockProj of size " << nslawSize << " x " << nslawSize << " for interaction " << inter->number() << std::endl;
#endif
indexSet->blockProj[*vi].reset(new SimpleMatrix(nslawSize, nslawSize));
}
if (!isLinear || !_hasBeenUpdated)
{
computeDiagonalInteractionBlock(*vi);
}
/* on a undirected graph, out_edges gives all incident edges */
InteractionsGraph::OEIterator oei, oeiend;
/* interactionBlock must be zeroed at init */
std::map<SP::SiconosMatrix, bool> initialized;
for (std11::tie(oei, oeiend) = indexSet->out_edges(*vi);
oei != oeiend; ++oei)
{
/* on adjoint graph there is at most 2 edges between source and target */
InteractionsGraph::EDescriptor ed1, ed2;
std11::tie(ed1, ed2) = indexSet->edges(indexSet->source(*oei), indexSet->target(*oei));
if (indexSet->upper_blockProj[ed1])
{
initialized[indexSet->upper_blockProj[ed1]] = false;
}
// if(indexSet->upper_blockProj[ed2])
// {
// initialized[indexSet->upper_blockProj[ed1]] = false;
// }
if (indexSet->lower_blockProj[ed1])
{
initialized[indexSet->lower_blockProj[ed2]] = false;
}
// if(indexSet->lower_blockProj[ed2])
// {
// initialized[indexSet->lower_blockProj[ed2]] = false;
// }
}
for (std11::tie(oei, oeiend) = indexSet->out_edges(*vi);
oei != oeiend; ++oei)
{
/* on adjoint graph there is at most 2 edges between source and target */
InteractionsGraph::EDescriptor ed1, ed2;
std11::tie(ed1, ed2) = indexSet->edges(indexSet->source(*oei), indexSet->target(*oei));
assert(*oei == ed1 || *oei == ed2);
/* the first edge as the lower index */
assert(indexSet->index(ed1) <= indexSet->index(ed2));
SP::Interaction inter1 = indexSet->bundle(indexSet->source(*oei));
SP::Interaction inter2 = indexSet->bundle(indexSet->target(*oei));
// Memory allocation if needed
unsigned int nslawSize1 = std11::static_pointer_cast<OSNSMatrixProjectOnConstraints>
(_M)->computeSizeForProjection(inter1);
unsigned int nslawSize2 = std11::static_pointer_cast<OSNSMatrixProjectOnConstraints>
(_M)->computeSizeForProjection(inter2);
unsigned int isrc = indexSet->index(indexSet->source(*oei));
unsigned int itar = indexSet->index(indexSet->target(*oei));
SP::SiconosMatrix currentInteractionBlock;
if (itar > isrc) // upper block
{
if (! indexSet->upper_blockProj[ed1])
{
indexSet->upper_blockProj[ed1].reset(new SimpleMatrix(nslawSize1, nslawSize2));
initialized[indexSet->upper_blockProj[ed1]] = false;
#ifdef MLCPPROJ_DEBUG
std::cout << "Allocation of upper_blockProj " << indexSet->upper_blockProj[ed1].get() << " of edge " << ed1 << " of size " << nslawSize1 << " x " << nslawSize2 << " for interaction " << inter1->number() << " and interaction " << inter2->number() << std::endl;
#endif
if (ed2 != ed1)
indexSet->upper_blockProj[ed2] = indexSet->upper_blockProj[ed1];
}
#ifdef MLCPPROJ_DEBUG
else
std::cout << "No Allocation of upper_blockProj of size " << nslawSize1 << " x " << nslawSize2 << std::endl;
#endif
currentInteractionBlock = indexSet->upper_blockProj[ed1];
#ifdef MLCPPROJ_DEBUG
std::cout << "currentInteractionBlock->size(0)" << currentInteractionBlock->size(0) << std::endl;
std::cout << "currentInteractionBlock->size(1)" << currentInteractionBlock->size(1) << std::endl;
std::cout << "inter1->display() " << inter1->number() << std::endl;
//inter1->display();
std::cout << "inter2->display() " << inter2->number() << std::endl;
//inter2->display();
#endif
}
else // lower block
{
if (! indexSet->lower_blockProj[ed1])
{
#ifdef MLCPPROJ_DEBUG
std::cout << "Allocation of lower_blockProj of size " << nslawSize1 << " x " << nslawSize2 << " for interaction " << inter1->number() << " and interaction " << inter2->number() << std::endl;
#endif
indexSet->lower_blockProj[ed1].reset(new SimpleMatrix(nslawSize1, nslawSize2));
initialized[indexSet->lower_blockProj[ed1]] = false;
if (ed2 != ed1)
indexSet->lower_blockProj[ed2] = indexSet->lower_blockProj[ed1];
}
#ifdef MLCPPROJ_DEBUG
else
std::cout << "No Allocation of lower_blockProj of size " << nslawSize1 << " x " << nslawSize2 << std::endl;
#endif
currentInteractionBlock = indexSet->lower_blockProj[ed1];
#ifdef MLCPPROJ_DEBUG
std::cout << "currentInteractionBlock->size(0)" << currentInteractionBlock->size(0) << std::endl;
std::cout << "currentInteractionBlock->size(1)" << currentInteractionBlock->size(1) << std::endl;
std::cout << "inter1->display() " << inter1->number() << std::endl;
//inter1->display();
std::cout << "inter2->display() " << inter2->number() << std::endl;
//inter2->display();
#endif
}
//assert(indexSet->index(ed1));
if (!initialized[currentInteractionBlock])
{
initialized[currentInteractionBlock] = true;
currentInteractionBlock->zero();
}
if (!isLinear || !_hasBeenUpdated)
{
if (isrc != itar)
computeInteractionBlock(*oei);
}
}
}
}
#ifdef MLCPPROJ_DEBUG
displayBlocks(indexSet);
#endif
}
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;
}
}
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");
}
for (std::vector<unsigned int>::iterator itindex = bc->velocityIndices()->begin() ;
itindex != bc->velocityIndices()->end();
++itindex)
{
// (nslawSize,sizeDS));
SP::SiconosVector coltmp(new SiconosVector(nslawSize));
coltmp->zero();
leftInteractionBlock->setCol(*itindex, *coltmp);
}
}
DEBUG_PRINT("leftInteractionBlock after application of boundary conditions\n");
DEBUG_EXPR(leftInteractionBlock->display(););
// (inter1 == inter2)
SP::SiconosMatrix work(new SimpleMatrix(*leftInteractionBlock));
work->trans();
SP::SiconosMatrix centralInteractionBlock = getOSIMatrix(Osi, ds);
DEBUG_EXPR(centralInteractionBlock->display(););
DEBUG_EXPR_WE(std::cout << std::boolalpha << " centralInteractionBlock->isPLUFactorized() = "<< centralInteractionBlock->isPLUFactorized() << std::endl;);
centralInteractionBlock->PLUForwardBackwardInPlace(*work);
//*currentInteractionBlock += *leftInteractionBlock ** work;
DEBUG_EXPR(work->display(););
prod(*leftInteractionBlock, *work, *currentInteractionBlock, false);
// gemm(CblasNoTrans,CblasNoTrans,1.0,*leftInteractionBlock,*work,1.0,*currentInteractionBlock);
//*currentInteractionBlock *=h;
DEBUG_EXPR(currentInteractionBlock->display(););
assert(currentInteractionBlock->isSymmetric(1e-10));
}
else RuntimeException::selfThrow("LinearOSNS::computeInteractionBlock not yet implemented for relation of type " + relationType);
// Set pos for next loop.
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);
}
}
// ================= Creation of the model =======================
void Disks::init()
{
SP::TimeDiscretisation timedisc_;
SP::TimeStepping simulation_;
SP::FrictionContact osnspb_;
// User-defined main parameters
double t0 = 0; // initial computation time
double T = std::numeric_limits<double>::infinity();
double h = 0.01; // time step
double g = 9.81;
double theta = 0.5; // theta for MoreauJeanOSI integrator
std::string solverName = "NSGS";
// -----------------------------------------
// --- Dynamical systems && interactions ---
// -----------------------------------------
double R;
double m;
try
{
// ------------
// --- Init ---
// ------------
std::cout << "====> Model loading ..." << std::endl << std::endl;
_plans.reset(new SimpleMatrix("plans.dat", true));
if (_plans->size(0) == 0)
{
/* default plans */
double A1 = P1A;
double B1 = P1B;
double C1 = P1C;
double A2 = P2A;
double B2 = P2B;
double C2 = P2C;
_plans.reset(new SimpleMatrix(6, 6));
_plans->zero();
(*_plans)(0, 0) = 0;
(*_plans)(0, 1) = 1;
(*_plans)(0, 2) = -GROUND;
(*_plans)(1, 0) = 1;
(*_plans)(1, 1) = 0;
(*_plans)(1, 2) = WALL;
(*_plans)(2, 0) = 1;
(*_plans)(2, 1) = 0;
(*_plans)(2, 2) = -WALL;
(*_plans)(3, 0) = 0;
(*_plans)(3, 1) = 1;
(*_plans)(3, 2) = -TOP;
(*_plans)(4, 0) = A1;
(*_plans)(4, 1) = B1;
(*_plans)(4, 2) = C1;
(*_plans)(5, 0) = A2;
(*_plans)(5, 1) = B2;
(*_plans)(5, 2) = C2;
}
/* set center positions */
for (unsigned int i = 0 ; i < _plans->size(0); ++i)
{
SP::DiskPlanR tmpr;
tmpr.reset(new DiskPlanR(1, (*_plans)(i, 0), (*_plans)(i, 1), (*_plans)(i, 2),
(*_plans)(i, 3), (*_plans)(i, 4), (*_plans)(i, 5)));
(*_plans)(i, 3) = tmpr->getXCenter();
(*_plans)(i, 4) = tmpr->getYCenter();
}
/* _moving_plans.reset(new FMatrix(1,6));
(*_moving_plans)(0,0) = &A;
(*_moving_plans)(0,1) = &B;
(*_moving_plans)(0,2) = &C;
(*_moving_plans)(0,3) = &DA;
(*_moving_plans)(0,4) = &DB;
(*_moving_plans)(0,5) = &DC;*/
SP::SiconosMatrix Disks;
Disks.reset(new SimpleMatrix("disks.dat", true));
// -- OneStepIntegrators --
SP::OneStepIntegrator osi;
osi.reset(new MoreauJeanOSI(theta));
// -- Model --
_model.reset(new Model(t0, T));
for (unsigned int i = 0; i < Disks->size(0); i++)
{
R = Disks->getValue(i, 2);
m = Disks->getValue(i, 3);
SP::SiconosVector qTmp;
SP::SiconosVector vTmp;
qTmp.reset(new SiconosVector(NDOF));
vTmp.reset(new SiconosVector(NDOF));
vTmp->zero();
(*qTmp)(0) = (*Disks)(i, 0);
(*qTmp)(1) = (*Disks)(i, 1);
SP::LagrangianDS body;
if (R > 0)
body.reset(new Disk(R, m, qTmp, vTmp));
else
body.reset(new Circle(-R, m, qTmp, vTmp));
// -- Set external forces (weight) --
SP::SiconosVector FExt;
FExt.reset(new SiconosVector(NDOF));
FExt->zero();
FExt->setValue(1, -m * g);
body->setFExtPtr(FExt);
// add the dynamical system to the one step integrator
osi->insertDynamicalSystem(body);
// add the dynamical system in the non smooth dynamical system
_model->nonSmoothDynamicalSystem()->insertDynamicalSystem(body);
}
_model->nonSmoothDynamicalSystem()->setSymmetric(true);
// ------------------
// --- Simulation ---
// ------------------
// -- Time discretisation --
timedisc_.reset(new TimeDiscretisation(t0, h));
// -- OneStepNsProblem --
osnspb_.reset(new FrictionContact(2));
osnspb_->numericsSolverOptions()->iparam[0] = 100; // Max number of
// iterations
osnspb_->numericsSolverOptions()->iparam[1] = 20; // compute error
// iterations
osnspb_->numericsSolverOptions()->dparam[0] = 1e-3; // Tolerance
osnspb_->setMaxSize(6 * ((3 * Ll * Ll + 3 * Ll) / 2 - Ll));
osnspb_->setMStorageType(1); // Sparse storage
osnspb_->setNumericsVerboseMode(0);
osnspb_->setKeepLambdaAndYState(true); // inject previous solution
// -- Simulation --
simulation_.reset(new TimeStepping(timedisc_));
std11::static_pointer_cast<TimeStepping>(simulation_)->setNewtonMaxIteration(3);
simulation_->insertIntegrator(osi);
simulation_->insertNonSmoothProblem(osnspb_);
simulation_->setCheckSolverFunction(localCheckSolverOuput);
// --- Simulation initialization ---
std::cout << "====> Simulation initialisation ..." << std::endl << std::endl;
SP::NonSmoothLaw nslaw(new NewtonImpactFrictionNSL(0, 0, 0.3, 2));
_playground.reset(new SpaceFilter(3, 6, _model, _plans, _moving_plans));
_playground->insert(nslaw, 0, 0);
_model->initialize(simulation_);
}
catch (SiconosException e)
{
std::cout << e.report() << std::endl;
exit(1);
}
catch (...)
{
std::cout << "Exception caught in Disks::init()" << std::endl;
exit(1);
}
}
void SchatzmanPaoliOSI::computeFreeState()
{
// This function computes "free" states of the DS belonging to this Integrator.
// "Free" means without taking non-smooth effects into account.
//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.
// Note: integration of r with a theta method has been removed
// SiconosVector *rold = static_cast<SiconosVector*>(d->rMemory()->getSiconosVector(0));
// Iteration through the set of Dynamical Systems.
//
DSIterator it; // Iterator through the set of DS.
SP::DynamicalSystem ds; // Current Dynamical System.
SP::SiconosMatrix W; // W SchatzmanPaoliOSI matrix of the current DS.
Type::Siconos dsType ; // Type of the current DS.
for (it = OSIDynamicalSystems->begin(); it != OSIDynamicalSystems->end(); ++it)
{
ds = *it; // the considered dynamical system
dsType = Type::value(*ds); // Its type
W = WMap[ds->number()]; // Its W SchatzmanPaoliOSI matrix of iteration.
//1 - Lagrangian Non Linear Systems
if (dsType == Type::LagrangianDS)
{
// // IN to be updated at current time: W, M, q, v, fL
// // IN at told: qi,vi, fLi
// // Note: indices i/i+1 corresponds to value at the beginning/end of the time step.
// // Index k stands for Newton iteration and thus corresponds to the last computed
// // value, ie the one saved in the DynamicalSystem.
// // "i" values are saved in memory vectors.
// // vFree = v_k,i+1 - W^{-1} ResiduFree
// // with
// // ResiduFree = M(q_k,i+1)(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)
// // -- 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);
// // --- ResiduFree computation ---
// // ResFree = M(v-vold) - h*[theta*forces(t) + (1-theta)*forces(told)]
// //
// // vFree pointer is used to compute and save ResiduFree in this first step.
// SP::SiconosVector vfree = d->workspace(DynamicalSystem::free);//workX[d];
// (*vfree)=*(d->workspace(DynamicalSystem::freeresidu));
// // -- Update W --
// // Note: during computeW, mass and jacobians of fL will be computed/
// computeW(t,d);
// SP::SiconosVector v = d->velocity(); // v = v_k,i+1
// // -- vfree = v - W^{-1} ResiduFree --
// // At this point vfree = residuFree
// // -> Solve WX = vfree and set vfree = X
// W->PLUForwardBackwardInPlace(*vfree);
// // -> compute real vfree
// *vfree *= -1.0;
// *vfree += *v;
RuntimeException::selfThrow("SchatzmanPaoliOSI::computeFreeState - not yet implemented for Dynamical system type: " + dsType);
}
// 2 - Lagrangian Linear Systems
else if (dsType == Type::LagrangianLinearTIDS)
{
// IN to be updated at current time: Fext
// IN at told: qi,vi, fext
// IN constants: K,C
// Note: indices i/i+1 corresponds to value at the beginning/end of the time step.
// "i" values are saved in memory vectors.
// vFree = v_i + W^{-1} ResiduFree // with
// ResiduFree = (-h*C -h^2*theta*K)*vi - h*K*qi + h*theta * Fext_i+1 + h*(1-theta)*Fext_i
// -- 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 qold = d->qMemory()->getSiconosVector(0); // q_k
// SP::SiconosVector vold = d->velocityMemory()->getSiconosVector(0); //v_k
// --- ResiduFree computation ---
// vFree pointer is used to compute and save ResiduFree in this first step.
// Velocity free and residu. vFree = RESfree (pointer equality !!).
SP::SiconosVector qfree = d->workspace(DynamicalSystem::free);//workX[d];
(*qfree) = *(d->workspace(DynamicalSystem::freeresidu));
W->PLUForwardBackwardInPlace(*qfree);
*qfree *= -1.0;
*qfree += *qold;
}
// 3 - Newton Euler Systems
else if (dsType == Type::NewtonEulerDS)
{
// // IN to be updated at current time: W, M, q, v, fL
// // IN at told: qi,vi, fLi
// // Note: indices i/i+1 corresponds to value at the beginning/end of the time step.
// // Index k stands for Newton iteration and thus corresponds to the last computed
// // value, ie the one saved in the DynamicalSystem.
// // "i" values are saved in memory vectors.
// // vFree = v_k,i+1 - W^{-1} ResiduFree
// // with
// // ResiduFree = M(q_k,i+1)(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)
// // -- Convert the DS into a Lagrangian one.
// SP::NewtonEulerDS d = std11::static_pointer_cast<NewtonEulerDS> (ds);
// computeW(t,d);
// // 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);
// // --- ResiduFree computation ---
// // ResFree = M(v-vold) - h*[theta*forces(t) + (1-theta)*forces(told)]
// //
// // vFree pointer is used to compute and save ResiduFree in this first step.
// SP::SiconosVector vfree = d->workspace(DynamicalSystem::free);//workX[d];
// (*vfree)=*(d->workspace(DynamicalSystem::freeresidu));
// //*(d->vPredictor())=*(d->workspace(DynamicalSystem::freeresidu));
// // -- Update W --
// // Note: during computeW, mass and jacobians of fL will be computed/
// SP::SiconosVector v = d->velocity(); // v = v_k,i+1
// // -- vfree = v - W^{-1} ResiduFree --
// // At this point vfree = residuFree
// // -> Solve WX = vfree and set vfree = X
// // std::cout<<"SchatzmanPaoliOSI::computeFreeState residu free"<<endl;
// // vfree->display();
// d->luW()->PLUForwardBackwardInPlace(*vfree);
// // std::cout<<"SchatzmanPaoliOSI::computeFreeState -WRfree"<<endl;
// // vfree->display();
// // scal(h,*vfree,*vfree);
// // -> compute real vfree
// *vfree *= -1.0;
// *vfree += *v;
RuntimeException::selfThrow("SchatzmanPaoliOSI::computeFreeState - not yet implemented for Dynamical system type: " + dsType);
}
else
RuntimeException::selfThrow("SchatzmanPaoliOSI::computeFreeState - not yet implemented for Dynamical system type: " + dsType);
}
}
/** Copy constructor
* \param SPO a PluggedObject we are going to copy
*/
SubPluggedObject(const SubPluggedObject& SPO): PluggedObject(SPO), _indx(SPO.getIndex()), _p(SPO.getp())
{
_parentfPtr = SPO.getParentfPtr();
_tmpMat.reset(new SimpleMatrix(SPO.getTmpMat()));
}
void DisksViewer::draw()
{
int i;
char qs[6];
float lbd, w;
float lbdmax = 0.;
DSIterator itDS;
SP::DynamicalSystemsSet involvedDS;
SP::InteractionsGraph I1;
SP::Interaction interaction;
SP::Relation relation;
if (Siconos_->model()->nonSmoothDynamicalSystem()->topology()->numberOfIndexSet() > 1)
{
I1 = Siconos_->model()->simulation()->indexSet(1);
// calibration
InteractionsGraph::VIterator ui, uiend;
for (boost::tie(ui, uiend) = I1->vertices(); ui != uiend; ++ui)
{
lbdmax = fmax(I1->bundle(*ui)->lambdaOld(1)->getValue(0), lbdmax);
}
for (boost::tie(ui, uiend) = I1->vertices(); ui != uiend; ++ui)
{
interaction = I1->bundle(*ui);
relation = interaction->relation();
lbd = interaction->lambdaOld(1)->getValue(0);
// screen width of interaction
w = lbd / (2 * fmax(lbdmax, 1.)) + .03;
// disk/disk
SP::DynamicalSystem d1 = I1->properties(*ui).source;
SP::DynamicalSystem d2 = I1->properties(*ui).target;
SP::SiconosVector q1 = ask<ForPosition>(*d1);
float x1 = (*q1)(0);
float y1 = (*q1)(1);
float r1 = ask<ForRadius>(*d1);
if (d1 != d2)
{
SP::SiconosVector q2 = ask<ForPosition>(*d2);
float x2 = (*q2)(0);
float y2 = (*q2)(1);
float r2 = ask<ForRadius>(*d2);
float d = hypotf(x1 - x2, y1 - y2);
glPushMatrix();
glColor3f(.0f, .0f, .0f);
drawRec(x1, y1, x1 + (x2 - x1)*r1 / d, y1 + (y2 - y1)*r1 / d, w);
drawRec(x2, y2, x2 + (x1 - x2)*r2 / d, y2 + (y1 - y2)*r2 / d, w);
glPopMatrix();
}
else
{
SP::SiconosMatrix jachq = ask<ForJachq>(*relation);
double jx = jachq->getValue(0, 0);
double jy = jachq->getValue(0, 1);
double dj = hypot(jx, jy);
glPushMatrix();
glColor3f(.0f, .0f, .0f);
drawRec(x1, y1, x1 - r1 * jx / dj, y1 - r1 * jy / dj, w);
glPopMatrix();
}
}
}
for (unsigned int i = 0; i < GETNDS(Siconos_); i++)
{
if (shapes_[i]->selected())
{
drawSelectedQGLShape(*shapes_[i]);
}
else
{
drawQGLShape(*shapes_[i]);
}
}
glColor3f(.45, .45, .45);
glLineWidth(1.);
drawGrid(100, 200);
setGridIsDrawn();
glColor3f(.1, .1, .3);
drawVec(-100, 0, 100, 0);
drawVec(0, -100, 0, 100);
glColor3f(0, 0, 1);
glLineWidth(4.);
if (Siconos_->plans())
{
for (unsigned int i = 0 ; i < Siconos_->plans()->size(0) ; ++i)
{
double A = (*Siconos_->plans())(i, 0);
double B = (*Siconos_->plans())(i, 1);
//double C = (*Siconos_->plans())(i,2);
double xc = (*Siconos_->plans())(i, 3);
double yc = (*Siconos_->plans())(i, 4);
double w = fmin(1e10, (*Siconos_->plans())(i, 5));
double H = hypot(A, B);
if (w == 0) w = 1e10;
// assert ( fabs(A*xc + B*yc + C) <= std::numeric_limits<double>::epsilon() );
drawVec(xc, yc, xc - 0.5 * w * B / H, yc + 0.5 * w * A / H);
drawVec(xc, yc, xc + 0.5 * w * B / H, yc - 0.5 * w * A / H);
}
}
if (Siconos_->movingPlans())
{
double time = Siconos_->model()->currentTime();
for (unsigned int i = 0 ; i < Siconos_->movingPlans()->size1() ; ++i)
{
double A = (*Siconos_->movingPlans())(i, 0)(time);
double B = (*Siconos_->movingPlans())(i, 1)(time);
double C = (*Siconos_->movingPlans())(i, 2)(time);
double w = 1e10;
double H = hypot(A, B);
double xc = 0.;
double yc = 0.;
if (fabs(C) > std::numeric_limits<double>::epsilon())
{
if (A == 0)
// By+C=0
{
yc = -C / B;
}
else if (B == 0)
// Ax+C=0
{
xc = -C / A;
}
else
// Ax+By+C=0
{
if (xc != 0)
yc = - (A * xc + C) / B;
else
xc = - (B * yc + C) / A;
}
}
drawVec(xc, yc, xc - 0.5 * w * B / H, yc + 0.5 * w * A / H);
drawVec(xc, yc, xc + 0.5 * w * B / H, yc - 0.5 * w * A / H);
}
}
glColor3f(.1, .1, .1);
glLineWidth(4.);
QGLViewer::drawArrow(qglviewer::Vec(0, 0, .1), qglviewer::Vec(1, 0, .1), .01, 3);
QGLViewer::drawArrow(qglviewer::Vec(0, 0, .1), qglviewer::Vec(0, 1, .1), .01, 3);
glLineWidth(1.);
for (i = -100; i <= 100; i += 5)
{
sprintf(qs, "%d", i);
// print((float)i,-.8,qs,small_text);
//print(-.8,(float)i,qs,small_text);
drawVec((float)i, -.2, (float)i, .2);
drawVec(-.2, (float)i, .2, (float)i);
}
for (i = -100; i <= 100; i++)
{
drawVec((float)i, -.1, (float)i, .1);
drawVec(-.1, (float)i, .1, (float)i);
}
}
