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
AfterConvergenceUniInPlaneFriction::ContactPoint126 inline void AfterConvergence()
127 {
128 lambdaPrev = lambda;
129 }
130
UpdateReactionUniInPlaneFriction::ContactPoint131 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
UpdateFrictionUniInPlaneFriction::ContactPoint150 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
ContactPoint(const Vec3 & offset,doublereal s)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
UpdateReaction(const grad::Vector<doublereal,3> & F1,const grad::Vector<doublereal,3> & M1,const grad::Vector<doublereal,3> & F2,const grad::Vector<doublereal,3> & M2,const doublereal dXn,const doublereal lambda)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
UpdateFriction(const grad::Vector<doublereal,2> & U,const grad::Vector<doublereal,2> & tau,const grad::Vector<doublereal,2> & z,const grad::Vector<doublereal,2> & zP)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
UniInPlaneFriction(unsigned uLabel,const DofOwner * pDO,DataManager * pDM,MBDynParser & HP)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
~UniInPlaneFriction(void)544 UniInPlaneFriction::~UniInPlaneFriction(void)
545 {
546 // destroy private data
547 }
548
iGetNumDof(void) const549 unsigned int UniInPlaneFriction::iGetNumDof(void) const
550 {
551 return ContactPoints1.size() * (1 + 2 * bEnableFriction);
552 }
553
GetDofType(unsigned int i) const554 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
GetEqType(unsigned int i) const573 DofOrder::Order UniInPlaneFriction::GetEqType(unsigned int i) const
574 {
575 return GetDofType(i);
576 }
577
DescribeDof(std::ostream & out,const char * prefix,bool bInitial) const578 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
DescribeEq(std::ostream & out,const char * prefix,bool bInitial) const600 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
iGetNumPrivData(void) const622 unsigned int UniInPlaneFriction::iGetNumPrivData(void) const
623 {
624 return iNumPrivDataGlobal + ContactPoints1.size() * iNumPrivData;
625 }
626
iGetPrivDataIdx(const char * s) const627 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
dGetPrivData(unsigned int i) const664 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
Output(OutputHandler & OH) const744 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
WorkSpaceDim(integer * piNumRows,integer * piNumCols) const763 UniInPlaneFriction::WorkSpaceDim(integer* piNumRows, integer* piNumCols) const
764 {
765 *piNumRows = -(ContactPoints1.size() * iNumDofGradient);
766 *piNumCols = iNumDofGradient;
767 }
768
769 VariableSubMatrixHandler&
AssJac(VariableSubMatrixHandler & WorkMatVar,doublereal dCoef,const VectorHandler & XCurr,const VectorHandler & XPrimeCurr)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&
AssRes(SubVectorHandler & WorkVec,doublereal dCoef,const VectorHandler & XCurr,const VectorHandler & XPrimeCurr)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
AssRes(grad::GradientAssVec<T> & WorkVec,doublereal dCoef,const grad::GradientVectorHandler<T> & XCurr,const grad::GradientVectorHandler<T> & XPrimeCurr,enum grad::FunctionCall func)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
AfterConvergence(const VectorHandler & X,const VectorHandler & XP)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
iGetNumConnectedNodes(void) const934 UniInPlaneFriction::iGetNumConnectedNodes(void) const
935 {
936 return 2;
937 }
938
939 void
GetConnectedNodes(std::vector<const Node * > & connectedNodes) const940 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
SetValue(DataManager * pDM,VectorHandler & X,VectorHandler & XP,SimulationEntity::Hints * ph)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&
Restart(std::ostream & out) const972 UniInPlaneFriction::Restart(std::ostream& out) const
973 {
974 return out;
975 }
976
977 unsigned int
iGetInitialNumDof(void) const978 UniInPlaneFriction::iGetInitialNumDof(void) const
979 {
980 return 0;
981 }
982
983 void
InitialWorkSpaceDim(integer * piNumRows,integer * piNumCols) const984 UniInPlaneFriction::InitialWorkSpaceDim(
985 integer* piNumRows,
986 integer* piNumCols) const
987 {
988 *piNumRows = 0;
989 *piNumCols = 0;
990 }
991
992 VariableSubMatrixHandler&
InitialAssJac(VariableSubMatrixHandler & WorkMat,const VectorHandler & XCurr)993 UniInPlaneFriction::InitialAssJac(
994 VariableSubMatrixHandler& WorkMat,
995 const VectorHandler& XCurr)
996 {
997 WorkMat.SetNullMatrix();
998
999 return WorkMat;
1000 }
1001
1002 SubVectorHandler&
InitialAssRes(SubVectorHandler & WorkVec,const VectorHandler & XCurr)1003 UniInPlaneFriction::InitialAssRes(
1004 SubVectorHandler& WorkVec,
1005 const VectorHandler& XCurr)
1006 {
1007 WorkVec.ResizeReset(0);
1008
1009 return WorkVec;
1010 }
1011 #endif
1012
uni_in_plane_set(void)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
module_init(const char * module_name,void * pdm,void * php)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
1058