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 #include "MoreauJeanCombinedProjectionOSI.hpp"
19 #include "Simulation.hpp"
20 #include "LagrangianDS.hpp"
21 #include "NewtonEulerDS.hpp"
22
23 #include "NewtonEulerR.hpp"
24 #include "LagrangianR.hpp"
25 #include "BlockVector.hpp"
26
27 #include "TypeName.hpp"
28 using namespace RELATION;
29
30
31 //#define DEBUG_STDOUT
32 //#define DEBUG_MESSAGES
33 //#define DEBUG_WHERE_MESSAGES
34 #include "siconos_debug.h"
35
MoreauJeanCombinedProjectionOSI(double theta)36 MoreauJeanCombinedProjectionOSI::MoreauJeanCombinedProjectionOSI(double theta) : MoreauJeanOSI(theta)
37 {
38 _levelMinForOutput= 0;
39 _levelMaxForOutput =1;
40 _levelMinForInput =0;
41 _levelMaxForInput =1;
42 //_integratorType = OSI::MOREAUDIRECTPROJECTIONOSI;
43 }
44
45
initializeWorkVectorsForDS(double t,SP::DynamicalSystem ds)46 void MoreauJeanCombinedProjectionOSI::initializeWorkVectorsForDS(double t, SP::DynamicalSystem ds)
47 {
48 DEBUG_BEGIN("MoreauJeanCombinedProjectionOSI::initializeWorkVectorsForDS( double t, SP::DynamicalSystem ds) \n");
49
50 MoreauJeanOSI::initializeWorkVectorsForDS(t, ds);
51
52 const DynamicalSystemsGraph::VDescriptor& dsv = _dynamicalSystemsGraph->descriptor(ds);
53 VectorOfVectors& workVectors = *_dynamicalSystemsGraph->properties(dsv).workVectors;
54 Type::Siconos dsType = Type::value(*ds);
55 if(dsType == Type::LagrangianDS || dsType == Type::LagrangianLinearTIDS)
56 {
57 SP::LagrangianDS d = std::static_pointer_cast<LagrangianDS> (ds);
58 workVectors[MoreauJeanOSI::QTMP].reset(new SiconosVector(d->dimension()));
59 }
60 else if(dsType == Type::NewtonEulerDS)
61 {
62 SP::NewtonEulerDS d = std::static_pointer_cast<NewtonEulerDS>(ds);
63 workVectors[MoreauJeanOSI::QTMP].reset(new SiconosVector(d->getqDim()));
64 }
65 else
66 {
67 THROW_EXCEPTION("MoreauJeanCombinedProjectionOSI::initialize() - DS not of the right type");
68 }
69 for(unsigned int k = _levelMinForInput ; k < _levelMaxForInput + 1; k++)
70 {
71 ds->initializeNonSmoothInput(k);
72 }
73
74 DEBUG_END("MoreauJeanCombinedProjectionOSI::initializeWorkVectorsForDS( double t, SP::DynamicalSystem ds) \n");
75 }
76
initializeWorkVectorsForInteraction(Interaction & inter,InteractionProperties & interProp,DynamicalSystemsGraph & DSG)77 void MoreauJeanCombinedProjectionOSI::initializeWorkVectorsForInteraction(Interaction &inter, InteractionProperties& interProp,
78 DynamicalSystemsGraph & DSG)
79 {
80 DEBUG_BEGIN("MoreauJeanCombinedProjectionOSI::initializeWorkVectorsForInteraction(Interaction &inter, InteractionProperties& interProp, DynamicalSystemsGraph & DSG)\n");
81
82 MoreauJeanOSI::initializeWorkVectorsForInteraction(inter, interProp,DSG);
83
84 SP::DynamicalSystem ds1= interProp.source;
85 SP::DynamicalSystem ds2= interProp.target;
86 assert(ds1);
87 assert(ds2);
88 VectorOfBlockVectors& DSlink = inter.linkToDSVariables();
89
90 Relation &relation = *inter.relation();
91 RELATION::TYPES relationType = relation.getType();
92
93 unsigned int p0 =0;
94 if(relationType == Lagrangian)
95 {
96 p0 = LagrangianR::p0;
97 }
98 else if(relationType == NewtonEuler)
99 {
100 p0 = NewtonEulerR::p0;
101 }
102 if(ds1 != ds2)
103 {
104 DEBUG_PRINT("ds1 != ds2\n");
105 if((!DSlink[p0]) || (DSlink[p0]->numberOfBlocks() !=2))
106 DSlink[p0].reset(new BlockVector(2));
107 }
108 else
109 {
110 if((!DSlink[p0]) || (DSlink[p0]->numberOfBlocks() !=1))
111 DSlink[p0].reset(new BlockVector(1));
112 }
113
114 if(checkOSI(DSG.descriptor(ds1)))
115 {
116 DEBUG_PRINTF("ds1->number() %i is taken into account\n", ds1->number());
117 assert(DSG.properties(DSG.descriptor(ds1)).workVectors);
118 if(relationType == Lagrangian)
119 {
120 LagrangianDS& lds = *std::static_pointer_cast<LagrangianDS> (ds1);
121 DSlink[p0]->setVectorPtr(0,lds.p(0));
122 }
123 else if(relationType == NewtonEuler)
124 {
125 NewtonEulerDS& neds = *std::static_pointer_cast<NewtonEulerDS> (ds1);
126 DSlink[p0]->setVectorPtr(0,neds.p(0));
127 }
128 }
129 DEBUG_PRINTF("ds1->number() %i\n",ds1->number());
130 DEBUG_PRINTF("ds2->number() %i\n",ds2->number());
131
132 if(ds1 != ds2)
133 {
134 DEBUG_PRINT("ds1 != ds2\n");
135 if(checkOSI(DSG.descriptor(ds2)))
136 {
137 DEBUG_PRINTF("ds2->number() %i is taken into account\n",ds2->number());
138 assert(DSG.properties(DSG.descriptor(ds2)).workVectors);
139 if(relationType == Lagrangian)
140 {
141 LagrangianDS& lds = *std::static_pointer_cast<LagrangianDS> (ds2);
142 DSlink[p0]->setVectorPtr(1,lds.p(0));
143 }
144 else if(relationType == NewtonEuler)
145 {
146 NewtonEulerDS& neds = *std::static_pointer_cast<NewtonEulerDS> (ds2);
147 DSlink[p0]->setVectorPtr(1,neds.p(0));
148 }
149 }
150 }
151
152
153
154 DEBUG_END("MoreauJeanCombinedProjectionOSI::initializeWorkVectorsForInteraction(Interaction &inter, InteractionProperties& interProp, DynamicalSystemsGraph & DSG)\n");
155
156
157 }
158
159
addInteractionInIndexSet(SP::Interaction inter,unsigned int i)160 bool MoreauJeanCombinedProjectionOSI::addInteractionInIndexSet(SP::Interaction inter, unsigned int i)
161 {
162 assert(i == 1 || i == 2);
163 //double h = _simulation->timeStep();
164 if(i == 1) // index set for resolution at the velocity
165 {
166 double y = (inter->y(0))->getValue(0); // y(0) is the position
167 DEBUG_PRINTF("MoreauJeanCombinedProjectionOSI::addInteractionInIndexSet yref=%e \n", y);
168 DEBUG_EXPR(
169 if(y <= 0)
170 printf("MoreauJeanCombinedProjectionOSI::addInteractionInIndexSet ACTIVATE in indexSet level = %i.\n", i);
171 )
172 return (y <= 0);
173 }
174 else if(i == 2) // special index for the projection
175 {
176 DEBUG_EXPR(
177 double lambda = 0;
178 lambda = (inter->lambda(1))->getValue(0); // lambda(1) is the contact impulse for MoreauJeanOSI scheme
179 printf("MoreauJeanCombinedProjectionOSI::addInteractionInIndexSet lambdaref=%e \n", lambda);
180 if(lambda > 0)
181 printf("MoreauJeanCombinedProjectionOSI::addInteractionInIndexSet ACTIVATE in indexSet level = %i.\n", i);
182 )
183 // return (lambda > 0);
184 return true;
185 }
186 else
187 {
188 return false;
189 }
190 }
191
192
removeInteractionFromIndexSet(SP::Interaction inter,unsigned int i)193 bool MoreauJeanCombinedProjectionOSI::removeInteractionFromIndexSet(SP::Interaction inter, unsigned int i)
194 {
195 assert(0);
196 return(0);
197 }
198
199