1 /* Siconos is a program dedicated to modeling, simulation and control
2 * of non smooth dynamical systems.
3 *
4 * Copyright 2021 INRIA.
5 *
6 * Licensed under the Apache License, Version 2.0 (the "License");
7 * you may not use this file except in compliance with the License.
8 * You may obtain a copy of the License at
9 *
10 * http://www.apache.org/licenses/LICENSE-2.0
11 *
12 * Unless required by applicable law or agreed to in writing, software
13 * distributed under the License is distributed on an "AS IS" BASIS,
14 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
15 * See the License for the specific language governing permissions and
16 * limitations under the License.
17 */
18
19 #include "MatrixIntegrator.hpp"
20 #include "SiconosAlgebra.hpp"
21 #include "FirstOrderLinearTIDS.hpp"
22 #include "EventDriven.hpp"
23 #include "SubPluggedObject.hpp"
24 #include "LsodarOSI.hpp"
25 #include "TimeDiscretisation.hpp"
26 #include "NonSmoothDynamicalSystem.hpp"
27 #include "EventsManager.hpp"
28
29 //#define DEBUG_WHERE_MESSAGES
30
31 // #define DEBUG_NOCOLOR
32 // #define DEBUG_STDOUT
33 // #define DEBUG_MESSAGES
34
35 #include "siconos_debug.h"
36
MatrixIntegrator(const DynamicalSystem & ds,const NonSmoothDynamicalSystem & nsds,const TimeDiscretisation & td,SP::SiconosMatrix E)37 MatrixIntegrator::MatrixIntegrator(const DynamicalSystem& ds, const NonSmoothDynamicalSystem& nsds, const TimeDiscretisation & td, SP::SiconosMatrix E): _E(E)
38 {
39 commonInit(ds, nsds, td);
40 _mat.reset(new SimpleMatrix(*E));
41 _mat->zero();
42 }
43
MatrixIntegrator(const DynamicalSystem & ds,const NonSmoothDynamicalSystem & nsds,const TimeDiscretisation & td,SP::PluggedObject plugin,const unsigned int p)44 MatrixIntegrator::MatrixIntegrator(const DynamicalSystem& ds, const NonSmoothDynamicalSystem& nsds, const TimeDiscretisation & td, SP::PluggedObject plugin, const unsigned int p):
45 _plugin(plugin)
46 {
47 commonInit(ds, nsds, td);
48 unsigned int n = ds.n();
49 _mat.reset(new SimpleMatrix(n, p, 0));
50 _spo.reset(new SubPluggedObject(*_plugin, n, p));
51 std::static_pointer_cast<FirstOrderLinearDS>(_DS)->setPluginB(_spo);
52 _isConst = false;
53 }
54
MatrixIntegrator(const DynamicalSystem & ds,const NonSmoothDynamicalSystem & nsds,const TimeDiscretisation & td)55 MatrixIntegrator::MatrixIntegrator(const DynamicalSystem& ds, const NonSmoothDynamicalSystem& nsds, const TimeDiscretisation & td)
56 {
57 unsigned int n = ds.n();
58 _mat.reset(new SimpleMatrix(n, n, 0));
59 commonInit(ds, nsds, td);
60 }
61
commonInit(const DynamicalSystem & ds,const NonSmoothDynamicalSystem & nsds,const TimeDiscretisation & td)62 void MatrixIntegrator::commonInit(const DynamicalSystem& ds, const NonSmoothDynamicalSystem& nsds, const TimeDiscretisation & td)
63 {
64 _TD.reset(new TimeDiscretisation(td));
65
66 DEBUG_EXPR(ds.display(););
67
68
69 Type::Siconos dsType = Type::value(ds);
70 if(dsType == Type::FirstOrderLinearTIDS)
71 {
72 _DS.reset(new FirstOrderLinearTIDS(static_cast<const FirstOrderLinearTIDS&>(ds)));
73 _isConst = _TD->hConst();
74 }
75 else if(dsType == Type::FirstOrderLinearDS)
76 {
77 const FirstOrderLinearDS& cfolds = static_cast<const FirstOrderLinearDS&>(ds);
78 _DS.reset(new FirstOrderLinearDS(cfolds));
79 // std::static_pointer_cast<FirstOrderLinearDS>(_DS)->zeroPlugin();
80 if(cfolds.getPluginA()->isPlugged())
81 {
82 std::static_pointer_cast<FirstOrderLinearDS>(_DS)->setPluginA(cfolds.getPluginA());
83 }
84 _isConst = (_TD->hConst()) && !(cfolds.getPluginA()->isPlugged()) ? true : false;
85 }
86
87 _DS->setNumber(9999999);
88 DEBUG_EXPR(_DS->display(););
89 // integration stuff
90 _nsds.reset(new NonSmoothDynamicalSystem(nsds.t0(), nsds.finalT()));
91
92 _OSI.reset(new LsodarOSI());
93 _nsds->insertDynamicalSystem(_DS);
94 _sim.reset(new EventDriven(_nsds, _TD, 0));
95 _sim->associate(_OSI, _DS);
96 _sim->setName("Matrix integrator simulation");
97 //change tolerance
98 _OSI->setTol(1, 10 * MACHINE_PREC, 5 * MACHINE_PREC);
99
100 }
101
integrate()102 void MatrixIntegrator::integrate()
103 {
104 DEBUG_BEGIN("MatrixIntegrator::integrate()\n");
105 SiconosVector& x0 = *_DS->x0();
106 SiconosVector& x = *_DS->x();
107
108 SP::SiconosVector Ecol = static_cast<FirstOrderLinearDS&>(*_DS).b();
109 if(!Ecol && _E)
110 {
111 Ecol.reset(new SiconosVector(_DS->n(), 0));
112 static_cast<FirstOrderLinearDS&>(*_DS).setbPtr(Ecol);
113 }
114 unsigned int p = _mat->size(1);
115 for(unsigned int i = 0; i < p; i++)
116 {
117 x0.zero();
118 if(_E)
119 _E->getCol(i, *Ecol);
120 else if(_plugin)
121 _spo->setIndex(i);
122 else
123 x0(i) = 1;
124
125 //Reset LsodarOSI
126 //_OSI->setIsInitialized(false);
127 _DS->resetToInitialState();
128 _sim->setIstate(1);
129 _sim->advanceToEvent();
130 _mat->setCol(i, x);
131 }
132
133 _sim->processEvents();
134 //_DS->resetToInitialState();
135
136 DEBUG_EXPR(_mat->display(););
137 DEBUG_EXPR(_DS->display(););
138 DEBUG_END("MatrixIntegrator::integrate()\n");
139 }
140