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;
}
}
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);
}
}
}