예제 #1
0
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;
  }



}
예제 #2
0
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);
            }
        }
    }