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 "LagrangianLinearTIDS.hpp"
19 #include "SimpleMatrixFriends.hpp" // for a*mat
20 #include "SiconosAlgebraProd.hpp" // for matrix-vector prod
21 #include "BlockMatrix.hpp"
22 // #define DEBUG_STDOUT
23 // #define DEBUG_MESSAGES
24 #include "siconos_debug.h"
25
26 #include <iostream>
27
28 // --- Constructor from a initial conditions and matrix-operators
LagrangianLinearTIDS(SP::SiconosVector newQ0,SP::SiconosVector newVelocity0,SP::SiconosMatrix newMass,SP::SiconosMatrix newK,SP::SiconosMatrix newC)29 LagrangianLinearTIDS::LagrangianLinearTIDS(SP::SiconosVector newQ0, SP::SiconosVector newVelocity0,
30 SP::SiconosMatrix newMass, SP::SiconosMatrix newK, SP::SiconosMatrix newC):
31 LagrangianDS(newQ0, newVelocity0, newMass)
32 {
33 assert((newK->size(0) == _ndof && newK->size(1) == _ndof) &&
34 "LagrangianLinearTIDS - constructor from data, inconsistent size between K and _ndof");
35
36 assert((newC->size(0) == _ndof && newC->size(1) == _ndof) &&
37 "LagrangianLinearTIDS - constructor from data, inconsistent size between C and ndof");
38
39 _K = newK;
40 _C = newC;
41 }
42
initRhs(double time)43 void LagrangianLinearTIDS::initRhs(double time)
44 {
45 // _rhsMatrices.resize(numberOfRhsMatrices);
46 // // Copy of Mass into _workMatrix for LU-factorization.
47 // _inverseMass.reset(new SimpleMatrix(*_mass));
48
49 // // compute x[1] (and thus _fExt if required)
50 // computeRhs(time);
51
52 LagrangianDS::initRhs(time);
53
54 // jacobianRhsx
55 if(_K)
56 {
57 // bloc10 of jacobianX is solution of Mass*Bloc10 = K
58 if(!_rhsMatrices[jacobianXBloc10])
59 _rhsMatrices[jacobianXBloc10].reset(new SimpleMatrix(-1 * *_K));
60 _inverseMass->Solve(*_rhsMatrices[jacobianXBloc10]);
61 }
62 else
63 _rhsMatrices[jacobianXBloc10] = _rhsMatrices[zeroMatrix] ;
64
65 if(_C)
66 {
67 // bloc11 of jacobianX is solution of Mass*Bloc11 = C
68 if(!_rhsMatrices[jacobianXBloc11])
69 _rhsMatrices[jacobianXBloc11].reset(new SimpleMatrix(-1 * *_C));
70 _inverseMass->Solve(*_rhsMatrices[jacobianXBloc11]);
71 }
72 else
73 _rhsMatrices[jacobianXBloc11] = _rhsMatrices[zeroMatrix] ;
74
75 if(_C || _K)
76 _jacxRhs.reset(new BlockMatrix(_rhsMatrices[zeroMatrix], _rhsMatrices[idMatrix],
77 _rhsMatrices[jacobianXBloc10], _rhsMatrices[jacobianXBloc11]));
78
79 }
80
setK(const SiconosMatrix & newValue)81 void LagrangianLinearTIDS::setK(const SiconosMatrix& newValue)
82 {
83 if(newValue.size(0) != _ndof || newValue.size(1) != _ndof)
84 THROW_EXCEPTION("LagrangianLinearTIDS - setK: inconsistent input matrix size ");
85
86 if(!_K)
87 _K.reset(new SimpleMatrix(newValue));
88 else
89 *_K = newValue;
90 }
91
setKPtr(SP::SiconosMatrix newPtr)92 void LagrangianLinearTIDS::setKPtr(SP::SiconosMatrix newPtr)
93 {
94 if(newPtr->size(0) != _ndof || newPtr->size(1) != _ndof)
95 THROW_EXCEPTION("LagrangianLinearTIDS - setKPtr: inconsistent input matrix size ");
96 _K = newPtr;
97 }
98
setC(const SiconosMatrix & newValue)99 void LagrangianLinearTIDS::setC(const SiconosMatrix& newValue)
100 {
101 if(newValue.size(0) != _ndof || newValue.size(1) != _ndof)
102 THROW_EXCEPTION("LagrangianLinearTIDS - setC: inconsistent input matrix size ");
103
104 if(!_C)
105 _C.reset(new SimpleMatrix(newValue));
106 else
107 *_C = newValue;
108 }
109
setCPtr(SP::SiconosMatrix newPtr)110 void LagrangianLinearTIDS::setCPtr(SP::SiconosMatrix newPtr)
111 {
112 if(newPtr->size(0) != _ndof || newPtr->size(1) != _ndof)
113 THROW_EXCEPTION("LagrangianLinearTIDS - setCPtr: inconsistent input matrix size ");
114
115 _C = newPtr;
116 }
117
display(bool brief) const118 void LagrangianLinearTIDS::display(bool brief) const
119 {
120 LagrangianDS::display();
121 std::cout << "===== Lagrangian Linear Time Invariant System display ===== " <<std::endl;
122 std::cout << "- Mass Matrix M : " <<std::endl;
123 if(_mass) _mass->display();
124 else std::cout << "-> nullptr" <<std::endl;
125 std::cout << "- Stiffness Matrix K : " <<std::endl;
126 if(_K) _K->display();
127 else std::cout << "-> nullptr" <<std::endl;
128 std::cout << "- Viscosity Matrix C : " <<std::endl;
129 if(_C) _C->display();
130 else std::cout << "-> nullptr" <<std::endl;
131 std::cout << "=========================================================== " <<std::endl;
132 }
133
computeForces(double time,SP::SiconosVector q2,SP::SiconosVector v2)134 void LagrangianLinearTIDS::computeForces(double time, SP::SiconosVector q2, SP::SiconosVector v2)
135 {
136 DEBUG_PRINT("LagrangianLinearTIDS::computeForces(double time, SP::SiconosVector q2, SP::SiconosVector v2) \n");
137
138 if(!_forces)
139 {
140 _forces.reset(new SiconosVector(_ndof));
141 }
142 else
143 _forces->zero();
144
145 if(_fExt)
146 {
147 computeFExt(time);
148 *_forces += *_fExt;
149 }
150 if(_K)
151 *_forces -= prod(*_K, *q2);
152 if(_C)
153 *_forces -= prod(*_C, *v2);
154 }
155
156