MBDyn-1.7.3
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Groups
module-ballbearing_contact.cc
Go to the documentation of this file.
1 /* $Header: /var/cvs/mbdyn/mbdyn/mbdyn-1.0/modules/module-ballbearing_contact/module-ballbearing_contact.cc,v 1.2 2015/06/07 11:31:07 masarati Exp $ */
2 /*
3  * MBDyn (C) is a multibody analysis code.
4  * http://www.mbdyn.org
5  *
6  * Copyright (C) 1996-2013
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) 2013(-2013) 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 <limits>
43 #include <iostream>
44 #include <iomanip>
45 #include <cfloat>
46 #include <cassert>
47 #include <cmath>
48 #include <cstring>
49 #include <ctime>
50 
51 #ifdef HAVE_CONFIG_H
52 #include "mbconfig.h" /* This goes first in every *.c,*.cc file */
53 #endif /* HAVE_CONFIG_H */
54 
55 #include <dataman.h>
56 #include <userelem.h>
57 
58 #include "module-ballbearing_contact.h"
59 #ifdef USE_AUTODIFF
60 #include <vector>
61 #include <gradient.h>
62 #include <matvec.h>
63 #include <matvecass.h>
64 
65 using namespace grad;
66 
67 class BallBearingContact: virtual public Elem, public UserDefinedElem
68 {
69 public:
70  BallBearingContact(unsigned uLabel, const DofOwner *pDO,
71  DataManager* pDM, MBDynParser& HP);
72  virtual ~BallBearingContact(void);
73  virtual void Output(OutputHandler& OH) const;
74  virtual void WorkSpaceDim(integer* piNumRows, integer* piNumCols) const;
75  VariableSubMatrixHandler&
76  AssJac(VariableSubMatrixHandler& WorkMat,
77  doublereal dCoef,
78  const VectorHandler& XCurr,
79  const VectorHandler& XPrimeCurr);
80  SubVectorHandler&
81  AssRes(SubVectorHandler& WorkVec,
82  doublereal dCoef,
83  const VectorHandler& XCurr,
84  const VectorHandler& XPrimeCurr);
85  template <typename T>
86  inline void
87  AssRes(GradientAssVec<T>& WorkVec,
88  doublereal dCoef,
89  const GradientVectorHandler<T>& XCurr,
90  const GradientVectorHandler<T>& XPrimeCurr,
91  enum FunctionCall func);
92  virtual void
93  AfterConvergence(const VectorHandler& X,
94  const VectorHandler& XP);
95  virtual unsigned int iGetNumPrivData(void) const;
96  virtual unsigned int iGetPrivDataIdx(const char *s) const;
97  virtual doublereal dGetPrivData(unsigned int i) const;
98  int iGetNumConnectedNodes(void) const;
99  void GetConnectedNodes(std::vector<const Node *>& connectedNodes) const;
100  void SetValue(DataManager *pDM, VectorHandler& X, VectorHandler& XP,
101  SimulationEntity::Hints *ph);
102  std::ostream& Restart(std::ostream& out) const;
103  virtual unsigned int iGetNumDof(void) const;
104  virtual std::ostream& DescribeDof(std::ostream& out,
105  const char *prefix, bool bInitial) const;
106  virtual std::ostream& DescribeEq(std::ostream& out,
107  const char *prefix, bool bInitial) const;
108  virtual DofOrder::Order GetDofType(unsigned int) const;
109  virtual DofOrder::Order GetEqType(unsigned int i) 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 index_type iNumADVars = 14;
121  const DataManager* const pDM;
122  const StructNode* pNode1;
123  doublereal R;
124  doublereal k;
125  Matrix<doublereal, 2, 2> Mk, Mk2, inv_Mk2, Ms, Ms2, sigma0, sigma1;
126  doublereal gamma, vs, beta;
127  doublereal dStictionStateEquScale;
128  doublereal dStictionStateDofScale;
129  doublereal tPrev;
130  doublereal tCurr;
131  doublereal dtMax;
132  doublereal dFMax;
133  doublereal dFMin;
134  bool bEnableFriction;
135  bool bFirstRes;
136 
137  struct Washer {
138  Washer()
139  : pNode2(0),
140  dPrev(0),
141  dCurr(0),
142  dd_dtPrev(0),
143  dd_dtCurr(0),
144  FnPrev(0),
145  FnCurr(0)
146  {
147 
148  }
149  const StructNode* pNode2;
150  Matrix<doublereal, 3, 3> Rt2;
151  Vector<doublereal, 3> o2;
152  LocalDofMap dof;
153  doublereal dPrev;
154  doublereal dCurr;
155  doublereal dd_dtPrev;
156  doublereal dd_dtCurr;
157  doublereal FnPrev;
158  doublereal FnCurr;
159  Vector<doublereal, 2> z;
160  Vector<doublereal, 2> zP;
161  Vector<doublereal, 2> uP;
162  };
163 
164  std::vector<Washer> washers;
165 
166  inline void CheckTimeStep(Washer& w, doublereal Fn, doublereal d, doublereal dn_dt);
167  inline void CheckTimeStep(Washer& w, const Gradient<iNumADVars>&, const Gradient<iNumADVars>&, const Gradient<iNumADVars>&);
168 
169  static const int iNumPrivData = 9;
170 
171  static const struct PrivData {
172  char szPattern[8];
173  } rgPrivData[iNumPrivData];
174 };
175 
176 const struct BallBearingContact::PrivData
177 BallBearingContact::rgPrivData[iNumPrivData] = {
178  {"d[%u]"},
179  {"dP[%u]"},
180  {"F[%u]"},
181  {"z1[%u]"},
182  {"z2[%u]"},
183  {"zP1[%u]"},
184  {"zP2[%u]"},
185  {"uP1[%u]"},
186  {"uP2[%u]"}
187 };
188 
189 BallBearingContact::BallBearingContact(
190  unsigned uLabel, const DofOwner *pDO,
191  DataManager* pDM, MBDynParser& HP)
192 : Elem(uLabel, flag(0)),
193  UserDefinedElem(uLabel, pDO),
194  pDM(pDM),
195  pNode1(0),
196  gamma(0.),
197  vs(0.),
198  beta(0.),
199  dStictionStateEquScale(0.),
200  dStictionStateDofScale(0.),
201  tPrev(-std::numeric_limits<doublereal>::max()),
202  tCurr(-std::numeric_limits<doublereal>::max()),
203  dtMax(std::numeric_limits<doublereal>::max()),
204  dFMax(std::numeric_limits<doublereal>::max()),
205  dFMin(0.),
206  bEnableFriction(false),
207  bFirstRes(true)
208 {
209  // help
210  if (HP.IsKeyWord("help")) {
211  silent_cout(
212  "\n"
213  "Module: ballbearing contact\n"
214  "\n"
215  " ball bearing contact,\n"
216  " ball, (label) <node1>,\n"
217  " ball radius, (Scalar) <R>,\n"
218  " washers, (Integer) <N>,\n"
219  " (label) <node2>,\n"
220  " [offset, (Vec3) <o2>,]\n"
221  " [orientation, (OrientationMatrix) <Rt2>,\n"
222  " ...\n"
223  " {elastic modulus, <E> |\n"
224  " elastic modulus ball, (Scalar) <E1>,\n"
225  " poisson ratio ball, (Scalar) <nu1>\n"
226  " elastic modulus washer, (Scalar) <E2>,\n"
227  " poisson ratio washer, (Scalar) <nu2>},\n"
228  " [damping factor, (Scalar) <ks>,]\n"
229  " {coulomb friction coefficient, (Scalar) <mu> |\n"
230  " coulomb friction coefficient x, (Scalar) <mux>,\n"
231  " coulomb friction coefficient y, (Scalar) <muy>},\n"
232  " [{static friction coefficient, (Scalar) <mus> |\n"
233  " static friction coefficient x, (Scalar) <musx>,\n"
234  " static friction coefficient y, (Scalar) <musy>},]\n"
235  " [sliding velocity coefficient, (Scalar) <vs>,]\n"
236  " [sliding velocity exponent, (Scalar) <gamma>,]\n"
237  " {micro slip stiffness, (Scalar) <sigma0> |\n"
238  " micro slip stiffness x, (Scalar) <sigma0x>,\n"
239  " micro slip stiffness y, (Scalar) <sigma0y>}\n"
240  " [,{micro slip damping, (Scalar) <sigma1>, |\n"
241  " micro slip damping x, (Scalar) <sigma1x>,\n"
242  " micro slip damping y, (Scalar) <sigma1y>}]\n"
243  << std::endl);
244 
245  if (!HP.IsArg()) {
246  /*
247  * Exit quietly if nothing else is provided
248  */
249  throw NoErr(MBDYN_EXCEPT_ARGS);
250  }
251  }
252 
253  if (!HP.IsKeyWord("ball")) {
254  silent_cerr("ball bearing contact" << GetLabel()
255  << "): keyword \"ball\" expected at line "
256  << HP.GetLineData() << std::endl);
257  throw ErrGeneric(MBDYN_EXCEPT_ARGS);
258  }
259 
260  pNode1 = pDM->ReadNode<const StructNode, Node::STRUCTURAL>(HP);
261 
262  if (!HP.IsKeyWord("ball" "radius") ) {
263  silent_cerr("ball bearing contact(" << GetLabel()
264  << "): keyword \"ball radius\" expected at line "
265  << HP.GetLineData() << std::endl);
266 
267  throw ErrGeneric(MBDYN_EXCEPT_ARGS);
268  }
269 
270  R = HP.GetReal();
271 
272  if (!HP.IsKeyWord("washers")) {
273  silent_cerr("ball bearing contact" << GetLabel()
274  << "): keyword \"washers\" expected at line "
275  << HP.GetLineData() << std::endl);
276  throw ErrGeneric(MBDYN_EXCEPT_ARGS);
277  }
278 
279  const integer N = HP.GetInt();
280 
281  washers.resize(N);
282 
283  for (std::vector<Washer>::iterator i = washers.begin(); i != washers.end(); ++i) {
284  i->pNode2 = pDM->ReadNode<const StructNode, Node::STRUCTURAL>(HP);
285 
286  if ( HP.IsKeyWord("offset") ) {
287  i->o2 = HP.GetPosRel(ReferenceFrame(i->pNode2));
288  } else {
289  i->o2 = Zero3;
290  }
291 
292  if (HP.IsKeyWord("orientation")) {
293  i->Rt2 = HP.GetRotRel(ReferenceFrame(i->pNode2));
294  } else {
295  i->Rt2 = Eye3;
296  }
297  }
298 
299  doublereal E;
300 
301  if ( HP.IsKeyWord("elastic" "modulus") ) {
302  E = HP.GetReal();
303  } else {
304  if ( !HP.IsKeyWord("elastic" "modulus" "ball") ) {
305  silent_cerr("ball bearing contact(" << GetLabel()
306  << "): keyword \"elastic modulus\" or "
307  "\"elastic modulus ball\" expected at line "
308  << HP.GetLineData() << std::endl);
309 
310  throw ErrGeneric(MBDYN_EXCEPT_ARGS);
311  }
312 
313  const doublereal E1 = HP.GetReal();
314 
315  if ( !HP.IsKeyWord("poisson" "ratio" "ball") ) {
316  silent_cerr("ball bearing contact(" << GetLabel()
317  << "): keyword \"poisson ratio ball\" expected at line "
318  << HP.GetLineData() << std::endl);
319 
320  throw ErrGeneric(MBDYN_EXCEPT_ARGS);
321  }
322 
323  const doublereal nu1 = HP.GetReal();
324 
325  if ( !HP.IsKeyWord("elastic" "modulus" "washer") ) {
326  silent_cerr("ball bearing contact(" << GetLabel()
327  << "): keyword \"elastic modulus washer\" expected at line "
328  << HP.GetLineData() << std::endl);
329 
330  throw ErrGeneric(MBDYN_EXCEPT_ARGS);
331  }
332 
333  const doublereal E2 = HP.GetReal();
334 
335  if ( !HP.IsKeyWord("poisson" "ratio" "washer") ) {
336  silent_cerr("ball bearing contact(" << GetLabel()
337  << "): keyword \"poisson ratio washer\" expected at line "
338  << HP.GetLineData() << std::endl);
339 
340  throw ErrGeneric(MBDYN_EXCEPT_ARGS);
341  }
342 
343  const doublereal nu2 = HP.GetReal();
344 
345  E = 1 / ((1 - nu1 * nu1) / E1 + (1 - nu2 * nu2) / E2);
346  }
347 
348  k = 4./3. * E * sqrt(R);
349 
350  if (HP.IsKeyWord("damping" "factor")) {
351  const doublereal ks = HP.GetReal();
352  beta = 3./2. * ks * k;
353  } else {
354  beta = 0.;
355  }
356 
357  if (HP.IsKeyWord("coulomb" "friction" "coefficient")
358  || HP.IsKeyWord("coulomb" "friction" "coefficient" "x")) {
359  bEnableFriction = true;
360 
361  const doublereal mukx = HP.GetReal();
362 
363  doublereal muky;
364 
365  if (HP.IsKeyWord("coulomb" "friction" "coefficient" "y")) {
366  muky = HP.GetReal();
367  } else {
368  muky = mukx;
369  }
370 
371  doublereal musx, musy;
372 
373  if (HP.IsKeyWord("static" "friction" "coefficient")
374  || HP.IsKeyWord("static" "friction" "coefficient" "x")) {
375  musx = HP.GetReal();
376 
377  if (HP.IsKeyWord("static" "friction" "coefficient" "y")) {
378  musy = HP.GetReal();
379  } else {
380  musy = musx;
381  }
382  } else {
383  musx = mukx;
384  musy = muky;
385  }
386 
387  if (HP.IsKeyWord("sliding" "velocity" "coefficient")) {
388  vs = HP.GetReal();
389  } else {
390  vs = 1.;
391  }
392 
393  if (HP.IsKeyWord("sliding" "velocity" "exponent")) {
394  gamma = HP.GetReal();
395  } else {
396  gamma = 1.;
397  }
398 
399  if (!(HP.IsKeyWord("micro" "slip" "stiffness") || HP.IsKeyWord("micro" "slip" "stiffness" "x"))) {
400  silent_cerr("ball bearing contact("
401  << GetLabel()
402  << "): keyword \"micro slip stiffness\" or \"micro slip stiffness x\" expected at line "
403  << HP.GetLineData() << std::endl);
404 
405  throw ErrGeneric(MBDYN_EXCEPT_ARGS);
406  }
407 
408  const doublereal sigma0x = HP.GetReal();
409 
410  doublereal sigma0y;
411 
412  if (HP.IsKeyWord("micro" "slip" "stiffness" "y")) {
413  sigma0y = HP.GetReal();
414  } else {
415  sigma0y = sigma0x;
416  }
417 
418  doublereal sigma1x, sigma1y;
419 
420  if (HP.IsKeyWord("micro" "slip" "damping") || HP.IsKeyWord("micro" "slip" "damping" "x")) {
421  sigma1x = HP.GetReal();
422 
423  if (HP.IsKeyWord("micro" "slip" "damping" "y")) {
424  sigma1y = HP.GetReal();
425  } else {
426  sigma1y = sigma1x;
427  }
428  } else {
429  sigma1x = 0.;
430  sigma1y = 0.;
431  }
432 
433  Mk(1, 1) = mukx;
434  Mk(2, 2) = muky;
435 
436  Mk2 = Mk * Mk;
437 
438  inv_Mk2 = Inv(Mk2);
439 
440  Ms(1, 1) = musx;
441  Ms(2, 2) = musy;
442 
443  Ms2 = Ms * Ms;
444 
445  sigma0(1, 1) = sigma0x;
446  sigma0(2, 2) = sigma0y;
447 
448  sigma1(1, 1) = sigma1x;
449  sigma1(2, 2) = sigma1y;
450 
451  dStictionStateEquScale = HP.IsKeyWord("stiction" "state" "equation" "scale") ? HP.GetReal() : 1.;
452  dStictionStateDofScale = HP.IsKeyWord("stiction" "state" "dof" "scale") ? HP.GetReal() : 1.;
453  }
454 
455  if (HP.IsKeyWord("max" "force" "increment")) {
456  dFMax = HP.GetReal();
457 
458  if (dFMax <= 0) {
459  silent_cerr("ball bearing contact(" << GetLabel()
460  << "): max force increment must be greater than zero at line "
461  << HP.GetLineData() << std::endl);
462  throw ErrGeneric(MBDYN_EXCEPT_ARGS);
463  }
464  }
465 
466  if (HP.IsKeyWord("min" "force" "increment")) {
467  dFMin = HP.GetReal();
468 
469  if (dFMin <= 0) {
470  silent_cerr("ball bearing contact(" << GetLabel()
471  << "): min force increment must be greater than zero at line "
472  << HP.GetLineData() << std::endl);
473  throw ErrGeneric(MBDYN_EXCEPT_ARGS);
474  }
475  }
476 
477  SetOutputFlag(pDM->fReadOutput(HP, Elem::LOADABLE));
478 }
479 
480 BallBearingContact::~BallBearingContact(void)
481 {
482 
483 }
484 
485 void
486 BallBearingContact::Output(OutputHandler& OH) const
487 {
488 
489 }
490 
491 void
492 BallBearingContact::WorkSpaceDim(integer* piNumRows, integer* piNumCols) const
493 {
494  integer iNumDof = 12;
495 
496  if (bEnableFriction) {
497  iNumDof += 2;
498  }
499 
500  *piNumRows = iNumDof * washers.size();
501  *piNumCols = iNumDof * washers.size();
502 }
503 
504 VariableSubMatrixHandler&
505 BallBearingContact::AssJac(VariableSubMatrixHandler& WorkMat,
506  doublereal dCoef,
507  const VectorHandler& XCurr,
508  const VectorHandler& XPrimeCurr)
509 {
510  GradientAssVec<Gradient<iNumADVars> >::AssJac(this,
511  WorkMat.SetSparse(),
512  dCoef,
513  XCurr,
514  XPrimeCurr,
515  REGULAR_JAC,
516  0);
517  return WorkMat;
518 }
519 
520 SubVectorHandler&
521 BallBearingContact::AssRes(SubVectorHandler& WorkVec,
522  doublereal dCoef,
523  const VectorHandler& XCurr,
524  const VectorHandler& XPrimeCurr)
525 {
526  GradientAssVec<doublereal>::AssRes(this,
527  WorkVec,
528  dCoef,
529  XCurr,
530  XPrimeCurr,
531  REGULAR_RES);
532 
533  return WorkVec;
534 }
535 
536 template <typename T>
537 inline void BallBearingContact::AssRes(GradientAssVec<T>& WorkVec,
538  doublereal dCoef,
539  const GradientVectorHandler<T>& XCurr,
540  const GradientVectorHandler<T>& XPrimeCurr,
541  enum FunctionCall func)
542 {
543  typedef Matrix<T, 3, 3> VMat3x3;
544  typedef Vector<T, 3> VVec3;
545  typedef Vector<T, 2> VVec2;
546 
547  VVec3 X1, X1P, omega1;
548  VMat3x3 R2;
549  VVec3 X2, X2P, omega2;
550  VVec2 z, zP;
551  VVec2 tau, Phi;
552  T norm_Fn, kappa;
553 
554  integer offset = std::numeric_limits<integer>::min();
555  const integer iFirstIndex = bEnableFriction ? iGetFirstIndex() : std::numeric_limits<integer>::min();
556  const integer iFirstMomIndexNode1 = pNode1->iGetFirstMomentumIndex();
557  dtMax = std::numeric_limits<doublereal>::max();
558 
559  for (std::vector<Washer>::iterator i = washers.begin(); i != washers.end(); ++i) {
560  i->dof.Reset(func);
561  pNode1->GetXCurr(X1, dCoef, func, &i->dof);
562  pNode1->GetVCurr(X1P, dCoef, func, &i->dof);
563  pNode1->GetWCurr(omega1, dCoef, func, &i->dof);
564 
565  i->pNode2->GetXCurr(X2, dCoef, func, &i->dof);
566  i->pNode2->GetVCurr(X2P, dCoef, func, &i->dof);
567  i->pNode2->GetRCurr(R2, dCoef, func, &i->dof);
568  i->pNode2->GetWCurr(omega2, dCoef, func, &i->dof);
569 
570  if (bEnableFriction) {
571  offset = 2 * (i - washers.begin());
572 
573  for (integer j = 1; j <= 2; ++j) {
574  XCurr.dGetCoef(iFirstIndex + j + offset, z(j), dCoef, &i->dof);
575  XPrimeCurr.dGetCoef(iFirstIndex + j + offset, zP(j), 1., &i->dof);
576  }
577 
578  z *= dStictionStateDofScale;
579  zP *= dStictionStateDofScale;
580 
581  for (int j = 1; j <= 2; ++j) {
582  i->z(j) = dGetValue(z(j));
583  i->zP(j) = dGetValue(zP(j));
584  }
585  }
586 
587  const VVec3 dX = X1 - X2;
588 
589  VVec3 v = Transpose(i->Rt2) * VVec3(Transpose(R2) * dX - i->o2);
590 
591  const T n = v(3);
592 
593  v(3) = 0.;
594 
595  const T dn_dt = Dot(R2 * i->Rt2.GetCol(3), X1P - X2P - Cross(omega2, dX));
596  const T d = R - n;
597 
598  if (R > n) {
599  norm_Fn = k * pow(d, 3./2.) - beta * sqrt(d) * dn_dt;
600  } else {
601  norm_Fn = 0.;
602  }
603 
604  if (norm_Fn < 0.) {
605  norm_Fn = 0.;
606  }
607 
608  CheckTimeStep(*i, norm_Fn, d, dn_dt);
609 
610  if (bEnableFriction) {
611  const VVec3 c1P = X1P - Cross(omega1, R2 * VVec3(i->Rt2.GetCol(3) * R));
612  const VVec3 c2P = X2P + Cross(omega2, R2 * VVec3(i->o2 + i->Rt2 * v));
613 
614  VVec3 l = v;
615  l(3) = R;
616  const VVec3 Deltac = X1 - X2 - R2 * VVec3(i->o2 + i->Rt2 * l);
617 
618  const VVec2 uP = Transpose(SubMatrix<1, 3, 1, 2>(i->Rt2)) * VVec3(Transpose(R2) * VVec3(c1P - c2P - Cross(omega2, Deltac)));
619 
620  for (int j = 1; j <= 2; ++j) {
621  i->uP(j) = dGetValue(uP(j));
622  }
623 
624  tau = (sigma0 * z + sigma1 * zP) * norm_Fn;
625  const T Norm_uP = Norm(uP);
626 
627  if (Norm_uP == 0.) {
628  kappa = 0.;
629  } else {
630  const T a0 = Norm(Mk2 * uP);
631  const T a1 = a0 / Norm(Mk * uP);
632  const T g = a1 + (Norm(Ms2 * uP) / Norm(Ms * uP) - a1) * exp(-pow(Norm_uP / vs, gamma));
633 
634  kappa = a0 / g;
635  }
636 
637  Phi = (uP - inv_Mk2 * VVec2(sigma0 * VVec2(z * kappa)) - zP) * dStictionStateEquScale;
638  }
639 
640  const VVec3 R2_Rt2_e3_n = R2 * VVec3(i->Rt2.GetCol(3) * n);
641 
642  const VVec3 F1 = R2 * VVec3(i->Rt2 * VVec3(-tau(1), -tau(2), norm_Fn));
643  const VVec3 M1 = -Cross(R2_Rt2_e3_n, F1);
644  const VVec3 F2 = -F1;
645  const VVec3 M2 = Cross(VVec3(dX - R2_Rt2_e3_n), F2);
646 
647  const integer iFirstMomIndexNode2 = i->pNode2->iGetFirstMomentumIndex();
648 
649  WorkVec.AddItem(iFirstMomIndexNode1 + 1, F1);
650  WorkVec.AddItem(iFirstMomIndexNode1 + 4, M1);
651  WorkVec.AddItem(iFirstMomIndexNode2 + 1, F2);
652  WorkVec.AddItem(iFirstMomIndexNode2 + 4, M2);
653 
654  if (bEnableFriction) {
655  for (int j = 1; j <= 2; ++j) {
656  WorkVec.AddItem(iFirstIndex + j + offset, Phi(j));
657  }
658  }
659  }
660 }
661 
662 inline void BallBearingContact::CheckTimeStep(Washer& w, doublereal Fn, doublereal d, doublereal dn_dt)
663 {
664  tCurr = pDM->dGetTime();
665  w.dCurr = d;
666  w.dd_dtCurr = -dn_dt;
667  w.FnCurr = Fn;
668 
669  if (bFirstRes) {
670  bFirstRes = false;
671  return;
672  }
673 
674  if (w.FnPrev < dFMin && w.FnCurr < dFMin) {
675  // Bypass time step control
676  // because the force is considered
677  // to be too small!
678  return;
679  }
680 
681  doublereal a;
682 
683  if ((w.dCurr > 0 && w.dPrev < 0) || (w.dCurr < 0 && w.dPrev > 0)) {
684  a = 0.;
685  } else if (w.dCurr >= 0. && w.dPrev >= 0.) {
686  const doublereal FnCurr = k * pow(w.dCurr, 3./2.);
687  const doublereal FnPrev = k * pow(w.dPrev, 3./2.);
688  const doublereal b = (FnPrev + copysign(dFMax, FnCurr - FnPrev)) / k;
689 
690  if (b >= 0.) {
691  a = std::pow(b, 2. / 3.);
692  } else {
693  a = 0.;
694  }
695  } else {
696  return;
697  }
698 
699  const doublereal dd_dt2 = (w.dd_dtCurr - w.dd_dtPrev) / (tCurr - tPrev);
700  const doublereal p = 2 * w.dd_dtPrev / dd_dt2;
701  const doublereal q = 2 * (w.dPrev - a) / dd_dt2;
702  const doublereal r = 0.25 * p * p - q;
703 
704  if (r >= 0) {
705  const doublereal dt[2] = {
706  -0.5 * p + sqrt(r),
707  -0.5 * p - sqrt(r)
708  };
709 
710  for (int i = 0; i < 2; ++i) {
711  if (dt[i] > 0. && dt[i] < dtMax) {
712  dtMax = dt[i];
713  }
714  }
715  }
716 }
717 
718 inline void BallBearingContact::CheckTimeStep(Washer& w, const Gradient<iNumADVars>&, const Gradient<iNumADVars>&, const Gradient<iNumADVars>&)
719 {
720  // Do nothing
721 }
722 
723 void BallBearingContact::AfterConvergence(const VectorHandler& X,
724  const VectorHandler& XP)
725 {
726  tPrev = tCurr;
727 
728  for (std::vector<Washer>::iterator i = washers.begin(); i != washers.end(); ++i) {
729  i->dPrev = i->dCurr;
730  i->dd_dtPrev = i->dd_dtCurr;
731  i->FnPrev = i->FnCurr;
732  }
733 }
734 
735 unsigned int
736 BallBearingContact::iGetNumPrivData(void) const
737 {
738  return 1u + washers.size() * iNumPrivData;
739 }
740 
741 unsigned int BallBearingContact::iGetPrivDataIdx(const char *s) const
742 {
743  if (0 == strcmp(s, "max" "dt")) {
744  return 1u;
745  } else {
746  for (int i = 0; i < iNumPrivData; ++i) {
747  unsigned iWasher;
748 
749  if (1 != sscanf(s, rgPrivData[i].szPattern, &iWasher)) {
750  continue;
751  }
752 
753  if (iWasher <= 0 || iWasher > washers.size()) {
754  return 0;
755  }
756 
757  return (iWasher - 1) * iNumPrivData + i + 2u;
758  }
759 
760  return 0;
761  }
762 }
763 
764 doublereal BallBearingContact::dGetPrivData(unsigned int i) const
765 {
766  if (i == 1u) {
767  return dtMax;
768  }
769 
770  const div_t d = div(i - 2u, iNumPrivData);
771 
772  if (d.quot < 0 || size_t(d.quot) >= washers.size()) {
773  silent_cerr("ballbearingcontact(" << GetLabel()
774  << "): invalid index " << i << std::endl);
775  throw ErrGeneric(MBDYN_EXCEPT_ARGS);
776  }
777 
778  const Washer& w = washers[d.quot];
779 
780  switch (d.rem) {
781  case 0:
782  return w.dCurr;
783 
784  case 1:
785  return w.dd_dtCurr;
786 
787  case 2:
788  return w.FnCurr;
789 
790  case 3:
791  case 4:
792  return w.z(d.rem - 2);
793 
794  case 5:
795  case 6:
796  return w.zP(d.rem - 4);
797 
798  case 7:
799  case 8:
800  return w.uP(d.rem - 6);
801 
802  default:
803  silent_cerr("ballbearingcontact(" << GetLabel()
804  << "): invalid index " << i << std::endl);
805  throw ErrGeneric(MBDYN_EXCEPT_ARGS);
806  }
807 }
808 
809 int
810 BallBearingContact::iGetNumConnectedNodes(void) const
811 {
812  return washers.size() + 1;
813 }
814 
815 void
816 BallBearingContact::GetConnectedNodes(std::vector<const Node *>& connectedNodes) const
817 {
818  connectedNodes.reserve(iGetNumConnectedNodes());
819  connectedNodes.clear();
820  connectedNodes.push_back(pNode1);
821 
822  for (std::vector<Washer>::const_iterator i = washers.begin(); i != washers.end(); ++i) {
823  connectedNodes.push_back(i->pNode2);
824  }
825 }
826 
827 void
828 BallBearingContact::SetValue(DataManager *pDM,
829  VectorHandler& X, VectorHandler& XP,
830  SimulationEntity::Hints *ph)
831 {
832 
833 }
834 
835 std::ostream&
836 BallBearingContact::Restart(std::ostream& out) const
837 {
838  return out;
839 }
840 
841 unsigned int BallBearingContact::iGetNumDof(void) const
842 {
843  return bEnableFriction ? 2 * washers.size() : 0;
844 }
845 
846 std::ostream& BallBearingContact::DescribeDof(std::ostream& out,
847  const char *prefix, bool bInitial) const
848 {
849  if (bEnableFriction) {
850  const integer iIndex = iGetFirstIndex();
851 
852  for (size_t i = 0; i < washers.size(); ++i) {
853  for (int j = 1; j <= 2; ++j) {
854  out << prefix << iIndex + j + 2 * i << ": stiction state z" << j << "[" << i << "]" << std::endl;
855  }
856  }
857  }
858 
859  return out;
860 }
861 
862 std::ostream& BallBearingContact::DescribeEq(std::ostream& out,
863  const char *prefix, bool bInitial) const
864 {
865  if (bEnableFriction) {
866  const integer iIndex = iGetFirstIndex();
867 
868  for (size_t i = 0; i < washers.size(); ++i) {
869  for (int j = 1; j <= 2; ++j) {
870  out << prefix << iIndex + j + 2 * i << ": ode for stiction state z" << j << "[" << i << "]" << std::endl;
871  }
872  }
873  }
874 
875  return out;
876 }
877 
878 DofOrder::Order BallBearingContact::GetDofType(unsigned int) const
879 {
880  return DofOrder::DIFFERENTIAL;
881 }
882 
883 DofOrder::Order BallBearingContact::GetEqType(unsigned int i) const
884 {
885  return DofOrder::DIFFERENTIAL;
886 }
887 
888 unsigned int
889 BallBearingContact::iGetInitialNumDof(void) const
890 {
891  return 0;
892 }
893 
894 void
895 BallBearingContact::InitialWorkSpaceDim(
896  integer* piNumRows,
897  integer* piNumCols) const
898 {
899  *piNumRows = 0;
900  *piNumCols = 0;
901 }
902 
903 VariableSubMatrixHandler&
904 BallBearingContact::InitialAssJac(
905  VariableSubMatrixHandler& WorkMat,
906  const VectorHandler& XCurr)
907 {
908  WorkMat.SetNullMatrix();
909 
910  return WorkMat;
911 }
912 
913 SubVectorHandler&
914 BallBearingContact::InitialAssRes(
915  SubVectorHandler& WorkVec,
916  const VectorHandler& XCurr)
917 {
918  WorkVec.ResizeReset(0);
919 
920  return WorkVec;
921 }
922 #endif
923 
924 bool ballbearing_contact_set(void)
925 {
926 #ifdef USE_AUTODIFF
927  UserDefinedElemRead *rf = new UDERead<BallBearingContact>;
928 
929  if (!SetUDE("ball" "bearing" "contact", rf))
930  {
931  delete rf;
932  return false;
933  }
934 
935  return true;
936 #else
937  return false;
938 #endif
939 }
940 
941 #ifndef STATIC_MODULES
942 
943 extern "C"
944 {
945 
946 int module_init(const char *module_name, void *pdm, void *php)
947 {
948  if (!ballbearing_contact_set())
949  {
950  silent_cerr("ballbearing_contact: "
951  "module_init(" << module_name << ") "
952  "failed" << std::endl);
953 
954  return -1;
955  }
956 
957  return 0;
958 }
959 
960 }
961 
962 #endif // ! STATIC_MODULE
963 
964 
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
grad::dGetValue
scalar_func_type dGetValue(scalar_func_type d)
Definition: gradient.h:2845
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
VectorHandler::ResizeReset
virtual void ResizeReset(integer)
Definition: vh.cc:55
HighParser::GetInt
virtual integer GetInt(integer iDefval=0)
Definition: parser.cc:1050
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
grad::index_type
integer index_type
Definition: gradient.h:104
module-ballbearing_contact.h
SimulationEntity::Hints
std::vector< Hint * > Hints
Definition: simentity.h:89
ballbearing_contact_set
bool ballbearing_contact_set(void)
Definition: module-ballbearing_contact.cc:924
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
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
copysign
doublereal copysign(doublereal x, doublereal y)
Definition: gradient.h:97
grad::FunctionCall
FunctionCall
Definition: gradient.h:1018
true
Definition: mbdyn.h:77
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
grad::sqrt
GradientExpression< UnaryExpr< FuncSqrt, Expr > > sqrt(const GradientExpression< Expr > &u)
Definition: gradient.h:2974
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
grad::Norm
VectorExpression< VectorExpr, N_rows >::ScalarType Norm(const VectorExpression< VectorExpr, N_rows > &u)
Definition: matvec.h:3163
HighParser::IsArg
virtual bool IsArg(void)
Definition: parser.cc:807
Elem
Definition: elem.h:75
SetUDE
bool SetUDE(const std::string &s, UserDefinedElemRead *rude)
Definition: userelem.cc:97
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
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-ballbearing_contact.cc:946
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
R
Mat3x3 R
Definition: mbdyn_util_oct.cc:74
HighParser::GetReal
virtual doublereal GetReal(const doublereal &dDefval=0.0)
Definition: parser.cc:1056