// ================= 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);
}
}
// ================= 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);
}
}
int main()
{
int cmp = 0;
/************************************************************/
/************************************************************/
int dimX = 4;
SP::SiconosVector x0(new SiconosVector(dimX));
//Point de départ hors arc singulier
x0->setValue(0, 3.4999939172);
x0->setValue(1, -2.2788416237);
x0->setValue(2, 1.1935988302);
x0->setValue(3, -0.6365413023);
double sT = 10;
double sStep = 2e-3;
unsigned int NBStep = floor(sT / sStep);
//NBStep =2;
// NBStep = 3;
//*****BUILD THE DYNAMIC SYSTEM
SP::MyDS aDS ;
aDS.reset(new MyDS(x0));
//******BUILD THE RELATION
SP::adjointInput aR;
aR.reset(new adjointInput());
int sN = 2;
//*****BUILD THE NSLAW
SP::NonSmoothLaw aNSL;
aNSL.reset(new ComplementarityConditionNSL(sN));
//****BUILD THE INTERACTION
SP::Interaction aI(new Interaction(sNSLawSize, aNSL, aR));
//****BUILD THE SYSTEM
SP::Model aM(new Model(0, sT));
aM->nonSmoothDynamicalSystem()->insertDynamicalSystem(aDS);
aM->nonSmoothDynamicalSystem()->link(aI,aDS);
SP::TimeDiscretisation aTD(new TimeDiscretisation(0, sStep));
SP::TimeStepping aS(new TimeStepping(aTD));
aS->setComputeResiduY(true);
aS->setComputeResiduR(true);
aS->setUseRelativeConvergenceCriteron(false);
//*****BUILD THE STEP INTEGRATOR
SP::OneStepIntegrator aEulerMoreauOSI ;
aEulerMoreauOSI.reset(new EulerMoreauOSI(0.5));
aS->insertIntegrator(aEulerMoreauOSI);
//**** BUILD THE STEP NS PROBLEM
SP::LCP aLCP ;
aLCP.reset(new LCP(SICONOS_LCP_ENUM));
// aLCP.reset(new LCP(SICONOS_LCP_NEWTONFB));
aS->insertNonSmoothProblem(aLCP);
aM->setSimulation(aS);
aM->initialize();
setNumericsVerbose(0);
SP::SiconosVector x = aDS->x();
SP::SiconosVector y = aI->y(0);
SP::SiconosVector lambda = aI->lambda(0);
unsigned int outputSize = 9; // number of required data
SimpleMatrix dataPlot(NBStep+10, outputSize);
SP::SiconosVector z = aDS->x();
SP::SiconosVector lambdaOut = aI->lambda(0);
SP::SiconosVector yOut = aI->y(0);
dataPlot(0, 0) = aM->t0(); // Initial time of the model
dataPlot(0, 1) = (*z)(0);
dataPlot(0, 2) = (*z)(1);
dataPlot(0, 3) = (*z)(2);
dataPlot(0, 4) = (*z)(3);
dataPlot(0, 5) = (*lambdaOut)(0);
dataPlot(0, 6) = (*lambdaOut)(1);
dataPlot(0, 7) = (*yOut)(0);
dataPlot(0, 8) = (*yOut)(1);
// do simulation while events remains in the "future events" list of events manager.
cout << " ==== Start of simulation : " << NBStep << " steps====" << endl;
boost::progress_display show_progress(NBStep);
boost::timer time;
time.restart();
unsigned int k = 0;
while (aS->hasNextEvent())
{
k++;
// if (cmp==150)
// setNumericsVerbose(à);
// else if (cmp==151)
setNumericsVerbose(1);
++show_progress;
cmp++;
// solve ...
// aS->computeOneStep();
aS->newtonSolve(1.1e-11, 50);
x = aDS->x();
lambda = aI->lambda(0);
dataPlot(k, 0) = aS->nextTime(); // Initial time of the model
dataPlot(k, 1) = (*x)(0);
dataPlot(k, 2) = (*x)(1);
dataPlot(k, 3) = (*x)(2);
dataPlot(k, 4) = (*x)(3);
dataPlot(k, 5) = (*lambda)(0);
dataPlot(k, 6) = (*lambda)(1);
dataPlot(k, 7) = (*yOut)(0);
dataPlot(k, 8) = (*yOut)(1);
aS->nextStep();
}
cout << "===== End of simulation. ==== " << endl;
dataPlot.resize(k+1, 9);
// --- Output files ---
cout << "====> Output file writing ..." << endl;
ioMatrix::write("OptimalControl.dat", "ascii", dataPlot, "noDim");
std::cout << "Comparison with a reference file: " ;
SimpleMatrix dataPlotRef(dataPlot);
dataPlotRef.zero();
ioMatrix::read("OptimalControl.ref", "ascii", dataPlotRef);
std::cout << "error="<< (dataPlot-dataPlotRef).normInf() <<std::endl;
if ((dataPlot - dataPlotRef).normInf() > 5e-11)
{
std::cout << "Warning. The results is rather different from the reference file." << std::endl;
return 1;
}
return 0;
}
int main(int argc, char *argv[])
{
//printf("argc %i\n", argc);
int cmp=0;
int dimX = 2;
char filename[50] = "simu.";
//***** Set the initial condition
SP::SiconosVector xti(new SiconosVector(dimX));
if (argc==1)
{
xti->setValue(0,1);
xti->setValue(1,6);
strncpy(&filename[5],"1.6.log",7);
}
else if (argc==3)
{
//printf("argv[0] %s\n", argv[0]);
printf("xti(0) is set to %f\n", atof(argv[1]));
printf("xti(1) is set to %f\n", atof(argv[2]));
xti->setValue(0,atof(argv[1]));
xti->setValue(1,atof(argv[2]));
int sizeofargv1 = strlen(argv[1]);
// printf("sizeofargv1 %i\n",sizeofargv1);
strncpy(&filename[5],argv[1],sizeofargv1);
int sizeofargv2 = strlen(argv[2]);
//printf("sizeofargv2 %i\n",sizeofargv2);
strncpy(&filename[5+sizeofargv1],".",1);
strncpy(&filename[5+sizeofargv1+1],argv[2],sizeofargv2);
strncpy(&filename[5+sizeofargv1+sizeofargv2+1],".log",4);
printf("Output is written in filename %s\n", filename);
}
else
{
cout << "wrong number of arguments = " << argc << endl;
}
int NBStep = (int) floor(sTf/sStep);
// NBStep =1;
//*****BUILD THE DYNAMICAL SYSTEM
SP::MyDS aDS ;
aDS.reset(new MyDS(xti));
DynamicalSystemsSet Inter_DS ;
Inter_DS.insert(aDS);
//******BUILD THE RELATION
SP::NonlinearRelationReduced2 aR;
aR.reset(new NonlinearRelationReduced2());
//*****BUILD THE NSLAW
SP::NonSmoothLaw aNSL;
RelayNSL * rNSL = new RelayNSL(sNSLawSize);
aNSL.reset(rNSL); //nr de lambda din pb LCP
rNSL->setLb(-0.0);
rNSL->setUb(1.0);
// unsigned int sNSLawSize = 4;
//****BUILD THE INTERACTION
SP::Interaction aI(new Interaction(sNSLawSize,aNSL,aR));
//****BUILD THE SYSTEM
SP::Model aM(new Model(0,sTf));
aM->nonSmoothDynamicalSystem()->insertDynamicalSystem(aDS);
aM->nonSmoothDynamicalSystem()->link(aI,aDS);
// -- (1) OneStepIntegrators --
SP::OneStepIntegrator aEulerMoreauOSI ;
aEulerMoreauOSI.reset(new EulerMoreauOSI(0.5));
// -- (2) Time discretisation --
SP::TimeDiscretisation aTD(new TimeDiscretisation(0,sStep));
// -- (3) Non smooth problem
SP::Relay osnspb(new Relay(SICONOS_RELAY_LEMKE));
osnspb->numericsSolverOptions()->dparam[0]=1e-08;
osnspb->numericsSolverOptions()->iparam[0]=0; // Multiple solutions 0 or 1
// osnspb->numericsSolverOptions()->iparam[3]=48;
osnspb->setNumericsVerboseMode(0);
// -- (4) Simulation setup with (1) (2) (3)
SP::TimeStepping aS(new TimeStepping(aTD,aEulerMoreauOSI,osnspb));
aS->setComputeResiduY(true);
aS->setUseRelativeConvergenceCriteron(false);
// Initialization
printf("-> Initialisation \n");
aM->setSimulation(aS);
aM->initialize();
printf("-> End of initialization \n");
// BUILD THE STEP INTEGRATOR
SP::SiconosVector x = aDS->x();
SP::SiconosVector vectorfield = aDS->rhs();
SP::SiconosVector y = aI->y(0);
SP::SiconosVector lambda = aI->lambda(0);
unsigned int outputSize = 9;
SimpleMatrix dataPlot(NBStep+1,outputSize);
cout << "=== Start of simulation: "<<NBStep<<" steps ===" << endl;
printf("=== Start of simulation: %d steps === \n", NBStep);
dataPlot(0, 0) = aM->t0();
dataPlot(0,1) = x->getValue(0);
dataPlot(0,2) = x->getValue(1);
dataPlot(0, 3) = lambda->getValue(0);
dataPlot(0, 7) = vectorfield->getValue(0);
dataPlot(0, 8) = vectorfield->getValue(1);
boost::progress_display show_progress(NBStep);
boost::timer time;
time.restart();
for(int k = 0 ; k < NBStep ; k++)
{
#ifdef SICONOS_DEBUG
std::cout<<"-> Running step:"<<k<<std::endl;
#endif
cmp++;
aS->newtonSolve(1e-4, 200);
dataPlot(cmp, 0) = aS->nextTime();
dataPlot(cmp, 1) = x->getValue(0);
dataPlot(cmp, 2) = x->getValue(1);
dataPlot(cmp, 3) = lambda->getValue(0);
aDS->computeRhs(aS->nextTime(),true);
if (cmp==1) // tricks just for display to avoid the computation of the initial Rhs
{
dataPlot(cmp-1, 7) = vectorfield->getValue(0);
dataPlot(cmp-1, 8) = vectorfield->getValue(1);
}
dataPlot(cmp, 7) = vectorfield->getValue(0);
dataPlot(cmp, 8) = vectorfield->getValue(1);
++show_progress;
aS->nextStep();
// (*fout)<<cmp<<" "<<x->getValue(0)<<" "<<x->getValue(1)<<" "<<lambda->getValue(0)<<" "<<lambda->getValue(1)<<" "<<lambda->getValue(2)<<" "<<lambda->getValue(3)<<endl;
}
dataPlot.resize(cmp,outputSize);
ioMatrix::write(filename, "ascii", dataPlot, "noDim");
cout << "=== End of simulation. === " << endl;
return 0;
}
int main()
{
// User-defined main parameters
double t0 = 0; // initial computation time
double T = 20.0; // end of computation time
double h = 0.005; // time step
double position_init = 10.0; // initial position
double velocity_init = 0.0; // initial velocity
double g = 9.81;
double theta = 0.5; // theta for MoreauJeanOSI integrator
// -----------------------------------------
// --- Dynamical systems && interactions ---
// -----------------------------------------
try
{
// ------------
// --- Init ---
// ------------
std::cout << "====> Model loading ..." << std::endl << std::endl;
// -- OneStepIntegrators --
SP::OneStepIntegrator osi;
osi.reset(new MoreauJeanOSI(theta));
// -- Model --
SP::Model model(new Model(t0, T));
std::vector<SP::BulletWeightedShape> shapes;
// note: no rebound with a simple Bullet Box, why ?
// the distance after the broadphase contact detection is negative
// and then stay negative.
// SP::btCollisionShape box(new btBoxShape(btVector3(1,1,1)));
// SP::BulletWeightedShape box1(new BulletWeightedShape(box,1.0));
// This is ok if we build one with btConveHullShape
SP::btCollisionShape box(new btConvexHullShape());
{
std11::static_pointer_cast<btConvexHullShape>(box)->addPoint(btVector3(-1.0, 1.0, -1.0));
std11::static_pointer_cast<btConvexHullShape>(box)->addPoint(btVector3(-1.0, -1.0, -1.0));
std11::static_pointer_cast<btConvexHullShape>(box)->addPoint(btVector3(-1.0, -1.0, 1.0));
std11::static_pointer_cast<btConvexHullShape>(box)->addPoint(btVector3(-1.0, 1.0, 1.0));
std11::static_pointer_cast<btConvexHullShape>(box)->addPoint(btVector3(1.0, 1.0, 1.0));
std11::static_pointer_cast<btConvexHullShape>(box)->addPoint(btVector3(1.0, 1.0, -1.0));
std11::static_pointer_cast<btConvexHullShape>(box)->addPoint(btVector3(1.0, -1.0, -1.0));
std11::static_pointer_cast<btConvexHullShape>(box)->addPoint(btVector3(1.0, -1.0, 1.0));
}
SP::BulletWeightedShape box1(new BulletWeightedShape(box, 1.0));
shapes.push_back(box1);
SP::SiconosVector q0(new SiconosVector(7));
SP::SiconosVector v0(new SiconosVector(6));
v0->zero();
q0->zero();
(*q0)(2) = position_init;
(*q0)(3) = 1.0;
(*v0)(2) = velocity_init;
// -- The dynamical system --
// -- the default contactor is the shape given in the constructor
// -- the contactor id is 0
SP::BulletDS body(new BulletDS(box1, q0, v0));
// -- Set external forces (weight) --
SP::SiconosVector FExt;
FExt.reset(new SiconosVector(3)); //
FExt->zero();
FExt->setValue(2, - g * box1->mass());
body->setFExtPtr(FExt);
// -- Add the dynamical system in the non smooth dynamical system
model->nonSmoothDynamicalSystem()->insertDynamicalSystem(body);
SP::btCollisionObject ground(new btCollisionObject());
ground->setCollisionFlags(btCollisionObject::CF_STATIC_OBJECT);
SP::btCollisionShape groundShape(new btBoxShape(btVector3(30, 30, .5)));
btMatrix3x3 basis;
basis.setIdentity();
ground->getWorldTransform().setBasis(basis);
ground->setCollisionShape(&*groundShape);
ground->getWorldTransform().getOrigin().setZ(-.50);
// ------------------
// --- Simulation ---
// ------------------
// -- Time discretisation --
SP::TimeDiscretisation timedisc(new TimeDiscretisation(t0, h));
// -- OneStepNsProblem --
SP::FrictionContact osnspb(new FrictionContact(3));
// -- Some configuration
osnspb->numericsSolverOptions()->iparam[0] = 1000; // Max number of
// iterations
osnspb->numericsSolverOptions()->dparam[0] = 1e-5; // 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 initialization ---
std::cout << "====> Simulation initialisation ..." << std::endl << std::endl;
int N = ceil((T - t0) / h); // Number of time steps
SP::NonSmoothLaw nslaw(new NewtonImpactFrictionNSL(0.8, 0., 0.0, 3));
// -- The space filter performs broadphase collision detection
SP::BulletSpaceFilter space_filter(new BulletSpaceFilter(model));
// -- insert a non smooth law for contactors id 0
space_filter->insert(nslaw, 0, 0);
// -- add multipoint iterations, this is needed to gather at least
// -- 3 contact points and avoid objects penetration, see Bullet
// -- documentation
space_filter->collisionConfiguration()->setConvexConvexMultipointIterations();
space_filter->collisionConfiguration()->setPlaneConvexMultipointIterations();
// -- The ground is a static object
// -- we give it a group contactor id : 0
space_filter->addStaticObject(ground, 0);
// -- MoreauJeanOSI Time Stepping with Bullet Dynamical Systems
SP::BulletTimeStepping simulation(new BulletTimeStepping(timedisc));
simulation->insertIntegrator(osi);
simulation->insertNonSmoothProblem(osnspb);
model->setSimulation(simulation);
model->initialize();
std::cout << "====> End of initialisation ..." << std::endl << std::endl;
// --- Get the values to be plotted ---
// -> saved in a matrix dataPlot
unsigned int outputSize = 4;
SimpleMatrix dataPlot(N + 1, outputSize);
dataPlot.zero();
SP::SiconosVector q = body->q();
SP::SiconosVector v = body->velocity();
dataPlot(0, 0) = model->t0();
dataPlot(0, 1) = (*q)(2);
dataPlot(0, 2) = (*v)(2);
// --- Time loop ---
std::cout << "====> Start computation ... " << std::endl << std::endl;
// ==== Simulation loop - Writing without explicit event handling =====
int k = 1;
boost::progress_display show_progress(N);
boost::timer time;
time.restart();
while (simulation->hasNextEvent())
{
space_filter->buildInteractions(model->currentTime());
simulation->computeOneStep();
// --- Get values to be plotted ---
dataPlot(k, 0) = simulation->nextTime();
dataPlot(k, 1) = (*q)(2);
dataPlot(k, 2) = (*v)(2);
// If broadphase collision detection shows some contacts then we may
// display contact forces.
if (space_filter->collisionWorld()->getDispatcher()->getNumManifolds() > 0)
{
// we *must* have an indexSet0, filled by Bullet broadphase
// collision detection and an indexSet1, filled by
// TimeStepping::updateIndexSet with the help of Bullet
// getDistance() function
if (model->nonSmoothDynamicalSystem()->topology()->numberOfIndexSet() == 2)
{
SP::InteractionsGraph index1 = simulation->indexSet(1);
// This is the narrow phase contact detection : if
// TimeStepping::updateIndexSet has filled indexSet1 then we
// have some contact forces to display
if (index1->size() > 0)
{
// Four contact points for a cube with a side facing the
// ground. Note : changing Bullet margin for collision
// detection may lead this assertion to be false.
if (index1->size() == 4)
{
InteractionsGraph::VIterator iur = index1->begin();
// different version of bullet may not gives the same
// contact points! So we only keep the summation.
dataPlot(k, 3) =
index1->bundle(*iur)-> lambda(1)->norm2() +
index1->bundle(*++iur)->lambda(1)->norm2() +
index1->bundle(*++iur)->lambda(1)->norm2() +
index1->bundle(*++iur)->lambda(1)->norm2();
}
}
}
}
simulation->nextStep();
++show_progress;
k++;
}
std::cout << std::endl << "End of computation - Number of iterations done: " << k - 1 << std::endl;
std::cout << "Computation Time " << time.elapsed() << std::endl;
// --- Output files ---
std::cout << "====> Output file writing ..." << std::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);
if ((dataPlot - dataPlotRef).normInf() > 1e-12)
{
std::cout << "Warning. The result is rather different from the reference file : "
<< (dataPlot - dataPlotRef).normInf() << std::endl;
return 1;
}
}
catch (SiconosException e)
{
std::cout << e.report() << std::endl;
exit(1);
}
catch (...)
{
std::cout << "Exception caught in BulletBouncingBox" << std::endl;
exit(1);
}
return 0;
}