// ================= 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);
}
}
// ================= Creation of the model =======================
void Disks::init()
{
SP::TimeDiscretisation timedisc_;
SP::TimeStepping simulation_;
SP::FrictionContact osnspb_;
// User-defined main parameters
double t0 = 0; // initial computation time
double T = std::numeric_limits<double>::infinity();
double h = 0.01; // time step
double g = 9.81;
double theta = 0.5; // theta for MoreauJeanOSI integrator
std::string solverName = "NSGS";
// -----------------------------------------
// --- Dynamical systems && interactions ---
// -----------------------------------------
double R;
double m;
try
{
// ------------
// --- Init ---
// ------------
std::cout << "====> Model loading ..." << std::endl << std::endl;
_plans.reset(new SimpleMatrix("plans.dat", true));
if (_plans->size(0) == 0)
{
/* default plans */
double A1 = P1A;
double B1 = P1B;
double C1 = P1C;
double A2 = P2A;
double B2 = P2B;
double C2 = P2C;
_plans.reset(new SimpleMatrix(6, 6));
_plans->zero();
(*_plans)(0, 0) = 0;
(*_plans)(0, 1) = 1;
(*_plans)(0, 2) = -GROUND;
(*_plans)(1, 0) = 1;
(*_plans)(1, 1) = 0;
(*_plans)(1, 2) = WALL;
(*_plans)(2, 0) = 1;
(*_plans)(2, 1) = 0;
(*_plans)(2, 2) = -WALL;
(*_plans)(3, 0) = 0;
(*_plans)(3, 1) = 1;
(*_plans)(3, 2) = -TOP;
(*_plans)(4, 0) = A1;
(*_plans)(4, 1) = B1;
(*_plans)(4, 2) = C1;
(*_plans)(5, 0) = A2;
(*_plans)(5, 1) = B2;
(*_plans)(5, 2) = C2;
}
/* set center positions */
for (unsigned int i = 0 ; i < _plans->size(0); ++i)
{
SP::DiskPlanR tmpr;
tmpr.reset(new DiskPlanR(1, (*_plans)(i, 0), (*_plans)(i, 1), (*_plans)(i, 2),
(*_plans)(i, 3), (*_plans)(i, 4), (*_plans)(i, 5)));
(*_plans)(i, 3) = tmpr->getXCenter();
(*_plans)(i, 4) = tmpr->getYCenter();
}
/* _moving_plans.reset(new FMatrix(1,6));
(*_moving_plans)(0,0) = &A;
(*_moving_plans)(0,1) = &B;
(*_moving_plans)(0,2) = &C;
(*_moving_plans)(0,3) = &DA;
(*_moving_plans)(0,4) = &DB;
(*_moving_plans)(0,5) = &DC;*/
SP::SiconosMatrix Disks;
Disks.reset(new SimpleMatrix("disks.dat", true));
// -- OneStepIntegrators --
SP::OneStepIntegrator osi;
osi.reset(new MoreauJeanOSI(theta));
// -- Model --
_model.reset(new Model(t0, T));
for (unsigned int i = 0; i < Disks->size(0); i++)
{
R = Disks->getValue(i, 2);
m = Disks->getValue(i, 3);
SP::SiconosVector qTmp;
SP::SiconosVector vTmp;
qTmp.reset(new SiconosVector(NDOF));
vTmp.reset(new SiconosVector(NDOF));
vTmp->zero();
(*qTmp)(0) = (*Disks)(i, 0);
(*qTmp)(1) = (*Disks)(i, 1);
SP::LagrangianDS body;
if (R > 0)
body.reset(new Disk(R, m, qTmp, vTmp));
else
body.reset(new Circle(-R, m, qTmp, vTmp));
// -- Set external forces (weight) --
SP::SiconosVector FExt;
FExt.reset(new SiconosVector(NDOF));
FExt->zero();
FExt->setValue(1, -m * g);
body->setFExtPtr(FExt);
// add the dynamical system to the one step integrator
osi->insertDynamicalSystem(body);
// add the dynamical system in the non smooth dynamical system
_model->nonSmoothDynamicalSystem()->insertDynamicalSystem(body);
}
_model->nonSmoothDynamicalSystem()->setSymmetric(true);
// ------------------
// --- Simulation ---
// ------------------
// -- Time discretisation --
timedisc_.reset(new TimeDiscretisation(t0, h));
// -- OneStepNsProblem --
osnspb_.reset(new FrictionContact(2));
osnspb_->numericsSolverOptions()->iparam[0] = 100; // Max number of
// iterations
osnspb_->numericsSolverOptions()->iparam[1] = 20; // compute error
// iterations
osnspb_->numericsSolverOptions()->dparam[0] = 1e-3; // Tolerance
osnspb_->setMaxSize(6 * ((3 * Ll * Ll + 3 * Ll) / 2 - Ll));
osnspb_->setMStorageType(1); // Sparse storage
osnspb_->setNumericsVerboseMode(0);
osnspb_->setKeepLambdaAndYState(true); // inject previous solution
// -- Simulation --
simulation_.reset(new TimeStepping(timedisc_));
std11::static_pointer_cast<TimeStepping>(simulation_)->setNewtonMaxIteration(3);
simulation_->insertIntegrator(osi);
simulation_->insertNonSmoothProblem(osnspb_);
simulation_->setCheckSolverFunction(localCheckSolverOuput);
// --- Simulation initialization ---
std::cout << "====> Simulation initialisation ..." << std::endl << std::endl;
SP::NonSmoothLaw nslaw(new NewtonImpactFrictionNSL(0, 0, 0.3, 2));
_playground.reset(new SpaceFilter(3, 6, _model, _plans, _moving_plans));
_playground->insert(nslaw, 0, 0);
_model->initialize(simulation_);
}
catch (SiconosException e)
{
std::cout << e.report() << std::endl;
exit(1);
}
catch (...)
{
std::cout << "Exception caught in Disks::init()" << std::endl;
exit(1);
}
}
void DisksViewer::draw()
{
int i;
char qs[6];
float lbd, w;
float lbdmax = 0.;
DSIterator itDS;
SP::DynamicalSystemsSet involvedDS;
SP::InteractionsGraph I1;
SP::Interaction interaction;
SP::Relation relation;
if (Siconos_->model()->nonSmoothDynamicalSystem()->topology()->numberOfIndexSet() > 1)
{
I1 = Siconos_->model()->simulation()->indexSet(1);
// calibration
InteractionsGraph::VIterator ui, uiend;
for (boost::tie(ui, uiend) = I1->vertices(); ui != uiend; ++ui)
{
lbdmax = fmax(I1->bundle(*ui)->lambdaOld(1)->getValue(0), lbdmax);
}
for (boost::tie(ui, uiend) = I1->vertices(); ui != uiend; ++ui)
{
interaction = I1->bundle(*ui);
relation = interaction->relation();
lbd = interaction->lambdaOld(1)->getValue(0);
// screen width of interaction
w = lbd / (2 * fmax(lbdmax, 1.)) + .03;
// disk/disk
SP::DynamicalSystem d1 = I1->properties(*ui).source;
SP::DynamicalSystem d2 = I1->properties(*ui).target;
SP::SiconosVector q1 = ask<ForPosition>(*d1);
float x1 = (*q1)(0);
float y1 = (*q1)(1);
float r1 = ask<ForRadius>(*d1);
if (d1 != d2)
{
SP::SiconosVector q2 = ask<ForPosition>(*d2);
float x2 = (*q2)(0);
float y2 = (*q2)(1);
float r2 = ask<ForRadius>(*d2);
float d = hypotf(x1 - x2, y1 - y2);
glPushMatrix();
glColor3f(.0f, .0f, .0f);
drawRec(x1, y1, x1 + (x2 - x1)*r1 / d, y1 + (y2 - y1)*r1 / d, w);
drawRec(x2, y2, x2 + (x1 - x2)*r2 / d, y2 + (y1 - y2)*r2 / d, w);
glPopMatrix();
}
else
{
SP::SiconosMatrix jachq = ask<ForJachq>(*relation);
double jx = jachq->getValue(0, 0);
double jy = jachq->getValue(0, 1);
double dj = hypot(jx, jy);
glPushMatrix();
glColor3f(.0f, .0f, .0f);
drawRec(x1, y1, x1 - r1 * jx / dj, y1 - r1 * jy / dj, w);
glPopMatrix();
}
}
}
for (unsigned int i = 0; i < GETNDS(Siconos_); i++)
{
if (shapes_[i]->selected())
{
drawSelectedQGLShape(*shapes_[i]);
}
else
{
drawQGLShape(*shapes_[i]);
}
}
glColor3f(.45, .45, .45);
glLineWidth(1.);
drawGrid(100, 200);
setGridIsDrawn();
glColor3f(.1, .1, .3);
drawVec(-100, 0, 100, 0);
drawVec(0, -100, 0, 100);
glColor3f(0, 0, 1);
glLineWidth(4.);
if (Siconos_->plans())
{
for (unsigned int i = 0 ; i < Siconos_->plans()->size(0) ; ++i)
{
double A = (*Siconos_->plans())(i, 0);
double B = (*Siconos_->plans())(i, 1);
//double C = (*Siconos_->plans())(i,2);
double xc = (*Siconos_->plans())(i, 3);
double yc = (*Siconos_->plans())(i, 4);
double w = fmin(1e10, (*Siconos_->plans())(i, 5));
double H = hypot(A, B);
if (w == 0) w = 1e10;
// assert ( fabs(A*xc + B*yc + C) <= std::numeric_limits<double>::epsilon() );
drawVec(xc, yc, xc - 0.5 * w * B / H, yc + 0.5 * w * A / H);
drawVec(xc, yc, xc + 0.5 * w * B / H, yc - 0.5 * w * A / H);
}
}
if (Siconos_->movingPlans())
{
double time = Siconos_->model()->currentTime();
for (unsigned int i = 0 ; i < Siconos_->movingPlans()->size1() ; ++i)
{
double A = (*Siconos_->movingPlans())(i, 0)(time);
double B = (*Siconos_->movingPlans())(i, 1)(time);
double C = (*Siconos_->movingPlans())(i, 2)(time);
double w = 1e10;
double H = hypot(A, B);
double xc = 0.;
double yc = 0.;
if (fabs(C) > std::numeric_limits<double>::epsilon())
{
if (A == 0)
// By+C=0
{
yc = -C / B;
}
else if (B == 0)
// Ax+C=0
{
xc = -C / A;
}
else
// Ax+By+C=0
{
if (xc != 0)
yc = - (A * xc + C) / B;
else
xc = - (B * yc + C) / A;
}
}
drawVec(xc, yc, xc - 0.5 * w * B / H, yc + 0.5 * w * A / H);
drawVec(xc, yc, xc + 0.5 * w * B / H, yc - 0.5 * w * A / H);
}
}
glColor3f(.1, .1, .1);
glLineWidth(4.);
QGLViewer::drawArrow(qglviewer::Vec(0, 0, .1), qglviewer::Vec(1, 0, .1), .01, 3);
QGLViewer::drawArrow(qglviewer::Vec(0, 0, .1), qglviewer::Vec(0, 1, .1), .01, 3);
glLineWidth(1.);
for (i = -100; i <= 100; i += 5)
{
sprintf(qs, "%d", i);
// print((float)i,-.8,qs,small_text);
//print(-.8,(float)i,qs,small_text);
drawVec((float)i, -.2, (float)i, .2);
drawVec(-.2, (float)i, .2, (float)i);
}
for (i = -100; i <= 100; i++)
{
drawVec((float)i, -.1, (float)i, .1);
drawVec(-.1, (float)i, .1, (float)i);
}
}
