1 //#**************************************************************
2 //#
3 //# filename: contact3D.cpp
4 //#
5 //# author: Gerstmayr Johannes, Peter Gruber
6 //#
7 //# generated: 17. October 2006
8 //# description: contact formulations
9 //#
10 //# remarks:
11 //#
12 //# Copyright (c) 2003-2013 Johannes Gerstmayr, Linz Center of Mechatronics GmbH, Austrian
13 //# Center of Competence in Mechatronics GmbH, Institute of Technical Mechanics at the
14 //# Johannes Kepler Universitaet Linz, Austria. All rights reserved.
15 //#
16 //# This file is part of HotInt.
17 //# HotInt is free software: you can redistribute it and/or modify it under the terms of
18 //# the HOTINT license. See folder 'licenses' for more details.
19 //#
20 //# bug reports are welcome!!!
21 //# WWW: www.hotint.org
22 //# email: bug_reports@hotint.org or support@hotint.org
23 //#***************************************************************************************
24
25
26 #include "element.h"
27 #include "constraint.h"
28 #include "body3d.h"
29 #include "geomelements.h"
30 #include "contact3d.h"
31 #include "femathhelperfunctions.h"
32 #include "contact_globalvariables.h"
33
34
35 int contact_cnt_gap = 0;
36 int contact_cnt_mastersearch = 0;
37 int contact_cnt_itemlist = 0;
38 int contact_cnt_foundmaster = 0;
39 int contact_cnt_list2 = 0;
40 int contact_cnt_list3 = 0;
41 int contact_cnt_list4 = 0;
42
43 int testcnt = 1000;
44 int last_buildsearchtree = 1000;
45
46 const int useconstantmastersearchtree = 0;
47 const int discstepcorrection = 1; //correct energy lost during discontinuous step (contact switching)
48
49 //++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
50 // GeneralContact3D
51 //++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
52
BuildSearchTrees()53 void GeneralContact3D::BuildSearchTrees()
54 {
55
56 if (!mastersearchtreeinitialized)
57 {
58 //+++++++++++++++++++++++++++++
59 //master segments:
60 Box3D box;
61 box.Clear();
62 for (int i=1; i <= NMC(); i++)
63 {
64 Box3D pbox;
65 GeomElement* ge = GeomElem(i);
66 pbox = ge->GetBoundingBox();
67 box.Add(pbox);
68 }
69 //UO() << "STbox=" << box.PMin() << "-" << box.PMax() << "\n";
70 box.Increase(bordersize);
71 mastersearchtreebox = box; //this box is currently not rebuilt-->if objects move outside initial box, search will be slower!
72
73 if (useconstantmastersearchtree)
74 {
75 //non-moving-objects:
76 constantmastersearchtree.ResetSearchTree((int)(1.5*searchtreeix), (int)(1.5*searchtreeiy), (int)(1.5*searchtreeiz), mastersearchtreebox); //define number of cubes and maximum size of search tree
77 for (int i=1; i <= NMC(); i++)
78 {
79 if (GeomElem(i)->GetElnum() == 0)
80 {
81 Box3D pbox = GeomElem(i)->GetBoundingBox();
82
83 pbox.Increase(bordersize);
84 constantmastersearchtree.AddItem(pbox,i);
85 }
86 }
87 }
88 mastersearchtreeinitialized = 1;
89 }
90
91 if (last_buildsearchtree++ < 0) return; //save time when not updating search trees so often
92
93 last_buildsearchtree = 0;
94
95 TMStartTimer(24); //build search trees
96
97 mastersearchtree.ResetSearchTree(searchtreeix, searchtreeiy, searchtreeiz, mastersearchtreebox); //define number of cubes and maximum size of search tree
98
99
100 for (int i=1; i <= NMC(); i++)
101 {
102 if (GeomElem(i)->GetElnum() != 0 || !useconstantmastersearchtree)
103 {
104 Box3D pbox = GeomElem(i)->GetBoundingBox();
105
106 pbox.Increase(bordersize);
107 mastersearchtree.AddItem(pbox,i);
108 }
109 }
110 TMStopTimer(24); //build search trees
111 }
112
GetSlaveBody(int i) const113 const Body3D& GeneralContact3D::GetSlaveBody(int i) const
114 {
115 return (const Body3D&)mbs->GetElement(slaveelements(i));
116 }
117
GetSlaveBody(int i)118 Body3D& GeneralContact3D::GetSlaveBody(int i)
119 {
120 return (Body3D&)mbs->GetElement(slaveelements(i));
121 }
122
123 //$ YV 2012-12-13: replaced GeomElem(i)->GetBody3D() by GeomElem(i)->GetElement() here and below
GetMasterBody(int i) const124 const Body3D& GeneralContact3D::GetMasterBody(int i) const
125 {
126 return (Body3D&)GeomElem(i)->GetElement();
127 }
128
GetMasterBody(int i)129 Body3D& GeneralContact3D::GetMasterBody(int i)
130 {
131 return (Body3D&)GeomElem(i)->GetElement();
132 }
133
GetSlaveNodePos(int i) const134 Vector3D GeneralContact3D::GetSlaveNodePos(int i) const
135 {
136 if (slaveNODEmode == 0)
137 {
138 return GetSlaveBody(i).GetPos(loccoords(i));
139 }
140 else
141 {
142 return GetSlaveBody(i).GetNodePos(locnodenums(i));
143 }
144 }
145
GetSlaveNodePosD(int i) const146 Vector3D GeneralContact3D::GetSlaveNodePosD(int i) const
147 {
148 if (slaveNODEmode == 0)
149 {
150 return GetSlaveBody(i).GetPosD(loccoords(i));
151 }
152 else
153 {
154 return GetSlaveBody(i).GetNodePosD(locnodenums(i));
155 }
156 }
157
GetSlaveNodeVel(int i) const158 Vector3D GeneralContact3D::GetSlaveNodeVel(int i) const
159 {
160 if (slaveNODEmode == 0)
161 {
162 return GetSlaveBody(i).GetVel(loccoords(i));
163 }
164 else
165 {
166 return GetSlaveBody(i).GetNodeVel(locnodenums(i));
167 }
168 }
169
GetMasterLocPos(const Vector3D & pp,int j,int locind) const170 Vector3D GeneralContact3D::GetMasterLocPos(const Vector3D& pp, int j, int locind) const
171 {
172 return GeomElem(j)->GetLocPos(locind, pp);
173 }
174
175 //approximate velocity at segment point pglob due linear interpolation
GetMasterSegVel(const Vector3D & pglob,int j,int locind) const176 Vector3D GeneralContact3D::GetMasterSegVel(const Vector3D& pglob, int j, int locind) const
177 {
178 return GeomElem(j)->GetVel(GeomElem(j)->GetLocPos(locind, pglob));
179 }
180
181 //get mindist and projected point pp; only for points, spheres return several elements!!!
GetNearestMasterSegment(int slavenum,const Vector3D & p,int & masternum,int & locind,double & dist,Vector3D & pp)182 void GeneralContact3D::GetNearestMasterSegment(int slavenum, const Vector3D& p, int& masternum,
183 int& locind, double& dist, Vector3D& pp)
184 {
185 Box3D box;
186 box.Add(p);
187 //box.Increase(pointradius(slavenum));
188
189 mastersearchtree.GetItemsInBox(box, itemlist);
190 if (useconstantmastersearchtree) constantmastersearchtree.AddItemsInBox(box, itemlist);
191
192 Vector3D pf;
193
194 double mindist = 1e20;
195 double d;
196 int fseg = 0;
197 locind = 0;
198 int flocind = 0;
199 double gap;
200
201 static IVector itemlist2;
202 itemlist2.SetLen(0);
203 //delete double elements in itemlist:
204 for (int i=1; i <= itemlist.Length(); i++)
205 {
206 if (tempitemlist(itemlist(i)) == 0)
207 {
208 itemlist2.Add(itemlist(i));
209 tempitemlist(itemlist(i)) = 1;
210 }
211 }
212
213 for (int i = 1; i <= itemlist2.Length(); i++)
214 {
215 int j = itemlist2(i);
216 tempitemlist(j) = 0;
217
218 if (masterbodyindex(j) < slavebodyindex(slavenum) &&
219 (masterisactive(j) == 0 || mathfunctions(masterisactive(j))->CheckCondition(GetMBS()->GetTime())) )
220 {
221
222 gap = GeomElem(j)->GetNearestPoint(p, locind, pf);//-pointradius(slavenum);
223 d = Dist(p,pf);//-pointradius(slavenum);
224
225 if (fabs(d) < fabs(mindist)
226 && fabs(gap) < contactmaxdist)
227 {
228 mindist = d;
229 fseg = j;
230 pp = pf;
231 flocind = locind;
232 }
233 }
234 }
235 //dist = mindist;
236 if (fseg)
237 {
238 dist = GeomElem(fseg)->GetNearestPoint(p, locind, pf);//-pointradius(slavenum);
239 }
240 else dist = mindist;
241
242 masternum = fseg;
243 }
244
245 //circle radius is radius of sphere!!!
GetNearestMasterSegments(int slavenum,const Vector3D & p,double circlerad,TArray<int> & a_masternum,TArray<int> & a_locind,TArray<double> & a_dist,TArray<Vector3D> & a_pp)246 void GeneralContact3D::GetNearestMasterSegments(int slavenum, const Vector3D& p, double circlerad, TArray<int>& a_masternum,
247 TArray<int>& a_locind, TArray<double>& a_dist, TArray<Vector3D>& a_pp)
248 {
249 contact_cnt_mastersearch++; //400.000/sec //50%
250
251 //TMStartTimer(28); //60% of GetNearestMasterSegs
252 Box3D box;
253 box.Add(p);
254 box.Increase(circlerad);
255
256 //int ind[6]; //indices for box
257 //mastersearchtree.GetBoxIndizes(box, ind); //8%
258 //mastersearchtree.GetItemsInBox(ind, itemlist); //10.8%
259
260 mastersearchtree.GetItemsInBox(box, itemlist); //2.500.000 Elements/sec
261 if (useconstantmastersearchtree) constantmastersearchtree.AddItemsInBox(box, itemlist);
262
263 Vector3D pf;
264 double d;
265 int flocind = 0;
266 double gap;
267
268 int locind = 0;
269 int masternum = 0;
270 double dist = 1e20;
271 Vector3D pp;
272
273 //check for all elements which penetrate -> store
274 //if no penetration, return nearest element!
275
276 a_masternum.SetLen(0);
277 a_locind.SetLen(0);
278 a_dist.SetLen(0);
279 a_pp.SetLen(0);
280
281
282 static IVector itemlist2;
283 itemlist2.SetLen(0);
284 //delete double elements in itemlist:
285 for (int i=1; i <= itemlist.Length(); i++)
286 {
287 if (tempitemlist(itemlist(i)) == 0)
288 {
289 itemlist2.Add(itemlist(i));
290 tempitemlist(itemlist(i)) = 1;
291 }
292 }
293 contact_cnt_itemlist += itemlist2.Length();
294 //itemlist = 33, itemlist2 = 10.027, list2=4.75744, list3=0.213187, foundmaster = 0.0111501
295 //n_gap=7.34539e+006, n_ms=3.9368e+007, itemlist=41.9261, itemlist2=13.5976, foundmaster=0.071787, list2=6.87892, list3=1.0026,
296
297 //TMStopTimer(28); //59% time spent up to here
298 Vector3D pc;
299 double rz;
300
301 for (int i = 1; i <= itemlist2.Length(); i++)
302 {
303 int j = itemlist2(i);
304 tempitemlist(j) = 0;
305
306 if (masterbodyindex(j) < slavebodyindex(slavenum) &&
307 (masterisactive(j) == 0 || mathfunctions(masterisactive(j))->CheckCondition(GetMBS()->GetTime())) )
308 {
309 //contact_cnt_list2++; //47% of items get here
310 if (GeomElem(j)->GetType() == TGeomSphere3D && circlerad != 0)
311 {
312 //TMStartTimer(29);
313 //contact_cnt_list3++;
314 locind = 1; //only one index possible in Sphere
315
316 pc = GeomElem(j)->GetElement().GetRefPos(); //center of sphere, global; ACCELERATE: getrefpos directly?
317 rz = ((GeomSphere3D*)GeomElem(j))->GetRadius();
318 pf = p-pc; //vector from center pc of sphere to p
319 gap = pf.Norm2(); //distance of pc and p
320
321 //gap = sqrt(gap);
322 //d = gap-rz-circlerad;
323 //if ((d < 0) != (gap < Sqr(rz+circlerad))) UO() << "problem in contact\n";
324
325 //if (d < 0)
326 if (gap < Sqr(rz+circlerad))
327 {
328 //contact_cnt_list4++;
329 gap = sqrt(gap);
330 if (gap != 0) pf *= (rz/gap); //pf=relative vector from center to surface of sphere
331 pf += pc; //pf=global projected point at surface of sphere
332 d = gap-rz-circlerad;
333 gap = d; //distance between p and projected point pf
334 }
335 else
336 {
337 d = 1; //so that d > 0 !!!
338 }
339 //TMStopTimer(29);//30% of total time here
340 }
341 else
342 {
343 gap = GeomElem(j)->GetNearestPoint(p, locind, pf) - circlerad; //gap value??
344 d = Dist(p,pf) - circlerad;
345
346 if (fabs(d) < fabs(dist) && fabs(d) < contactmaxdist) //only for case when circlerad==0
347 {
348 dist = d;
349 masternum = j;
350 pp = pf;
351 flocind = locind;
352 }
353 }
354
355 if (circlerad != 0 && d < 0 && fabs(d) < contactmaxdist)
356 {
357 //0.1% of items get here
358 //this is practically done for every master element
359 // if (!GetMBS()->IsJacobianComputation() && 0)
360 // UO() << "GeomElem" << j << ", gap=" << d << "\n";
361
362 //if (!Find(j, a_masternum))
363 {
364 int f = 0;
365 int k = 1;
366 while (k <= a_masternum.Length() && !f)
367 {
368 //if (masterbodyindex(a_masternum(k)) == masterbodyindex(j)) f = k;
369 if (Dist2(pf, a_pp(k)) < 1e-16)
370 {
371 f = k;
372 }
373 k++;
374 }
375
376 if (!f)
377 {
378 a_masternum.Add(j);
379 a_dist.Add(d);
380 a_pp.Add(pf);
381 a_locind.Add(locind);
382 }
383 else
384 {
385 if (fabs(d) < fabs(a_dist(f)))
386 {
387 a_masternum(f) = j;
388 a_dist(f) = d;
389 a_pp(f) = pf;
390 a_locind(f) = locind;
391 }
392 }
393 }
394 }
395 }
396 }
397 if (masternum && a_masternum.Length() == 0 && circlerad == 0)
398 {
399 dist = GeomElem(masternum)->GetNearestPoint(p, locind, pf) - circlerad;
400 //dist = Dist(p,pf) - circlerad;
401
402 if (dist < 0)
403 {
404 a_dist.Add(dist);
405 a_masternum.Add(masternum);
406 a_pp.Add(pp);
407 a_locind.Add(flocind);
408 }
409 }
410 contact_cnt_foundmaster += a_masternum.Length();
411 //otherwise leave arrays empty
412 }
413
GetTangentStickForce(int i,const Vector3D & t,double tvel,const Vector3D & pp,const Vector3D & lastpp,Vector3D & tforce)414 void GeneralContact3D::GetTangentStickForce(int i, const Vector3D& t, double tvel, const Vector3D& pp, const Vector3D& lastpp, Vector3D& tforce)
415 {
416 double friction_penalty = 0.1*c_contact(i);
417
418 tforce = 0;
419 if (islaststick(i) && isstick(i)) tforce = friction_penalty*(lastpp-pp);
420 //else tforce = (-friction_penalty*tvel)*t;
421 }
422
ConvertRestitution(double restcoeff)423 double GeneralContact3D::ConvertRestitution(double restcoeff)
424 {
425 double x = restcoeff;
426 return 0.02842673968087363+1.235462852658231*pow(x,7)-27.76611509389485*pow(x,4)-8.758510573837406*pow(x,6)+22.67054845547086*pow(x,5)+16.29462160441537*pow(x,3)
427 -4.061556592190019*pow(x,2)+1.357122607696947*x;
428 }
429
GetContactForce(int i,double gap,double gapp)430 double GeneralContact3D::GetContactForce(int i, double gap, double gapp)
431 {
432 //double e = 0.95; //coeff of restitution; 0.95 ususally, with ambrosio
433 double e = restitution_coeff; //coeff of restitution; 0.95 ususally
434 double nh = hertzian_contact_param; //coeff for Hertzian contact; 1 works good
435
436 double gi = fabs(gapp_init(i));
437 if (fabs(gapp) > fabs(gapp_init(i))) gi = fabs(gapp);
438
439 switch (contactmode)
440 {
441 case 0: //Hertian contact
442 return -Sgn(gap)*c_contact(i)*(1.+0.75*(1.-Sqr(e))*(-gapp)/gi)*pow(fabs(gap),nh); //original, XG(i) is compression force, Hertzian contact model
443 //following model has been used in some numerical examples:
444 //return -Sgn(gap)*c_contact(i)*(1.+8*(1.-Sqr(e))*gapp/gi)*pow(fabs(gap),nh); //XG(i) is compression force, Hertzian contact model
445 break;
446 case 1: //tied, linear elastic
447 return -c_contact(i)*gap;
448 break;
449 case 3: //linear contact model
450 return -c_contact(i)*gap;
451 break;
452 case 5: //Model which gives correct restitution coefficient from 0..1
453 return -Sgn(gap)*c_contact(i)*(1.+0.75*(1.-Sqr(e))/(1.-Sqr(1.-e))*(-gapp)/gi)*pow(fabs(gap),nh); //XG(i) is compression force, Hertzian contact model
454 break;
455 default: UO() << "ERROR: contactmode in GetContactForce\n"; return 0;
456 }
457 }
458
PostprocessingStep()459 void GeneralContact3D::PostprocessingStep()
460 {
461 //TMStartTimer(24);
462 for (int i = 1; i <= NMC(); i++)
463 {
464 mastertoslave_last[i-1] = mastertoslave[i-1];
465 mastertoslave_actlast[i-1] = mastertoslave[i-1];
466 }
467
468 for (int i = 1; i <= NSC(); i++)
469 {
470 if (pointradius(i) != 0)
471 {
472 *multimastercontact_last(i) = *multimastercontact(i);
473 *multimastercontact_actlast(i) = *multimastercontact(i);
474 if (multimastercontact_last(i)->Length() == 0) gapp_init(i) = gapp_init_min(i); //reset should be done earlier for each contact pair!!!
475 }
476 }
477
478 for (int i = 1; i <= NSC(); i++)
479 {
480 if (pointradius(i) == 0)
481 {
482 if (!iscontact(i)) gapp_init(i) = gapp_init_min(i);
483
484 if (isfriction)
485 {
486 if (iscontact(i) && isstick(i))
487 {
488 //UO() << "stick" << i << "\n";
489 if (islaststick(i) != isstick(i))
490 {
491 int j = isstick(i);
492
493 Vector3D pglob = GetSlaveNodePos(i);
494
495 //store local mastersegment position:
496 laststickp(i) = GetMasterLocPos(pglob, j, masterlocind(j));
497 islaststick(i) = isstick(i); //master segment number
498 }
499 }
500 else
501 {
502 if (!iscontact(i))
503 {
504 islaststick(i) = 0;
505 }
506 else
507 {
508 int j = iscontact(i);
509
510 Vector3D pglob = GetSlaveNodePos(i);
511
512 //store local mastersegment position:
513 laststickp(i) = GetMasterLocPos(pglob, j, masterlocind(j));
514 }
515
516 }
517 if (abs(slipdir(i)) == 2) slipdir(i) /= 2;
518 }
519 }
520 }
521
522
523
524 BuildSearchTrees();
525 nlstepcnt = 0;
526 //TMStopTimer(24);
527 }
528
529 //ofstream gapout("..\\..\\output\\gap.txt"); //�
530
PostNewtonStep(double t)531 double GeneralContact3D::PostNewtonStep(double t)
532 {
533 TMStartTimer(25);
534
535 if (contactmode == 1 || contactmode == 2) //always contact
536 {
537 UO() << "ERROR: contact mode 1 and 2 not yet implemented for General Contact\n";
538 }
539
540 int maxnlit = 4; //2 works fine, but ignores many special cases
541 if (nlstepcnt > maxnlit)
542 {
543 TMStopTimer(25);
544 return 0;
545 }
546 double nlerror = 0;
547
548 for (int i = 1; i <= NMC(); i++)
549 {
550 mastertoslave[i-1].SetLen(0);
551 }
552
553 for (int i = 1; i <= NSC(); i++) //i is index of slave element
554 {
555 Vector3D p, pp;
556 double gap;
557 int mnum, mlocind;
558
559 p = GetSlaveNodePos(i);
560
561 int nseg=1;
562 if (pointradius(i) == 0)
563 {
564 GetNearestMasterSegment(i, p, mnum, mlocind, gap, pp); //gap < 0 means penetration; mnum is master element
565 if (mnum == 0) nseg = 0;
566 }
567 else
568 {
569 TMStartTimer(27);
570 GetNearestMasterSegments(i, p, pointradius(i), tempmasternum, templocind, tempdist, temp_pp);
571 nseg=tempmasternum.Length();
572 TMStopTimer(27);
573 }
574
575 //?????????????
576 //Add multimastercontact_actlast elements somehow to tempmasternum_actlast list ...
577 //then switching back from contact will be implemented correctly
578
579 for (int fi=1; fi <= nseg; fi++)
580 {
581 //if (mnum != 0)
582 //{
583 if (pointradius(i) != 0)
584 {
585 mnum = tempmasternum(fi); //mnum is master element
586 mlocind = templocind(fi);
587 gap = tempdist(fi);
588 pp = temp_pp(fi);
589 }
590
591 double gapp, tvel, forcefact, forceadd;
592 Vector3D t1, t2, n, ploc;
593
594 //gapp is d/dt(gap)
595 ComputeGap(i, mnum, gap, gapp, p, pp, ploc, t1, t2, n, tvel, forcefact, forceadd, 1); //called from PostNewtonStep
596
597 double c_force;
598 if (islagrange)
599 c_force = XG(i);
600 else
601 c_force = GetContactForce(i, gap, gapp);
602
603 c_force *= forcefact;
604 c_force += forceadd;
605
606 if (pointradius(i) != 0)
607 {
608 if (Find(mnum,*multimastercontact(i))) //compare with last nonlinear iteration / assumption of contact!
609 {
610 iscontact(i) = mnum; //this variable is reused in multimastercontact mode ....!
611 //if (GetMBS()->GetStepSize() > 1e-7) nlerror = -1;//GetMBS()->GetStepSize(); //adaptive stepsize for contact
612 }
613 else
614 {
615 iscontact(i) = 0;
616 nlerror += 1;
617 }
618 }
619 else if (iscontact(i) && mnum != iscontact(i))
620 {
621 iscontact(i) = mnum;
622 nlerror += 1;
623 }
624
625 if (gap < 0 && iscontact(i) == 0 && nlstepcnt < maxnlit)
626 {
627 iscontact(i) = mnum; //master element number
628 nlerror += fabs(gap);
629 }
630 else if (gap > 0 && iscontact(i) != 0)
631 {
632 iscontact(i) = 0;
633 nlerror += fabs(c_force)/c_contact(i);
634 }
635
636 if (fabs(gapp_init(i)) < fabs(gapp) && iscontact(i) != 0)
637 {
638 nlerror += fabs(fabs(gapp_init(i)) - fabs(gapp))/Maximum(fabs(gapp_init(i)),fabs(gapp));
639 gapp_init(i) = fabs(gapp);
640 }
641
642 if (isfriction && pointradius(i) == 0)
643 {
644 if (isstick(i))
645 {
646 UO() << "Contact3D::not implemented!!!\n";
647 Vector3D t_force;
648 Vector3D lastpp = GeomElem(isstick(i))->GetPos(laststickp(i));
649 GetTangentStickForce(i, t1, tvel, pp, lastpp, t_force);
650
651 //if (t >= 0.16 && t <= 0.1601) UO() << "slave" << i << ": gap=" << gap << ", cforce=" << c_force << ", tforce=" << t_force << "\n";
652
653 if (t_force.Norm() > frictioncoeff_stick*c_force)
654 {
655 isstick(i) = 0;
656 nlerror += fabs(t_force.Norm() - frictioncoeff_stick*c_force)/c_contact(i);
657 if (abs(slipdir(i)) == 2) slipdir(i) = -slipdir(i) / 2;
658 }
659 }
660 else
661 {
662 //isstick(i) = mnum;
663
664 if ((double)slipdir(i)*tvel < 0)
665 {
666 //assumed slip direction not equal tangential slip velocity -> try stick or other velocity:
667 isstick(i) = mnum;
668 nlerror += fabs(tvel);
669 slipdir(i) *= 2;
670 }
671 }
672
673 }
674
675 if (iscontact(i) != 0)
676 {
677 mastertoslave[mnum-1].Add(i);
678 }
679
680 if (pointradius(i) != 0)
681 {
682 tempmasternum(fi) = iscontact(i);
683 }
684
685 } //end fi
686 if (pointradius(i) != 0)
687 {
688 //check if contact was active in last step, but is not active in current step:
689 for (int fi=1; fi <= multimastercontact(i)->Length(); fi++)
690 {
691 if (!Find(multimastercontact(i)->Get(fi),tempmasternum))
692 nlerror += 1;
693 }
694
695 multimastercontact(i)->SetLen(0);
696 for (int fi=1; fi <= tempmasternum.Length(); fi++)
697 {
698 if (tempmasternum(fi) != 0) multimastercontact(i)->Add(tempmasternum(fi));
699 }
700
701 //multimastercontact_actlast should contain all actual contacts and contacts which were active in last time step
702 *multimastercontact_actlast(i) = *multimastercontact(i);
703 for (int fi = 1; fi <= multimastercontact_last(i)->Length(); fi++)
704 {
705 if (!Find(multimastercontact_last(i)->Get(fi),*multimastercontact_actlast(i)))
706 {
707 multimastercontact_actlast(i)->Add(multimastercontact_last(i)->Get(fi));
708
709 //check gap in this elements as well:
710 double gapp, tvel, forcefact, forceadd;
711 Vector3D t1, t2, n, ploc;
712 ComputeGap(i, multimastercontact_last(i)->Get(fi), gap, gapp, p, pp, ploc, t1, t2, n, tvel, forcefact, forceadd, 1); //called from PostNewtonStep
713 }
714 }
715 //if (multimastercontact(i)->Length() || multimastercontact_last(i)->Length())
716 //UO() << "MMC=" << *multimastercontact(i) << ", mmc_last=" << *multimastercontact_last(i) << ", mmc_actlast=" << *multimastercontact_actlast(i) << "\n";
717 }
718 }
719
720 for (int mnum = 1; mnum <= NMC(); mnum++) //update master to slave actual list
721 {
722 mastertoslave_actlast[mnum-1] = mastertoslave[mnum-1];
723
724 for (int fi = 1; fi <= mastertoslave_last[mnum-1].Length(); fi++)
725 {
726 if (!Find(mastertoslave_last[mnum-1].Get(fi),mastertoslave_actlast[mnum-1]))
727 {
728 mastertoslave_actlast[mnum-1].Add(mastertoslave_last[mnum-1].Get(fi));
729 //UO() << "contact M" << mnum << "-" << mastertoslave_last[mnum-1].Get(fi) << " active before\n";
730 }
731 }
732 }
733
734 //if (nlerror > 0.01) UO() << "nlerror=" << nlerror << "\n";
735
736 nlstepcnt++;
737 TMStopTimer(25);
738
739 return nlerror;
740 }
741
742
EvalG(Vector & f,double t)743 void GeneralContact3D::EvalG(Vector& f, double t)
744 {
745 /*
746 UO() << "cmode=" << contactmode << "\n";
747 UO() << "islagrange=" << islagrange << "\n";
748 UO() << "isfriction=" << isfriction << "\n";*/
749
750 if (!islagrange) return;
751
752 if (isfriction) UO() << "ERROR: GeneralContact3D:EvalG, lagrange contact for friction not implemented!\n";
753
754 if (slaveelements.Length() == 0)
755 {
756 UO() << "ERROR: GeneralContact3D::EvalG, number of elements == 0\n"; return;
757 }
758
759 if (MaxIndex()<=3)
760 {
761 Vector3D p, pp;
762 for (int i = 1; i <= NSC(); i++)
763 {
764 if (pointradius(i) != 0) UO() << "ERROR: GeneralContact3D:EvalG, Lagrange contact for circle-elements not implemented!\n";
765
766 //UO() << "iscontact(" << i << ")=" << iscontact(i) << "\n";
767 if (iscontact(i) == 0)
768 {
769 f(i) = XG(i);
770 }
771 else
772 {
773 double gap;
774 p = GetSlaveNodePos(i);
775
776 //mastersegment number is iscontact(i)
777 int mnum = iscontact(i);
778
779
780 gap = GeomElem(mnum)->GetNearestPoint(p, masterlocind(mnum), pp) - pointradius(i);
781
782 Vector3D ploc = GetMasterLocPos(pp, mnum, masterlocind(mnum));
783 Vector3D mpmid = GeomElem(mnum)->GetPos(ploc);
784 Vector3D n;
785 if (pointradius(i) == 0) n = GeomElem(mnum)->GetNormal(masterlocind(mnum), ploc); //normal, outwards
786 else {n = p - mpmid; n.Normalize();}
787
788 Vector3D t1 = GeomElem(mnum)->GetTangent(masterlocind(mnum), ploc);
789 Vector3D t2 = n.Cross(t1);
790
791 double gapp;
792 Vector3D mv = GetMasterSegVel(pp, mnum, masterlocind(mnum)); //master segment approximate velocity
793 gapp = ((GetSlaveNodeVel(i) - mv) * n); //project velocity to contact surface normal
794 //double tvel = ((GetSlaveNodeVel(i) - mv) * tt); //project velocity tangential to surface
795
796 gap = (p-mpmid)*n - pointradius(i);
797
798
799 switch (contactmode)
800 {
801 case 0: //Hertian contact
802 f(i) = GetContactForce(i, gap, gapp) - XG(i); //XG(i) is compression force, Hertzian contact model
803 break;
804 case 1: //tied, linear elastic
805 f(i) = GetContactForce(i, gap, gapp) - XG(i);
806 break;
807 case 2: //tied, rigid, Lagrange multiplier
808 f(i) = gapp;
809 break;
810 case 3: //linear contact model
811 f(i) = GetContactForce(i, gap, gapp) - XG(i);
812 //f(i) = -gap - XG(i)/c_contact(i); //better condition number???
813 break;
814 case 4: //Lagrange multiplier contact, velocity based
815 f(i) = gapp;
816 break;
817 case 5: //corrected restitution
818 f(i) = GetContactForce(i, gap, gapp) - XG(i);
819 break;
820 default: UO() << "ERROR: contactmode\n"; f(i) = 0;
821 }
822 }
823 }
824 }
825
826 };
827
ComputeGap(int i,int mnum,double & gap,double & gapp,Vector3D & p,Vector3D & pp,Vector3D & ploc,Vector3D & t1,Vector3D & t2,Vector3D & n,double & tvel,double & forcefact,double & forceadd,int nlstep=0)828 void GeneralContact3D::ComputeGap(int i, int mnum, double& gap, double& gapp, Vector3D& p, Vector3D& pp,
829 Vector3D& ploc, Vector3D& t1, Vector3D& t2, Vector3D& n, double& tvel, double& forcefact, double& forceadd, int nlstep = 0)
830 {
831 contact_cnt_gap++;
832
833 TMStartTimer(26); //gap
834 gap = GeomElem(mnum)->GetNearestPoint(p, masterlocind(mnum), pp) - pointradius(i);
835
836 ploc = GetMasterLocPos(pp, mnum, masterlocind(mnum));
837 Vector3D mpmid = GeomElem(mnum)->GetPos(ploc);
838
839 if (pointradius(i) == 0) n = GeomElem(mnum)->GetNormal(masterlocind(mnum), ploc); //normal, outwards
840 else {n = p - mpmid; n.Normalize();}
841 t1 = GeomElem(mnum)->GetTangent(masterlocind(mnum), ploc);
842 t2 = n.Cross(t1);
843
844 Vector3D mv = GetMasterSegVel(pp, mnum, masterlocind(mnum)); //master segment approximate velocity
845 gapp = ((GetSlaveNodeVel(i) - mv) * n); //project velocity to contact surface normal
846
847 //project velocity tangential to surface
848 tvel = sqrt(Sqr((GetSlaveNodeVel(i) - mv) * t1) + Sqr((GetSlaveNodeVel(i) - mv) * t2));
849
850 gap = (p-mpmid)*n - pointradius(i); //better approximation of gap
851
852
853 //if (GetMBS()->GetTime() >= 0.2202 && nlstep) UO() << "t=" << GetMBS()->GetTime() << ", gap=" << gap << ", gapp=" << gapp << ", dt=" << GetMBS()->GetStepSize() << "\n";
854
855 //++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
856 int iscontactA = 1; //contact at beginning of time step
857 int iscontactB = 1; //contact at end of time step
858
859 forcefact = 1; //correction of force, because contact force is not applied for the whole time step
860 forceadd = 0;
861
862 if (!Find(mnum, *multimastercontact_last(i)))
863 //if (!Find(i, mastertoslave_last[mnum-1])) //check if contact was active at beginning of step!
864 {
865 iscontactA = 0;
866 }
867 if (!Find(mnum, *multimastercontact(i)))
868 //if (!Find(i, mastertoslave[mnum-1])) //check if contact is active now!
869 {
870 iscontactB = 0;
871 }
872
873
874 int simplemode = 1;
875
876 if (!simplemode)
877 {
878 //correction of force:
879 if (iscontactA != iscontactB)
880 {
881 //++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
882 //compute gap0 and gapp0:
883 double* oldptr = GetMBS()->GetXact().GetVecPtr();
884 int oldlen = GetMBS()->GetXact().Length();
885 GetMBS()->SetActState(GetMBS()->GetLastNLItSolVector());
886
887 Vector3D p0 = GetSlaveNodePos(i);
888 Vector3D pp0, ploc0, n0, t10, t20;
889
890 double gap0 = GeomElem(mnum)->GetNearestPoint(p0, masterlocind(mnum), pp0) - pointradius(i);
891
892 ploc0 = GetMasterLocPos(pp0, mnum, masterlocind(mnum));
893 Vector3D mpmid0 = GeomElem(mnum)->GetPos(ploc0);
894
895 if (pointradius(i) == 0) n0 = GeomElem(mnum)->GetNormal(masterlocind(mnum), ploc0); //normal, outwards
896 else {n0 = p0 - mpmid0; n0.Normalize();}
897 t10 = GeomElem(mnum)->GetTangent(masterlocind(mnum), ploc0);
898 t20 = n0.Cross(t10);
899
900 Vector3D mv0 = GetMasterSegVel(pp0, mnum, masterlocind(mnum)); //master segment approximate velocity
901 double gapp0 = ((GetSlaveNodeVel(i) - mv0) * n0); //project velocity to contact surface normal
902
903 //project velocity tangential to surface
904 double tvel0 = sqrt(Sqr((GetSlaveNodeVel(i) - mv0) * t10) + Sqr((GetSlaveNodeVel(i) - mv0) * t20));
905
906 gap0 = (p0-mpmid0)*n0 - pointradius(i); //better approximation of gap
907
908 GetMBS()->SetActState(oldptr, oldlen);
909 //++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
910
911 //UO() << "cA=" << iscontactA << ", cB=" << iscontactB << "\n";
912 //gapp = -d/dt(gap) .......
913 double dt = GetMBS()->GetStepSize();
914 double gaptot = fabs(gap0) + fabs(gap);
915 if (dt != 0 && gaptot != 0)
916 {
917 double t2 = fabs(gap)/gaptot*dt; //estimated time since last switch
918 double t1 = dt-t2;
919
920 //t2 = gap/gapp;
921
922 //UO() << "t2/dt=" << t2/dt << ",gap0=" << gap0 << ",gap=" << gap << ",gapp0=" << gapp0 << ",gapp=" << gapp << "\n";
923 int conservingmode = 1;
924 if (nlstep)
925 {
926 if (t1 < GetMBS()->GetStepRecommendation() && t1 < GetMBS()->GetStepSize() && fabs(gap) > 2.e-7)
927 {
928 GetMBS()->SetStepRecommendation(t1);
929 //if (t1 > 1e-6) GetMBS()->SetStepRecommendation(1e-6);
930
931 //UO() << "contact rec dt=" << t1;// << "\n";
932 //UO() << "c" << iscontactA << iscontactB << ", t=" << GetMBS()->GetTime() << ", gap=" << gap << ", gapp=" << gapp << ", dt=" << GetMBS()->GetStepSize() << "\n";
933 }
934 }
935
936 if (!iscontactA && iscontactB)
937 {
938 if (conservingmode)
939 {
940 forcefact = 0;// 0
941 //forceadd = -1.361207*c_contact(i)*(gap*(t2/dt)); //for first step right with forcefact = 0 ...
942
943 //forceadd = GetContactForce(i, gap, gapp)*(t2/dt)*(1.+2.*Sqr(t1/dt)); //for first step right with forcefact = 0 ...
944 forceadd = GetContactForce(i, gap, gapp)*(t2/dt); //for first step right with forcefact = 0 ...
945 }
946 else
947 {
948 forcefact = 1;// 0
949 forceadd = 0;
950 }
951 //forceadd = -c_contact(i)*gap0*(t1/dt);
952
953 //forceadd = -c_contact(i)*(-Sqr(t2)*dt*gapp0+Sqr(dt)*gap0+Cub(dt)*gapp0-Sqr(t2)*gap0)/Sqr(dt); //equation from acceleration
954 //forceadd = -c_contact(i)*t2*(dt*gap0+dt*t1*gapp0-t2*gap0)/Sqr(dt); //equation from velocities
955
956 //if (!GetMBS()->IsJacobianComputation()) UO() << "t=" << GetMBS()->GetTime() << ", t2/dt=" << t2/dt << ", fact=" << forcefact << ", f=" << forceadd << ", gap=" << gap << ", gapp=" << gapp << "\n";
957 }
958
959 if (iscontactA && !iscontactB && gap != 0)
960 {
961 if (conservingmode)
962 {
963 forcefact = 0; //too big???
964 //forceadd = -c_contact(i)*gap0*(t1/dt); //somehow right with forcefact = 0 ...
965 //forceadd = c_contact(i)*gap*(t2/dt)*(1.+2.*Sqr(t1/dt)); //somehow right with forcefact = 0 ...
966 forceadd = -0*GetContactForce(i, gap, gapp)*(t2/dt); //somehow right with forcefact = 0 ...
967
968 //if (!GetMBS()->IsJacobianComputation()) UO() << "t=" << GetMBS()->GetTime() << ", t2/dt=" << t2/dt << ", fact=" << forcefact << ", f=" << forceadd << ", gap=" << gap << ", gapp=" << gapp << "\n";
969 }
970 else
971 {
972 forcefact = 0; //too big???
973 forceadd = 0;
974 gap = 0;
975 }
976 }
977 }
978 }
979 }
980 else
981 {
982 forcefact = 1;
983 forceadd = 0;
984 }
985 TMStopTimer(26); //gap
986 //++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
987 }
988
989
AddElementCqTLambda(double t,int locelemind,Vector & f)990 void GeneralContact3D::AddElementCqTLambda(double t, int locelemind, Vector& f)
991 {
992
993 //-C_q^T \lambda = [dC/dx; dC/dy; dC/dphi]^T [\lambda1;\lambda2]
994 //C = p_ref+R*p_loc
995 double sign = 1;
996
997 Vector3D p, pp;
998 double gap;
999
1000 if (locelemind <= NSC())
1001 {
1002 //SLAVE
1003 int i = locelemind; //slave elements
1004
1005 //if (GetMBS()->GetTime() > 0.5704) UO() << "t2=" << GetMBS()->GetTime() << "mmc=" << *multimastercontact(i) << ", mmc_last=" << *multimastercontact_last(i) << ", mmc_actlast=" << *multimastercontact_actlast(i) << "\n";
1006
1007 if ((pointradius(i) == 0 && iscontact(i)) || (pointradius(i) != 0 && multimastercontact_actlast(i)->Length() != 0))
1008 {
1009 p = GetSlaveNodePos(i);
1010
1011 int n = 1;
1012 if (pointradius(i) != 0) n = multimastercontact_actlast(i)->Length();
1013 for (int mi = 1; mi <= n; mi++)
1014 {
1015
1016 //mastersegment number is iscontact(i)
1017 int mnum;
1018 if (pointradius(i) == 0) mnum = iscontact(i);
1019 else mnum = multimastercontact_actlast(i)->Get(mi);
1020
1021 double gapp, tvel, forcefact, forceadd;
1022 Vector3D t1, t2, n, ploc;
1023
1024 //gapp is d/dt(gap)
1025 ComputeGap(i, mnum, gap, gapp, p, pp, ploc, t1, t2, n, tvel, forcefact, forceadd);
1026
1027 double c_force;
1028 if (islagrange)
1029 c_force = XG(i);
1030 else
1031 c_force = GetContactForce(i, gap, gapp);
1032
1033 c_force *= forcefact;
1034 c_force += forceadd;
1035
1036 Vector3D totalforce(sign*n.X()*c_force, sign*n.Y()*c_force, sign*n.Z()*c_force);
1037
1038 /*
1039 if (gap < 0.0 && (GetMBS()->GetTime() >= 0.0))
1040 {
1041 UO() << "gap" << i << "=" << gap << ", force=" << totalforce << "\n";
1042 }*/
1043
1044 if (isfriction)
1045 {
1046 UO() << "Contact3D:: Tangent stick force must be corrected for friction!\n";
1047 if (isstick(i))
1048 {
1049 Vector3D t_force;
1050 Vector3D lastpp = GeomElem(isstick(i))->GetPos(laststickp(i));
1051 GetTangentStickForce(i, t1, tvel, pp, lastpp, t_force); //not correct!!!
1052
1053 totalforce += sign*t_force;
1054
1055 }
1056 else
1057 {
1058 //slipdir->slipvector2D !!!
1059 //totalforce += -sign*slipdir(i)*frictioncoeff*fabs(c_force)*(t1...t2);
1060 }
1061 }
1062
1063 if (slaveNODEmode)
1064 {
1065 int locn = locnodenums(i);
1066 double fact;
1067 /*
1068 fact = 1.5;
1069 if (locn >= 1 && locn <= 4) fact = 3./4.;
1070 */
1071 fact = 1;
1072 totalforce *= -fact; //correction of quadratic elements, //negative sign: needs to be subtracted from f!!!!
1073
1074 GetSlaveBody(i).AddNodedPosdqTLambda(locnodenums(i),totalforce,f);
1075 }
1076 else
1077 {
1078 GetSlaveBody(i).GetdPosdqT(loccoords(i),dpdq);
1079 //UO() << "dpdq" << i << "=" << dpdq << "\n";
1080 //UO() << "force=" << totalforce << "\n";
1081 //UO() << "loccoords=" << loccoords(i) << "\n";
1082
1083
1084 for (int j=1; j <= f.Length(); j++)
1085 {
1086 f(j) -= -(dpdq(j,1)*totalforce.X()+dpdq(j,2)*totalforce.Y()+dpdq(j,3)*totalforce.Z());
1087 }
1088 }
1089 }
1090 }
1091 }
1092 else
1093 {
1094 //MASTER:
1095 int j = locelemind - NSC(); //master elements
1096 sign = -1;
1097
1098 if (GetMasterElnum(j))
1099 {
1100 for (int k = 1; k <= mastertoslave_actlast[j-1].Length(); k++)
1101 {
1102 int i = mastertoslave_actlast[j-1](k);
1103
1104 p = GetSlaveNodePos(i);
1105
1106 //mastersegment number is j
1107 int mnum = j;
1108 //mnum = iscontact(i);
1109
1110
1111 double gapp, tvel, forcefact, forceadd;
1112 Vector3D t1, t2, n, ploc;
1113
1114 ComputeGap(i, mnum, gap, gapp, p, pp, ploc, t1, t2, n, tvel, forcefact, forceadd);
1115
1116 double c_force;
1117 if (islagrange)
1118 c_force = XG(i);
1119 else
1120 c_force = GetContactForce(i, gap, gapp);
1121
1122 c_force *= forcefact;
1123 c_force += forceadd;
1124
1125 if (IsNaN(c_force))
1126 UO() << "ERROR: c_force is NAN\n";
1127 if (IsNaN(gap))
1128 {
1129 UO() << "ERROR: gap is NAN\n";
1130 UO() << "mnum=" << mnum << ", p=" << p << ", pp=" << pp << "\n";
1131 }
1132
1133 Vector3D totalforce(sign*n.X()*c_force, sign*n.Y()*c_force, sign*n.Z()*c_force);
1134
1135 if (isfriction)
1136 {
1137 UO() << "Contact3D::friction, master\n";
1138 if (isstick(i))
1139 {
1140 Vector3D t_force;
1141 Vector3D lastpp = GeomElem(isstick(i))->GetPos(laststickp(i));
1142 GetTangentStickForce(i, t1, tvel, pp, lastpp, t_force); //correct ...
1143
1144 totalforce += sign*t_force;
1145
1146 }
1147 else
1148 {
1149 //slipdir->slipvector2D !!!
1150 //totalforce += -sign*slipdir(i)*frictioncoeff*fabs(c_force)*(t1...t2);
1151 }
1152 }
1153
1154 GetMasterBody(j).GetdPosdqT(ploc,dpdq);
1155
1156 for (int k=1; k <= f.Length(); k++)
1157 {
1158 f(k) -= -(dpdq(k,1)*totalforce.X()+dpdq(k,2)*totalforce.Y()+dpdq(k,3)*totalforce.Z());
1159 }
1160 }
1161 }
1162
1163 }
1164 };
1165
GetRefPosD() const1166 Vector3D GeneralContact3D::GetRefPosD() const
1167 {
1168 Vector3D v;
1169 if (NSC() != 0)
1170 {
1171 v = GetSlaveNodePosD(1);
1172 }
1173 return Vector3D(v.X(),v.Y(),0);
1174 }
1175
DrawElement()1176 void GeneralContact3D::DrawElement()
1177 {
1178 Vector3D p, pp;
1179 for (int i = 1; i <= NSC(); i++)
1180 {
1181 if (GetMBS()->GetIOption(112))
1182 {
1183 p = GetSlaveNodePosD(i);
1184
1185 //draw contact node:
1186 mbs->SetColor(GetCol());
1187 //mbs->DrawSphere(GetSlaveBody(i).ToP3D(p),1*0.5*draw_dim.X(),4);
1188
1189 /*
1190 //mastersegment number is iscontact(i)
1191 int mnum = iscontact(i);
1192 if (mnum && 0)
1193 {
1194 GeomElem(mnum)->GetNearestPoint(p, masterlocind(mnum), pp);
1195
1196 mbs->DrawSphere(GetSlaveBody(i).ToP3D(pp),1.*draw_dim.X(),3);
1197 }*/
1198 }
1199 }
1200
1201 for (int i = 1; i <= NMC(); i++)
1202 {
1203 //if (GetMBS()->GetIOption(112))
1204 if (masterisactive(i) == 0 || mathfunctions(masterisactive(i))->CheckCondition(GetMBS()->GetDrawTime()))
1205 {
1206 GeomElem(i)->DrawYourself();
1207 }
1208 }
1209 };
1210
1211
1212
1213
1214
1215
1216
1217
1218