MBDyn-1.7.3
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Groups
module-nonsmooth-node.cc
Go to the documentation of this file.
1 /* $Header: /var/cvs/mbdyn/mbdyn/mbdyn-1.0/modules/module-nonsmooth-node/module-nonsmooth-node.cc,v 1.17 2017/01/12 14:55:58 masarati Exp $ */
2 /*
3  * MBDyn (C) is a multibody analysis code.
4  * http://www.mbdyn.org
5  *
6  * Copyright (C) 1996-2017
7  *
8  * Pierangelo Masarati <masarati@aero.polimi.it>
9  *
10  * Dipartimento di Ingegneria Aerospaziale - Politecnico di Milano
11  * via La Masa, 34 - 20156 Milano, Italy
12  * http://www.aero.polimi.it
13  *
14  * Changing this copyright notice is forbidden.
15  *
16  * This program is free software; you can redistribute it and/or modify
17  * it under the terms of the GNU General Public License as published by
18  * the Free Software Foundation (version 2 of the License).
19  *
20  *
21  * This program is distributed in the hope that it will be useful,
22  * but WITHOUT ANY WARRANTY; without even the implied warranty of
23  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
24  * GNU General Public License for more details.
25  *
26  * You should have received a copy of the GNU General Public License
27  * along with this program; if not, write to the Free Software
28  * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
29  */
30 /*
31  * Author: Matteo Fancello <matteo.fancello@gmail.com>
32  * Nonsmooth dynamics element;
33  * uses SICONOS <http://siconos.gforge.inria.fr/>
34  */
35 
36 #include "mbconfig.h" /* This goes first in every *.c,*.cc file */
37 #include <iostream>
38 #include <cfloat>
39 #include "dataman.h"
40 #include "userelem.h"
41 #include "stlvh.h"
42 
43 #include "mbdyn_siconos.h"
44 
45 
46 enum Constraining {
47  // the interaction with MBDyn results
48  // in imposing the position of the node associated
49  // to the nonsmooth integration and in the force Lambda exchanged
50  POSITION_ONLY,
51 
52  // the interaction with MBDyn results
53  // in imposing the velocity of the node associated
54  // to the nonsmooth integration and in the force Lambda exchanged
55  VELOCITY_ONLY,
56 
57  // the interaction with MBDyn results
58  // in imposing the position and velocity of the node associated
59  // to the nonsmooth integration and in the force Lambda exchanged
60  POSITION_AND_VELOCITY
61 };
62 
63 struct plane {
64  // Definizione del piano di vincolo:
65  // nodo alla cui rotazione รจ ancorato, punto per cui passa, rotazione relativa al sys di rife del nodo
66  const StructNode *AttNode;
67  Vec3 Point;
68  Mat3x3 RotRel;
69 
70  // H: relazione tra posizione e reazione, al passo k e k+1
71  Mat3x3 H_k;
72  Mat3x3 H_kp1;
73 
74  doublereal Gap;
75  doublereal e_rest;
76  doublereal mu;
77 };
78 
79 struct NS_subsys {
80  // Contains all the parameters of the nonsmooth system and its actual status
81 
82  // ---- NONSMOOTH SUBSYSTEM STATUS--------
83  Vec3 Xns_k, Xns_kp1; // NonSmooth node global position at time k, k+1
84  Vec3 Vns_k, Vns_kp1; // NonSmooth node global velocity at time k
85  Vec3 Lambda_k, Lambda_kp1; // Lagrange multiplier for node 1 at step k and k+1
86 
87  Vec3 GravityAcceleration;
88  bool bGravity;
89  doublereal hstep;
90 
91  // ---- NONSMOOTH SUBSYSTEM INPUT PARAMETERS -----
92  doublereal mass_ns, radius_ns;
93  int Np; // number of planes which constitute a unilateral constraint each
94  std::vector<plane> constr;
95 
96  doublereal theta_ts, gamma_pred; // integration step
97  solver_parameters solparam; // solver parameters: tolerance, max iterations,
98 
99  Constraining constraint_type;
100  int iterations;
101  bool bVerbose;
102 
103  // ---- NONSMOOTH SUBSYSTEM DEBUG AND OUTPUT -----
104 
105  double pkp1; // norm of the reaion impulse
106 // double bLCP, WNN, pkp1, Gap, Pkplus1, Un_k, Un_kp1, vfree;
107  Vec3 mu;
108  int ia; // size of the vector of the active constraints
109 
110  Vec3 Piter;
111  int niter;
112  int niterLCPmax;
113  Vec3 Poutput;
114 };
115 
116 class ModuleNonsmoothNode
117  : virtual public Elem, public UserDefinedElem
118 {
119 private:
120  const DataManager *m_pDM;
121 
122  // Node associated to the nonsmooth subsystem
123  const StructDispNode *m_pNode;
124 
125  // all the parameters of the nonsmooth subsystem and the updated variables
126  NS_subsys NS_data;
127 
128  // It allows to know if step changes (for the bVerbose output mainly)
129  bool bStepToggle;
130 
131  // Constraint relaxation variable
132  Vec3 Mu;
133 
134  // Input parameter to set a frictional problem
135  bool bFrictional;
136 
137  // iteration number
138  int iIter;
139 
140  // function timestepping a frictionless subsys
141  void mbs_get_force(NS_subsys&);
142 
143  // function timestepping a frictional subsys
144  void mbs_get_force_frictional(NS_subsys&);
145 
146 public:
147  ModuleNonsmoothNode(unsigned uLabel, const DofOwner *pDO,
148  DataManager* pDM, MBDynParser& HP);
149  virtual ~ModuleNonsmoothNode(void);
150 
151  /* inherited from SimulationEntity */
152  virtual unsigned int iGetNumDof(void) const;
153 
154 #if 0 // TODO
155  virtual std::ostream& DescribeDof(std::ostream& out,
156  const char *prefix = "", bool bInitial = false) const;
157  virtual void DescribeDof(std::vector<std::string>& desc,
158  bool bInitial = false, int i = -1) const;
159  virtual std::ostream& DescribeEq(std::ostream& out,
160  const char *prefix = "", bool bInitial = false) const;
161  virtual void DescribeEq(std::vector<std::string>& desc,
162  bool bInitial = false, int i = -1) const;
163 #endif
164 
165  virtual DofOrder::Order GetDofType(unsigned int) const;
166 
167  virtual void Output(OutputHandler& OH) const;
168  virtual void WorkSpaceDim(integer* piNumRows, integer* piNumCols) const;
169  virtual void AfterPredict(VectorHandler& X, VectorHandler& XP);
170  virtual void AfterConvergence(const VectorHandler& X,
171  const VectorHandler& XP);
172  VariableSubMatrixHandler&
173  AssJac(VariableSubMatrixHandler& WorkMat,
174  doublereal dCoef,
175  const VectorHandler& XCurr,
176  const VectorHandler& XPrimeCurr);
177  SubVectorHandler&
178  AssRes(SubVectorHandler& WorkVec,
179  doublereal dCoef,
180  const VectorHandler& XCurr,
181  const VectorHandler& XPrimeCurr);
182  unsigned int iGetNumPrivData(void) const;
183  int iGetNumConnectedNodes(void) const;
184  void GetConnectedNodes(std::vector<const Node *>& connectedNodes) const;
185  void SetValue(DataManager *pDM, VectorHandler& X, VectorHandler& XP,
186  SimulationEntity::Hints *ph);
187  std::ostream& Restart(std::ostream& out) const;
188  virtual unsigned int iGetInitialNumDof(void) const;
189  virtual void
190  InitialWorkSpaceDim(integer* piNumRows, integer* piNumCols) const;
191  VariableSubMatrixHandler&
192  InitialAssJac(VariableSubMatrixHandler& WorkMat,
193  const VectorHandler& XCurr);
194  SubVectorHandler&
195  InitialAssRes(SubVectorHandler& WorkVec, const VectorHandler& XCurr);
196 };
197 
198 // number of facets of the discretized friction cone
199 static const int omegan = 16;
200 
201 // Creo matrici fullmh IPa e IRa contenenti i vettori delle direzioni delle facets del friction cone outer discretized
202 static const doublereal dIPa[2][omegan - 2] = {
203  { 1., 0. },
204  { 0., 1. }
205 };
206 
207 // dIRa[2][omegan - 2]
208 static const doublereal dIRa[2][omegan - 2] = {
209  { 0.92388, 0.70711, 0.38268, -0.38268, -0.70711, -0.92388, -1.00000, -0.92388, -0.70711, -0.38268, -0.00000, 0.38268, 0.70711, 0.92388 },
210  { 0.38268, 0.70711, 0.92388, 0.92388, 0.70711, 0.38268, 0.00000, -0.38268, -0.70711, -0.92388, -1.00000, -0.92388, -0.70711, -0.38268 }
211 };
212 
213 static FullMatrixHandler IRa;
214 static FullMatrixHandler IRat;
215 static FullMatrixHandler IPa;
216 static bool bIRa(false);
217 
218 
219 ModuleNonsmoothNode::ModuleNonsmoothNode( unsigned uLabel, const DofOwner *pDO,
220  DataManager* pDM, MBDynParser& HP)
221 : Elem(uLabel, flag(0)),
222 UserDefinedElem(uLabel, pDO),
223 m_pDM(pDM),
224 m_pNode(0),
225 bStepToggle(false),
226 Mu(::Zero3),
227 bFrictional(false),
228 iIter(0)
229 {
230  NS_data.bVerbose = false;
231  NS_data.niter = 0;
232  NS_data.Piter = ::Zero3;
233 
234  // help
235  if (HP.IsKeyWord("help")) {
236  silent_cout(
237 "\n"
238 "Module: nonsmooth-node\n"
239 "Author: Matteo Fancello <matteo.fancello@gmail.com>\n"
240 " Pierangelo Masarati <masarati@aero.polimi.it>\n"
241 "Organization: Dipartimento di Ingegneria Aerospaziale\n"
242 " Politecnico di Milano\n"
243 " http://www.aero.polimi.it/\n"
244 "\n"
245 " All rights reserved\n"
246 "\n"
247 " Defines a unilateral constraint in form of a contact\n"
248 " between a node and one or more planes, optionally with friction.\n"
249 "\n"
250 " # in the \"control data\" block: increase by 1 the count of \"loadable elements\"\n"
251 "\n"
252 " # in the \"elements\" block: add a user-defined element according to the syntax\n"
253 " user defined :\n"
254 " <element_label> , nonsmooth node ,\n"
255 " [ frictional , { yes | no | (bool) <frictional> } , ]\n"
256 " (StructDispNode) <NonsmoothNODELABEL> ,\n"
257 " mass , (real) <mass> , radius , (real) <radius> ,\n"
258 " planes , (int) <number_of_planes> ,\n"
259 " <PlaneDefiningNODELABEL> ,\n"
260 " position ,\n"
261 " (Vec3) <relative_plane_position> ,\n"
262 " rotation orientation ,\n"
263 " (OrientationMatrix) <rel_rot_orientation_1> ,\n"
264 " restitution , (real) <rest_coeff>\n"
265 " [ , frictional coefficient , <mu> ]\n"
266 " [ , <other planes> ]\n"
267 " [ , constraint type , { position | velocity | both } ] # default: both\n"
268 " [ , theta , <theta> ] [ , gamma , <gamma> ]\n"
269 " [ , LCP solver , <solver> ]\n"
270 " [ , tolerance , <tolerance> ][ , max iterations , <num_iter> ]\n"
271 " # these options depend on LCP solver support, see\n"
272 " # http://siconos.gforge.inria.fr/Numerics/LCPSolvers.html\n"
273 " [ , limit iterations , <niterations> ]\n"
274 " [ , limit LCP iterations , <niterations> ]\n"
275 " [ , verbose , { yes | no | (bool) <verbose> } ] ;\n"
276 "\n"
277 " <solver> ::= lexico lemke # the default\n"
278 " | rpgs\n"
279 " | qp\n"
280 " | cpg\n"
281 " | pgs\n"
282 " | psor\n"
283 " | nsqp\n"
284 " | latin\n"
285 " | latin_w\n"
286 " | newton_min\n"
287 " | newton_FB\n"
288 "\n"
289 " OUTPUT:\n"
290 " 1: element label\n"
291 " 2-4: impulse on NonSmooth node in global ref. frame\n"
292 " 5-7: position of NonSmooth node in global ref. frame\n"
293 " 8-10: velocity of Nonsmooth node in global ref. frame\n"
294 " 11-13: Lambda: constr. reaction between mbs node and NS node\n"
295 " 14: p: norm of the impulse reaction in global ref. frame\n"
296 " 15: Ia: number of active constraints in this step\n"
297 "\n"
298 " if verbose, also:\n"
299 " 16-18: Mu: constraint relaxation factor\n"
300 " 19: LCPsolver status: 0 -> convergence, 1 -> iter=maxiter, >1 -> fail\n"
301 " (only for some solvers)\n"
302 " 20: LCPsolver resulting_error\n"
303 " (only for some solvers)\n"
304  << std::endl);
305 
306 
307  if (!HP.IsArg()) {
308  /*
309  * Exit quietly if nothing else is provided
310  */
311  throw NoErr(MBDYN_EXCEPT_ARGS);
312  }
313  }
314 
315  m_pNode = pDM->ReadNode<const StructDispNode, Node::STRUCTURAL>(HP);
316 
317  if (!HP.IsKeyWord("mass")) {
318  silent_cerr("ModuleNonsmoothNode(" << uLabel << "): \"mass\" expected at line "
319  << HP.GetLineData() << std::endl);
320  throw ErrGeneric(MBDYN_EXCEPT_ARGS);
321  }
322 
323  NS_data.mass_ns = HP.GetReal();
324  if (NS_data.mass_ns <= std::numeric_limits<doublereal>::epsilon()) {
325  silent_cerr("ModuleNonsmoothNode(" << uLabel << "): \"mass\" too small at line "
326  << HP.GetLineData() << std::endl);
327  throw ErrGeneric(MBDYN_EXCEPT_ARGS);
328  }
329 
330  if (!HP.IsKeyWord("radius")) {
331  silent_cerr("ModuleNonsmoothNode(" << uLabel << "): \"radius\" expected at line "
332  << HP.GetLineData() << std::endl);
333  throw ErrGeneric(MBDYN_EXCEPT_ARGS);
334  }
335 
336  NS_data.radius_ns = HP.GetReal();
337 
338  if (!HP.IsKeyWord("planes")) {
339  silent_cerr("ModuleNonsmoothNode(" << uLabel << "): \"planes\" expected at line "
340  << HP.GetLineData() << std::endl);
341  throw ErrGeneric(MBDYN_EXCEPT_ARGS);
342  }
343 
344  NS_data.Np = HP.GetInt();
345  NS_data.constr.resize(NS_data.Np);
346 
347  for (int i = 0; i < NS_data.Np; i++) {
348  NS_data.constr[i].AttNode = pDM->ReadNode<const StructNode, Node::STRUCTURAL>(HP);
349 
350  ReferenceFrame RFnode(NS_data.constr[i].AttNode);
351 
352  if (!HP.IsKeyWord("position")) {
353  silent_cerr("ModuleNonsmoothNode(" << uLabel << "): \"position\" expected at line "
354  << HP.GetLineData() << std::endl);
355  throw ErrGeneric(MBDYN_EXCEPT_ARGS);
356  }
357 
358  NS_data.constr[i].Point = HP.GetPosRel(RFnode);
359 
360  if (!HP.IsKeyWord("rotation" "orientation")) {
361  silent_cerr("ModuleNonsmoothNode(" << uLabel << "): \"rotation orientation \" expected at line "
362  << HP.GetLineData() << std::endl);
363  throw ErrGeneric(MBDYN_EXCEPT_ARGS);
364  }
365 
366  NS_data.constr[i].RotRel = HP.GetRotRel(RFnode);
367 
368  if (!HP.IsKeyWord("restitution")) {
369  silent_cerr("ModuleNonsmoothNode(" << uLabel << "): \"restitution \" expected at line "
370  << HP.GetLineData() << std::endl);
371  throw ErrGeneric(MBDYN_EXCEPT_ARGS);
372  }
373 
374  NS_data.constr[i].e_rest = HP.GetReal();
375 
376  if (HP.IsKeyWord("friction" "coefficient")) {
377  bFrictional = true;
378  NS_data.constr[i].mu = HP.GetReal();
379 
380  } else if (bFrictional) {
381  silent_cerr("ModuleNonsmoothNode(" << uLabel << "): no friction coefficient for plane #" << i << " of " << NS_data.Np << " at line "
382  << HP.GetLineData() << std::endl);
383  }
384  }
385 
386  NS_data.constraint_type = POSITION_AND_VELOCITY;
387 
388  if (HP.IsKeyWord("constraint" "type")) {
389  if (HP.IsKeyWord("position")) {
390  NS_data.constraint_type = POSITION_ONLY;
391  } else if (HP.IsKeyWord("velocity")) {
392  NS_data.constraint_type = VELOCITY_ONLY;
393  } else if (HP.IsKeyWord("both")) {
394  NS_data.constraint_type = POSITION_AND_VELOCITY;
395  } else {
396  silent_cerr("ModuleNonsmoothNode: invalid constraint_type"
397  << " at line " << HP.GetLineData() << std::endl);
398  throw ErrGeneric(MBDYN_EXCEPT_ARGS);
399  }
400  }
401 
402  if (HP.IsKeyWord("theta")) {
403  NS_data.theta_ts = HP.GetReal();
404 
405  } else {
406  NS_data.theta_ts = 0.5;
407  }
408 
409  if (HP.IsKeyWord("gamma")) {
410  NS_data.gamma_pred = HP.GetReal();
411 
412  } else {
413  NS_data.gamma_pred = 1.;
414  }
415 
416  // default: LEXICO_LEMKE
417  NS_data.solparam.solver = LEXICO_LEMKE;
418  if (HP.IsKeyWord("LCP" "solver")) {
419  if (HP.IsKeyWord("lexico" "lemke")) {
420  NS_data.solparam.solver = LEXICO_LEMKE;
421  } else if (HP.IsKeyWord("rpgs")) {
422  NS_data.solparam.solver = RPGS;
423  } else if (HP.IsKeyWord("qp")) {
424  NS_data.solparam.solver = QP;
425  } else if (HP.IsKeyWord("cpg")) {
426  NS_data.solparam.solver = CPG;
427  } else if (HP.IsKeyWord("pgs")) {
428  NS_data.solparam.solver = PGS;
429  } else if (HP.IsKeyWord("psor")) {
430  NS_data.solparam.solver = PSOR;
431  } else if (HP.IsKeyWord("nsqp")) {
432  NS_data.solparam.solver = NSQP;
433  } else if (HP.IsKeyWord("latin")) {
434  NS_data.solparam.solver = LATIN;
435  } else if (HP.IsKeyWord("latin_w")) {
436  NS_data.solparam.solver = LATIN_W;
437  } else if (HP.IsKeyWord("newton_min")) {
438  NS_data.solparam.solver = NEWTON_MIN;
439  } else if (HP.IsKeyWord("newton_FB")) {
440  NS_data.solparam.solver = NEWTON_FB;
441  } else {
442  silent_cerr("ModuleNonsmoothNode(" << GetLabel() << "): invalid lcp solver type"
443  << " at line " << HP.GetLineData() << std::endl);
444  throw ErrGeneric(MBDYN_EXCEPT_ARGS);
445  }
446  }
447 
448  NS_data.solparam.solvertol = 1.e-14;
449  if (HP.IsKeyWord("tolerance")) {
450  NS_data.solparam.solvertol = HP.GetReal();
451  }
452 
453  NS_data.solparam.solveritermax = 100;
454  if (HP.IsKeyWord("max" "iterations")) {
455  NS_data.solparam.solveritermax = HP.GetInt();
456  }
457 
458  NS_data.iterations = 0;
459  if (HP.IsKeyWord("limit" "iterations")) {
460  NS_data.iterations = HP.GetInt();
461  }
462 
463  NS_data.niterLCPmax = 0;
464  if (HP.IsKeyWord("limit" "LCP" "iterations")) {
465  NS_data.niterLCPmax = HP.GetInt();
466  }
467 
468  if (HP.IsKeyWord("verbose")) {
469  NS_data.bVerbose = HP.GetYesNoOrBool(true);
470  }
471 
472  SetOutputFlag(pDM->fReadOutput(HP, Elem::LOADABLE));
473 
474  //----------------------------------------------
475  // CHECK GAP
476  //----------------------------------------------
477 
478  for (int i = 0; i < NS_data.Np; i++) {
479  NS_data.constr[i].H_kp1 = NS_data.constr[i].AttNode->GetRCurr() * NS_data.constr[i].RotRel;
480  Vec3 Plane_point = NS_data.constr[i].AttNode->GetXCurr() + NS_data.constr[i].AttNode->GetRCurr() * NS_data.constr[i].Point;
481  double Gap = (m_pNode->GetXCurr() - Plane_point) * NS_data.constr[i].H_kp1.GetVec(3) - NS_data.radius_ns;
482 
483  if (NS_data.bVerbose) {
484  silent_cout("ModuleNonsmoothNode(" << GetLabel() << ")" << std::endl
485  << " initial nonsmooth node position " << m_pNode->GetXCurr() << std::endl
486  << " Plane_point " << Plane_point << std::endl
487  << " normal " << NS_data.constr[i].H_kp1.GetVec(3) << std::endl
488  << " Gap " << Gap << std::endl
489  << " RFnode " << NS_data.constr[i].AttNode->GetRCurr() << std::endl);
490  }
491 
492  if (Gap <= 0.) {
493  silent_cerr("ModuleNonsmoothNode(" << GetLabel() << "): "
494  "unilateral constraint " << i << " violated before simulation; Gap=" << Gap << std::endl);
495  throw ErrGeneric(MBDYN_EXCEPT_ARGS);
496  }
497  }
498 
499  //Inizializzazione del sottosistema NonSmooth (struct) nel costruttore:
500  // integration step
501  NS_data.hstep = m_pDM->pGetDrvHdl()->dGetTimeStep();
502 
503  // Inizializzazione dello stato
504  NS_data.Lambda_kp1 = Zero3;
505  NS_data.Lambda_k = Zero3;
506  NS_data.Xns_kp1 = m_pNode->GetXCurr(); //NS state at step k +1
507  NS_data.Vns_kp1 = m_pNode->GetVCurr(); //NS velocity at step k + 1
508  NS_data.Xns_k = NS_data.Xns_kp1; //NS state at step k
509  NS_data.Vns_k = NS_data.Vns_kp1; //NS velocity at step k
510 
511  NS_data.solparam.info = 0;
512  NS_data.solparam.resulting_error = 0;
513  NS_data.solparam.processed_iterations = -1;
514 
515  if (NS_data.bVerbose) {
516  silent_cout("ModuleNonsmoothNode(" << GetLabel() << "): initial nonsmooth node position " << NS_data.Xns_kp1 << std::endl);
517  }
518 
519  // Set-up approximated friction cone
520  // TODO: allow per-element discretization
521  if (bFrictional && !bIRa) {
522  bIRa = true;
523 
524  IRa.Resize(2, omegan - 2);
525  for (int r = 0; r < 2; r++) {
526  for (int c = 0; c < omegan - 2; c++) {
527  IRa(r + 1, c + 1) = dIRa[r][c];
528  }
529  }
530 
531  IRat.Resize(omegan - 2, 2);
532  for (int r = 0; r < omegan - 2; r++) {
533  for (int c = 0; c < 2; c++) {
534  IRat(r + 1, c + 1) = dIRa[c][r];
535  }
536  }
537 
538  IPa.Resize(2, 2);
539  for (int r = 0; r < 2; r++) {
540  for (int c = 0; c < 2; c++) {
541  IPa(r + 1, c + 1) = dIPa[r][c];
542  }
543  }
544  }
545 }
546 
547 ModuleNonsmoothNode::~ModuleNonsmoothNode(void)
548 {
549  // destroy private data
550  NO_OP;
551 }
552 
553 unsigned int
554 ModuleNonsmoothNode::iGetNumDof(void) const
555 {
556  switch (NS_data.constraint_type) {
557  case POSITION_ONLY:
558  return 3;
559 
560  case VELOCITY_ONLY:
561  return 3;
562 
563  case POSITION_AND_VELOCITY:
564  return 6;
565  }
566 
567  // impossible
568  ASSERT(0);
569  throw ErrGeneric(MBDYN_EXCEPT_ARGS);
570 }
571 
572 
573 #if 0
574 std::ostream&
575 ModuleNonsmoothNode::DescribeDof(std::ostream& out, const char *prefix, bool bInitial) const
576 {
577 }
578 
579 void
580 ModuleNonsmoothNode::DescribeDof(std::vector<std::string>& desc, bool bInitial, int i) const
581 {
582 }
583 
584 std::ostream&
585 ModuleNonsmoothNode::DescribeEq(std::ostream& out, const char *prefix, bool bInitial) const
586 {
587 }
588 
589 void
590 ModuleNonsmoothNode::DescribeEq(std::vector<std::string>& desc, bool bInitial, int i) const
591 {
592 }
593 #endif
594 
595 DofOrder::Order
596 ModuleNonsmoothNode::GetDofType(unsigned int) const
597 {
598  return DofOrder::ALGEBRAIC;
599 }
600 
601 void
602 ModuleNonsmoothNode::WorkSpaceDim(integer* piNumRows, integer* piNumCols) const
603 {
604  switch (NS_data.constraint_type) {
605  case POSITION_ONLY:
606  *piNumRows = 6;
607  *piNumCols = 6;
608  break;
609 
610  case VELOCITY_ONLY:
611  *piNumRows = 6;
612  *piNumCols = 6;
613  break;
614 
615  case POSITION_AND_VELOCITY:
616  *piNumRows = 9;
617  *piNumCols = 9;
618  break;
619 
620  default:
621  ASSERT(0);
622  }
623 }
624 
625 void
626 ModuleNonsmoothNode::AfterPredict(VectorHandler& X, VectorHandler& XP)
627 {
628  // called right after prediction, before any iteration within the time step
629  // estrai Lambda1 e Lambda2 da XCurr e "mettili via" per il non-smooth
630  integer iFirstIndex = iGetFirstIndex();
631  NS_data.Lambda_kp1 = Vec3(X, iFirstIndex + 1);
632  // Inizializzazione dello stato
633  NS_data.Xns_kp1 = m_pNode->GetXCurr(); //NS state at step k +1
634  NS_data.Vns_kp1 = m_pNode->GetVCurr(); //NS velocity at step k + 1
635 }
636 
637 void
638 ModuleNonsmoothNode::AfterConvergence(const VectorHandler& X,
639  const VectorHandler& XP)
640 {
641  // called after a step converged
642  // store lambda at step k
643  NS_data.Lambda_k = NS_data.Lambda_kp1;
644  bStepToggle = false;
645  iIter = 0;
646  NS_data.niter = 0;
647  NS_data.Piter = Zero3;
648 }
649 
650 VariableSubMatrixHandler&
651 ModuleNonsmoothNode::AssJac(VariableSubMatrixHandler& WorkMat,
652  doublereal dCoef,
653  const VectorHandler& XCurr,
654  const VectorHandler& XPrimeCurr)
655 {
656  SparseSubMatrixHandler& WM = WorkMat.SetSparse();
657  integer iNode1FirstPositionIndex = m_pNode->iGetFirstPositionIndex();
658  integer iNode1FirstMomentumIndex = m_pNode->iGetFirstMomentumIndex();
659  integer iFirstIndex = iGetFirstIndex();
660 
661  switch (NS_data.constraint_type) {
662  case POSITION_ONLY:
663  WM.ResizeReset(6, 1);
664  // add + I Dl = -lambda to existing momentum equation
665  WM.PutItem(1, iNode1FirstMomentumIndex + 1, iFirstIndex + 1, 1.);
666  WM.PutItem(2, iNode1FirstMomentumIndex + 2, iFirstIndex + 2, 1.);
667  WM.PutItem(3, iNode1FirstMomentumIndex + 3, iFirstIndex + 3, 1.);
668  // constraint equation on position: I dq = q-q
669  WM.PutItem(4, iFirstIndex + 1, iNode1FirstPositionIndex + 1, 1.);
670  WM.PutItem(5, iFirstIndex + 2, iNode1FirstPositionIndex + 2, 1.);
671  WM.PutItem(6, iFirstIndex + 3, iNode1FirstPositionIndex + 3, 1.);
672  break;
673 
674  case VELOCITY_ONLY:
675  WM.ResizeReset(6, 1);
676  // add + I Dl = -lambda to existing momentum equation
677  WM.PutItem(1, iNode1FirstMomentumIndex + 1, iFirstIndex + 1, 1.);
678  WM.PutItem(2, iNode1FirstMomentumIndex + 2, iFirstIndex + 2, 1.);
679  WM.PutItem(3, iNode1FirstMomentumIndex + 3, iFirstIndex + 3, 1.);
680  // constraint equation on velocity: I Dv = v-v
681  WM.PutItem(4, iFirstIndex + 1, iNode1FirstPositionIndex + 1, 1.);
682  WM.PutItem(5, iFirstIndex + 2, iNode1FirstPositionIndex + 2, 1.);
683  WM.PutItem(6, iFirstIndex + 3, iNode1FirstPositionIndex + 3, 1.);
684  break;
685 
686  case POSITION_AND_VELOCITY:
687  WM.ResizeReset(12, 1);
688  // add + I Dl = -lambda to existing momentum equation
689  WM.PutItem(1, iNode1FirstMomentumIndex + 1, iFirstIndex + 1, 1.);
690  WM.PutItem(2, iNode1FirstMomentumIndex + 2, iFirstIndex + 2, 1.);
691  WM.PutItem(3, iNode1FirstMomentumIndex + 3, iFirstIndex + 3, 1.);
692  // constraint equation on position, relaxed: I dq + I dmu = q-q -mu
693  WM.PutItem(4, iFirstIndex + 1, iNode1FirstPositionIndex + 1, dCoef);
694  WM.PutItem(5, iFirstIndex + 2, iNode1FirstPositionIndex + 2, dCoef);
695  WM.PutItem(6, iFirstIndex + 3, iNode1FirstPositionIndex + 3, dCoef);
696  WM.PutItem(7, iFirstIndex + 1, iFirstIndex + 4, 1.);
697  WM.PutItem(8, iFirstIndex + 2, iFirstIndex + 5, 1.);
698  WM.PutItem(9, iFirstIndex + 3, iFirstIndex + 6, 1.);
699  // constraint equation on velocity: I Dv = v-v
700  WM.PutItem(10, iFirstIndex + 4, iNode1FirstPositionIndex + 1, 1.);
701  WM.PutItem(11, iFirstIndex + 5, iNode1FirstPositionIndex + 2, 1.);
702  WM.PutItem(12, iFirstIndex + 6, iNode1FirstPositionIndex + 3, 1.);
703  break;
704 
705  default:
706  // impossible
707  ASSERT(0);
708  }
709 
710  return WorkMat;
711 }
712 
713 SubVectorHandler&
714 ModuleNonsmoothNode::AssRes(SubVectorHandler& WorkVec,
715  doublereal dCoef,
716  const VectorHandler& XCurr,
717  const VectorHandler& XPrimeCurr)
718 {
719  integer iNode1FirstIndex = m_pNode->iGetFirstMomentumIndex();
720  integer iFirstIndex = iGetFirstIndex();
721  // Variables needed for the iteration:
722  // timestep value, gravity acceleration (if it is set),
723  // position and velocity of the node coincident with the nonsmooth node,
724  // forces applied on that node by the rest of the model.
725  NS_data.hstep = m_pDM->pGetDrvHdl()->dGetTimeStep();
726  const Vec3& Xn = m_pNode->GetXCurr();
727  NS_data.bGravity = GravityOwner::bGetGravity(Xn, NS_data.GravityAcceleration);
728 
729  NS_data.Xns_k = m_pNode->GetXPrev();
730  NS_data.Vns_k = m_pNode->GetVPrev();
731 
732  NS_data.Xns_kp1 = m_pNode->GetXCurr();
733 // std::cout << "NS_data.Xns_kp1="<<NS_data.Xns_kp1;
734  NS_data.Vns_kp1 = m_pNode->GetVCurr();
735  NS_data.Lambda_kp1 = Vec3(XCurr, iFirstIndex + 1);
736 
737  // integra un'iterazione del systema NonSmooth e restituisci il nuovo Lambda
738  // e Xns1 e Vns1 all'interno di NS_data
739 
740  if (((NS_data.iterations > 0) && (iIter < NS_data.iterations)) || (NS_data.iterations == 0)) {
741  if (bFrictional) {
742  mbs_get_force_frictional(NS_data);
743 
744  } else {
745  mbs_get_force(NS_data);
746  }
747  iIter++;
748  }
749 
750  Vec3 DX1(NS_data.Xns_kp1 - m_pNode->GetXCurr());
751  Vec3 DV1(NS_data.Vns_kp1 - m_pNode->GetVCurr());
752 
753  if (NS_data.bVerbose) {
754  silent_cout("ModuleNonsmoothNode(" << GetLabel() << ") step=" << m_pDM->pGetDrvHdl()->iGetStep() << std::endl
755  << " NS_data.Xns_kp1={" << NS_data.Xns_kp1 << "} DX={" << DX1 << "}" << std::endl
756  << " NS_data.Vns_kp1={" << NS_data.Vns_kp1 << "} DV={" << DV1 << "}" << std::endl
757  << " NS_data.Lambda_kp1={" << NS_data.Lambda_kp1 << "}" << std::endl);
758  }
759 
760  switch (NS_data.constraint_type) {
761  case POSITION_ONLY:
762  WorkVec.ResizeReset(6);
763  WorkVec.PutItem(1, iNode1FirstIndex + 1, - NS_data.Lambda_kp1(1));
764  WorkVec.PutItem(2, iNode1FirstIndex + 2, - NS_data.Lambda_kp1(2));
765  WorkVec.PutItem(3, iNode1FirstIndex + 3, - NS_data.Lambda_kp1(3));
766  WorkVec.PutItem(4, iFirstIndex + 1, DX1(1)/dCoef);
767  WorkVec.PutItem(5, iFirstIndex + 2, DX1(2)/dCoef);
768  WorkVec.PutItem(6, iFirstIndex + 3, DX1(3)/dCoef);
769  break;
770 
771  case VELOCITY_ONLY:
772  WorkVec.ResizeReset(6);
773  WorkVec.PutItem(1, iNode1FirstIndex + 1, - NS_data.Lambda_kp1(1));
774  WorkVec.PutItem(2, iNode1FirstIndex + 2, - NS_data.Lambda_kp1(2));
775  WorkVec.PutItem(3, iNode1FirstIndex + 3, - NS_data.Lambda_kp1(3));
776  WorkVec.PutItem(4, iFirstIndex + 1, DV1(1));
777  WorkVec.PutItem(5, iFirstIndex + 2, DV1(2));
778  WorkVec.PutItem(6, iFirstIndex + 3, DV1(3));
779  break;
780 
781  case POSITION_AND_VELOCITY:
782  WorkVec.ResizeReset(9);
783  Mu = Vec3(XCurr, iFirstIndex + 4);
784  // residuo per le equazioni di congruenza del nodo 1
785  DX1 -= Mu;
786  NS_data.mu = Mu;
787  WorkVec.PutItem(1, iNode1FirstIndex + 1, - NS_data.Lambda_kp1(1));
788  WorkVec.PutItem(2, iNode1FirstIndex + 2, - NS_data.Lambda_kp1(2));
789  WorkVec.PutItem(3, iNode1FirstIndex + 3, - NS_data.Lambda_kp1(3));
790  WorkVec.PutItem(4, iFirstIndex + 1, DX1(1));
791  WorkVec.PutItem(5, iFirstIndex + 2, DX1(2));
792  WorkVec.PutItem(6, iFirstIndex + 3, DX1(3));
793  WorkVec.PutItem(7, iFirstIndex + 4, DV1(1));
794  WorkVec.PutItem(8, iFirstIndex + 5, DV1(2));
795  WorkVec.PutItem(9, iFirstIndex + 6, DV1(3));
796  break;
797  }
798 
799  if (NS_data.bVerbose) {
800  silent_cout("ModuleNonsmoothNode(" << GetLabel() << ") workvec:" << std::endl << WorkVec << std::endl);
801  }
802 
803  return WorkVec;
804 }
805 
806 unsigned int
807 ModuleNonsmoothNode::iGetNumPrivData(void) const
808 {
809  return 0;
810 }
811 
812 int
813 ModuleNonsmoothNode::iGetNumConnectedNodes(void) const
814 {
815  // silent_cerr("start iGetNumConnectedNodes " << std::endl);
816  return 1 + NS_data.constr.size();
817 }
818 
819 void
820 ModuleNonsmoothNode::GetConnectedNodes(std::vector<const Node *>& connectedNodes) const
821 {
822  connectedNodes.resize(iGetNumConnectedNodes());
823  connectedNodes[0] = m_pNode;
824  unsigned i = 0;
825  for (; i < NS_data.constr.size(); i++) {
826  connectedNodes[i + 1] = NS_data.constr[i].AttNode;
827  }
828 }
829 
830 void
831 ModuleNonsmoothNode::SetValue(DataManager *pDM,
832  VectorHandler& X, VectorHandler& XP,
833  SimulationEntity::Hints *ph)
834 {
835  NO_OP;
836 }
837 
838 std::ostream&
839 ModuleNonsmoothNode::Restart(std::ostream& out) const
840 {
841  return out << "# ModuleNonsmoothNode: not implemented" << std::endl;
842 }
843 
844 unsigned int
845 ModuleNonsmoothNode::iGetInitialNumDof(void) const
846 {
847  return 0;
848 }
849 
850 void
851 ModuleNonsmoothNode::InitialWorkSpaceDim(integer* piNumRows, integer* piNumCols) const
852 {
853  *piNumRows = 0;
854  *piNumCols = 0;
855 }
856 
857 VariableSubMatrixHandler&
858 ModuleNonsmoothNode::InitialAssJac(
859  VariableSubMatrixHandler& WorkMat,
860  const VectorHandler& XCurr)
861 {
862  // should not be called, since initial workspace is empty
863  ASSERT(0);
864  WorkMat.SetNullMatrix();
865  return WorkMat;
866 }
867 
868 SubVectorHandler&
869 ModuleNonsmoothNode::InitialAssRes(
870  SubVectorHandler& WorkVec,
871  const VectorHandler& XCurr)
872 {
873  // should not be called, since initial workspace is empty
874  ASSERT(0);
875  WorkVec.ResizeReset(0);
876  return WorkVec;
877 }
878 
879 void
880 ModuleNonsmoothNode::Output(OutputHandler& OH) const
881 {
882  if (bToBeOutput()) {
883  std::ostream& out = OH.Loadable();
884  out << std::setw(8) << GetLabel()
885  << " " << NS_data.Poutput
886  << " " << NS_data.Xns_kp1
887  << " " << NS_data.Vns_kp1
888  << " " << NS_data.Lambda_kp1
889  << " " << NS_data.pkp1
890  << " " << NS_data.ia;
891 
892  if (NS_data.bVerbose) {
893  out
894  << " " << NS_data.mu
895  << " " << NS_data.solparam.info
896  << " " << NS_data.solparam.resulting_error
897  << " " << NS_data.solparam.processed_iterations;
898  }
899 
900  out << std::endl;
901  }
902 
903  // TODO: use NetCDF?
904 }
905 
906 void
907 ModuleNonsmoothNode::mbs_get_force(NS_subsys& NS)
908 {
909  // Some Output initialization
910  Vec3 p_kp1 = Zero3;
911  NS.pkp1 = 0.;
912  NS.ia = 0;
913 
914 // Mat3x3 M_ns = Eye3 * NS.mass_ns;
915 // Mat3x3 K_ns = Zero3x3;
916 // Mat3x3 C_ns = Zero3x3;
917 // Mat3x3 Mhat = M_ns + C_ns * (NS.hstep * NS.theta_ts) + K_ns * (NS.hstep * NS.hstep * NS.theta_ts * NS.theta_ts);
918 // Mat3x3 invMhat = Mhat.Inv();
919  Mat3x3 M_ns = Eye3 * NS.mass_ns;
920  Mat3x3 invMhat = Eye3 * (1./NS.mass_ns);
921 
922  // Determine the predicted NS state at the step k+1, to verify if the gap is closed
923  Vec3 x_NS_predicted = NS.Xns_k + NS.Vns_k * (NS.hstep * NS.gamma_pred);
924 
925  // Create index of active constraints: Ia, through evaluation of the gap distance.
926  std::vector<int> Ia;
927  for (int i = 0; i < NS.Np; i++) {
928  // Store the orientation matrix H of the reference frame with Z aligned with contact direction
929  NS.constr[i].H_kp1 = NS.constr[i].AttNode->GetRCurr() * NS.constr[i].RotRel;
930  NS.constr[i].H_k = NS.constr[i].AttNode->GetRPrev() * NS.constr[i].RotRel;
931 
932  Vec3 Plane_point = NS.constr[i].AttNode->GetXCurr() + NS.constr[i].AttNode->GetRCurr() * NS.constr[i].Point;
933  NS.constr[i].Gap = (x_NS_predicted - Plane_point) * NS.constr[i].H_kp1.GetVec(3) - NS.radius_ns;
934 
935  // Valuto gap(xpredicted) e creo indice vincoli attivi
936  if (NS.constr[i].Gap <= 0.) {
937  Ia.push_back(i);
938  }
939  }
940 
941 #if 0
942  // NOTE: if no active contact, directly integrate the dynamics of the mass
943  if (Ia.empty()) {
944  }
945 #endif
946 
947  // External forces: gravity and the reaction passed from the rest of the system integrated by MBDYN
948  Vec3 F_k = NS.Lambda_k;
949  Vec3 F_kp1 = NS.Lambda_kp1;
950  Vec3 Fg;
951  if (NS.bGravity) {
952  Fg = NS.GravityAcceleration * NS.mass_ns;
953  F_k += Fg;
954  F_kp1 += Fg;
955  }
956 
957  // Velocity of the non-smooth subsystem calculated without the contribute of contact impulse
958  // Vec3 vfree_kp1= NS.Vns_k + invMhat * ( - C_ns*NS.Vns_k*NS.hstep - K_ns*NS.Xns_k*NS.hstep - K_ns*NS.Vns_k*(NS.hstep*NS.hstep*NS.theta_ts) + (F_kp1*NS.theta_ts + F_k* (1.-NS.theta_ts) ) *NS.hstep);
959  Vec3 vfree_kp1 = NS.Vns_k + invMhat * (F_kp1*NS.theta_ts + F_k* (1. - NS.theta_ts)) * NS.hstep;
960 
961  Vec3 VfreeRel = vfree_kp1 - NS.constr[0].AttNode->GetVCurr(); // VfreeMinusVplane
962  Vec3 VnskRel = NS.Vns_k - NS.constr[0].AttNode->GetVCurr();
963 
964  // Composing and solving the Linear Complementarity System to determine the contact impulse p_kp1
965  if (!Ia.empty()) {
966  Mat3xN Hfull_kp1(Ia.size());
967  Mat3xN Hfull_k(Ia.size());
968 
969  for (unsigned i = 0; i < Ia.size(); i++) {
970  Hfull_kp1.PutVec(i + 1, NS.constr[Ia[i]].H_kp1.GetVec(3));
971  Hfull_k.PutVec(i + 1, NS.constr[Ia[i]].H_k.GetVec(3));
972  }
973 
974  MatNx3 Hfull_kp1T(Ia.size());
975  Hfull_kp1T.Transpose(Hfull_kp1);
976  MatNx3 Hfull_kT(Ia.size());
977  Hfull_kT.Transpose(Hfull_k);
978  MatNx3 HTW(Ia.size());
979  HTW.RightMult(Hfull_kp1T, invMhat);
980  MatNxN W_delassus(Ia.size(), 0.);
981  W_delassus.Mult(HTW, Hfull_kp1);
982 
983  // Attenzione, matrici riempite alla FORTRAN! Passo a Siconos le matrici come le vuole lui.
984  double W_passed[Ia.size()*Ia.size()];
985  int indice = 0;
986  for (unsigned i = 1; i < Ia.size() + 1; i++) {
987  for (unsigned j = 1; j < Ia.size() + 1; j++) {
988  W_passed[indice] = W_delassus(j, i);
989  indice++;
990  }
991  }
992 
993  double bLCP_new[Ia.size()];
994  for (unsigned i = 0; i < Ia.size(); i++) {
995  bLCP_new[i] = Hfull_kp1T.GetVec(i + 1) * VfreeRel + Hfull_kT.GetVec(i + 1) * VnskRel * NS.constr[Ia[i]].e_rest;
996  }
997 
998  VecN P(Ia.size(), 0.);
999  VecN U(Ia.size(), 0.);
1000 
1001  if ((NS.niter < NS.niterLCPmax) || (NS.niterLCPmax == 0)) {
1002  // Solving the LCP with one of Siconos solvers
1003  mbdyn_siconos_LCP_call(Ia.size(), W_passed, bLCP_new, P.pGetVec(), U.pGetVec(), NS.solparam);
1004 
1005  // Reazioni nel sistema di riferimento globale
1006  p_kp1 = Hfull_kp1 * P;
1007 
1008  NS.Piter = p_kp1;
1009 
1010  NS.niter++;
1011  }
1012  }
1013 
1014  // Updating: state actualization with the contribution of the contact impulse
1015  NS.Vns_kp1 = vfree_kp1 + invMhat * NS.Piter;
1016  NS.Xns_kp1 = NS.Xns_k + (NS.Vns_kp1 * NS.theta_ts + NS.Vns_k* (1. - NS.theta_ts)) * NS.hstep;
1017  // Reaction force to be exchanged with the rest of the system in MBDYN
1018  NS.Lambda_kp1 = p_kp1 / NS.hstep - M_ns * (NS.Vns_kp1 - NS.Vns_k) / NS.hstep + NS.Lambda_k*(1. - NS.theta_ts);
1019  if (NS.bGravity) {
1020  NS.Lambda_kp1 += Fg;
1021  }
1022  NS.Lambda_kp1 /= -NS.theta_ts;
1023 
1024  // Store output
1025  NS.ia = Ia.size();
1026  NS.pkp1 = p_kp1.Norm();
1027  NS.Poutput = p_kp1;
1028  // p_kp1 = NS.Piter;
1029 
1030  // Indicatore di nuovo step, per evitare troppo output con bVerbose.
1031  bStepToggle = true;
1032 }
1033 
1034 void
1035 ModuleNonsmoothNode::mbs_get_force_frictional(NS_subsys& NS)
1036 {
1037 //------------------------------------------------------------------------------
1038 
1039 // Mat3x3 M_ns = Eye3 * NS.mass_ns;
1040 // Mat3x3 K_ns = Zero3x3;
1041 // Mat3x3 C_ns = Zero3x3;
1042 // Mat3x3 Mhat = M_ns + C_ns * (NS.hstep * NS.theta_ts) + K_ns * (NS.hstep * NS.hstep * NS.theta_ts * NS.theta_ts);
1043 // Mat3x3 invMhat = Mhat.Inv();
1044  Mat3x3 M_ns = Eye3 * NS.mass_ns;
1045  Mat3x3 invMhat = Eye3 * (1./NS.mass_ns);
1046 
1047  Vec3 p_kp1 = Zero3;
1048  NS.Poutput = Zero3;
1049  NS.ia = 0;
1050 
1051  // Determine the predicted NS state at the step k+1, to verify if the gap is closed
1052  Vec3 x_NS_predicted = NS.Xns_k + NS.Vns_k * NS.hstep * NS.gamma_pred;
1053 
1054  // Create index of active constraints: Ia, through evaluation of the gap distance.
1055  std::vector<int> Ia;
1056  for (int i = 0; i < NS.Np; i++) {
1057  // Store the orientation matrix H of the reference frame with Z aligned with contact direction
1058  NS.constr[i].H_kp1 = NS.constr[i].AttNode->GetRCurr() * NS.constr[i].RotRel;
1059  NS.constr[i].H_k = NS.constr[i].AttNode->GetRPrev() * NS.constr[i].RotRel;
1060 
1061  Vec3 Plane_point = NS.constr[i].AttNode->GetXCurr() + NS.constr[i].AttNode->GetRCurr() * NS.constr[i].Point;
1062  NS.constr[i].Gap = (x_NS_predicted - Plane_point) * NS.constr[i].H_kp1.GetVec(3) - NS.radius_ns;
1063 
1064  // Valuto gap(xpredicted) e creo indice vincoli attivi
1065  if (NS.constr[i].Gap <= 0.) {
1066  Ia.push_back(i);
1067  }
1068  }
1069 
1070 #if 0
1071  // NOTE: if no active contact, directly integrate the dynamics of the mass
1072  if (Ia.empty()) {
1073  }
1074 #endif
1075 
1076  // External forces: gravity and the reaction passed from the rest of the system integrated by MBDYN
1077  Vec3 F_k = NS.Lambda_k;
1078  Vec3 F_kp1 = NS.Lambda_kp1;
1079  Vec3 Fg;
1080  if (NS.bGravity) {
1081  Fg = NS.GravityAcceleration * NS.mass_ns;
1082  F_k += Fg;
1083  F_kp1 += Fg;
1084  }
1085 
1086  // Velocity of the non-smooth subsystem calculated without the contribute of contact impulse
1087  // Vec3 vfree_kp1= NS.Vns_k + invMhat * ( - C_ns*NS.Vns_k*NS.hstep - K_ns*NS.Xns_k*NS.hstep - K_ns*NS.Vns_k*(NS.hstep*NS.hstep*NS.theta_ts) + (F_kp1*NS.theta_ts + F_k* (1.-NS.theta_ts) ) *NS.hstep);
1088  Vec3 vfree_kp1 = NS.Vns_k + invMhat * (F_kp1*NS.theta_ts + F_k * (1. - NS.theta_ts)) * NS.hstep;
1089  Vec3 VfreeRel = vfree_kp1 - NS.constr[0].AttNode->GetVCurr(); // VfreeMinusVplane
1090 
1091  if (Ia.size() > 0) {
1092  int ac = Ia.size(); // active constraints !!!
1093  FullMatrixHandler H_NN(3, ac);
1094  FullMatrixHandler H_TT(3, 2*ac);
1095  FullMatrixHandler H_NNt(ac, 3);
1096  FullMatrixHandler H_TTt(2*ac, 3);
1097 
1098  FullMatrixHandler Htot(3, 3*ac);
1099 
1100  FullMatrixHandler I_P(2*ac, 2*ac);
1101  FullMatrixHandler I_R(2*ac, (omegan - 2)*ac);
1102  FullMatrixHandler I_Rt((omegan - 2)*ac, 2*ac);
1103  FullMatrixHandler mu_P(2*ac, ac);
1104  FullMatrixHandler mu_R((omegan - 2)*ac, ac);
1105  FullMatrixHandler en(1, ac);
1106 
1107  for (unsigned i = 0; i < Ia.size(); i++) {
1108  en(1, i + 1) = NS.constr[Ia[i]].e_rest;
1109 
1110  H_NN.Put(1, i + 1, NS.constr[Ia[i]].H_kp1.GetVec(3));
1111  H_TT.Put(1, 2*i + 1, NS.constr[Ia[i]].H_kp1.GetVec(1));
1112  H_TT.Put(1, 2*i + 2, NS.constr[Ia[i]].H_kp1.GetVec(2));
1113  H_NNt.PutT(i + 1, 1, NS.constr[Ia[i]].H_kp1.GetVec(3));
1114  H_TTt.PutT(2*i + 1, 1, NS.constr[Ia[i]].H_kp1.GetVec(1));
1115  H_TTt.PutT(2*i + 2, 1, NS.constr[Ia[i]].H_kp1.GetVec(2));
1116 
1117  I_P.Put(2*(i + 1 - 1) + 1, 2*(i + 1 - 1) + 1, IPa);
1118  I_R.Put(2*(i + 1 - 1) + 1, (omegan - 2)*(i + 1 - 1) + 1, IRa);
1119  I_Rt.Put((omegan - 2)*(i + 1 - 1) + 1, 2*(i + 1 - 1) + 1, IRat);
1120 
1121  FullMatrixHandler mu_Piw(2, 1);
1122  mu_Piw(1, 1) = NS.constr[Ia[i]].mu;
1123  mu_Piw(2, 1) = NS.constr[Ia[i]].mu;
1124 
1125  FullMatrixHandler mu_Riw(omegan - 2, 1);
1126 
1127  for (int in = 1; in < omegan - 2 + 1; in++) {
1128  mu_Riw(in, 1) = NS.constr[Ia[i]].mu;
1129  }
1130 
1131  mu_P.Put(2*(i + 1 - 1) + 1, i + 1, mu_Piw);
1132  mu_R.Put((omegan - 2)*(i + 1 - 1) + 1, i + 1, mu_Riw);
1133  }
1134 
1135  Htot.Put(1, 1, H_TT);
1136  Htot.Put(1, 2*ac + 1, H_NN);
1137 
1138  FullMatrixHandler v_free(3, 1);
1139  v_free.Put(1, 1, VfreeRel);
1140  FullMatrixHandler U_Nfree(1*ac, 1);
1141  FullMatrixHandler U_Tfree(2*ac, 1);
1142  U_Nfree.MatMul(H_NNt, v_free);
1143  U_Tfree.MatMul(H_TTt, v_free);
1144 
1145  FullMatrixHandler iMhat(3, 3);
1146  iMhat.Put(1, 1, invMhat);
1147 
1148  FullMatrixHandler WNNl(ac, 3);
1149  WNNl.MatMul(H_NNt, iMhat);
1150  FullMatrixHandler WNN(ac, ac);
1151  WNN.MatMul(WNNl, H_NN);
1152 
1153  FullMatrixHandler WTTl(2*ac, 3);
1154  WTTl.MatMul(H_TTt, iMhat);
1155  FullMatrixHandler WTT(2*ac, 2*ac);
1156  WTT.MatMul(WTTl, H_TT);
1157 
1158  FullMatrixHandler WNTl(ac, 3);
1159  WNTl.MatMul(H_NNt, iMhat);
1160  FullMatrixHandler WNT(ac, 2*ac);
1161  WNT.MatMul(WNTl, H_TT);
1162 
1163  FullMatrixHandler WTNl(2*ac, 3);
1164  WTNl.MatMul(H_TTt, iMhat);
1165  FullMatrixHandler WTN(2*ac, ac);
1166  WTN.MatMul(WTNl, H_NN);
1167 
1168  // FIXME: adesso รจ facile perchรจ eye(2*ac)
1169  FullMatrixHandler IPmT(2*ac, 2*ac);
1170  for (int ipmti = 1; ipmti < 2*ac + 1; ipmti++) {
1171  IPmT(ipmti, ipmti) = 1;
1172  }
1173 
1174  // MLCP construction
1175  int ProbDim = (3 + omegan - 2)*ac;
1176  FullMatrixHandler Mlcp(ProbDim, ProbDim);
1177  FullMatrixHandler mlcp11(ac, ac);
1178  FullMatrixHandler mlcp12(ac, 2*ac);
1179  FullMatrixHandler mlcp21(2*ac, ac);
1180  FullMatrixHandler mlcp22(2*ac, 2*ac);
1181  FullMatrixHandler mlcp23(2*ac, (omegan - 2)*ac);
1182  FullMatrixHandler mlcp31((omegan - 2)*ac, ac);
1183  FullMatrixHandler mlcp32((omegan - 2)*ac, 2*ac);
1184 
1185  FullMatrixHandler WNT_IPmT(ac, 2*ac);
1186  WNT_IPmT.MatMul(WNT, IPmT);
1187  FullMatrixHandler WNT_IPmT_muP(ac, ac);
1188  WNT_IPmT_muP.MatMul(WNT_IPmT, mu_P);
1189 
1190  mlcp11.Put(1, 1, WNN);
1191  mlcp11.Add(1, 1, WNT_IPmT_muP);
1192  mlcp12.Sub(1, 1, WNT_IPmT);
1193 
1194  FullMatrixHandler WTT_IPmT(2*ac, 2*ac);
1195  WTT_IPmT.MatMul(WTT, IPmT);
1196  FullMatrixHandler WTT_IPmT_muP(2*ac, ac);
1197  WTT_IPmT_muP.MatMul(WTT_IPmT, mu_P);
1198  WTT_IPmT_muP.Add(1, 1, WTN);
1199  mlcp21.Sub(1, 1, WTT_IPmT_muP);
1200 
1201  mlcp22.Put(1, 1, WTT);
1202 
1203  mlcp23.Sub(1, 1, I_R);
1204 
1205  FullMatrixHandler I_Rt_mu_P((omegan - 2)*ac, ac);
1206  I_Rt_mu_P.MatMul(I_Rt, mu_P);
1207 
1208  mlcp31.Add(1, 1, mu_R);
1209  mlcp31.Sub(1, 1, I_Rt_mu_P);
1210  mlcp32.Add(1, 1, I_Rt);
1211 
1212  Mlcp.Put(1, 1, mlcp11);
1213  Mlcp.Put(1, ac + 1, mlcp12);
1214  Mlcp.Put(ac + 1, 1, mlcp21);
1215  Mlcp.Put(ac + 1, ac + 1, mlcp22);
1216  Mlcp.Put(ac + 1, 3*ac + 1, mlcp23);
1217  Mlcp.Put(3*ac + 1, 1, mlcp31);
1218  Mlcp.Put(3*ac + 1, ac + 1, mlcp32);
1219 
1220  // FIXME: use STLVectorHandler?
1221  FullMatrixHandler qlcp(ProbDim, 1);
1222  FullMatrixHandler qlcp1(ac, 1);
1223 
1224  STLVectorHandler VnskMinusVplane(3);
1225  VnskMinusVplane.Put(1, NS.Vns_k - NS.constr[0].AttNode->GetVCurr());
1226 
1227  STLVectorHandler U_N(ac);
1228  H_NNt.MatVecMul(U_N, VnskMinusVplane);
1229 
1230  for (int ien = 1; ien < ac + 1; ien++) {
1231  qlcp1(ien, 1) = U_N(ien)*en(1, ien);
1232  }
1233 
1234  qlcp1.Add(1, 1, U_Nfree);
1235  qlcp.Put(1, 1, qlcp1);
1236  qlcp.Sub(ac + 1, 1, U_Tfree);
1237 
1238 #if 0
1239  // SOME OUTPUT TO DEBUG
1240 
1241  std::cout<<" H_NN" << std::endl << H_NN << std::endl;
1242  std::cout<<" H_TT" << std::endl << H_TT << std::endl;
1243  std::cout<<" I_P" << std::endl << I_P << std::endl;
1244  std::cout<<" I_R" << std::endl << I_R << std::endl;
1245  std::cout<<" I_Rt" << std::endl << I_Rt << std::endl;
1246  std::cout<<" mu_P" << std::endl << mu_P << std::endl;
1247  std::cout<<" mu_R" << std::endl << mu_R << std::endl;
1248  std::cout<<" WNN" << std::endl << WNN << std::endl;
1249  std::cout<<" WNT" << std::endl << WNT << std::endl;
1250  std::cout<<" WTN" << std::endl << WTN << std::endl;
1251  std::cout<<" WTT" << std::endl << WTT << std::endl;
1252  std::cout<<" IPmT" << std::endl << IPmT << std::endl;
1253 // std::cout<< "MLCP" << std::endl << MLCP << std::endl;
1254 // std::cout<< "qLCP" << std::endl << qLCP << std::endl;
1255 #endif
1256 
1257  double Pkp1[ProbDim];
1258  double Unkp1[ProbDim];
1259 
1260  // NOTE: resetting Pkp1, Unkp1 is mandatory because some times
1261  // they are not (r)eset in mbdyn_siconos_LCP_call()
1262  memset(Pkp1, 0, sizeof(Pkp1));
1263  memset(Unkp1, 0, sizeof(Unkp1));
1264 
1265  if ((NS.niter < NS.niterLCPmax) || (NS.niterLCPmax == 0)) {
1266  // Solving the LCP with one of Siconos solvers
1267  mbdyn_siconos_LCP_call(ProbDim, Mlcp.pdGetMat(), qlcp.pdGetMat(), &Pkp1[0], &Unkp1[0], NS.solparam);
1268 
1269  STLVectorHandler PN(ac);
1270  STLVectorHandler sigmaP(2*ac);
1271  STLVectorHandler PT(2*ac);
1272  STLVectorHandler Ptot(3*ac);
1273  STLVectorHandler ptot(3);
1274 
1275  for (int i = 1; i <= ac; i++) {
1276  PN(i) = Pkp1[i - 1];
1277  }
1278  for (int i = ac + 1; i <= 3*ac; i++) {
1279  sigmaP(i - ac) = Pkp1[i - 1];
1280  }
1281  mu_P.MatVecMul(PT, PN);
1282  PT -= sigmaP;
1283  for (int i = 1; i <= 2*ac; i++) {
1284  Ptot(i) = PT(i);
1285  }
1286  for (int i = 2*ac + 1; i <= 3*ac; i++) {
1287  Ptot(i) = PN(i - (2*ac));
1288  }
1289  Htot.MatVecMul(ptot, Ptot);
1290 
1291  p_kp1(1) = ptot(1);
1292  p_kp1(2) = ptot(2);
1293  p_kp1(3) = ptot(3);
1294 
1295  NS.Piter = p_kp1;
1296 
1297  NS.niter++;
1298  }
1299 
1300 // // Solving the LCP with one of Siconos solvers
1301 // mbdyn_siconos_LCP_call(ProbDim, Mlcp_passed, qlcp_passed, Pkp1, Unkp1, NS.solparam);
1302 
1303 // FullMatrixHandler PN(ac, 1);
1304 // FullMatrixHandler sigmaP(2*ac, 1);
1305 // FullMatrixHandler PT(2*ac, 1);
1306 // FullMatrixHandler Ptot(3*ac, 1);
1307 // FullMatrixHandler ptot(3, 1);
1308 //
1309 // for (int i = 1; i < ac + 1; i++) {
1310 // PN(i, 1) = Pkp1[i - 1];
1311 // }
1312 // for (int i = ac; i < 3*ac; i++) {
1313 // sigmaP(i - ac + 1, 1) = Pkp1[i];
1314 // }
1315 // PT.MatMul(mu_P, PN);
1316 // PT.Sub(1, 1, sigmaP);
1317 // for (int i = 1; i < 2*ac + 1; i++) {
1318 // Ptot(i, 1) = PT(i, 1);
1319 // }
1320 // for (int i = 2*ac + 1; i < 3*ac + 1; i++) {
1321 // Ptot(i, 1) = PN(i - (2*ac), 1);
1322 // }
1323 //
1324 // ptot.MatMul(Htot, Ptot);
1325 //
1326 // p_kp1(1) = ptot(1, 1);
1327 // p_kp1(2) = ptot(2, 1);
1328 // p_kp1(3) = ptot(3, 1);
1329  }
1330 
1331  // Updating: state actualization with the contribution of the contact impulse
1332 // NS.Vns_kp1 = vfree_kp1 + invMhat * p_kp1;
1333  NS.Vns_kp1 = vfree_kp1 + invMhat * NS.Piter;
1334  NS.Xns_kp1 = NS.Xns_k + (NS.Vns_kp1 * NS.theta_ts + NS.Vns_k* (1. - NS.theta_ts)) * NS.hstep;
1335 
1336  // Reaction force to be exchanged with the rest of the system in MBDYN
1337  NS.Lambda_kp1 = p_kp1 / NS.hstep - M_ns * (NS.Vns_kp1 - NS.Vns_k) / NS.hstep + NS.Lambda_k*(1. - NS.theta_ts);
1338  if (NS.bGravity) {
1339  NS.Lambda_kp1 += Fg;
1340  }
1341  NS.Lambda_kp1 /= -NS.theta_ts;
1342 
1343  // Store Output
1344  NS.ia = Ia.size();
1345  NS.pkp1 = p_kp1.Norm();
1346  NS.Poutput = p_kp1;
1347 
1348  // Indicatore di nuovo step, per evitare troppo output con bVerbose.
1349  bStepToggle = true;
1350 }
1351 
1352 extern "C" int
1353 module_init(const char *module_name, void *pdm, void *php)
1354 {
1355  UserDefinedElemRead *rf = new UDERead<ModuleNonsmoothNode>;
1356 
1357  if (!SetUDE("nonsmooth" "node", rf)) {
1358  delete rf;
1359  silent_cerr("module-nonsmooth-node: "
1360  "module_init(" << module_name << ") "
1361  "failed" << std::endl);
1362  return -1;
1363  }
1364 
1365  return 0;
1366 }
1367 
DataManager::fReadOutput
flag fReadOutput(MBDynParser &HP, const T &t) const
Definition: dataman.h:1064
FullMatrixHandler::PutT
void PutT(integer iRow, integer iCol, const FullMatrixHandler &source)
Definition: fullmh.cc:1338
MBDynParser::GetRotRel
Mat3x3 GetRotRel(const ReferenceFrame &rf)
Definition: mbpar.cc:1795
solver_parameters
Definition: mbdyn_siconos.h:83
FullMatrixHandler::MatMul
void MatMul(const FullMatrixHandler &m1, const FullMatrixHandler &m2)
Definition: fullmh.cc:433
NS_subsys::constraint_type
Constraining constraint_type
Definition: module-nonsmooth-node.cc:99
mbdyn_siconos_LCP_call
void mbdyn_siconos_LCP_call(int sizep, double W_NN[], double bLCP[], double Pkp1[], double wlem[], solver_parameters &solparam)
Definition: mbdyn_siconos.cc:43
solver_parameters::processed_iterations
int processed_iterations
Definition: mbdyn_siconos.h:93
Zero3
const Vec3 Zero3(0., 0., 0.)
flag
long int flag
Definition: mbdyn.h:43
ToBeOutput::bToBeOutput
virtual bool bToBeOutput(void) const
Definition: output.cc:890
MBDYN_EXCEPT_ARGS
#define MBDYN_EXCEPT_ARGS
Definition: except.h:63
Vec3
Definition: matvec3.h:98
VectorHandler::ResizeReset
virtual void ResizeReset(integer)
Definition: vh.cc:55
HighParser::GetInt
virtual integer GetInt(integer iDefval=0)
Definition: parser.cc:1050
ModuleNonsmoothNode::AssRes
SubVectorHandler & AssRes(SubVectorHandler &WorkVec, doublereal dCoef, const VectorHandler &XCurr, const VectorHandler &XPrimeCurr)
Definition: module-nonsmooth-node.cc:714
Vec3::Norm
doublereal Norm(void) const
Definition: matvec3.h:263
NEWTON_MIN
Definition: mbdyn_siconos.h:71
FullMatrixHandler::Put
void Put(integer iRow, integer iCol, const FullMatrixHandler &source)
Definition: fullmh.cc:1113
StructDispNode::GetXPrev
virtual const Vec3 & GetXPrev(void) const
Definition: strnode.h:304
ModuleNonsmoothNode::AfterPredict
virtual void AfterPredict(VectorHandler &X, VectorHandler &XP)
Definition: module-nonsmooth-node.cc:626
ModuleNonsmoothNode::iGetNumDof
virtual unsigned int iGetNumDof(void) const
Definition: module-nonsmooth-node.cc:554
Eye3
const Mat3x3 Eye3(1., 0., 0., 0., 1., 0., 0., 0., 1.)
ModuleNonsmoothNode::iGetNumConnectedNodes
int iGetNumConnectedNodes(void) const
Definition: module-nonsmooth-node.cc:813
IRat
static FullMatrixHandler IRat
Definition: module-nonsmooth-node.cc:214
FullMatrixHandler::Add
void Add(integer iRow, integer iCol, const FullMatrixHandler &source)
Definition: fullmh.cc:1039
NS_subsys::Lambda_kp1
Vec3 Lambda_kp1
Definition: module-nonsmooth-node.cc:85
DataManager::pGetDrvHdl
const DriveHandler * pGetDrvHdl(void) const
Definition: dataman.h:340
SparseSubMatrixHandler::ResizeReset
void ResizeReset(integer iNewRow, integer iNewCol)
Definition: submat.cc:1084
GravityOwner::bGetGravity
virtual bool bGetGravity(const Vec3 &X, Vec3 &Acc) const
Definition: gravity.cc:208
ModuleNonsmoothNode::GetDofType
virtual DofOrder::Order GetDofType(unsigned int) const
Definition: module-nonsmooth-node.cc:596
ModuleNonsmoothNode::Output
virtual void Output(OutputHandler &OH) const
Definition: module-nonsmooth-node.cc:880
QP
Definition: mbdyn_siconos.h:42
NS_subsys::Piter
Vec3 Piter
Definition: module-nonsmooth-node.cc:110
NO_OP
#define NO_OP
Definition: myassert.h:74
SimulationEntity::Hints
std::vector< Hint * > Hints
Definition: simentity.h:89
MatNxN::Mult
const MatNxN & Mult(const MatNx3 &m, const Mat3xN &n)
Definition: matvec3n.cc:1017
FullMatrixHandler::Resize
virtual void Resize(integer iNewRows, integer iNewCols)
Definition: fullmh.cc:174
NSQP
Definition: mbdyn_siconos.h:58
bIRa
static bool bIRa(false)
CPG
Definition: mbdyn_siconos.h:45
ModuleNonsmoothNode::GetConnectedNodes
void GetConnectedNodes(std::vector< const Node * > &connectedNodes) const
Definition: module-nonsmooth-node.cc:820
ModuleNonsmoothNode::iGetInitialNumDof
virtual unsigned int iGetInitialNumDof(void) const
Definition: module-nonsmooth-node.cc:845
NS_subsys::Vns_kp1
Vec3 Vns_kp1
Definition: module-nonsmooth-node.cc:84
NS_subsys::theta_ts
doublereal theta_ts
Definition: module-nonsmooth-node.cc:96
LATIN_W
Definition: mbdyn_siconos.h:64
NS_subsys::solparam
solver_parameters solparam
Definition: module-nonsmooth-node.cc:97
SubVectorHandler::PutItem
virtual void PutItem(integer iSubRow, integer iRow, const doublereal &dCoef)
Definition: submat.h:1445
ModuleNonsmoothNode::iGetNumPrivData
unsigned int iGetNumPrivData(void) const
Definition: module-nonsmooth-node.cc:807
HighParser::GetYesNoOrBool
virtual bool GetYesNoOrBool(bool bDefval=false)
Definition: parser.cc:1038
NS_subsys::bGravity
bool bGravity
Definition: module-nonsmooth-node.cc:88
NS_subsys::bVerbose
bool bVerbose
Definition: module-nonsmooth-node.cc:101
solver_parameters::solveritermax
int solveritermax
Definition: mbdyn_siconos.h:87
MatNx3::Transpose
const MatNx3 & Transpose(const Mat3xN &n)
Definition: matvec3n.cc:898
NS_subsys::constr
std::vector< plane > constr
Definition: module-nonsmooth-node.cc:94
MatNx3::RightMult
const MatNx3 & RightMult(const MatNx3 &n, const Mat3x3 &m)
Definition: matvec3n.cc:814
SparseSubMatrixHandler::PutItem
void PutItem(integer iSubIt, integer iRow, integer iCol, const doublereal &dCoef)
Definition: submat.h:997
ModuleNonsmoothNode::m_pDM
const DataManager * m_pDM
Definition: module-nonsmooth-node.cc:120
RPGS
Definition: mbdyn_siconos.h:52
NEWTON_FB
Definition: mbdyn_siconos.h:74
ModuleNonsmoothNode::ModuleNonsmoothNode
ModuleNonsmoothNode(unsigned uLabel, const DofOwner *pDO, DataManager *pDM, MBDynParser &HP)
Definition: module-nonsmooth-node.cc:219
NS_subsys::Xns_kp1
Vec3 Xns_kp1
Definition: module-nonsmooth-node.cc:83
DriveHandler::iGetStep
integer iGetStep(void) const
Definition: drive.h:400
VecN::pGetVec
const doublereal * pGetVec(void) const
Definition: matvec3n.h:125
MBDynParser::GetPosRel
Vec3 GetPosRel(const ReferenceFrame &rf)
Definition: mbpar.cc:1331
IPa
static FullMatrixHandler IPa
Definition: module-nonsmooth-node.cc:215
NS_subsys
Definition: module-nonsmooth-node.cc:79
LATIN
Definition: mbdyn_siconos.h:61
VariableSubMatrixHandler::SetNullMatrix
void SetNullMatrix(void)
Definition: submat.h:1159
NS_subsys::mu
Vec3 mu
Definition: module-nonsmooth-node.cc:107
NS_subsys::radius_ns
doublereal radius_ns
Definition: module-nonsmooth-node.cc:92
NS_subsys::Np
int Np
Definition: module-nonsmooth-node.cc:93
false
Definition: mbdyn.h:76
HighParser::IsKeyWord
virtual bool IsKeyWord(const char *sKeyWord)
Definition: parser.cc:910
STLVectorHandler::Put
virtual void Put(integer iRow, const Vec3 &v)
Definition: stlvh.cc:181
ModuleNonsmoothNode::InitialAssJac
VariableSubMatrixHandler & InitialAssJac(VariableSubMatrixHandler &WorkMat, const VectorHandler &XCurr)
Definition: module-nonsmooth-node.cc:858
OutputHandler::Loadable
std::ostream & Loadable(void) const
Definition: output.h:506
DriveHandler::dGetTimeStep
doublereal dGetTimeStep(void) const
Definition: drive.h:393
ModuleNonsmoothNode::mbs_get_force_frictional
void mbs_get_force_frictional(NS_subsys &)
Definition: module-nonsmooth-node.cc:1035
NS_subsys::gamma_pred
doublereal gamma_pred
Definition: module-nonsmooth-node.cc:96
StructDispNode::GetVPrev
virtual const Vec3 & GetVPrev(void) const
Definition: strnode.h:316
ModuleNonsmoothNode::AfterConvergence
virtual void AfterConvergence(const VectorHandler &X, const VectorHandler &XP)
Definition: module-nonsmooth-node.cc:638
dIRa
static const doublereal dIRa[2][omegan-2]
Definition: module-nonsmooth-node.cc:208
dIPa
static const doublereal dIPa[2][omegan-2]
Definition: module-nonsmooth-node.cc:202
ModuleNonsmoothNode::InitialAssRes
SubVectorHandler & InitialAssRes(SubVectorHandler &WorkVec, const VectorHandler &XCurr)
Definition: module-nonsmooth-node.cc:869
ModuleNonsmoothNode::mbs_get_force
void mbs_get_force(NS_subsys &)
Definition: module-nonsmooth-node.cc:907
Elem::DescribeEq
virtual std::ostream & DescribeEq(std::ostream &out, const char *prefix="", bool bInitial=false) const
Definition: elem.cc:137
NS_subsys::mass_ns
doublereal mass_ns
Definition: module-nonsmooth-node.cc:92
StructDispNode::iGetFirstMomentumIndex
virtual integer iGetFirstMomentumIndex(void) const =0
StructDispNode::iGetFirstPositionIndex
virtual integer iGetFirstPositionIndex(void) const
Definition: strnode.h:452
WithLabel::uLabel
unsigned int uLabel
Definition: withlab.h:44
NS_subsys::GravityAcceleration
Vec3 GravityAcceleration
Definition: module-nonsmooth-node.cc:87
omegan
static const int omegan
Definition: module-nonsmooth-node.cc:199
ModuleNonsmoothNode::WorkSpaceDim
virtual void WorkSpaceDim(integer *piNumRows, integer *piNumCols) const
Definition: module-nonsmooth-node.cc:602
ASSERT
#define ASSERT(expression)
Definition: colamd.c:977
NS_subsys::pkp1
double pkp1
Definition: module-nonsmooth-node.cc:105
NS_subsys::Vns_k
Vec3 Vns_k
Definition: module-nonsmooth-node.cc:84
ModuleNonsmoothNode::Restart
std::ostream & Restart(std::ostream &out) const
Definition: module-nonsmooth-node.cc:839
StructDispNode::GetXCurr
virtual const Vec3 & GetXCurr(void) const
Definition: strnode.h:310
FullMatrixHandler::Sub
void Sub(integer iRow, integer iCol, const FullMatrixHandler &source)
Definition: fullmh.cc:1076
LEXICO_LEMKE
Definition: mbdyn_siconos.h:68
NoErr
Definition: except.h:79
NS_subsys::ia
int ia
Definition: module-nonsmooth-node.cc:108
ModuleNonsmoothNode::m_pNode
const StructDispNode * m_pNode
Definition: module-nonsmooth-node.cc:123
ModuleNonsmoothNode::~ModuleNonsmoothNode
virtual ~ModuleNonsmoothNode(void)
Definition: module-nonsmooth-node.cc:547
c
static std::stack< cleanup * > c
Definition: cleanup.cc:59
plane::AttNode
const StructNode * AttNode
Definition: module-nonsmooth-node.cc:66
FullMatrixHandler::pdGetMat
const doublereal * pdGetMat(void) const
Definition: fullmh.h:144
PSOR
Definition: mbdyn_siconos.h:55
plane::e_rest
doublereal e_rest
Definition: module-nonsmooth-node.cc:75
ModuleNonsmoothNode::SetValue
void SetValue(DataManager *pDM, VectorHandler &X, VectorHandler &XP, SimulationEntity::Hints *ph)
Definition: module-nonsmooth-node.cc:831
NS_subsys::Xns_k
Vec3 Xns_k
Definition: module-nonsmooth-node.cc:83
HighParser::IsArg
virtual bool IsArg(void)
Definition: parser.cc:807
Elem
Definition: elem.h:75
solver_parameters::info
int info
Definition: mbdyn_siconos.h:90
NS_subsys::Lambda_k
Vec3 Lambda_k
Definition: module-nonsmooth-node.cc:85
ModuleNonsmoothNode::InitialWorkSpaceDim
virtual void InitialWorkSpaceDim(integer *piNumRows, integer *piNumCols) const
Definition: module-nonsmooth-node.cc:851
SetUDE
bool SetUDE(const std::string &s, UserDefinedElemRead *rude)
Definition: userelem.cc:97
plane::Gap
doublereal Gap
Definition: module-nonsmooth-node.cc:74
ModuleNonsmoothNode::AssJac
VariableSubMatrixHandler & AssJac(VariableSubMatrixHandler &WorkMat, doublereal dCoef, const VectorHandler &XCurr, const VectorHandler &XPrimeCurr)
Definition: module-nonsmooth-node.cc:651
Mat3xN::PutVec
void PutVec(integer iCol, const Vec3 &v)
Definition: matvec3n.cc:565
PGS
Definition: mbdyn_siconos.h:48
StructDispNode::GetVCurr
virtual const Vec3 & GetVCurr(void) const
Definition: strnode.h:322
VecN
Definition: matvec3n.h:76
NS_subsys::niterLCPmax
int niterLCPmax
Definition: module-nonsmooth-node.cc:112
module_init
int module_init(const char *module_name, void *pdm, void *php)
This function registers our user defined element for the math parser.
Definition: module-nonsmooth-node.cc:1353
VariableSubMatrixHandler::SetSparse
SparseSubMatrixHandler & SetSparse(void)
Definition: submat.h:1178
ToBeOutput::SetOutputFlag
virtual void SetOutputFlag(flag f=flag(1))
Definition: output.cc:896
mbdyn_siconos.h
NS_subsys::iterations
int iterations
Definition: module-nonsmooth-node.cc:100
DofOwnerOwner::iGetFirstIndex
virtual integer iGetFirstIndex(void) const
Definition: dofown.h:127
doublereal
double doublereal
Definition: colamd.c:52
NS_subsys::Poutput
Vec3 Poutput
Definition: module-nonsmooth-node.cc:113
integer
long int integer
Definition: colamd.c:51
IncludeParser::GetLineData
virtual HighParser::ErrOut GetLineData(void) const
Definition: parsinc.cc:697
plane::mu
doublereal mu
Definition: module-nonsmooth-node.cc:76
WithLabel::GetLabel
unsigned int GetLabel(void) const
Definition: withlab.cc:62
DataManager::ReadNode
Node * ReadNode(MBDynParser &HP, Node::Type type) const
Definition: dataman3.cc:2309
solver_parameters::resulting_error
double resulting_error
Definition: mbdyn_siconos.h:94
MatrixHandler::MatVecMul
virtual VectorHandler & MatVecMul(VectorHandler &out, const VectorHandler &in) const
Definition: mh.cc:332
NS_subsys::niter
int niter
Definition: module-nonsmooth-node.cc:111
Elem::DescribeDof
virtual std::ostream & DescribeDof(std::ostream &out, const char *prefix="", bool bInitial=false) const
Definition: elem.cc:124
NS_subsys::hstep
doublereal hstep
Definition: module-nonsmooth-node.cc:89
solver_parameters::solvertol
double solvertol
Definition: mbdyn_siconos.h:86
solver_parameters::solver
LCPsolver solver
Definition: mbdyn_siconos.h:85
IRa
static FullMatrixHandler IRa
Definition: module-nonsmooth-node.cc:213
HighParser::GetReal
virtual doublereal GetReal(const doublereal &dDefval=0.0)
Definition: parser.cc:1056