MBDyn-1.7.3
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Groups
module-uni_in_plane.cc
Go to the documentation of this file.
1 /* $Header: /var/cvs/mbdyn/mbdyn/mbdyn-1.0/modules/module-uni_in_plane/module-uni_in_plane.cc,v 1.6 2017/01/12 14:58:31 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  * Paolo Mantegazza <mantegazza@aero.polimi.it>
10  *
11  * Dipartimento di Ingegneria Aerospaziale - Politecnico di Milano
12  * via La Masa, 34 - 20156 Milano, Italy
13  * http://www.aero.polimi.it
14  *
15  * Changing this copyright notice is forbidden.
16  *
17  * This program is free software; you can redistribute it and/or modify
18  * it under the terms of the GNU General Public License as published by
19  * the Free Software Foundation (version 2 of the License).
20  *
21  *
22  * This program is distributed in the hope that it will be useful,
23  * but WITHOUT ANY WARRANTY; without even the implied warranty of
24  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
25  * GNU General Public License for more details.
26  *
27  * You should have received a copy of the GNU General Public License
28  * along with this program; if not, write to the Free Software
29  * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
30  */
31 
32 /*
33  AUTHOR: Reinhard Resch <r.resch@secop.com>
34  Copyright (C) 2015(-2017) all rights reserved.
35 
36  The copyright of this code is transferred
37  to Pierangelo Masarati and Paolo Mantegazza
38  for use in the software MBDyn as described
39  in the GNU Public License version 2.1
40 */
41 
42 #include <algorithm>
43 #include <cassert>
44 #include <cfloat>
45 #include <cmath>
46 #include <cstdlib>
47 #include <cstring>
48 #include <iostream>
49 #include <iomanip>
50 #include <sstream>
51 #include <limits>
52 #include <memory>
53 #include <vector>
54 
55 using namespace std;
56 
57 #ifdef HAVE_CONFIG_H
58 #include "mbconfig.h" /* This goes first in every *.c,*.cc file */
59 #endif /* HAVE_CONFIG_H */
60 
61 #include <dataman.h>
62 #include <userelem.h>
63 
64 #include <gradient.h>
65 #include <matvec.h>
66 #include <matvecass.h>
67 #include "module-uni_in_plane.h"
68 
69 #ifdef USE_AUTODIFF
70 class UniInPlaneFriction: virtual public Elem, public UserDefinedElem
71 {
72 public:
73  UniInPlaneFriction(unsigned uLabel, const DofOwner *pDO,
74  DataManager* pDM, MBDynParser& HP);
75  virtual ~UniInPlaneFriction(void);
76  virtual unsigned int iGetNumDof(void) const;
77  virtual DofOrder::Order GetDofType(unsigned int i) const;
78  virtual DofOrder::Order GetEqType(unsigned int i) const;
79  virtual std::ostream& DescribeDof(std::ostream& out, const char *prefix, bool bInitial) const;
80  virtual std::ostream& DescribeEq(std::ostream& out, const char *prefix, bool bInitial) const;
81  virtual unsigned int iGetNumPrivData(void) const;
82  virtual unsigned int iGetPrivDataIdx(const char *s) const;
83  virtual doublereal dGetPrivData(unsigned int i) const;
84  virtual void Output(OutputHandler& OH) const;
85  virtual void WorkSpaceDim(integer* piNumRows, integer* piNumCols) const;
86  VariableSubMatrixHandler&
87  AssJac(VariableSubMatrixHandler& WorkMat,
88  doublereal dCoef,
89  const VectorHandler& XCurr,
90  const VectorHandler& XPrimeCurr);
91  SubVectorHandler&
92  AssRes(SubVectorHandler& WorkVec,
93  doublereal dCoef,
94  const VectorHandler& XCurr,
95  const VectorHandler& XPrimeCurr);
96  template <typename T>
97  inline void
98  AssRes(grad::GradientAssVec<T>& WorkVec,
99  doublereal dCoef,
100  const grad::GradientVectorHandler<T>& XCurr,
101  const grad::GradientVectorHandler<T>& XPrimeCurr,
102  enum grad::FunctionCall func);
103  virtual void AfterConvergence(const VectorHandler& X,
104  const VectorHandler& XP);
105  int iGetNumConnectedNodes(void) const;
106  void GetConnectedNodes(std::vector<const Node *>& connectedNodes) const;
107  void SetValue(DataManager *pDM, VectorHandler& X, VectorHandler& XP,
108  SimulationEntity::Hints *ph);
109  std::ostream& Restart(std::ostream& out) const;
110  virtual unsigned int iGetInitialNumDof(void) const;
111  virtual void
112  InitialWorkSpaceDim(integer* piNumRows, integer* piNumCols) const;
113  VariableSubMatrixHandler&
114  InitialAssJac(VariableSubMatrixHandler& WorkMat,
115  const VectorHandler& XCurr);
116  SubVectorHandler&
117  InitialAssRes(SubVectorHandler& WorkVec, const VectorHandler& XCurr);
118 
119  private:
120  static const int iNumDofGradient = 15;
121 
122  struct ContactPoint
123  {
124  inline explicit ContactPoint(const Vec3& offset=Zero3, doublereal s=std::numeric_limits<doublereal>::max());
125 
126  inline void AfterConvergence()
127  {
128  lambdaPrev = lambda;
129  }
130 
131  inline void UpdateReaction(
132  const grad::Vector<grad::Gradient<iNumDofGradient>, 3>&,
133  const grad::Vector<grad::Gradient<iNumDofGradient>, 3>&,
134  const grad::Vector<grad::Gradient<iNumDofGradient>, 3>&,
135  const grad::Vector<grad::Gradient<iNumDofGradient>, 3>&,
136  const grad::Gradient<iNumDofGradient>&,
137  const grad::Gradient<iNumDofGradient>&)
138  {
139 
140  }
141 
142  inline void UpdateReaction(
143  const grad::Vector<doublereal, 3>& F1,
144  const grad::Vector<doublereal, 3>& M1,
145  const grad::Vector<doublereal, 3>& F2,
146  const grad::Vector<doublereal, 3>& M2,
147  doublereal dXn,
148  doublereal lambda);
149 
150  inline void UpdateFriction(const grad::Vector<grad::Gradient<iNumDofGradient>, 2>& U,
151  const grad::Vector<grad::Gradient<iNumDofGradient>, 2>& tau,
152  const grad::Vector<grad::Gradient<iNumDofGradient>, 2>& z,
153  const grad::Vector<grad::Gradient<iNumDofGradient>, 2>& zP)
154  {
155 
156  }
157 
158  inline void UpdateFriction(const grad::Vector<doublereal, 2>& U,
159  const grad::Vector<doublereal, 2>& tau,
160  const grad::Vector<doublereal, 2>& z,
161  const grad::Vector<doublereal, 2>& zP);
162 
163  grad::Vector<doublereal, 3> o1;
164  doublereal s;
165  doublereal lambda, lambdaPrev, dXn;
166  grad::Vector<doublereal, 2> U, tau, z, zP;
167  grad::Vector<doublereal, 3> F1, M1, F2, M2;
168  grad::LocalDofMap dof;
169  };
170 
171  static const int iNumPrivData = 22;
172  static const int iNumPrivDataGlobal = 1;
173 
174  static const struct PrivateData
175  {
176  enum Index
177  {
178  LAMBDA,
179  DXN,
180  TAU1,
181  TAU2,
182  U1,
183  U2,
184  Z1,
185  Z2,
186  ZP1,
187  ZP2,
188  F1X,
189  F1Y,
190  F1Z,
191  M1X,
192  M1Y,
193  M1Z,
194  F2X,
195  F2Y,
196  F2Z,
197  M2X,
198  M2Y,
199  M2Z
200  };
201  char name[10];
202  } rgPrivData[iNumPrivData];
203 
204  private:
205  typedef std::vector<ContactPoint> ContactPointVec_t;
206  typedef ContactPointVec_t::iterator ContactPointIter_t;
207  typedef ContactPointVec_t::const_iterator const_ContactPointIter_t;
208 
209  const DataManager* const pDM;
210  const StructNode* pNode1;
211  ContactPointVec_t ContactPoints1;
212  const StructNode* pNode2;
213  grad::Vector<doublereal, 3> o2;
214  grad::Matrix<doublereal, 3, 3> Rp;
215  doublereal epsilon, dFmax, tCurr, tPrev;
216  bool bEnableFriction;
217  grad::Matrix<doublereal, 2, 2> Mk, Ms, Mk2, Ms2, invMk2_sigma0;
218  grad::Matrix<doublereal, 2, 2> sigma0, sigma1, sigma2;
219  doublereal vs, a;
220  DriveOwner m_oInitialAssembly, m_oOffset;
221 };
222 
223 UniInPlaneFriction::ContactPoint::ContactPoint(const Vec3& offset, doublereal s)
224  :o1(offset),
225  s(s),
226  lambda(0.),
227  lambdaPrev(0.),
228  dXn(0.),
229  F1(Zero3),
230  M1(Zero3),
231  F2(Zero3),
232  M2(Zero3)
233 {
234 
235 }
236 
237 void UniInPlaneFriction::ContactPoint::UpdateReaction(
238  const grad::Vector<doublereal, 3>& F1,
239  const grad::Vector<doublereal, 3>& M1,
240  const grad::Vector<doublereal, 3>& F2,
241  const grad::Vector<doublereal, 3>& M2,
242  const doublereal dXn,
243  const doublereal lambda)
244 {
245  this->F1 = F1;
246  this->M1 = M1;
247  this->F2 = F2;
248  this->M2 = M2;
249  this->dXn = dXn;
250  this->lambda = lambda;
251 }
252 
253 void UniInPlaneFriction::ContactPoint::UpdateFriction(const grad::Vector<doublereal, 2>& U,
254  const grad::Vector<doublereal, 2>& tau,
255  const grad::Vector<doublereal, 2>& z,
256  const grad::Vector<doublereal, 2>& zP)
257 {
258  this->U = U;
259  this->tau = tau;
260  this->z = z;
261  this->zP = zP;
262 }
263 
264 const UniInPlaneFriction::PrivateData UniInPlaneFriction::rgPrivData[iNumPrivData] =
265 {
266  {"lambda"},
267  {"dXn"},
268  {"tau1"},
269  {"tau2"},
270  {"U1"},
271  {"U2"},
272  {"z1"},
273  {"z2"},
274  {"zP1"},
275  {"zP2"},
276  {"F1x"},
277  {"F1y"},
278  {"F1z"},
279  {"M1x"},
280  {"M1y"},
281  {"M1z"},
282  {"F2x"},
283  {"F2y"},
284  {"F2z"},
285  {"M2x"},
286  {"M2y"},
287  {"M2z"}
288 };
289 
290 UniInPlaneFriction::UniInPlaneFriction(
291  unsigned uLabel, const DofOwner *pDO,
292  DataManager* pDM, MBDynParser& HP)
293 : Elem(uLabel, flag(0)),
294  UserDefinedElem(uLabel, pDO),
295  pDM(pDM),
296  pNode1(0),
297  pNode2(0),
298  o2(Zero3),
299  Rp(Eye3),
300  epsilon(0.),
301  dFmax(0.),
302  bEnableFriction(false)
303 {
304  tCurr = tPrev = pDM->dGetTime();
305 
306  using namespace grad;
307  // help
308  if (HP.IsKeyWord("help"))
309  {
310  silent_cout(
311  "\n"
312  "Module: UniInPlaneFriction\n"
313  "\n"
314  " This element implements the unilateral contact between a point and a plane\n"
315  "\n"
316  " unilateral in plane,\n"
317  " node1, (label) <node1>,\n"
318  " [ offset, (integer) <count>,\n"
319  " (Vec3) <offset1>,\n"
320  " [stiffness, (real) <stiffness1>,]\n"
321  " [ ... , ] ]\n"
322  " epsilon, (real) <epsilon>,\n"
323  " node2, (label) <node2>,\n"
324  " [ offset, (Vec3) <offset>, ]\n"
325  " [ hinge, (Mat3x3) <hinge>, ]\n"
326  " [ initial assembly, (DriveCaller) <initial_assembly>, ]\n"
327  " [ offset plane, (DriveCaller) <normal_offset> ]"
328  "\n"
329  << std::endl);
330 
331  if (!HP.IsArg())
332  {
333  /*
334  * Exit quietly if nothing else is provided
335  */
336  throw NoErr(MBDYN_EXCEPT_ARGS);
337  }
338  }
339 
340  if ( !HP.IsKeyWord("node1") )
341  {
342  silent_cerr("unilateral in plane(" << GetLabel() << "): keyword \"node1\" expected at line " << HP.GetLineData() << std::endl);
343  throw ErrGeneric(MBDYN_EXCEPT_ARGS);
344  }
345 
346  pNode1 = pDM->ReadNode<StructNode, Node::STRUCTURAL>(HP);
347 
348  if (!pNode1) {
349  silent_cerr("unilateral in plane(" << GetLabel() << "): structural node expected at line " << HP.GetLineData() << std::endl);
350  throw ErrGeneric(MBDYN_EXCEPT_ARGS);
351  }
352 
353  if ( HP.IsKeyWord("offset") )
354  {
355  const integer N = HP.GetInt();
356 
357  if (N < 1)
358  {
359  silent_cerr("unilateral in plan(" << GetLabel()
360  << "): number of offsets must be greater than zero at line "
361  << HP.GetLineData() << std::endl);
362  throw ErrGeneric(MBDYN_EXCEPT_ARGS);
363  }
364 
365  const ReferenceFrame refNode1(pNode1);
366 
367  ContactPoints1.reserve(N);
368 
369  for (int i = 0; i < N; ++i)
370  {
371  const Vec3 o1 = HP.GetPosRel(refNode1);
372  const doublereal s = HP.IsKeyWord("stiffness") ? HP.GetReal() : std::numeric_limits<doublereal>::max();
373  ContactPoints1.push_back(ContactPoint(o1, s));
374  }
375  }
376  else
377  {
378  ContactPoints1.push_back(ContactPoint(Zero3));
379  }
380 
381  if ( !HP.IsKeyWord("epsilon") )
382  {
383  silent_cerr("unilateral in plane(" << GetLabel() << "): keyword \"epsilon\" expected at line " << HP.GetLineData() << std::endl);
384  throw ErrGeneric(MBDYN_EXCEPT_ARGS);
385  }
386 
387  epsilon = HP.GetReal();
388 
389  if ( epsilon <= 0 )
390  {
391  silent_cerr("unilateral in plane(" << GetLabel() << "): epsilon must be greater than zero at line " << HP.GetLineData() << std::endl);
392  throw ErrGeneric(MBDYN_EXCEPT_ARGS);
393  }
394 
395  dFmax = HP.IsKeyWord("max" "force" "increment") ? HP.GetReal() : std::numeric_limits<doublereal>::max();
396 
397  if ( !HP.IsKeyWord("node2") )
398  {
399  silent_cerr("unilateral in plane(" << GetLabel() << "): keyword \"node2\" expected at line " << HP.GetLineData() << std::endl);
400  throw ErrGeneric(MBDYN_EXCEPT_ARGS);
401  }
402 
403  pNode2 = pDM->ReadNode<StructNode, Node::STRUCTURAL>(HP);
404 
405  if (!pNode2) {
406  silent_cerr("unilateral in plane(" << GetLabel() << "): structural node expected at line " << HP.GetLineData() << std::endl);
407  throw ErrGeneric(MBDYN_EXCEPT_ARGS);
408  }
409 
410  const ReferenceFrame refNode2(pNode2);
411 
412  if ( HP.IsKeyWord("offset") )
413  o2 = HP.GetPosRel(refNode2);
414 
415  if ( HP.IsKeyWord("hinge") )
416  {
417  Rp = HP.GetRotRel(refNode2);
418  }
419 
420  if (HP.IsKeyWord("coulomb" "friction" "coefficient") || HP.IsKeyWord("coulomb" "friction" "coefficient" "x"))
421  {
422  bEnableFriction = true;
423 
424  const doublereal mukx = HP.GetReal();
425 
426  doublereal muky;
427 
428  if (HP.IsKeyWord("coulomb" "friction" "coefficient" "y")) {
429  muky = HP.GetReal();
430  } else {
431  muky = mukx;
432  }
433 
434  doublereal musx, musy;
435 
436  if (HP.IsKeyWord("static" "friction" "coefficient")
437  || HP.IsKeyWord("static" "friction" "coefficient" "x")) {
438  musx = HP.GetReal();
439 
440  if (HP.IsKeyWord("static" "friction" "coefficient" "y")) {
441  musy = HP.GetReal();
442  } else {
443  musy = musx;
444  }
445  } else {
446  musx = mukx;
447  musy = muky;
448  }
449 
450  if (HP.IsKeyWord("sliding" "velocity" "coefficient")) {
451  vs = HP.GetReal();
452  } else {
453  vs = 1.;
454  }
455 
456  if (HP.IsKeyWord("sliding" "velocity" "exponent")) {
457  a = HP.GetReal();
458  } else {
459  a = 1.;
460  }
461 
462  if (!(HP.IsKeyWord("micro" "slip" "stiffness") || HP.IsKeyWord("micro" "slip" "stiffness" "x"))) {
463  silent_cerr("unilateral in plane("
464  << GetLabel()
465  << "): keyword \"micro slip stiffness\" or \"micro slip stiffness x\" expected at line "
466  << HP.GetLineData() << std::endl);
467 
468  throw ErrGeneric(MBDYN_EXCEPT_ARGS);
469  }
470 
471  const doublereal sigma0x = HP.GetReal();
472 
473  doublereal sigma0y;
474 
475  if (HP.IsKeyWord("micro" "slip" "stiffness" "y")) {
476  sigma0y = HP.GetReal();
477  } else {
478  sigma0y = sigma0x;
479  }
480 
481  doublereal sigma1x, sigma1y;
482 
483  if (HP.IsKeyWord("micro" "slip" "damping") || HP.IsKeyWord("micro" "slip" "damping" "x")) {
484  sigma1x = HP.GetReal();
485 
486  if (HP.IsKeyWord("micro" "slip" "damping" "y")) {
487  sigma1y = HP.GetReal();
488  } else {
489  sigma1y = sigma1x;
490  }
491  } else {
492  sigma1x = 0.;
493  sigma1y = 0.;
494  }
495 
496  if (HP.IsKeyWord("viscous" "friction" "coefficient") || HP.IsKeyWord("viscous" "friction" "coefficient" "x"))
497  {
498  sigma2(1, 1) = HP.GetReal();
499  }
500 
501  if (HP.IsKeyWord("viscous" "friction" "coefficient" "y"))
502  {
503  sigma2(2, 2) = HP.GetReal();
504  }
505  else
506  {
507  sigma2(2, 2) = sigma2(1, 1);
508  }
509 
510  Mk(1, 1) = mukx;
511  Mk(2, 2) = muky;
512 
513  Mk2 = Mk * Mk;
514 
515  Ms(1, 1) = musx;
516  Ms(2, 2) = musy;
517 
518  Ms2 = Ms * Ms;
519 
520  sigma0(1, 1) = sigma0x;
521  sigma0(2, 2) = sigma0y;
522 
523  sigma1(1, 1) = sigma1x;
524  sigma1(2, 2) = sigma1y;
525 
526  invMk2_sigma0 = Inv(Mk2) * sigma0;
527  }
528 
529  m_oInitialAssembly.Set(HP.IsKeyWord("initial" "assembly") ? HP.GetDriveCaller() : new OneDriveCaller);
530  m_oOffset.Set(HP.IsKeyWord("offset" "plane") ? HP.GetDriveCaller() : new NullDriveCaller);
531 
532  SetOutputFlag(pDM->fReadOutput(HP, Elem::LOADABLE));
533 
534  std::ostream& out = pDM->GetLogFile();
535 
536  out << "unilateral in plane: " << GetLabel() << " " << pNode1->GetLabel() << " " << ContactPoints1.size() << " ";
537 
538  for ( const_ContactPointIter_t it = ContactPoints1.begin(); it != ContactPoints1.end(); ++it )
539  out << it->o1 << " ";
540 
541  out << pNode2->GetLabel() << " " << o2 << " " << Rp << std::endl;
542 }
543 
544 UniInPlaneFriction::~UniInPlaneFriction(void)
545 {
546  // destroy private data
547 }
548 
549 unsigned int UniInPlaneFriction::iGetNumDof(void) const
550 {
551  return ContactPoints1.size() * (1 + 2 * bEnableFriction);
552 }
553 
554 DofOrder::Order UniInPlaneFriction::GetDofType(unsigned int i) const
555 {
556  const int iNumDof = 1 + 2 * bEnableFriction;
557 
558  switch (i % iNumDof)
559  {
560  case 0:
561  return DofOrder::ALGEBRAIC;
562 
563  case 1:
564  case 2:
565  return DofOrder::DIFFERENTIAL;
566 
567  default:
568  ASSERT(false);
569  throw ErrGeneric(MBDYN_EXCEPT_ARGS);
570  }
571 }
572 
573 DofOrder::Order UniInPlaneFriction::GetEqType(unsigned int i) const
574 {
575  return GetDofType(i);
576 }
577 
578 std::ostream& UniInPlaneFriction::DescribeDof(std::ostream& out, const char *prefix, bool bInitial) const
579 {
580  integer iIndex = iGetFirstIndex();
581 
582  const int N = ContactPoints1.size();
583 
584  for ( int i = 1; i <= N; ++i )
585  {
586  out << prefix << ++iIndex << ": reaction force [lambda(" << i << ")]" << std::endl;
587 
588  if (bEnableFriction)
589  {
590  for (int j = 1; j <= 2; ++j)
591  {
592  out << prefix << ++iIndex << ": sticktion state [z" << j << "(" << i << ")]" << std::endl;
593  }
594  }
595  }
596 
597  return out;
598 }
599 
600 std::ostream& UniInPlaneFriction::DescribeEq(std::ostream& out, const char *prefix, bool bInitial) const
601 {
602  integer iIndex = iGetFirstIndex();
603 
604  const int N = ContactPoints1.size();
605 
606  for ( int i = 1; i <= N; ++i )
607  {
608  out << prefix << ++iIndex << ": constraint equation [c" << i << "]" << std::endl;
609 
610  if (bEnableFriction)
611  {
612  for (int j = 1; j <= 2; ++j)
613  {
614  out << prefix << ++iIndex << ": sticktion state variation [Phi" << j << "(" << i << ")]" << std::endl;
615  }
616  }
617  }
618 
619  return out;
620 }
621 
622 unsigned int UniInPlaneFriction::iGetNumPrivData(void) const
623 {
624  return iNumPrivDataGlobal + ContactPoints1.size() * iNumPrivData;
625 }
626 
627 unsigned int UniInPlaneFriction::iGetPrivDataIdx(const char *s) const
628 {
629  if (0 == strcmp(s, "max" "dt"))
630  {
631  return 1u;
632  }
633 
634  std::istringstream is(s);
635  std::string tok1;
636  char tok2;
637  unsigned int i;
638 
639  std::getline(is, tok1, '[');
640 
641  is >> i >> tok2;
642 
643  if ( !is.good() ||
644  tok2 != ']' ||
645  i == 0 ||
646  i > ContactPoints1.size() )
647  {
648  goto error_return;
649  }
650 
651  for (int j = 0; j < iNumPrivData; ++j)
652  {
653  if (tok1 == rgPrivData[j].name)
654  {
655  return iNumPrivDataGlobal + (i - 1) * iNumPrivData + j + 1;
656  }
657  }
658 
659 error_return:
660  silent_cerr("unilateral in plane(" << GetLabel() << "): private data \"" << s << "\" not available" << std::endl);
661  return 0;
662 }
663 
664 doublereal UniInPlaneFriction::dGetPrivData(unsigned int i) const
665 {
666  if (i == 1u)
667  {
668  doublereal dtmax = std::numeric_limits<doublereal>::max();
669 
670  for (const_ContactPointIter_t j = ContactPoints1.begin(); j != ContactPoints1.end(); ++j)
671  {
672  const doublereal dF = j->lambda - j->lambdaPrev;
673 
674  if (std::abs(dF) > dFmax)
675  {
676  const doublereal dt = tCurr - tPrev;
677  dtmax = std::min(dtmax, std::abs(dt / dF * dFmax));
678  }
679  }
680 
681  return dtmax;
682  }
683 
684  const div_t idx = div(i - 1 - iNumPrivDataGlobal, iNumPrivData);
685 
686  if ( idx.quot < 0 || ::size_t(idx.quot) >= ContactPoints1.size() || idx.rem < 0 || idx.rem >= iNumPrivData )
687  {
688  silent_cerr("unilateral in plane(" << GetLabel() << "): index " << i << " out of range" << std::endl);
689  throw ErrGeneric(MBDYN_EXCEPT_ARGS);
690  }
691 
692  const ContactPoint& cont = ContactPoints1[idx.quot];
693 
694  switch (idx.rem)
695  {
696  case PrivateData::LAMBDA:
697  return cont.lambda;
698 
699  case PrivateData::DXN:
700  return cont.dXn;
701 
702  case PrivateData::TAU1:
703  case PrivateData::TAU2:
704  return cont.tau(idx.rem - PrivateData::TAU1 + 1);
705 
706  case PrivateData::U1:
707  case PrivateData::U2:
708  return cont.U(idx.rem - PrivateData::U1 + 1);
709 
710  case PrivateData::Z1:
711  case PrivateData::Z2:
712  return cont.z(idx.rem - PrivateData::Z1 + 1);
713 
714  case PrivateData::ZP1:
715  case PrivateData::ZP2:
716  return cont.zP(idx.rem - PrivateData::ZP1 + 1);
717 
718  case PrivateData::F1X:
719  case PrivateData::F1Y:
720  case PrivateData::F1Z:
721  return cont.F1(idx.rem - PrivateData::F1X + 1);
722 
723  case PrivateData::M1X:
724  case PrivateData::M1Y:
725  case PrivateData::M1Z:
726  return cont.M1(idx.rem - PrivateData::M1X + 1);
727 
728  case PrivateData::F2X:
729  case PrivateData::F2Y:
730  case PrivateData::F2Z:
731  return cont.F2(idx.rem - PrivateData::F2X + 1);
732 
733  case PrivateData::M2X:
734  case PrivateData::M2Y:
735  case PrivateData::M2Z:
736  return cont.M2(idx.rem - PrivateData::M2X + 1);
737 
738  default:
739  throw ErrGeneric(MBDYN_EXCEPT_ARGS);
740  }
741 }
742 
743 void
744 UniInPlaneFriction::Output(OutputHandler& OH) const
745 {
746  if ( bToBeOutput() )
747  {
748  if ( OH.UseText(OutputHandler::LOADABLE) )
749  {
750  std::ostream& os = OH.Loadable();
751 
752  os << std::setw(8) << GetLabel();
753 
754  for (const_ContactPointIter_t it = ContactPoints1.begin(); it != ContactPoints1.end(); ++it)
755  os << " " << it->dXn << " " << it->F1 << " " << it->M1 << " " << it->F2 << " " << it->M2;
756 
757  os << std::endl;
758  }
759  }
760 }
761 
762 void
763 UniInPlaneFriction::WorkSpaceDim(integer* piNumRows, integer* piNumCols) const
764 {
765  *piNumRows = -(ContactPoints1.size() * iNumDofGradient);
766  *piNumCols = iNumDofGradient;
767 }
768 
769 VariableSubMatrixHandler&
770 UniInPlaneFriction::AssJac(VariableSubMatrixHandler& WorkMatVar,
771  doublereal dCoef,
772  const VectorHandler& XCurr,
773  const VectorHandler& XPrimeCurr)
774 {
775  using namespace grad;
776 
777  GradientAssVec<Gradient<iNumDofGradient> >::AssJac(this, WorkMatVar.SetSparse(), dCoef, XCurr, XPrimeCurr, REGULAR_JAC, 0);
778 
779  return WorkMatVar;
780 }
781 SubVectorHandler&
782 UniInPlaneFriction::AssRes(SubVectorHandler& WorkVec,
783  doublereal dCoef,
784  const VectorHandler& XCurr,
785  const VectorHandler& XPrimeCurr)
786 {
787  using namespace grad;
788 
789  tCurr = pDM->dGetTime();
790 
791  GradientAssVec<doublereal>::AssRes(this, WorkVec, dCoef, XCurr, XPrimeCurr, REGULAR_RES);
792 
793  return WorkVec;
794 }
795 
796 template <typename T>
797 inline void
798 UniInPlaneFriction::AssRes(grad::GradientAssVec<T>& WorkVec,
799  doublereal dCoef,
800  const grad::GradientVectorHandler<T>& XCurr,
801  const grad::GradientVectorHandler<T>& XPrimeCurr,
802  enum grad::FunctionCall func)
803 {
804  using namespace grad;
805  typedef Vector<T, 2> Vec2;
806  typedef Vector<T, 3> Vec3;
807  typedef Matrix<T, 3, 3> Mat3x3;
808 
809  const integer iFirstMomentumIndexNode1 = pNode1->iGetFirstMomentumIndex();
810  const integer iFirstMomentumIndexNode2 = pNode2->iGetFirstMomentumIndex();
811  const integer iFirstIndex = iGetFirstIndex();
812  integer iDofIndex = iFirstIndex;
813 
814  const integer N = ContactPoints1.size();
815  const doublereal alpha = m_oInitialAssembly.dGet();
816 
817  Vec3 X1, X2;
818  Mat3x3 R1, R2;
819  Vec2 z, zP, tau;
820  T lambda, kappa;
821 
822  for ( integer i = 1; i <= N; ++i )
823  {
824  ContactPoint& cont = ContactPoints1[i - 1];
825  cont.dof.Reset(func);
826 
827  pNode1->GetXCurr(X1, dCoef, func, &cont.dof);
828  pNode1->GetRCurr(R1, dCoef, func, &cont.dof);
829  pNode2->GetXCurr(X2, dCoef, func, &cont.dof);
830  pNode2->GetRCurr(R2, dCoef, func, &cont.dof);
831 
832  XCurr.dGetCoef(++iDofIndex, lambda, 1., &cont.dof);
833 
834  const Vector<doublereal, 3>& o1 = cont.o1;
835  const Vec3 dX = X1 + R1 * o1 - X2 - R2 * o2;
836  const T dXn = Dot(Rp.GetCol(3), Transpose(R2) * dX) - m_oOffset.dGet() + lambda / cont.s;
837 
838  const T d = 0.5 * (dXn - lambda);
839  const T c = 0.5 * (dXn + lambda) - sqrt(d * d + epsilon);
840 
841  if ( alpha != 0. )
842  WorkVec.AddItem(iDofIndex, c * alpha / dCoef);
843  else WorkVec.AddItem(iDofIndex, lambda / dCoef);
844 
845  Vec3 F1p = Rp.GetCol(3) * lambda;
846 
847  if (bEnableFriction)
848  {
849  Vec3 X1P, X2P, omega1, omega2;
850 
851  pNode1->GetVCurr(X1P, dCoef, func, &cont.dof);
852  pNode1->GetWCurr(omega1, dCoef, func, &cont.dof);
853  pNode2->GetVCurr(X2P, dCoef, func, &cont.dof);
854  pNode2->GetWCurr(omega2, dCoef, func, &cont.dof);
855 
856  const Vec3 dXP = X1P + Cross(omega1, Vec3(R1 * o1)) - X2P - Cross(omega2, Vec3(R2 * o2));
857  const Vec2 U = Transpose(SubMatrix<1,3,1,2>(Rp)) * Vec3(Transpose(R2) * Vec3(dXP + Cross(dX, omega2)));
858  const T norm_U = sqrt(Dot(U, U));
859 
860  if (norm_U == 0.) {
861  kappa = 0.;
862  } else {
863  const Vec2 Mk_U = Mk * U;
864  const Vec2 Ms_U = Ms * U;
865  const Vec2 Mk2_U = Mk2 * U;
866  const Vec2 Ms2_U = Ms2 * U;
867  const T norm_Mk2_U = sqrt(Dot(Mk2_U, Mk2_U));
868  const T a0 = norm_Mk2_U / sqrt(Dot(Mk_U, Mk_U));
869  const T a1 = sqrt(Dot(Ms2_U, Ms2_U)) / sqrt(Dot(Ms_U, Ms_U));
870  const T g = a0 + (a1 - a0) * exp(-pow(norm_U / vs, a));
871 
872  kappa = norm_Mk2_U / g;
873  }
874 
875  for (int i = 1; i <= 2; ++i)
876  {
877  XCurr.dGetCoef(iDofIndex + i, z(i), dCoef, &cont.dof);
878  XPrimeCurr.dGetCoef(iDofIndex + i, zP(i), 1., &cont.dof);
879  }
880 
881  const Vec2 Phi = (U - invMk2_sigma0 * z * kappa - zP) * alpha;
882 
883  for (int i = 1; i <= 2; ++i)
884  {
885  ++iDofIndex;
886 
887  if (alpha != 0.)
888  {
889  WorkVec.AddItem(iDofIndex, Phi(i));
890  }
891  else
892  {
893  WorkVec.AddItem(iDofIndex, z(i));
894  }
895  }
896 
897  tau = (sigma0 * z + sigma1 * zP) * lambda + sigma2 * U;
898 
899  for (int i = 1; i <= 2; ++i)
900  {
901  F1p -= Rp.GetCol(i) * tau(i);
902  }
903 
904  cont.UpdateFriction(U, tau, z, zP);
905  }
906 
907  F1p *= alpha;
908  const Vec3 F1 = R2 * F1p;
909  const Vec3 M1 = Cross(Vec3(R1 * o1), F1);
910  const Vec3 F2 = -F1;
911  const Vec3 M2 = Cross(Vec3(X1 + R1 * o1 - X2), F2);
912 
913  WorkVec.AddItem(iFirstMomentumIndexNode1 + 1, F1);
914  WorkVec.AddItem(iFirstMomentumIndexNode1 + 4, M1);
915  WorkVec.AddItem(iFirstMomentumIndexNode2 + 1, F2);
916  WorkVec.AddItem(iFirstMomentumIndexNode2 + 4, M2);
917 
918  cont.UpdateReaction(F1, M1, F2, M2, dXn, lambda);
919  }
920 }
921 
922 void UniInPlaneFriction::AfterConvergence(const VectorHandler& X,
923  const VectorHandler& XP)
924 {
925  tPrev = tCurr;
926 
927  for (ContactPointIter_t i = ContactPoints1.begin(); i != ContactPoints1.end(); ++i)
928  {
929  i->AfterConvergence();
930  }
931 }
932 
933 int
934 UniInPlaneFriction::iGetNumConnectedNodes(void) const
935 {
936  return 2;
937 }
938 
939 void
940 UniInPlaneFriction::GetConnectedNodes(std::vector<const Node *>& connectedNodes) const
941 {
942  connectedNodes.resize(iGetNumConnectedNodes());
943  connectedNodes[0] = pNode1;
944  connectedNodes[1] = pNode2;
945 }
946 
947 void
948 UniInPlaneFriction::SetValue(DataManager *pDM,
949  VectorHandler& X, VectorHandler& XP,
950  SimulationEntity::Hints *ph)
951 {
952  const int N = ContactPoints1.size();
953 
954  integer iDofIndex = iGetFirstIndex();
955 
956  for ( int i = 0; i < N; ++i )
957  {
958  X.PutCoef(++iDofIndex, ContactPoints1[i].lambda);
959 
960  if (bEnableFriction)
961  {
962  for (int j = 1; j <= 2; ++j)
963  {
964  X.PutCoef(++iDofIndex, ContactPoints1[i].z(j));
965  XP.PutCoef(iDofIndex, ContactPoints1[i].zP(j));
966  }
967  }
968  }
969 }
970 
971 std::ostream&
972 UniInPlaneFriction::Restart(std::ostream& out) const
973 {
974  return out;
975 }
976 
977 unsigned int
978 UniInPlaneFriction::iGetInitialNumDof(void) const
979 {
980  return 0;
981 }
982 
983 void
984 UniInPlaneFriction::InitialWorkSpaceDim(
985  integer* piNumRows,
986  integer* piNumCols) const
987 {
988  *piNumRows = 0;
989  *piNumCols = 0;
990 }
991 
992 VariableSubMatrixHandler&
993 UniInPlaneFriction::InitialAssJac(
994  VariableSubMatrixHandler& WorkMat,
995  const VectorHandler& XCurr)
996 {
997  WorkMat.SetNullMatrix();
998 
999  return WorkMat;
1000 }
1001 
1002 SubVectorHandler&
1003 UniInPlaneFriction::InitialAssRes(
1004  SubVectorHandler& WorkVec,
1005  const VectorHandler& XCurr)
1006 {
1007  WorkVec.ResizeReset(0);
1008 
1009  return WorkVec;
1010 }
1011 #endif
1012 
1013 bool uni_in_plane_set(void)
1014 {
1015 #ifdef USE_AUTODIFF
1016  UserDefinedElemRead *rf = new UDERead<UniInPlaneFriction>;
1017 
1018  if (!SetUDE("unilateral" "in" "plane",rf))
1019  {
1020  delete rf;
1021  return false;
1022  }
1023 #endif
1024 
1025  return true;
1026 }
1027 
1028 #ifndef STATIC_MODULES
1029 
1030 #ifdef __CYGWIN__
1031 #error Dynamic linking is not supported on cygwin!
1032 #error Use STATIC_MODULES instead!
1033 #else
1034 
1035 extern "C"
1036 {
1037 
1038 int module_init(const char *module_name, void *pdm, void *php)
1039 {
1040  if (!unilateral_in_plane_friction_set())
1041  {
1042  silent_cerr("contact: "
1043  "module_init(" << module_name << ") "
1044  "failed" << std::endl);
1045 
1046  return -1;
1047  }
1048 
1049  return 0;
1050 }
1051 
1052 }
1053 
1054 #endif
1055 
1056 #endif // ! STATIC_MODULE
1057 
DataManager::fReadOutput
flag fReadOutput(MBDynParser &HP, const T &t) const
Definition: dataman.h:1064
grad::exp
GradientExpression< UnaryExpr< FuncExp, Expr > > exp(const GradientExpression< Expr > &u)
Definition: gradient.h:2975
MBDynParser::GetRotRel
Mat3x3 GetRotRel(const ReferenceFrame &rf)
Definition: mbpar.cc:1795
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-uni_in_plane.cc:1038
Zero3
const Vec3 Zero3(0., 0., 0.)
flag
long int flag
Definition: mbdyn.h:43
grad::Transpose
MatrixExpression< TransposedMatrix< MatrixDirectExpr< Matrix< T, N_rows, N_cols > > >, N_cols, N_rows > Transpose(const Matrix< T, N_rows, N_cols > &A)
Definition: matvec.h:2206
grad::pow
GradientExpression< BinaryExpr< FuncPow, LhsExpr, RhsExpr > > pow(const GradientExpression< LhsExpr > &u, const GradientExpression< RhsExpr > &v)
Definition: gradient.h:2961
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
module-uni_in_plane.h
DataManager::dGetTime
doublereal dGetTime(void) const
Definition: dataman2.cc:165
Eye3
const Mat3x3 Eye3(1., 0., 0., 0., 1., 0., 0., 0., 1.)
iNumPrivData
static const unsigned int iNumPrivData
Definition: beam.cc:249
SimulationEntity::Hints
std::vector< Hint * > Hints
Definition: simentity.h:89
func
void func(const T &u, const T &v, const T &w, doublereal e, T &f)
Definition: gradienttest.cc:309
MBDynParser::GetPosRel
Vec3 GetPosRel(const ReferenceFrame &rf)
Definition: mbpar.cc:1331
grad::Inv
Matrix< T, 2, 2 > Inv(const Matrix< T, 2, 2 > &A)
Definition: matvec.h:3282
uni_in_plane_set
bool uni_in_plane_set(void)
Definition: module-uni_in_plane.cc:1013
VariableSubMatrixHandler::SetNullMatrix
void SetNullMatrix(void)
Definition: submat.h:1159
false
Definition: mbdyn.h:76
dof
static const char * dof[]
Definition: drvdisp.cc:195
HighParser::IsKeyWord
virtual bool IsKeyWord(const char *sKeyWord)
Definition: parser.cc:910
OutputHandler::Loadable
std::ostream & Loadable(void) const
Definition: output.h:506
grad::FunctionCall
FunctionCall
Definition: gradient.h:1018
grad::Dot
DotTraits< VectorExprLhs, VectorExprRhs, N_rows, N_rows >::ExpressionType Dot(const VectorExpression< VectorExprLhs, N_rows > &u, const VectorExpression< VectorExprRhs, N_rows > &v)
Definition: matvec.h:3133
ASSERT
#define ASSERT(expression)
Definition: colamd.c:977
grad::sqrt
GradientExpression< UnaryExpr< FuncSqrt, Expr > > sqrt(const GradientExpression< Expr > &u)
Definition: gradient.h:2974
VectorHandler::PutCoef
virtual void PutCoef(integer iRow, const doublereal &dCoef)=0
grad::Cross
VectorExpression< VectorCrossExpr< VectorLhsExpr, VectorRhsExpr >, 3 > Cross(const VectorExpression< VectorLhsExpr, 3 > &u, const VectorExpression< VectorRhsExpr, 3 > &v)
Definition: matvec.h:3248
NoErr
Definition: except.h:79
c
static std::stack< cleanup * > c
Definition: cleanup.cc:59
HighParser::IsArg
virtual bool IsArg(void)
Definition: parser.cc:807
Elem
Definition: elem.h:75
DataManager::GetLogFile
std::ostream & GetLogFile(void) const
Definition: dataman.h:326
SetUDE
bool SetUDE(const std::string &s, UserDefinedElemRead *rude)
Definition: userelem.cc:97
MBDynParser::GetDriveCaller
DriveCaller * GetDriveCaller(bool bDeferred=false)
Definition: mbpar.cc:2033
a
static const doublereal a
Definition: hfluid_.h:289
VariableSubMatrixHandler::SetSparse
SparseSubMatrixHandler & SetSparse(void)
Definition: submat.h:1178
integer
long int integer
Definition: colamd.c:51
IncludeParser::GetLineData
virtual HighParser::ErrOut GetLineData(void) const
Definition: parsinc.cc:697
DataManager::ReadNode
Node * ReadNode(MBDynParser &HP, Node::Type type) const
Definition: dataman3.cc:2309
OutputHandler::UseText
bool UseText(int out) const
Definition: output.cc:446
HighParser::GetReal
virtual doublereal GetReal(const doublereal &dDefval=0.0)
Definition: parser.cc:1056