SP::SiconosVector Simulation::lambda(unsigned int level, unsigned int coor)
{
// return input(level) (ie with lambda[level]) for all Interactions.
// assert(level>=0);
DEBUG_BEGIN("Simulation::input(unsigned int level, unsigned int coor)\n");
DEBUG_PRINTF("with level = %i and coor = %i \n", level,coor);
InteractionsGraph::VIterator ui, uiend;
SP::Interaction inter;
SP::InteractionsGraph indexSet0 = _nsds->topology()->indexSet0();
SP::SiconosVector lambda (new SiconosVector (_nsds->topology()->indexSet0()->size() ));
int i=0;
for (std11::tie(ui, uiend) = indexSet0->vertices(); ui != uiend; ++ui)
{
inter = indexSet0->bundle(*ui);
assert(inter->lowerLevelForOutput() <= level);
assert(inter->upperLevelForOutput() >= level);
lambda->setValue(i,inter->lambda(level)->getValue(coor));
i++;
}
DEBUG_END("Simulation::input(unsigned int level, unsigned int coor)\n");
return lambda;
}
void EventDriven::updateIndexSetsWithDoubleCondition()
{
assert(_nsds);
assert(_nsds->topology());
// for all Interactions in indexSet[i-1], compute y[i-1] and
// update the indexSet[i]
SP::Topology topo = _nsds->topology();
SP::InteractionsGraph indexSet2 = topo->indexSet(2);
InteractionsGraph::VIterator ui, uiend, vnext;
std11::tie(ui, uiend) = indexSet2->vertices();
for (vnext = ui; ui != uiend; ui = vnext)
{
++vnext;
SP::Interaction inter = indexSet2->bundle(*ui);
double gamma = inter->getYRef(2);
double F = inter->getLambdaRef(2);
if (fabs(F) < _TOL_ED)
indexSet2->remove_vertex(inter);
else if ((gamma < -_TOL_ED) || (F < -_TOL_ED))
RuntimeException::selfThrow("EventDriven::updateIndexSetsWithDoubleCondition(), output[2] and lambda[2] for Interactionof indexSet[2] must be higher or equal to zero.");
else if (((fabs(gamma) > _TOL_ED) && (fabs(F) > _TOL_ED)))
RuntimeException::selfThrow("EventDriven::updateIndexSetsWithDoubleCondition(), something is wrong for the LCP resolution.");
}
}
std::pair<DynamicalSystemsGraph::EDescriptor, InteractionsGraph::VDescriptor>
Topology::link(SP::Interaction inter, SP::DynamicalSystem ds, SP::DynamicalSystem ds2)
{
DEBUG_PRINTF("Topology::link : inter %p, ds1 %p, ds2 %p\n", &*inter, &*ds,
&*ds2);
if (indexSet0()->is_vertex(inter))
{
removeInteractionFromIndexSet(inter);
}
unsigned int sumOfDSSizes = 0, sumOfZSizes = 0;
sumOfDSSizes += ds->getDim();
if(ds->z())
sumOfZSizes += ds->z()->size();
if(ds2)
{
sumOfDSSizes += ds2->getDim();
if(ds->z())
sumOfZSizes += ds2->z()->size();
inter->setHas2Bodies(true);
}
DEBUG_PRINTF("sumOfDSSizes = %i\t, sumOfZSizes = %i\n ", sumOfDSSizes, sumOfZSizes);
inter->setDSSizes(sumOfDSSizes, sumOfZSizes);
return addInteractionInIndexSet0(inter, ds, ds2);
}
void MLCPProjectOnConstraints::postCompute()
{
_hasBeenUpdated = true;
// This function is used to set y/lambda values using output from
// lcp_driver (w,z). Only Interactions (ie Interactions) of
// indexSet(leveMin) are concerned.
// === Get index set from Topology ===
SP::InteractionsGraph indexSet = simulation()->indexSet(indexSetLevel());
// y and lambda vectors
SP::SiconosVector lambda;
SP::SiconosVector y;
// === Loop through "active" Interactions (ie present in
// indexSets[1]) ===
/** We chose to do a small step _alpha in view of stabilized the algorithm.*/
#ifdef MLCPPROJ_DEBUG
printf("MLCPProjectOnConstraints::postCompute damping value = %f\n", _alpha);
#endif
(*_z) *= _alpha;
unsigned int pos = 0;
#ifdef MLCPPROJ_DEBUG
printf("MLCPProjectOnConstraints::postCompute _z\n");
_z->display();
display();
#endif
InteractionsGraph::VIterator ui, uiend;
for (std11::tie(ui, uiend) = indexSet->vertices(); ui != uiend; ++ui)
{
SP::Interaction inter = indexSet->bundle(*ui);
// Get the relative position of inter-interactionBlock in the vector w
// or z
pos = _M->getPositionOfInteractionBlock(*inter);
RELATION::TYPES relationType = inter->relation()->getType();
if (relationType == NewtonEuler)
{
postComputeNewtonEulerR(inter, pos);
}
else if (relationType == Lagrangian)
{
postComputeLagrangianR(inter, pos);
}
else
{
RuntimeException::selfThrow("MLCPProjectOnConstraints::computeInteractionBlock - relation type is not from Lagrangian type neither NewtonEuler.");
}
}
}
void visit(const Hem5OSI&)
{
unsigned int lowerLevelForOutput = LEVELMAX;
unsigned int upperLevelForOutput = 0;
unsigned int lowerLevelForInput = LEVELMAX;
unsigned int upperLevelForInput = 0;
Type::Siconos dsType = Type::value(*_ds);
/** there is only a test on the dstype and simulation since we assume that
* we implicitely the nonsmooth law when a DS type is chosen
*/
if (dsType == Type::LagrangianDS || dsType == Type::LagrangianLinearTIDS || dsType == Type::NewtonEulerDS)
{
if (Type::value(*_parent) == Type::EventDriven)
{
Type::Siconos nslType = Type::value(*_nonSmoothLaw);
if (nslType == Type::NewtonImpactNSL || nslType == Type::MultipleImpactNSL)
{
lowerLevelForOutput = 0;
upperLevelForOutput = 2 ;
lowerLevelForInput = 1;
upperLevelForInput = 2;
}
else if (nslType == Type::NewtonImpactFrictionNSL)
{
lowerLevelForOutput = 0;
upperLevelForOutput = 4;
lowerLevelForInput = 1;
upperLevelForInput = 2;
RuntimeException::selfThrow("Simulation::SetupLevels::visit - simulation of type: " + Type::name(*_parent) + " not yet implemented for nonsmooth law of type NewtonImpactFrictionNSL");
}
else
{
RuntimeException::selfThrow("Simulation::SetupLevels::visit - simulation of type: " + Type::name(*_parent) + "not yet implemented for nonsmooth of type");
}
}
else
RuntimeException::selfThrow("Simulation::SetupLevels::visit - unknown simulation type: " + Type::name(*_parent));
}
else RuntimeException::selfThrow("Simulation::SetupLevels::visit - not yet implemented for Dynamical system type :" + dsType);
_parent->_levelMinForInput = std::min<int>(lowerLevelForInput, _parent->_levelMinForInput);
_parent->_levelMaxForInput = std::max<int>(upperLevelForInput, _parent->_levelMaxForInput);
_parent->_levelMinForOutput = std::min<int>(lowerLevelForOutput, _parent->_levelMinForInput);
_parent->_levelMaxForOutput = std::max<int>(upperLevelForOutput, _parent->_levelMaxForInput);
_parent->_numberOfIndexSets = std::max<int>(_parent->_levelMaxForOutput + 1, _parent->_numberOfIndexSets);
_interaction->setLowerLevelForOutput(lowerLevelForOutput);
_interaction->setUpperLevelForOutput(upperLevelForOutput);
_interaction->setLowerLevelForInput(lowerLevelForInput);
_interaction->setUpperLevelForInput(upperLevelForInput);
_interaction->setSteps(1);
};
void visit(const NewtonImpactFrictionNSL& nslaw)
{
double e;
e = nslaw.en();
// Only the normal part is multiplied by e
SP::SiconosVector y_k_1 ;
y_k_1 = _inter->yMemory(_osnsp->inputOutputLevel())->getSiconosVector(1);
(*_inter->yForNSsolver())(0) += e * (*y_k_1)(0);
}
void visit(const NewtonImpactNSL& nslaw)
{
double e;
e = nslaw.e();
Index subCoord(4);
subCoord[0] = 0;
subCoord[1] = _inter->nonSmoothLaw()->size();
subCoord[2] = 0;
subCoord[3] = subCoord[1];
subscal(e, *_inter->yOld(_osnsp->inputOutputLevel()), *(_inter->yForNSsolver()), subCoord, false); // q = q + e * q
}
void MLCPProjectOnConstraints::computeqBlock(InteractionsGraph::VDescriptor& vertex_inter, unsigned int pos)
{
SP::InteractionsGraph indexSet = simulation()->indexSet(indexSetLevel());
SP::Interaction inter = indexSet->bundle(vertex_inter);
unsigned int sizeY = std11::static_pointer_cast<OSNSMatrixProjectOnConstraints>
(_M)->computeSizeForProjection(inter);
for (unsigned int i = 0; i < sizeY; i++)
_q->setValue(pos + i, inter->y(0)->getValue(0 + i));
#ifdef MLCPPROJ_DEBUG
printf("MLCPProjectOnConstraints::computeqBlock, _q from y(0)\n");
_q->display();
#endif
}
void MLCPProjectOnConstraints::postComputeNewtonEulerR(SP::Interaction inter, unsigned int pos)
{
SP::NewtonEulerR ner = (std11::static_pointer_cast<NewtonEulerR>(inter->relation()));
SP::SiconosVector lambda = inter->lambda(0);
SP::SiconosVector y = inter->y(0);
unsigned int sizeY = std11::static_pointer_cast<OSNSMatrixProjectOnConstraints>
(_M)->computeSizeForProjection(inter);
// Copy _w/_z values, starting from index pos into y/lambda.
//setBlock(*_w, y, sizeY, pos, 0);
setBlock(*_z, lambda, sizeY, pos, 0);
}
void visit(const MoreauJeanCombinedProjectionOSI& moreauCPOSI)
{
unsigned int lowerLevelForOutput = LEVELMAX;
unsigned int upperLevelForOutput = 0;
unsigned int lowerLevelForInput = LEVELMAX;
unsigned int upperLevelForInput = 0;
Type::Siconos dsType = Type::value(*_ds);
if (dsType == Type::LagrangianDS || dsType == Type::LagrangianLinearTIDS || dsType == Type::NewtonEulerDS)
{
if (Type::value(*_parent) == Type::TimeStepping)
{
lowerLevelForOutput = 0;
upperLevelForOutput = 1;
lowerLevelForInput = 1;
upperLevelForInput = 1;
}
else if (Type::value(*_parent) == Type::TimeSteppingCombinedProjection)
{
// Warning : we never enter this case !!!
lowerLevelForOutput = 0;
upperLevelForOutput = 1 ;
lowerLevelForInput = 0;
upperLevelForInput = 1;
}
else
{
RuntimeException::selfThrow("Simulation::SetupLevels::visit(const MoreauJeanCombinedProjectionOSI) - unknown simulation type: " + Type::name(*_parent));
}
}
else RuntimeException::selfThrow("Simulation::SetupLevels::visit(const MoreauJeanCombinedProjectionOSI) - not yet implemented for Dynamical system type :" + dsType);
_parent->_levelMinForInput = std::min<int>(lowerLevelForInput, _parent->_levelMinForInput);
_parent->_levelMaxForInput = std::max<int>(upperLevelForInput, _parent->_levelMaxForInput);
_parent->_levelMinForOutput = std::min<int>(lowerLevelForOutput, _parent->_levelMinForInput);
_parent->_levelMaxForOutput = std::max<int>(upperLevelForOutput, _parent->_levelMaxForInput);
_parent->_numberOfIndexSets = std::max<int>(_parent->_levelMaxForOutput + 1, _parent->_numberOfIndexSets);
_interaction->setLowerLevelForOutput(lowerLevelForOutput);
_interaction->setUpperLevelForOutput(upperLevelForOutput);
_interaction->setLowerLevelForInput(lowerLevelForInput);
_interaction->setUpperLevelForInput(upperLevelForInput);
_interaction->setSteps(1);
};
void visit(const D1MinusLinearOSI& d1OSI)
{
unsigned int lowerLevelForOutput = LEVELMAX;
unsigned int upperLevelForOutput = 0;
unsigned int lowerLevelForInput = LEVELMAX;
unsigned int upperLevelForInput = 0;
Type::Siconos dsType = Type::value(*_ds);
/** there is only a test on the dstype and simulation since we assume that
* we implicitely the nonsmooth law when a DS type is chosen
*/
if (dsType == Type::LagrangianDS || dsType == Type::LagrangianLinearTIDS || dsType == Type::NewtonEulerDS)
{
if (Type::value(*_parent) == Type::TimeSteppingD1Minus)
{
lowerLevelForOutput = 0;
upperLevelForOutput = 2 ;
lowerLevelForInput = 1;
upperLevelForInput = 2;
}
else
RuntimeException::selfThrow("Simulation::SetupLevels::visit(const D1MinusLinearOSI&) - unknown simulation type: " + Type::name(*_parent));
}
else RuntimeException::selfThrow("Simulation::SetupLevels::visit(const D1MinusLinearOSI&) - not yet implemented for Dynamical system type :" + dsType);
_parent->_levelMinForInput = std::min<int>(lowerLevelForInput, _parent->_levelMinForInput);
_parent->_levelMaxForInput = std::max<int>(upperLevelForInput, _parent->_levelMaxForInput);
_parent->_levelMinForOutput = std::min<int>(lowerLevelForOutput, _parent->_levelMinForInput);
_parent->_levelMaxForOutput = std::max<int>(upperLevelForOutput, _parent->_levelMaxForInput);
/* Get the number of required index sets. */
unsigned int nbIndexSets = d1OSI.numberOfIndexSets();
_parent->_numberOfIndexSets = std::max<int>(nbIndexSets, _parent->_numberOfIndexSets);
_interaction->setLowerLevelForOutput(lowerLevelForOutput);
_interaction->setUpperLevelForOutput(upperLevelForOutput);
_interaction->setLowerLevelForInput(lowerLevelForInput);
_interaction->setUpperLevelForInput(upperLevelForInput);
_interaction->setSteps(2); // Two evaluations of lambda(2) are made for each time--step
};
void visit(const NewtonImpactNSL& nslaw)
{
double e;
e = nslaw.e();
Index subCoord(4);
subCoord[0] = 0;
subCoord[1] = _inter->nonSmoothLaw()->size();
subCoord[2] = 0;
subCoord[3] = subCoord[1];
// Only the normal part is multiplied by e
SP::SiconosVector y_k_1 ;
y_k_1 = _inter->yMemory(_osnsp->inputOutputLevel())->getSiconosVector(1);
// std::cout << "y_k_1 " << std::endl;
// y_k_1->display();
subscal(e, *y_k_1, *(_inter->yForNSsolver()), subCoord, false);
}
void visit(const NewtonImpactNSL& nslaw)
{
double e;
e = nslaw.e();
Index subCoord(4);
subCoord[0] = 0;
subCoord[1] = _inter->nonSmoothLaw()->size();
subCoord[2] = 0;
subCoord[3] = subCoord[1];
// Only the normal part is multiplied by e
const SiconosVector& y_k_1(
_inter->yMemory(_osnsp->inputOutputLevel()).getSiconosVector(1));
DEBUG_PRINTF("_osnsp->inputOutputLevel() = %i \n ",_osnsp->inputOutputLevel() );
DEBUG_EXPR(y_k_1.display());;
SiconosVector & osnsp_rhs = *(*_interProp.workVectors)[SchatzmanPaoliOSI::OSNSP_RHS];
subscal(e, y_k_1, osnsp_rhs, subCoord, false);
}
void visit(const NewtonImpactFrictionNSL& nslaw)
{
double e;
e = nslaw.en();
// Only the normal part is multiplied by e
const SiconosVector& y_k_1(
_inter->yMemory(_osnsp->inputOutputLevel()).getSiconosVector(1));
SiconosVector & osnsp_rhs = *(*_interProp.workVectors)[SchatzmanPaoliOSI::OSNSP_RHS];
osnsp_rhs (0) += e * (y_k_1)(0);
}
void Simulation::updateOutput(unsigned int level)
{
// To compute output(level) (ie with y[level]) for all Interactions.
// assert(level>=0);
DEBUG_PRINTF("Simulation::updateOutput(unsigned int level) starts for level = %i\n", level);
double time = model()->currentTime();
InteractionsGraph::VIterator ui, uiend;
SP::Interaction inter;
SP::InteractionsGraph indexSet0 = model()->nonSmoothDynamicalSystem()->topology()->indexSet0();
for (std11::tie(ui, uiend) = indexSet0->vertices(); ui != uiend; ++ui)
{
inter = indexSet0->bundle(*ui);
assert(inter->lowerLevelForOutput() <= level);
assert(inter->upperLevelForOutput() >= level);
inter->computeOutput(time, indexSet0->properties(*ui), level);
}
DEBUG_PRINTF("Simulation::updateOutput(unsigned int level) ends for level = %i\n", level);
}
void Simulation::updateInput(unsigned int level)
{
// To compute input(level) (ie with lambda[level]) for all Interactions.
// assert(level>=0);
// double time = nextTime();
double time = model()->currentTime();
// Set dynamical systems non-smooth part to zero.
reset(level);
// We compute input using lambda(level).
InteractionsGraph::VIterator ui, uiend;
SP::Interaction inter;
SP::InteractionsGraph indexSet0 = model()->nonSmoothDynamicalSystem()->topology()->indexSet0();
for (std11::tie(ui, uiend) = indexSet0->vertices(); ui != uiend; ++ui)
{
inter = indexSet0->bundle(*ui);
assert(inter->lowerLevelForInput() <= level);
assert(inter->upperLevelForInput() >= level);
inter->computeInput(time, indexSet0->properties(*ui), level);
}
}
void Simulation::initializeInteraction(double time, SP::Interaction inter)
{
// determine which (lower and upper) levels are required for this Interaction
// in this Simulation.
computeLevelsForInputAndOutput(inter);
// Get the interaction properties from the topology for initialization.
SP::InteractionsGraph indexSet0 = _nsds->topology()->indexSet0();
InteractionsGraph::VDescriptor ui = indexSet0->descriptor(inter);
// This calls computeOutput() and initializes qMemory and q_k.
inter->initialize(time, indexSet0->properties(ui));
}
void visit(const SchatzmanPaoliOSI&)
{
unsigned int lowerLevelForOutput = LEVELMAX;
unsigned int upperLevelForOutput = 0;
unsigned int lowerLevelForInput = LEVELMAX;
unsigned int upperLevelForInput = 0;
Type::Siconos dsType = Type::value(*_ds);
if (dsType == Type::LagrangianDS || dsType == Type::LagrangianLinearTIDS || dsType == Type::NewtonEulerDS)
{
if (Type::value(*_parent) == Type::TimeStepping)
{
lowerLevelForOutput = 0;
upperLevelForOutput = 0;
lowerLevelForInput = 0;
upperLevelForInput = 0;
}
else
RuntimeException::selfThrow("Simulation::SetupLevels::visit - unknown simulation type: " + Type::name(*_parent));
}
else RuntimeException::selfThrow("Simulation::SetupLevels::visit - not yet implemented for Dynamical system type :" + dsType);
_parent->_levelMinForInput = std::min<int>(lowerLevelForInput, _parent->_levelMinForInput);
_parent->_levelMaxForInput = std::max<int>(upperLevelForInput, _parent->_levelMaxForInput);
_parent->_levelMinForOutput = std::min<int>(lowerLevelForOutput, _parent->_levelMinForInput);
_parent->_levelMaxForOutput = std::max<int>(upperLevelForOutput, _parent->_levelMaxForInput);
_parent->_numberOfIndexSets = std::max<int>(_parent->_levelMaxForOutput + 1, _parent->_numberOfIndexSets);
_interaction->setLowerLevelForOutput(lowerLevelForOutput);
_interaction->setUpperLevelForOutput(upperLevelForOutput);
_interaction->setLowerLevelForInput(lowerLevelForInput);
_interaction->setUpperLevelForInput(upperLevelForInput);
_interaction->setSteps(2);
};
void SchatzmanPaoliOSI::computeFreeOutput(InteractionsGraph::VDescriptor& vertex_inter, OneStepNSProblem* osnsp)
{
DEBUG_BEGIN("SchatzmanPaoliOSI::computeFreeOutput(InteractionsGraph::VDescriptor& vertex_inter, OneStepNSProblem* osnsp)\n");
/** \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 = _simulation->oneStepNSProblems();
VectorOfBlockVectors& inter_work_block = *indexSet->properties(vertex_inter).workBlockVectors;
// 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& osnsp_rhs = *(*indexSet->properties(vertex_inter).workVectors)[SchatzmanPaoliOSI::OSNSP_RHS];
SP::SiconosVector e;
SP::BlockVector Xfree = inter_work_block[SchatzmanPaoliOSI::xfree];;
assert(Xfree);
SP::Interaction mainInteraction = inter;
assert(mainInteraction);
assert(mainInteraction->relation());
DEBUG_EXPR(inter->display(););
void BulletSpaceFilter::buildInteractions(double time)
{
if (! _dynamicCollisionsObjectsInserted)
{
DynamicalSystemsGraph& dsg = *(_model->nonSmoothDynamicalSystem()->dynamicalSystems());
DynamicalSystemsGraph::VIterator dsi, dsiend;
std11::tie(dsi, dsiend) = dsg.vertices();
for (; dsi != dsiend; ++dsi)
{
CollisionObjects& collisionObjects = *ask<ForCollisionObjects>(*dsg.bundle(*dsi));
for (CollisionObjects::iterator ico = collisionObjects.begin();
ico != collisionObjects.end(); ++ico)
{
_collisionWorld->addCollisionObject(const_cast<btCollisionObject*>((*ico).first));
DEBUG_PRINTF("add dynamic collision object %p\n", const_cast<btCollisionObject*>((*ico).first));
}
}
_dynamicCollisionsObjectsInserted = true;
}
if (! _staticCollisionsObjectsInserted)
{
for(StaticObjects::iterator
ic = _staticObjects->begin(); ic != _staticObjects->end(); ++ic)
{
_collisionWorld->addCollisionObject(const_cast<btCollisionObject*>((*ic).first));
DEBUG_PRINTF("add static collision object %p\n", const_cast<btCollisionObject*>((*ic).first));
}
_staticCollisionsObjectsInserted = true;
}
DEBUG_PRINT("-----start build interaction\n");
// 1. perform bullet collision detection
gOrphanedInteractions.clear();
_collisionWorld->performDiscreteCollisionDetection();
// 2. collect old contact points from Siconos graph
std::map<btManifoldPoint*, bool> contactPoints;
std::map<Interaction*, bool> activeInteractions;
SP::InteractionsGraph indexSet0 = model()->nonSmoothDynamicalSystem()->topology()->indexSet(0);
InteractionsGraph::VIterator ui0, ui0end, v0next;
std11::tie(ui0, ui0end) = indexSet0->vertices();
for (v0next = ui0 ;
ui0 != ui0end; ui0 = v0next)
{
++v0next; // trick to iterate on a dynamic bgl graph
SP::Interaction inter0 = indexSet0->bundle(*ui0);
if (gOrphanedInteractions.find(&*inter0) != gOrphanedInteractions.end())
{
model()->nonSmoothDynamicalSystem()->removeInteraction(inter0);
}
else
{
SP::btManifoldPoint cp = ask<ForContactPoint>(*(inter0->relation()));
if(cp)
{
DEBUG_PRINTF("Contact point in interaction : %p\n", &*cp);
contactPoints[&*cp] = false;
}
}
}
unsigned int numManifolds =
_collisionWorld->getDispatcher()->getNumManifolds();
DEBUG_PRINTF("number of manifolds : %d\n", numManifolds);
for (unsigned int i = 0; i < numManifolds; ++i)
{
btPersistentManifold* contactManifold =
_collisionWorld->getDispatcher()->getManifoldByIndexInternal(i);
DEBUG_PRINTF("processing manifold %d : %p\n", i, contactManifold);
const btCollisionObject* obA = contactManifold->getBody0();
const btCollisionObject* obB = contactManifold->getBody1();
DEBUG_PRINTF("object A : %p, object B: %p\n", obA, obB);
// contactManifold->refreshContactPoints(obA->getWorldTransform(),obB->getWorldTransform());
unsigned int numContacts = contactManifold->getNumContacts();
if ( (obA->getUserPointer() && obA->getUserPointer() != obB->getUserPointer()) ||
(obB->getUserPointer() && obA->getUserPointer() != obB->getUserPointer()) )
{
for (unsigned int z = 0; z < numContacts; ++z)
{
SP::btManifoldPoint cpoint(createSPtrbtManifoldPoint(contactManifold->getContactPoint(z)));
DEBUG_PRINTF("manifold %d, contact %d, &contact %p, lifetime %d\n", i, z, &*cpoint, cpoint->getLifeTime());
// should no be mixed with something else that use UserPointer!
SP::BulletDS dsa;
SP::BulletDS dsb;
unsigned long int gid1, gid2;
if(obA->getUserPointer())
{
dsa = static_cast<BulletDS*>(obA->getUserPointer())->shared_ptr();
assert(dsa->collisionObjects()->find(contactManifold->getBody0()) !=
dsa->collisionObjects()->end());
gid1 = boost::get<2>((*((*dsa->collisionObjects()).find(obA))).second);
}
else
{
gid1 = (*_staticObjects->find(obA)).second.second;
}
SP::NonSmoothLaw nslaw;
if (obB->getUserPointer())
{
dsb = static_cast<BulletDS*>(obB->getUserPointer())->shared_ptr();
assert(dsb->collisionObjects()->find(obB) != dsb->collisionObjects()->end());
gid2 = boost::get<2>((*((*dsb->collisionObjects()).find(obB))).second);
}
else
{
gid2 = (*_staticObjects->find(obB)).second.second;
}
DEBUG_PRINTF("collision between group %ld and %ld\n", gid1, gid2);
nslaw = (*_nslaws)(gid1, gid2);
if (!nslaw)
{
RuntimeException::selfThrow(
(boost::format("Cannot find nslaw for collision between group %1% and %2%") %
gid1 % gid2).str());
}
assert(nslaw);
std::map<btManifoldPoint*, bool>::iterator itc;
itc = contactPoints.find(&*cpoint);
DEBUG_EXPR(if (itc == contactPoints.end())
{
DEBUG_PRINT("contact point not found\n");
for(std::map<btManifoldPoint*, bool>::iterator itd=contactPoints.begin();
itd != contactPoints.end(); ++itd)
{
DEBUG_PRINTF("-->%p != %p\n", &*cpoint, &*(*itd).first);
}
});
if (itc == contactPoints.end() || !cpoint->m_userPersistentData)
{
/* new interaction */
SP::Interaction inter;
if (nslaw->size() == 3)
{
SP::BulletR rel(new BulletR(cpoint, createSPtrbtPersistentManifold(*contactManifold)));
inter.reset(new Interaction(3, nslaw, rel, 4 * i + z));
}
else
{
if (nslaw->size() == 1)
{
SP::BulletFrom1DLocalFrameR rel(new BulletFrom1DLocalFrameR(cpoint));
inter.reset(new Interaction(1, nslaw, rel, 4 * i + z));
}
}
if (obB->getUserPointer())
{
SP::BulletDS dsb(static_cast<BulletDS*>(obB->getUserPointer())->shared_ptr());
if (dsa != dsb)
{
DEBUG_PRINTF("LINK obA:%p obB:%p inter:%p\n", obA, obB, &*inter);
assert(inter);
cpoint->m_userPersistentData = &*inter;
link(inter, dsa, dsb);
}
/* else collision shapes belong to the same object do nothing */
}
else
{
DEBUG_PRINTF("LINK obA:%p inter :%p\n", obA, &*inter);
assert(inter);
cpoint->m_userPersistentData = &*inter;
link(inter, dsa);
}
}
if (cpoint->m_userPersistentData)
{
activeInteractions[static_cast<Interaction *>(cpoint->m_userPersistentData)] = true;
DEBUG_PRINTF("Interaction %p = true\n", static_cast<Interaction *>(cpoint->m_userPersistentData));
DEBUG_PRINTF("cpoint %p = true\n", &*cpoint);
}
else
{
assert(false);
DEBUG_PRINT("cpoint->m_userPersistentData is empty\n");
}
contactPoints[&*cpoint] = true;
DEBUG_PRINTF("cpoint %p = true\n", &*cpoint);
}
}
}
SetupLevels(SP::Simulation s, SP::Interaction inter,
SP::DynamicalSystem ds) :
_parent(s), _interaction(inter), _ds(ds)
{
_nonSmoothLaw = inter->nonSmoothLaw();
};
void EulerMoreauOSI::computeW(double t, DynamicalSystem& ds, DynamicalSystemsGraph::VDescriptor& dsgVD)
{
// Compute W matrix of the Dynamical System ds, at time t and for the current ds state.
// When this function is called, WMap[ds] is supposed to exist and not to be null
// Memory allocation has been done during initW.
unsigned int dsN = ds.number();
assert((WMap.find(dsN) != WMap.end()) &&
"EulerMoreauOSI::computeW(t,ds) - W(ds) does not exists. Maybe you forget to initialize the osi?");
double h = simulationLink->timeStep();
Type::Siconos dsType = Type::value(ds);
SiconosMatrix& W = *WMap[dsN];
// 1 - First order non linear systems
if (dsType == Type::FirstOrderNonLinearDS)
{
// W = M - h*_theta* [jacobian_x f(t,x,z)]
FirstOrderNonLinearDS& d = static_cast<FirstOrderNonLinearDS&> (ds);
// Copy M or I if M is Null into W
if (d.M())
W = *d.M();
else
W.eye();
d.computeJacobianfx(t);
// Add -h*_theta*jacobianfx to W
scal(-h * _theta, *d.jacobianfx(), W, false);
}
// 2 - First order linear systems
else if (dsType == Type::FirstOrderLinearDS || dsType == Type::FirstOrderLinearTIDS)
{
FirstOrderLinearDS& d = static_cast<FirstOrderLinearDS&> (ds);
if (dsType == Type::FirstOrderLinearDS)
d.computeA(t);
if (d.M())
W = *d.M();
else
W.eye();
scal(-h * _theta, *d.A(), W, false);
}
else RuntimeException::selfThrow("EulerMoreauOSI::computeW - not yet implemented for Dynamical system type :" + dsType);
// if (_useGamma)
{
Topology& topo = *simulationLink->model()->nonSmoothDynamicalSystem()->topology();
DynamicalSystemsGraph& DSG0 = *topo.dSG(0);
InteractionsGraph& indexSet = *topo.indexSet(0);
DynamicalSystemsGraph::OEIterator oei, oeiend;
InteractionsGraph::VDescriptor ivd;
SP::SiconosMatrix K;
SP::Interaction inter;
for (std11::tie(oei, oeiend) = DSG0.out_edges(dsgVD); oei != oeiend; ++oei)
{
inter = DSG0.bundle(*oei);
ivd = indexSet.descriptor(inter);
FirstOrderR& rel = static_cast<FirstOrderR&>(*inter->relation());
K = rel.K();
if (!K) K = (*indexSet.properties(ivd).workMatrices)[FirstOrderR::mat_K];
if (K)
{
scal(-h * _gamma, *K, W, false);
}
}
}
// Remark: W is not LU-factorized here.
// Function PLUForwardBackward will do that if required.
}
void EventDriven::updateIndexSet(unsigned int i)
{
assert(_nsds);
assert(_nsds->topology());
SP::Topology topo = _nsds->topology();
assert(i < topo->indexSetsSize() &&
"EventDriven::updateIndexSet(i), indexSets[i] does not exist.");
// IndexSets[0] must not be updated in simulation, since it belongs to Topology.
assert(i > 0 &&
"EventDriven::updateIndexSet(i=0), indexSets[0] cannot be updated.");
// For all Interactions in indexSet[i-1], compute y[i-1] and
// update the indexSet[i].
SP::InteractionsGraph indexSet1 = topo->indexSet(1);
SP::InteractionsGraph indexSet2 = topo->indexSet(2);
assert(_indexSet0);
assert(indexSet1);
assert(indexSet2);
// DEBUG_PRINTF("update indexSets start : indexSet0 size : %ld\n", indexSet0->size());
// DEBUG_PRINTF("update IndexSets start : indexSet1 size : %ld\n", indexSet1->size());
// DEBUG_PRINTF("update IndexSets start : indexSet2 size : %ld\n", indexSet2->size());
InteractionsGraph::VIterator uibegin, uipend, uip;
std11::tie(uibegin, uipend) = _indexSet0->vertices();
// loop over all vertices of the indexSet[i-1]
for (uip = uibegin; uip != uipend; ++uip)
{
SP::Interaction inter = _indexSet0->bundle(*uip);
if (i == 1) // IndexSet[1]
{
// if indexSet[1]=>getYRef(0): output y
// if indexSet[2]=>getYRef(1): output ydot
double y = inter->getYRef(0); // output to define the IndexSets at this Interaction
if (y < -_TOL_ED) // y[0] < 0
{
inter->display();
cout << "y = " << y << " < -_TOL_ED = " << -_TOL_ED <<endl;
RuntimeException::selfThrow("EventDriven::updateIndexSet, output of level 0 must be positive!!! ");
}
// 1 - If the Interaction is not yet in the set
if (!indexSet1->is_vertex(inter)) // Interaction is not yet in the indexSet[i]
{
if (fabs(y) <= _TOL_ED)
{
// vertex and edges insertions
indexSet1->copy_vertex(inter, *_indexSet0);
}
}
else // if the Interaction was already in the set
{
if (fabs(y) > _TOL_ED)
{
indexSet1->remove_vertex(inter); // remove the Interaction from IndexSet[1]
inter->lambda(1)->zero(); // reset the lambda[1] to zero
}
}
}
else if (i == 2) // IndexSet[2]
{
if (indexSet1->is_vertex(inter)) // Interaction is in the indexSet[1]
{
double y = inter->getYRef(1); // output of level 1 at this Interaction
if (!indexSet2->is_vertex(inter)) // Interaction is not yet in the indexSet[2]
{
if (fabs(y) <= _TOL_ED)
{
// vertex and edges insertions
indexSet2->copy_vertex(inter, *_indexSet0);
}
}
else // if the Interaction was already in the set
{
if (fabs(y) > _TOL_ED)
{
indexSet2->remove_vertex(inter); // remove the Interaction from IndexSet[1]
inter->lambda(2)->zero(); // reset the lambda[i] to zero
}
}
}
else // Interaction is not in the indexSet[1]
{
if (indexSet2->is_vertex(inter)) // Interaction is in the indexSet[2]
{
indexSet2->remove_vertex(inter); // remove the Interaction from IndexSet[2]
inter->lambda(2)->zero(); // reset the lambda[i] to zero
}
}
}
else
{
RuntimeException::selfThrow("EventDriven::updateIndexSet, IndexSet[i > 2] doesn't exist");
}
}
// DEBUG_PRINTF("update indexSets end : indexSet0 size : %ld\n", indexSet0->size());
// DEBUG_PRINTF("update IndexSets end : indexSet1 size : %ld\n", indexSet1->size());
// DEBUG_PRINTF("update IndexSets end : indexSet2 size : %ld\n", indexSet2->size());
}
void EventDriven::computeg(SP::OneStepIntegrator osi,
integer * sizeOfX, doublereal* time,
doublereal* x, integer * ng,
doublereal * gOut)
{
assert(_nsds);
assert(_nsds->topology());
InteractionsGraph::VIterator ui, uiend;
SP::Topology topo = _nsds->topology();
SP::InteractionsGraph indexSet2 = topo->indexSet(2);
unsigned int nsLawSize, k = 0 ;
SP::SiconosVector y, ydot, yddot, lambda;
SP::LsodarOSI lsodar = std11::static_pointer_cast<LsodarOSI>(osi);
// Solve LCP at acceleration level to calculate the lambda[2] at Interaction of indexSet[2]
lsodar->fillXWork(sizeOfX, x);
//
double t = *time;
if (!_allNSProblems->empty())
{
if (((*_allNSProblems)[SICONOS_OSNSP_ED_SMOOTH_ACC]->hasInteractions()))
{
(*_allNSProblems)[SICONOS_OSNSP_ED_SMOOTH_ACC]->compute(t);
}
};
/*
double * xdottmp = (double *)malloc(*sizeOfX*sizeof(double));
computef(osi, sizeOfX,time,x,xdottmp);
free(xdottmp);
*/
// Update the output from level 0 to level 1
_nsds->updateOutput(t,0);
_nsds->updateOutput(t,1);
_nsds->updateOutput(t,2);
//
for (std11::tie(ui, uiend) = _indexSet0->vertices(); ui != uiend; ++ui)
{
SP::Interaction inter = _indexSet0->bundle(*ui);
nsLawSize = inter->nonSmoothLaw()->size();
y = inter->y(0); // output y at this Interaction
ydot = inter->y(1); // output of level 1 at this Interaction
yddot = inter->y(2);
lambda = inter->lambda(2); // input of level 2 at this Interaction
if (!(indexSet2->is_vertex(inter))) // if Interaction is not in the indexSet[2]
{
for (unsigned int i = 0; i < nsLawSize; ++i)
{
if ((*y)(i) > _TOL_ED)
{
gOut[k] = (*y)(i);
}
else
{
if ((*ydot)(i) > -_TOL_ED)
{
gOut[k] = 100 * _TOL_ED;
}
else
{
gOut[k] = (*y)(i);
}
}
k++;
}
}
else // If Interaction is in the indexSet[2]
{
for (unsigned int i = 0; i < nsLawSize; ++i)
{
if ((*lambda)(i) > _TOL_ED)
{
gOut[k] = (*lambda)(i); // g = lambda[2]
}
else
{
if ((*yddot)(i) > _TOL_ED)
{
gOut[k] = (*lambda)(i);
}
else
{
gOut[k] = 100 * _TOL_ED;
}
}
k++;
}
}
}
}
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);
}
}
}
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(););
std::pair<DynamicalSystemsGraph::EDescriptor, InteractionsGraph::VDescriptor>
Topology::addInteractionInIndexSet0(SP::Interaction inter, SP::DynamicalSystem ds1, SP::DynamicalSystem ds2)
{
// !! Private function !!
//
// Add inter and ds into IG/DSG
// Compute number of constraints
unsigned int nsLawSize = inter->nonSmoothLaw()->size();
unsigned int m = inter->getSizeOfY() / nsLawSize;
if (m > 1)
RuntimeException::selfThrow("Topology::addInteractionInIndexSet0 - m > 1. Obsolete !");
_numberOfConstraints += nsLawSize;
SP::DynamicalSystem ds2_ = ds2;
// _DSG is the hyper forest : (vertices : dynamical systems, edges :
// Interactions)
//
// _IG is the hyper graph : (vertices : Interactions, edges :
// dynamical systems)
assert(_DSG[0]->edges_number() == _IG[0]->size());
// _IG = L(_DSG), L is the line graph transformation
// vector of the Interaction
DynamicalSystemsGraph::VDescriptor dsgv1, dsgv2;
dsgv1 = _DSG[0]->add_vertex(ds1);
SP::VectorOfVectors workVds1 = _DSG[0]->properties(dsgv1).workVectors;
SP::VectorOfVectors workVds2;
if (!workVds1)
{
workVds1.reset(new VectorOfVectors());
_DSG[0]->properties(dsgv1).workMatrices.reset(new VectorOfMatrices());
ds1->initWorkSpace(*workVds1, *_DSG[0]->properties(dsgv1).workMatrices);
}
if(ds2)
{
dsgv2 = _DSG[0]->add_vertex(ds2);
workVds2 = _DSG[0]->properties(dsgv2).workVectors;
if (!workVds2)
{
workVds2.reset(new VectorOfVectors());
_DSG[0]->properties(dsgv2).workMatrices.reset(new VectorOfMatrices());
ds2->initWorkSpace(*workVds2, *_DSG[0]->properties(dsgv2).workMatrices);
}
}
else
{
dsgv2 = dsgv1;
ds2_ = ds1;
workVds2 = workVds1;
}
// this may be a multi edges graph
assert(!_DSG[0]->is_edge(dsgv1, dsgv2, inter));
assert(!_IG[0]->is_vertex(inter));
InteractionsGraph::VDescriptor ig_new_ve;
DynamicalSystemsGraph::EDescriptor new_ed;
std11::tie(new_ed, ig_new_ve) = _DSG[0]->add_edge(dsgv1, dsgv2, inter, *_IG[0]);
InteractionProperties& interProp = _IG[0]->properties(ig_new_ve);
interProp.DSlink.reset(new VectorOfBlockVectors);
interProp.workVectors.reset(new VectorOfVectors);
interProp.workMatrices.reset(new VectorOfSMatrices);
unsigned int nslawSize = inter->nonSmoothLaw()->size();
interProp.block.reset(new SimpleMatrix(nslawSize, nslawSize));
inter->setDSLinkAndWorkspace(interProp, *ds1, *workVds1, *ds2_, *workVds2);
// add self branches in vertex properties
// note : boost graph SEGFAULT on self branch removal
// see https://svn.boost.org/trac/boost/ticket/4622
_IG[0]->properties(ig_new_ve).source = ds1;
_IG[0]->properties(ig_new_ve).source_pos = 0;
if(!ds2)
{
_IG[0]->properties(ig_new_ve).target = ds1;
_IG[0]->properties(ig_new_ve).target_pos = 0;
}
else
{
_IG[0]->properties(ig_new_ve).target = ds2;
_IG[0]->properties(ig_new_ve).target_pos = ds1->getDim();
}
assert(_IG[0]->bundle(ig_new_ve) == inter);
assert(_IG[0]->is_vertex(inter));
assert(_DSG[0]->is_edge(dsgv1, dsgv2, inter));
assert(_DSG[0]->edges_number() == _IG[0]->size());
return std::pair<DynamicalSystemsGraph::EDescriptor, InteractionsGraph::VDescriptor>(new_ed, ig_new_ve);
}
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
double EventDriven::detectEvents(bool updateIstate)
{
double _minResiduOutput = 0.0; // maximum of g_i with i running over all activated or deactivated contacts
// Loop over all interactions to detect whether some constraints are activated or deactivated
bool _IsContactClosed = false;
bool _IsContactOpened = false;
bool _IsFirstTime = true;
InteractionsGraph::VIterator ui, uiend;
SP::SiconosVector y, ydot, lambda;
SP::Topology topo = _nsds->topology();
SP::InteractionsGraph indexSet2 = topo->indexSet(2);
//
#ifdef DEBUG_MESSAGES
cout << "======== In EventDriven::detectEvents =========" <<endl;
#endif
for (std11::tie(ui, uiend) = _indexSet0->vertices(); ui != uiend; ++ui)
{
SP::Interaction inter = _indexSet0->bundle(*ui);
double nsLawSize = inter->nonSmoothLaw()->size();
if (nsLawSize != 1)
{
RuntimeException::selfThrow("In EventDriven::detectEvents, the interaction size > 1 has not been implemented yet!!!");
}
y = inter->y(0); // output y at this Interaction
ydot = inter->y(1); // output of level 1 at this Interaction
lambda = inter->lambda(2); // input of level 2 at this Interaction
if (!(indexSet2->is_vertex(inter))) // if Interaction is not in the indexSet[2]
{
if ((*y)(0) < _TOL_ED) // gap at the current interaction <= 0
{
_IsContactClosed = true;
}
if (_IsFirstTime)
{
_minResiduOutput = (*y)(0);
_IsFirstTime = false;
}
else
{
if (_minResiduOutput > (*y)(0))
{
_minResiduOutput = (*y)(0);
}
}
}
else // If interaction is in the indexSet[2]
{
if ((*lambda)(0) < _TOL_ED) // normal force at the current interaction <= 0
{
_IsContactOpened = true;
}
if (_IsFirstTime)
{
_minResiduOutput = (*lambda)(0);
_IsFirstTime = false;
}
else
{
if (_minResiduOutput > (*lambda)(0))
{
_minResiduOutput = (*lambda)(0);
}
}
}
//
#ifdef DEBUG_MESSAGES
cout.precision(15);
cout << "Contact number: " << inter->number() <<endl;
cout << "Contact gap: " << (*y)(0) <<endl;
cout << "Contact force: " << (*lambda)(0) <<endl;
cout << "Is contact is closed: " << _IsContactClosed <<endl;
cout << "Is contact is opened: " << _IsContactOpened <<endl;
#endif
//
}
//
if (updateIstate)
{
if ((!_IsContactClosed) && (!_IsContactOpened))
{
_istate = 2; //no event is detected
}
else if ((_IsContactClosed) && (!_IsContactOpened))
{
_istate = 3; // Only some contacts are closed
}
else if ((!_IsContactClosed) && (_IsContactOpened))
{
_istate = 4; // Only some contacts are opened
}
else
{
_istate = 5; // Some contacts are closed AND some contacts are opened
}
}
//
return _minResiduOutput;
}
// Fill the SparseMat
void BlockCSRMatrix::fill(SP::InteractionsGraph indexSet)
{
// ======> Aim: find inter1 and inter2 both in indexSets[level] and which
// have common DynamicalSystems. Then get the corresponding matrix
// from map blocks.
assert(indexSet);
// Number of blocks in a row = number of active constraints.
_nr = indexSet->size();
// (re)allocate memory for ublas matrix
_blockCSR->resize(_nr, _nr, false);
_diagsize0->resize(_nr);
_diagsize1->resize(_nr);
// === Loop through "active" Interactions (ie present in
// indexSets[level]) ===
int sizeV = 0;
InteractionsGraph::VIterator vi, viend;
for (std11::tie(vi, viend) = indexSet->vertices();
vi != viend; ++vi)
{
SP::Interaction inter = indexSet->bundle(*vi);
assert(inter->nonSmoothLaw()->size() > 0);
sizeV += inter->nonSmoothLaw()->size();
(*_diagsize0)[indexSet->index(*vi)] = sizeV;
(*_diagsize1)[indexSet->index(*vi)] = sizeV;
assert((*_diagsize0)[indexSet->index(*vi)] > 0);
assert((*_diagsize1)[indexSet->index(*vi)] > 0);
(*_blockCSR)(indexSet->index(*vi), indexSet->index(*vi)) =
indexSet->properties(*vi).block->getArray();
}
InteractionsGraph::EIterator ei, eiend;
for (std11::tie(ei, eiend) = indexSet->edges();
ei != eiend; ++ei)
{
InteractionsGraph::VDescriptor vd1 = indexSet->source(*ei);
InteractionsGraph::VDescriptor vd2 = indexSet->target(*ei);
SP::Interaction inter1 = indexSet->bundle(vd1);
SP::Interaction inter2 = indexSet->bundle(vd2);
assert(indexSet->index(vd1) < _nr);
assert(indexSet->index(vd2) < _nr);
assert(indexSet->is_vertex(inter2));
assert(vd2 == indexSet->descriptor(inter2));
assert(indexSet->index(vd2) == indexSet->index(indexSet->descriptor(inter2)));
unsigned int pos = indexSet->index(vd1);
unsigned int col = indexSet->index(vd2);
assert(pos != col);
(*_blockCSR)(std::min(pos, col), std::max(pos, col)) =
indexSet->properties(*ei).upper_block->getArray();
(*_blockCSR)(std::max(pos, col), std::min(pos, col)) =
indexSet->properties(*ei).lower_block->getArray();
}
DEBUG_EXPR(display(););
int main(int argc, char* argv[])
{
try
{
// ================= Creation of the model =======================
// User-defined main parameters
unsigned int nDof = 3; // degrees of freedom for the ball
double t0 = 0; // initial computation time
double T = 2.0; // final computation time
double h = 0.0005; // time step
double position_init = 1.0; // initial position for lowest bead.
double velocity_init = 0.0; // initial velocity for lowest bead.
double theta = 0.5; // theta for MoreauJeanOSI integrator
double R = 0.1; // Ball radius
double m = 1; // Ball mass
double g = 9.81; // Gravity
// -------------------------
// --- Dynamical systems ---
// -------------------------
cout << "====> Model loading ..." << endl << endl;
// Number of Beads
unsigned int nBeads = 10;
double initialGap = 0.25;
double alert = 0.02;
SP::SiconosMatrix Mass(new SimpleMatrix(nDof, nDof));
(*Mass)(0, 0) = m;
(*Mass)(1, 1) = m;
(*Mass)(2, 2) = 3. / 5 * m * R * R;
// -- Initial positions and velocities --
std::vector<SP::SiconosVector> q0(nBeads);
std::vector<SP::SiconosVector> v0(nBeads);
for (unsigned int i = 0; i < nBeads; i++)
{
(q0[i]).reset(new SiconosVector(nDof));
(v0[i]).reset(new SiconosVector(nDof));
(q0[i])->setValue(0, position_init + i * initialGap);
(v0[i])->setValue(0, velocity_init);
}
// -- The dynamical system --
SP::SiconosVector weight(new SiconosVector(nDof));
(*weight)(0) = -m * g;
std::vector<SP::LagrangianLinearTIDS> beads(nBeads);
for (unsigned int i = 0; i < nBeads; i++)
{
beads[i].reset(new LagrangianLinearTIDS(q0[i], v0[i], Mass));
// -- Set external forces (weight) --
beads[i]->setFExtPtr(weight);
}
// --------------------
// --- Interactions ---
// --------------------
// -- nslaw --
double e = 0.9;
// Interaction ball-floor
//
SP::SimpleMatrix H(new SimpleMatrix(1, nDof));
(*H)(0, 0) = 1.0;
SP::SiconosVector b(new SiconosVector(1));
(*b)(0) = -R;
SP::NonSmoothLaw nslaw(new NewtonImpactNSL(e));
SP::Relation relation(new LagrangianLinearTIR(H, b));
SP::Interaction inter;
// beads/beads interactions
SP::SimpleMatrix HOfBeads(new SimpleMatrix(1, 2 * nDof));
(*HOfBeads)(0, 0) = -1.0;
(*HOfBeads)(0, 3) = 1.0;
SP::SiconosVector bOfBeads(new SiconosVector(1));
(*bOfBeads)(0) = -2 * R;
// This doesn't work !!!
//SP::Relation relationOfBeads(new LagrangianLinearTIR(HOfBeads));
//std::vector<SP::Interaction > interOfBeads(nBeads-1);
// for (unsigned int i =0; i< nBeads-1; i++)
// {
// interOfBeads[i].reset(new Interaction(1, nslaw, relationOfBeads));
// }
// This works !!
std::vector<SP::Relation > relationOfBeads(nBeads - 1);
std::vector<SP::Interaction > interOfBeads(nBeads - 1);
// for (unsigned int i =0; i< nBeads-1; i++)
// {
// relationOfBeads[i].reset(new LagrangianLinearTIR(HOfBeads,bOfBeads));
// interOfBeads[i].reset(new Interaction(1, nslaw, relationOfBeads[i]));
// }
// --------------------------------------
// --- Model and simulation ---
// --------------------------------------
SP::Model columnOfBeads(new Model(t0, T));
// -- (1) OneStepIntegrators --
SP::MoreauJeanOSI OSI(new MoreauJeanOSI(theta));
// add the dynamical system in the non smooth dynamical system
for (unsigned int i = 0; i < nBeads; i++)
{
columnOfBeads->nonSmoothDynamicalSystem()->insertDynamicalSystem(beads[i]);
}
// // link the interaction and the dynamical system
// for (unsigned int i =0; i< nBeads-1; i++)
// {
// columnOfBeads->nonSmoothDynamicalSystem()->link(interOfBeads[i],beads[i]);
// columnOfBeads->nonSmoothDynamicalSystem()->link(interOfBeads[i],beads[i+1]);
// }
// -- (2) Time discretisation --
SP::TimeDiscretisation t(new TimeDiscretisation(t0, h));
// -- (3) one step non smooth problem
SP::OneStepNSProblem osnspb(new LCP());
// -- (4) Simulation setup with (1) (2) (3)
SP::TimeStepping s(new TimeStepping(t, OSI, osnspb));
columnOfBeads->setSimulation(s);
// =========================== End of model definition ===========================
// ================================= Computation =================================
// --- Simulation initialization ---
cout << "====> Initialisation ..." << endl << endl;
columnOfBeads->initialize();
int N = ceil((T - t0) / h); // Number of time steps
// --- Get the values to be plotted ---
// -> saved in a matrix dataPlot
unsigned int outputSize = 1 + nBeads * 4;
SimpleMatrix dataPlot(N + 1, outputSize);
dataPlot(0, 0) = columnOfBeads->t0();
for (unsigned int i = 0; i < nBeads; i++)
{
dataPlot(0, 1 + i * 2) = (beads[i]->q())->getValue(0);
dataPlot(0, 2 + i * 2) = (beads[i]->velocity())->getValue(0);
// dataPlot(0,3+i*4) = (beads[i]->p(1))->getValue(0);
}
// for (unsigned int i =1; i< nBeads; i++)
// {
// dataPlot(0,4+i*4) = (interOfBeads[i-1]->lambda(1))->getValue(0);
// }
// --- Time loop ---
cout << "====> Start computation ... " << endl << endl;
// ==== Simulation loop - Writing without explicit event handling =====
int k = 1;
boost::progress_display show_progress(N);
boost::timer time;
time.restart();
int ncontact = 0 ;
bool isOSNSinitialized = false;
while (s->hasNextEvent())
{
// Rough contact detection
for (unsigned int i = 0; i < nBeads - 1; i++)
{
// Between first bead and plane
if (abs(((beads[i])->q())->getValue(0) - R) < alert)
{
if (!inter)
{
ncontact++;
// std::cout << "Number of contact = " << ncontact << std::endl;
inter.reset(new Interaction(1, nslaw, relation));
columnOfBeads->nonSmoothDynamicalSystem()->link(inter, beads[0]);
s->initializeInteraction(s->nextTime(), inter);
if (!isOSNSinitialized)
{
s->initOSNS();
isOSNSinitialized = true;
}
assert(inter->y(0)->getValue(0) >= 0);
}
}
// Between two beads
if (abs(((beads[i + 1])->q())->getValue(0) - ((beads[i])->q())->getValue(0) - 2 * R) < alert)
{
//std::cout << "Alert distance for declaring contact = ";
//std::cout << abs(((beads[i])->q())->getValue(0)-((beads[i+1])->q())->getValue(0)) <<std::endl;
if (!interOfBeads[i].get())
{
ncontact++;
// std::cout << "Number of contact = " << ncontact << std::endl;
relationOfBeads[i].reset(new LagrangianLinearTIR(HOfBeads, bOfBeads));
interOfBeads[i].reset(new Interaction(1, nslaw, relationOfBeads[i]));
columnOfBeads->nonSmoothDynamicalSystem()->link(interOfBeads[i], beads[i], beads[i+1]);
s->initializeInteraction(s->nextTime(), interOfBeads[i]);
if (!isOSNSinitialized)
{
s->initOSNS();
isOSNSinitialized = true;
}
assert(interOfBeads[i]->y(0)->getValue(0) >= 0);
}
}
}
s->computeOneStep();
// --- Get values to be plotted ---
dataPlot(k, 0) = s->nextTime();
for (unsigned int i = 0; i < nBeads; i++)
{
dataPlot(k, 1 + i * 2) = (beads[i]->q())->getValue(0);
dataPlot(k, 2 + i * 2) = (beads[i]->velocity())->getValue(0);
}
// for (unsigned int i =1; i< nBeads; i++)
// {
// dataPlot(k,4+i*4) = (interOfBeads[i-1]->lambda(1))->getValue(0);
// }
// for (unsigned int i =1; i< nBeads; i++)
// {
// std::cout << (interOfBeads[i-1]->y(0))->getValue(0) << std::endl ;
// }
s->nextStep();
++show_progress;
k++;
}
cout << endl << "End of computation - Number of iterations done: " << k - 1 << endl;
cout << "Computation Time " << time.elapsed() << endl;
// --- Output files ---
cout << "====> Output file writing ..." << endl;
dataPlot.resize(k, outputSize);
ioMatrix::write("result.dat", "ascii", dataPlot, "noDim");
// Comparison with a reference file
SimpleMatrix dataPlotRef(dataPlot);
dataPlotRef.zero();
ioMatrix::read("result.ref", "ascii", dataPlotRef);
cout << "====> Comparison with reference file ..." << endl;
std::cout << "Error w.r.t. reference file : " << (dataPlot - dataPlotRef).normInf() << std::endl;
if ((dataPlot - dataPlotRef).normInf() > 1e-12)
{
std::cout << "Warning. The result is rather different from the reference file." << std::endl;
return 1;
}
}
catch (SiconosException e)
{
cout << e.report() << endl;
}
catch (...)
{
cout << "Exception caught in ColumnOfBeadsTS.cpp" << endl;
}
}
