MBDyn-1.7.3
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Groups
vehj3.cc
Go to the documentation of this file.
1 /* $Header: /var/cvs/mbdyn/mbdyn/mbdyn-1.0/mbdyn/struct/vehj3.cc,v 1.44 2017/07/23 15:09:35 zanoni 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 /* Cerniera deformabile */
33 
34 #include "mbconfig.h" /* This goes first in every *.c,*.cc file */
35 
36 #include "dataman.h"
37 #include "vehj3.h"
38 
39 #include "matvecexp.h"
40 #include "Rot.hh"
41 
42 // helper for efficiency
43 static void
44 MultRMRtGammam1(Mat6x6& M, const Mat3x3& R, const Mat3x3& Gammam1)
45 {
46  Mat3x3 GM1RT(Gammam1.MulMT(R));
47 
48  M.PutMat11(R*(M.GetMat11().MulMT(R)));
49  M.PutMat12(R*(M.GetMat12()*GM1RT));
50  M.PutMat21(R*(M.GetMat21().MulMT(R)));
51  M.PutMat22(R*(M.GetMat22()*GM1RT));
52 }
53 
54 /* DeformableJoint - begin */
55 
56 /* Costruttore non banale */
58  const DofOwner* pDO,
59  const ConstitutiveLaw6D* pCL,
60  const StructNode* pN1,
61  const StructNode* pN2,
62  const Vec3& tilde_f1,
63  const Vec3& tilde_f2,
64  const Mat3x3& tilde_R1h,
65  const Mat3x3& tilde_R2h,
66  const OrientationDescription& od,
67  flag fOut)
68 : Elem(uL, fOut),
69 Joint(uL, pDO, fOut),
71 pNode1(pN1), pNode2(pN2),
72 tilde_f1(tilde_f1), tilde_f2(tilde_f2),
73 tilde_R1h(tilde_R1h), tilde_R2h(tilde_R2h),
74 od(od),
75 tilde_k(Zero6), tilde_kPrime(Zero6),
76 #ifdef USE_NETCDF
77 Var_tilde_d(0),
78 Var_tilde_dPrime(0),
79 Var_d(0),
80 Var_dPrime(0),
81 Var_Phi(0),
82 Var_Omega(0),
83 #endif // USE_NETCDF
84 bFirstRes(false)
85 {
86  ASSERT(pNode1 != NULL);
87  ASSERT(pNode2 != NULL);
90 
92 }
93 
94 
95 /* Distruttore */
97 {
98  NO_OP;
99 }
100 
101 
102 /* Contributo al file di restart */
103 std::ostream&
104 DeformableJoint::Restart(std::ostream& out) const
105 {
106  Joint::Restart(out) << ", deformable joint, "
107  << pNode1->GetLabel() << ", reference, node, ",
108  tilde_f1.Write(out, ", ") << ", hinge, reference, node, 1, ",
109  (tilde_R1h.GetVec(1)).Write(out, ", ")
110  << ", 2, ", (tilde_R1h.GetVec(2)).Write(out, ", ") << ", "
111  << pNode2->GetLabel() << ", reference, node, ",
112  tilde_f2.Write(out, ", ") << ", hinge, reference, node, 1, ",
113  (tilde_R2h.GetVec(1)).Write(out, ", ")
114  << ", 2, ", (tilde_R2h.GetVec(2)).Write(out, ", ") << ", ";
115  return pGetConstLaw()->Restart(out) << ';' << std::endl;
116 }
117 
118 void
120 {
121  if (bToBeOutput()) {
122 #ifdef USE_NETCDF
124  std::string name;
125  OutputPrepare_int("deformable joint", OH, name);
126  Var_tilde_d = OH.CreateVar<Vec3>(name + "d", "m",
127  "relative position in local frame (x, y, z)");
128  Var_tilde_dPrime = OH.CreateVar<Vec3>(name + "dPrime", "m/s",
129  "relative linear velocity in local frame (x, y, z)");
130  Var_d = OH.CreateVar<Vec3>(name + "D", "m",
131  "relative position in global frame (x, y, z)");
132  Var_dPrime = OH.CreateVar<Vec3>(name + "DPrime", "m/s",
133  "relative linear velocity in global frame (x, y, z)");
134  Var_Phi = OH.CreateRotationVar(name, "", od, "global");
135  Var_Omega = OH.CreateVar<Vec3>(name + "Omega", "radian/s",
136  "local relative angular velocity (x, y, z)");
137  }
138 #endif
139  }
140 }
141 
142 void
144 {
145  if (bToBeOutput()) {
148  Mat3x3 R(R1h.MulTM(R2h));
149  Vec3 F(GetF().GetVec1());
150  Vec3 M(GetF().GetVec2());
151  Vec3 E;
152 
153  // angular strain
154  switch (od) {
155  case EULER_123:
157  break;
158 
159  case EULER_313:
161  break;
162 
163  case EULER_321:
165  break;
166 
167  case ORIENTATION_VECTOR:
168  E = RotManip::VecRot(R);
169  break;
170 
171  case ORIENTATION_MATRIX:
172  break;
173 
174  default:
175  /* impossible */
176  break;
177  }
178 
179 
180 #ifdef USE_NETCDF
182  Var_F_local->put_rec((R1h*F).pGetVec(), OH.GetCurrentStep());
183  Var_M_local->put_rec((R1h*M).pGetVec(), OH.GetCurrentStep());
184  Var_F_global->put_rec(F.pGetVec(), OH.GetCurrentStep());
185  Var_M_global->put_rec(M.pGetVec(), OH.GetCurrentStep());
186 
187  switch (od) {
188  case EULER_123:
189  case EULER_313:
190  case EULER_321:
191  case ORIENTATION_VECTOR:
192  Var_Phi->put_rec(E.pGetVec(), OH.GetCurrentStep());
193  break;
194  case ORIENTATION_MATRIX:
195  Var_Phi->put_rec(R.pGetMat(), OH.GetCurrentStep());
196  break;
197  default:
198  /* impossible */
199  break;
200  }
201  }
202 #endif // USE_NETCDF
203  if (OH.UseText(OutputHandler::JOINTS)) {
204  Joint::Output(OH.Joints(), "DeformableJoint", GetLabel(),
205  F, M, R1h*F, R1h*M);
206 
207  // linear strain
208  OH.Joints() << " " << tilde_k.GetVec1() << " ";
209 
210  // Angular strain
211  switch (od) {
212  case EULER_123:
213  case EULER_313:
214  case EULER_321:
215  case ORIENTATION_VECTOR:
216  OH.Joints() << E;
217  break;
218  case ORIENTATION_MATRIX:
219  OH.Joints() << R;
220  break;
221  default:
222  /* impossible */
223  break;
224  }
225 
227  OH.Joints() << " " << tilde_kPrime;
228  }
229 
230  OH.Joints() << std::endl;
231  }
232  }
233 }
234 
235 void
239 {
240  if (ph) {
241  for (unsigned i = 0; i < ph->size(); i++) {
242  Joint::JointHint *pjh = dynamic_cast<Joint::JointHint *>((*ph)[i]);
243 
244  if (pjh) {
245  if (dynamic_cast<Joint::OffsetHint<1> *>(pjh)) {
246  Mat3x3 R1t(pNode1->GetRCurr().Transpose());
247  Vec3 f2(pNode2->GetRCurr()*tilde_f2);
248 
249  tilde_f1 = R1t*(pNode2->GetXCurr() + f2 - pNode1->GetXCurr());
250 
251  } else if (dynamic_cast<Joint::OffsetHint<2> *>(pjh)) {
252  Mat3x3 R2t(pNode2->GetRCurr().Transpose());
253  Vec3 f1(pNode1->GetRCurr()*tilde_f1);
254 
255  tilde_f2 = R2t*(pNode1->GetXCurr() + f1 - pNode2->GetXCurr());
256 
257  } else if (dynamic_cast<Joint::HingeHint<1> *>(pjh)) {
259 
260  } else if (dynamic_cast<Joint::HingeHint<2> *>(pjh)) {
262 
263  } else if (dynamic_cast<Joint::ReactionsHint *>(pjh)) {
264  /* TODO */
265  }
266 
267  continue;
268  }
269 
270  /* else, pass to constitutive law */
271  ConstitutiveLaw6DOwner::SetValue(pDM, X, XP, ph);
272  }
273  }
274 }
275 
276 /* inverse dynamics capable element */
277 bool
279 {
280  return true;
281 }
282 
283 Hint *
284 DeformableJoint::ParseHint(DataManager *pDM, const char *s) const
285 {
286  if (strncasecmp(s, "offset{" /*}*/ , STRLENOF("offset{" /*}*/ )) == 0)
287  {
288  s += STRLENOF("offset{" /*}*/ );
289 
290  if (strcmp(&s[1], /*{*/ "}") != 0) {
291  return 0;
292  }
293 
294  switch (s[0]) {
295  case '1':
296  return new Joint::OffsetHint<1>;
297 
298  case '2':
299  return new Joint::OffsetHint<2>;
300  }
301 
302  } else if (strncasecmp(s, "hinge{" /*}*/, STRLENOF("hinge{" /*}*/)) == 0) {
303  s += STRLENOF("hinge{" /*}*/);
304 
305  if (strcmp(&s[1], /*{*/ "}") != 0) {
306  return 0;
307  }
308 
309  switch (s[0]) {
310  case '1':
311  return new Joint::HingeHint<1>;
312 
313  case '2':
314  return new Joint::HingeHint<2>;
315  }
316  }
317 
318  return ConstitutiveLaw6DOwner::ParseHint(pDM, s);
319 }
320 
321 unsigned int
323 {
325 }
326 
327 unsigned int
329 {
330  ASSERT(s != NULL);
331 
332  unsigned idx = 0;
333 
334  switch (s[0]) {
335  case 'd':
336  break;
337 
338  case 'r':
339  idx += 3;
340  break;
341 
342  case 'v':
343  idx += 6;
344  break;
345 
346  case 'w':
347  idx += 9;
348  break;
349 
350  case 'F':
351  idx += 12;
352  break;
353 
354  case 'M':
355  idx += 15;
356  break;
357 
358  default:
359  {
360  size_t l = STRLENOF("constitutiveLaw.");
361  if (strncmp(s, "constitutiveLaw.", l) == 0) {
363  if (idx > 0) {
364  return 18 + idx;
365  }
366  }
367  return 0;
368  }
369  }
370 
371  switch (s[1]) {
372  case 'x':
373  idx += 1;
374  break;
375  case 'y':
376  idx += 2;
377  break;
378  case 'z':
379  idx += 3;
380  break;
381  default:
382  return 0;
383  }
384 
385  if (s[2] != '\0') {
386  return 0;
387  }
388 
389  return idx;
390 }
391 
393 DeformableJoint::dGetPrivData(unsigned int i) const
394 {
395  ASSERT(i > 0);
396 
397  ASSERT(i <= iGetNumPrivData());
398 
399  switch (i) {
400  case 1:
401  case 2:
402  case 3:
403  {
404  Vec3 f1(pNode1->GetRCurr()*tilde_f1);
405  Vec3 f2(pNode2->GetRCurr()*tilde_f2);
407  Vec3 tilde_d(R1hT*(pNode2->GetXCurr() + f2 - pNode1->GetXCurr() - f1));
408 
409  return tilde_d(i);
410  }
411 
412  case 4:
413  case 5:
414  case 6:
415  {
418 
419  Vec3 tilde_Theta(RotManip::VecRot(R1hT*R2h));
420 
421  return tilde_Theta(i - 3);
422  }
423 
424  case 7:
425  case 8:
426  case 9:
427  {
428  Vec3 f2(pNode2->GetRCurr()*tilde_f2);
429  Mat3x3 R1hT(pNode1->GetRCurr().Transpose());
430  Vec3 tilde_dPrime(R1hT*(pNode2->GetVCurr() - pNode1->GetVCurr()
432  - f2.Cross(pNode2->GetWCurr() - pNode1->GetWCurr())));
433 
434  return tilde_dPrime(i - 6);
435  }
436 
437  case 10:
438  case 11:
439  case 12:
440  {
442  Vec3 tilde_Omega = R1hT*(pNode2->GetWCurr() - pNode1->GetWCurr());
443 
444  return tilde_Omega(i - 9);
445  }
446 
447  case 13:
448  case 14:
449  case 15:
450  case 16:
451  case 17:
452  case 18:
453  return GetF()(i - 12);
454 
455  default:
457  }
458 }
459 
460 void
462 {
463  d2 = pNode2->GetRCurr()*tilde_f2;
464  d1 = pNode2->GetXCurr() + d2 - pNode1->GetXCurr();
465 
466  Vec3 FTmp(F.GetVec1()*dCoef);
467  Mat3x3 FCross(MatCross, FTmp);
468  Mat3x3 MCross(MatCross, F.GetVec2()*dCoef);
469 
470  WM.Add(1, 4, FCross);
471  WM.Sub(4, 1, FCross);
472  WM.Add(4, 6 + 1, FCross);
473  WM.Sub(6 + 1, 4, FCross);
474 
475  Mat3x3 MTmp(MatCrossCross, FTmp, d2);
476 
477  WM.Add(6 + 4, 6 + 4, MTmp);
478  WM.Sub(4, 6 + 4, MTmp);
479 
480  MTmp = Mat3x3(MatCrossCross, d2, FTmp) + MCross;
481 
482  WM.Sub(6 + 4, 4, MTmp);
483 
484  MTmp = Mat3x3(MatCrossCross, d1, FTmp) + MCross;
485 
486  WM.Add(4, 4, MTmp);
487 }
488 
489 void
491  const Mat6x6& FDE)
492 {
493  Mat3x3 F_d = FDE.GetMat11()*dCoef;
494  Mat3x3 F_theta = FDE.GetMat12()*dCoef;
495  Mat3x3 M_d = FDE.GetMat21()*dCoef;
496  Mat3x3 M_theta = FDE.GetMat22()*dCoef;
497 
498  /* D11 */
499  WM.Add(1, 1, F_d);
500  WM.Sub(1, 6 + 1, F_d);
501  WM.Sub(6 + 1, 1, F_d);
502  WM.Add(6 + 1, 6 + 1, F_d);
503 
504  /* D11 * [d1 x] */
505  Mat3x3 FTmp = F_d*Mat3x3(MatCross, d1) - F_theta;
506 
507  WM.Sub(1, 4, FTmp);
508  WM.Add(6 + 1, 4, FTmp);
509 
510  /* */
511  Mat3x3 MTmp(M_d*Mat3x3(MatCross, d1) - M_theta);
512 
513  WM.Sub(4, 4, d1.Cross(FTmp) + MTmp);
514  WM.Add(6 + 4, 4, d2.Cross(FTmp) + MTmp);
515 
516  /* D11 * [d2 x] */
517  FTmp = F_d*Mat3x3(MatCross, d2) - F_theta;
518 
519  WM.Add(1, 6 + 4, FTmp);
520  WM.Sub(6 + 1, 6 + 4, FTmp);
521 
522  /* */
523  MTmp = M_d*Mat3x3(MatCross, d2) - M_theta;
524 
525  WM.Add(4, 6 + 4, d1.Cross(FTmp) + MTmp);
526  WM.Sub(6 + 4, 6 + 4, d2.Cross(FTmp) + MTmp);
527 
528  /* [d1 x] * D11 */
529  FTmp = d1.Cross(F_d) + M_d;
530 
531  WM.Add(4, 1, FTmp);
532  WM.Sub(4, 6 + 1, FTmp);
533 
534  /* [d2 x] * D11 */
535  FTmp = d2.Cross(F_d) + M_d;
536 
537  WM.Sub(6 + 4, 1, FTmp);
538  WM.Add(6 + 4, 6 + 1, FTmp);
539 }
540 
541 
542 void
545  doublereal dCoef, const Mat6x6& FDEPrime)
546 {
547  const Mat3x3& F_dPrime = FDEPrime.GetMat11();
548  const Mat3x3& F_thetaPrime = FDEPrime.GetMat12();
549  const Mat3x3& M_dPrime = FDEPrime.GetMat21();
550  const Mat3x3& M_thetaPrime = FDEPrime.GetMat22();
551 
552  const Vec3& Omega1 = pNode1->GetWCurr();
553  const Vec3& Omega2 = pNode2->GetWCurr();
554 
555  /* D11 */
556  WMB.Add(1, 1, F_dPrime);
557  WMB.Sub(1, 6 + 1, F_dPrime);
558  WMB.Sub(6 + 1, 1, F_dPrime);
559  WMB.Add(6 + 1, 6 + 1, F_dPrime);
560 
561  /* D11 * [d1 x] */
562  Mat3x3 FTmp = F_dPrime*Mat3x3(MatCross, d1) - F_thetaPrime;
563 
564  WMB.Sub(1, 4, FTmp);
565  WMB.Add(6 + 1, 4, FTmp);
566 
567  /* */
568  Mat3x3 MTmp(M_dPrime*Mat3x3(MatCross, d1) - M_thetaPrime);
569 
570  WMB.Sub(4, 4, d1.Cross(FTmp) + MTmp);
571  WMB.Add(6 + 4, 4, d2.Cross(FTmp) + MTmp);
572 
573  /* D11 * [d2 x] */
574  FTmp = F_dPrime*Mat3x3(MatCross, d2) - F_thetaPrime;
575 
576  WMB.Add(1, 6 + 4, FTmp);
577  WMB.Sub(6 + 1, 6 + 4, FTmp);
578 
579  /* */
580  MTmp = M_dPrime*Mat3x3(MatCross, d2) - M_thetaPrime;
581 
582  WMB.Add(4, 6 + 4, d1.Cross(FTmp) + MTmp);
583  WMB.Sub(6 + 4, 6 + 4, d2.Cross(FTmp) + MTmp);
584 
585  /* [d1 x] * D11 */
586  FTmp = d1.Cross(F_dPrime) + M_dPrime;
587 
588  WMB.Add(4, 1, FTmp);
589  WMB.Sub(4, 6 + 1, FTmp);
590 
591  /* [d2 x] * D11 */
592  FTmp = d2.Cross(F_dPrime) + M_dPrime;
593 
594  WMB.Sub(6 + 4, 1, FTmp);
595  WMB.Add(6 + 4, 6 + 1, FTmp);
596 
597  // ~~~ o ~~~ o ~~~ o ~~~
598 
599  MTmp = F_dPrime*Mat3x3(MatCross, Omega1*dCoef);
600 
601  WMA.Sub(1, 1, MTmp);
602  WMA.Add(6 + 1, 1, MTmp);
603  WMA.Add(1, 6 + 1, MTmp);
604  WMA.Sub(6 + 1, 6 + 1, MTmp);
605 
606  Mat3x3 A1dP(d1.Cross(F_dPrime) + M_dPrime);
607  Mat3x3 A2dP(d2.Cross(F_dPrime) + M_dPrime);
608 
609  Mat3x3 A1tPw((d1.Cross(F_thetaPrime) + M_thetaPrime)*Mat3x3(MatCross, Omega2*dCoef));
610  Mat3x3 A2tPw((d2.Cross(F_thetaPrime) + M_thetaPrime)*Mat3x3(MatCross, Omega2*dCoef));
611 
612  Mat3x3 D1(Mat3x3(MatCross, d1Prime*dCoef) - Mat3x3(MatCrossCross, Omega1, d1*dCoef));
613  Mat3x3 D2(Mat3x3(MatCross, d2Prime*dCoef) - Mat3x3(MatCrossCross, Omega1, d2*dCoef));
614 
615  MTmp = A1dP*Mat3x3(MatCross, Omega1*dCoef);
616 
617  WMA.Sub(4, 1, MTmp);
618  WMA.Add(4, 6 + 1, MTmp);
619 
620  MTmp = A2dP*Mat3x3(MatCross, Omega1*dCoef);
621 
622  WMA.Add(6 + 4, 1, MTmp);
623  WMA.Sub(6 + 4, 6 + 1, MTmp);
624 
625  FTmp = F_thetaPrime*Mat3x3(MatCross, Omega2*dCoef);
626 
627  MTmp = F_dPrime*D1 + FTmp;
628 
629  WMA.Sub(1, 4, MTmp);
630  WMA.Add(6 + 1, 4, MTmp);
631 
632  MTmp = F_dPrime*D2 + FTmp;
633 
634  WMA.Add(1, 6 + 4, MTmp);
635  WMA.Sub(6 + 1, 6 + 4, MTmp);
636 
637  WMA.Sub(4, 4, A1dP*D1 + A1tPw);
638 
639  WMA.Add(6 + 4, 4, A2dP*D1 + A2tPw);
640 
641  WMA.Add(4, 6 + 4, A1dP*D2 + A1tPw);
642 
643  WMA.Sub(6 + 4, 6 + 4, A2dP*D2 + A2tPw);
644 }
645 
646 /* assemblaggio jacobiano */
649  doublereal dCoef,
650  const VectorHandler& /* XCurr */ ,
651  const VectorHandler& /* XPrimeCurr */ )
652 {
653  FullSubMatrixHandler& WM = WorkMat.SetFull();
654 
655  /* Dimensiona e resetta la matrice di lavoro */
656  integer iNumRows = 0;
657  integer iNumCols = 0;
658  WorkSpaceDim(&iNumRows, &iNumCols);
659  WM.ResizeReset(iNumRows, iNumCols);
660 
661  /* Recupera gli indici */
662  integer iNode1FirstPosIndex = pNode1->iGetFirstPositionIndex();
663  integer iNode1FirstMomIndex = pNode1->iGetFirstMomentumIndex();
664  integer iNode2FirstPosIndex = pNode2->iGetFirstPositionIndex();
665  integer iNode2FirstMomIndex = pNode2->iGetFirstMomentumIndex();
666 
667  /* Setta gli indici della matrice */
668  for (int iCnt = 1; iCnt <= 6; iCnt++) {
669  WM.PutRowIndex(iCnt, iNode1FirstMomIndex + iCnt);
670  WM.PutColIndex(iCnt, iNode1FirstPosIndex + iCnt);
671  WM.PutRowIndex(6 + iCnt, iNode2FirstMomIndex + iCnt);
672  WM.PutColIndex(6 + iCnt, iNode2FirstPosIndex + iCnt);
673  }
674 
675  AssMats(WM, WM, dCoef);
676 
677  return WorkMat;
678 }
679 
680 /* Jacobian matrix assembly - all but Elastic */
681 void
683  VariableSubMatrixHandler& WorkMatB,
684  const VectorHandler& /* XCurr */ ,
685  const VectorHandler& /* XPrimeCurr */ )
686 {
687  FullSubMatrixHandler& WMA = WorkMatA.SetFull();
688  FullSubMatrixHandler& WMB = WorkMatB.SetFull();
689 
690  /* Dimensiona e resetta la matrice di lavoro */
691  integer iNumRows = 0;
692  integer iNumCols = 0;
693  WorkSpaceDim(&iNumRows, &iNumCols);
694  WMA.ResizeReset(iNumRows, iNumCols);
695  WMB.ResizeReset(iNumRows, iNumCols);
696 
697  /* Recupera gli indici */
698  integer iNode1FirstPosIndex = pNode1->iGetFirstPositionIndex();
699  integer iNode1FirstMomIndex = pNode1->iGetFirstMomentumIndex();
700  integer iNode2FirstPosIndex = pNode2->iGetFirstPositionIndex();
701  integer iNode2FirstMomIndex = pNode2->iGetFirstMomentumIndex();
702 
703  /* Setta gli indici della matrice */
704  for (int iCnt = 1; iCnt <= 6; iCnt++) {
705  WMA.PutRowIndex(iCnt, iNode1FirstMomIndex + iCnt);
706  WMA.PutColIndex(iCnt, iNode1FirstPosIndex + iCnt);
707  WMA.PutRowIndex(6 + iCnt, iNode2FirstMomIndex + iCnt);
708  WMA.PutColIndex(6 + iCnt, iNode2FirstPosIndex + iCnt);
709 
710  WMB.PutRowIndex(iCnt, iNode1FirstMomIndex + iCnt);
711  WMB.PutColIndex(iCnt, iNode1FirstPosIndex + iCnt);
712  WMB.PutRowIndex(6 + iCnt, iNode2FirstMomIndex + iCnt);
713  WMB.PutColIndex(6 + iCnt, iNode2FirstPosIndex + iCnt);
714  }
715 
716  AssMats(WMA, WMB, 1.);
717 }
718 
719 /* assemblaggio residuo */
722  doublereal /* dCoef */ ,
723  const VectorHandler& /* XCurr */ ,
724  const VectorHandler& /* XPrimeCurr */ )
725 {
726  /* Dimensiona e resetta la matrice di lavoro */
727  integer iNumRows = 0;
728  integer iNumCols = 0;
729  WorkSpaceDim(&iNumRows, &iNumCols);
730  WorkVec.ResizeReset(iNumRows);
731 
732  /* Recupera gli indici */
733  integer iNode1FirstMomIndex = pNode1->iGetFirstMomentumIndex();
734  integer iNode2FirstMomIndex = pNode2->iGetFirstMomentumIndex();
735 
736  /* Setta gli indici della matrice */
737  for (int iCnt = 1; iCnt <= 6; iCnt++) {
738  WorkVec.PutRowIndex(iCnt, iNode1FirstMomIndex + iCnt);
739  WorkVec.PutRowIndex(6 + iCnt, iNode2FirstMomIndex + iCnt);
740  }
741 
742  AssVec(WorkVec);
743 
744  return WorkVec;
745 }
746 
747 /* Inverse Dynamics Residual Assembly */
750  const VectorHandler& /* XCurr */,
751  const VectorHandler& /* XPrimeCurr */,
752  const VectorHandler& /* XPrimePrimeCurr */,
753  InverseDynamics::Order iOrder)
754 {
756  || (iOrder == InverseDynamics::POSITION && bIsErgonomy()));
757 
758  bFirstRes = false;
759 
760  /* Dimensiona e resetta la matrice di lavoro */
761  integer iNumRows = 0;
762  integer iNumCols = 0;
763  WorkSpaceDim(&iNumRows, &iNumCols);
764  WorkVec.ResizeReset(iNumRows);
765 
766  /* Recupera gli indici */
767  integer iNode1FirstMomIndex = pNode1->iGetFirstPositionIndex();
768  integer iNode2FirstMomIndex = pNode2->iGetFirstPositionIndex();
769 
770  /* Setta gli indici della matrice */
771  for (int iCnt = 1; iCnt <= 6; iCnt++) {
772  WorkVec.PutRowIndex(iCnt, iNode1FirstMomIndex + iCnt);
773  WorkVec.PutRowIndex(6 + iCnt, iNode2FirstMomIndex + iCnt);
774  }
775 
776  AssVec(WorkVec);
777 
778  return WorkVec;
779 }
780 
781 /* Contributo al residuo durante l'assemblaggio iniziale */
784  const VectorHandler& /* XCurr */ )
785 {
786  /* Dimensiona e resetta la matrice di lavoro */
787  integer iNumRows = 0;
788  integer iNumCols = 0;
789  InitialWorkSpaceDim(&iNumRows, &iNumCols);
790  WorkVec.ResizeReset(iNumRows);
791 
792  /* Recupera gli indici */
793  integer iNode1FirstPosIndex = pNode1->iGetFirstPositionIndex();
794  integer iNode2FirstPosIndex = pNode2->iGetFirstPositionIndex();
795 
796  /* Setta gli indici della matrice */
797  for (int iCnt = 1; iCnt <= 6; iCnt++) {
798  WorkVec.PutRowIndex(iCnt, iNode1FirstPosIndex + iCnt);
799  WorkVec.PutRowIndex(6 + iCnt, iNode2FirstPosIndex + iCnt);
800  }
801 
802  AssVec(WorkVec);
803 
804  return WorkVec;
805 }
806 
807 /* DeformableJoint - end */
808 
809 
810 /* ElasticJoint - begin */
811 
813  const DofOwner* pDO,
814  const ConstitutiveLaw6D* pCL,
815  const StructNode* pN1,
816  const StructNode* pN2,
817  const Vec3& tilde_f1,
818  const Vec3& tilde_f2,
819  const Mat3x3& tilde_R1h,
820  const Mat3x3& tilde_R2h,
821  const OrientationDescription& od,
822  flag fOut)
823 : Elem(uL, fOut),
824 DeformableJoint(uL, pDO, pCL, pN1, pN2, tilde_f1, tilde_f2, tilde_R1h, tilde_R2h, od, fOut),
825 ThetaRef(Zero3)
826 {
827  /*
828  * Chiede la matrice tangente di riferimento
829  * e la porta nel sistema globale
830  */
832 }
833 
835 {
836  NO_OP;
837 }
838 
839 void
841  const VectorHandler& XP)
842 {
844 }
845 
846 /* assemblaggio jacobiano */
847 void
849  VariableSubMatrixHandler& WorkMatB,
850  const VectorHandler& /* XCurr */ ,
851  const VectorHandler& /* XPrimeCurr */ )
852 {
853  FullSubMatrixHandler& WMA = WorkMatA.SetFull();
854  WorkMatB.SetNullMatrix();
855 
856  /* Dimensiona e resetta la matrice di lavoro */
857  integer iNumRows = 0;
858  integer iNumCols = 0;
859  WorkSpaceDim(&iNumRows, &iNumCols);
860  WMA.ResizeReset(iNumRows, iNumCols);
861 
862  /* Recupera gli indici */
863  integer iNode1FirstPosIndex = pNode1->iGetFirstPositionIndex();
864  integer iNode1FirstMomIndex = pNode1->iGetFirstMomentumIndex();
865  integer iNode2FirstPosIndex = pNode2->iGetFirstPositionIndex();
866  integer iNode2FirstMomIndex = pNode2->iGetFirstMomentumIndex();
867 
868  /* Setta gli indici della matrice */
869  for (int iCnt = 1; iCnt <= 6; iCnt++) {
870  WMA.PutRowIndex(iCnt, iNode1FirstMomIndex + iCnt);
871  WMA.PutColIndex(iCnt, iNode1FirstPosIndex + iCnt);
872  WMA.PutRowIndex(6 + iCnt, iNode2FirstMomIndex + iCnt);
873  WMA.PutColIndex(6 + iCnt, iNode2FirstPosIndex + iCnt);
874  }
875 
876  AssMats(WMA, WMA, 1.);
877 }
878 
879 void
881  FullSubMatrixHandler& /* WMB */ ,
882  doublereal dCoef)
883 {
884  AssMatCommon(WMA, dCoef);
885  AssMatElastic(WMA, dCoef, FDE);
886 }
887 
888 /* Inverse Dynamics Jacobian matrix assembly */
891  const VectorHandler& XCurr)
892 {
893  ASSERT(bIsErgonomy());
894 
895  // HACK? Need to call AfterPredict() here to update MDE and so
896  // const_cast because AfterPredict() does not modify X, XP
897  AfterPredict(const_cast<VectorHandler&>(XCurr), const_cast<VectorHandler&>(XCurr));
898 
899  return DeformableJoint::AssJac(WorkMat, 1., XCurr, XCurr);
900 }
901 
902 /* Inverse Dynamics update */
903 void
905 {
906  NO_OP;
907 }
908 
909 /* Inverse Dynamics after convergence */
910 void
912  const VectorHandler& XP, const VectorHandler& XPP)
913 {
915 }
916 
917 void
919 {
920  if (bFirstRes) {
921  bFirstRes = false;
922 
923  } else {
925 
926  d2 = pNode2->GetRCurr()*tilde_f2;
927  d1 = pNode2->GetXCurr() + d2 - pNode1->GetXCurr();
928 
930  Vec3 f1(pNode1->GetRCurr()*tilde_f1);
931 
932  tilde_k = Vec6(R1h.MulTV(d1 - f1), RotManip::VecRot(R1h.MulTM(R2h)));
933 
935  }
936 
937  F = MultRV(GetF(), R1h);
938 
939  WorkVec.Add(1, F.GetVec1());
940  WorkVec.Add(4, d1.Cross(F.GetVec1()) + F.GetVec2());
941  WorkVec.Sub(6 + 1, F.GetVec1());
942  WorkVec.Sub(6 + 4, d2.Cross(F.GetVec1()) + F.GetVec2());
943 }
944 
945 void
947  VectorHandler& /* XP */ )
948 {
949  /* Calcola le deformazioni, aggiorna il legame costitutivo
950  * e crea la FDE */
951 
952  /* Recupera i dati */
954 
955  /* Calcola la deformazione corrente nel sistema locale (nodo a) */
957 
958  /* Calcola l'inversa di Gamma di ThetaRef */
959  Mat3x3 GammaRefm1 = RotManip::DRot_I(ThetaRef);
960 
961  d2 = pNode2->GetRRef()*tilde_f2;
962  d1 = pNode2->GetXCurr() + d2 - pNode1->GetXCurr();
963  Vec3 f1(pNode1->GetRRef()*tilde_f1);
964 
965  tilde_k = Vec6(R1h.MulTV(d1 - f1), ThetaRef);
966 
967  /* Aggiorna il legame costitutivo */
969 
970  /* Chiede la matrice tangente di riferimento e la porta
971  * nel sistema globale */
973  MultRMRtGammam1(FDE, R1h, GammaRefm1);
974 
975  bFirstRes = true;
976 }
977 
978 
979 /* Contributo allo jacobiano durante l'assemblaggio iniziale */
982  const VectorHandler& /* XCurr */ )
983 {
984  FullSubMatrixHandler& WM = WorkMat.SetFull();
985 
986  /* Dimensiona e resetta la matrice di lavoro */
987  integer iNumRows = 0;
988  integer iNumCols = 0;
989  InitialWorkSpaceDim(&iNumRows, &iNumCols);
990  WM.ResizeReset(iNumRows, iNumCols);
991 
992  /* Recupera gli indici */
993  integer iNode1FirstPosIndex = pNode1->iGetFirstPositionIndex();
994  integer iNode2FirstPosIndex = pNode2->iGetFirstPositionIndex();
995 
996  /* Setta gli indici della matrice */
997  for (int iCnt = 1; iCnt <= 6; iCnt++) {
998  WM.PutRowIndex(iCnt, iNode1FirstPosIndex + iCnt);
999  WM.PutColIndex(iCnt, iNode1FirstPosIndex + iCnt);
1000  WM.PutRowIndex(6 + iCnt, iNode2FirstPosIndex + iCnt);
1001  WM.PutColIndex(6 + iCnt, iNode2FirstPosIndex + iCnt);
1002  }
1003 
1004  AssMats(WM, WM, 1.);
1005 
1006  return WorkMat;
1007 }
1008 
1009 /* ElasticJoint - end */
1010 
1011 
1012 /* ElasticJointInv - begin */
1013 
1015  const DofOwner* pDO,
1016  const ConstitutiveLaw6D* pCL,
1017  const StructNode* pN1,
1018  const StructNode* pN2,
1019  const Vec3& tilde_f1,
1020  const Vec3& tilde_f2,
1021  const Mat3x3& tilde_R1h,
1022  const Mat3x3& tilde_R2h,
1023  const OrientationDescription& od,
1024  flag fOut)
1025 : Elem(uL, fOut),
1026 DeformableJoint(uL, pDO, pCL, pN1, pN2, tilde_f1, tilde_f2, tilde_R1h, tilde_R2h, od, fOut),
1027 ThetaRef(Zero3)
1028 {
1029  /*
1030  * Chiede la matrice tangente di riferimento
1031  * e la porta nel sistema globale
1032  */
1034 }
1035 
1037 {
1038  NO_OP;
1039 }
1040 
1041 void
1043  const VectorHandler& XP)
1044 {
1046 }
1047 
1048 /* assemblaggio jacobiano */
1049 void
1051  VariableSubMatrixHandler& WorkMatB,
1052  const VectorHandler& /* XCurr */ ,
1053  const VectorHandler& /* XPrimeCurr */ )
1054 {
1055  FullSubMatrixHandler& WMA = WorkMatA.SetFull();
1056  WorkMatB.SetNullMatrix();
1057 
1058  /* Dimensiona e resetta la matrice di lavoro */
1059  integer iNumRows = 0;
1060  integer iNumCols = 0;
1061  WorkSpaceDim(&iNumRows, &iNumCols);
1062  WMA.ResizeReset(iNumRows, iNumCols);
1063 
1064  /* Recupera gli indici */
1065  integer iNode1FirstPosIndex = pNode1->iGetFirstPositionIndex();
1066  integer iNode1FirstMomIndex = pNode1->iGetFirstMomentumIndex();
1067  integer iNode2FirstPosIndex = pNode2->iGetFirstPositionIndex();
1068  integer iNode2FirstMomIndex = pNode2->iGetFirstMomentumIndex();
1069 
1070  /* Setta gli indici della matrice */
1071  for (int iCnt = 1; iCnt <= 6; iCnt++) {
1072  WMA.PutRowIndex(iCnt, iNode1FirstMomIndex + iCnt);
1073  WMA.PutColIndex(iCnt, iNode1FirstPosIndex + iCnt);
1074  WMA.PutRowIndex(6 + iCnt, iNode2FirstMomIndex + iCnt);
1075  WMA.PutColIndex(6 + iCnt, iNode2FirstPosIndex + iCnt);
1076  }
1077 
1078  AssMats(WMA, WMA, 1.);
1079 }
1080 
1081 void
1083  FullSubMatrixHandler& /* WMB */ ,
1084  doublereal dCoef)
1085 {
1086  AssMatCommon(WMA, dCoef);
1087  AssMatElastic(WMA, dCoef, FDE);
1088 }
1089 
1090 void
1092 {
1093  R1h = pNode1->GetRRef()*tilde_R1h;
1094 
1095  Mat3x3 R2h(pNode2->GetRRef()*tilde_R2h);
1096  Vec3 ThetaCurr = RotManip::VecRot(R1h.Transpose()*R2h);
1097  Mat3x3 tilde_R = RotManip::Rot(ThetaCurr/2.);
1098  Mat3x3 hat_R(R1h*tilde_R);
1099 
1100  Mat3x3 hat_I = hat_R*((Eye3 + tilde_R).Inv()).MulMT(hat_R);
1101 
1102  d1 = pNode1->GetRCurr()*tilde_f1;
1103  d2 = pNode2->GetRCurr()*tilde_f2;
1104  Vec3 d(pNode2->GetXCurr() + d2 - pNode1->GetXCurr() - d1);
1105 
1106  if (bFirstRes) {
1107  bFirstRes = false;
1108 
1109  } else {
1110  tilde_k = Vec6(hat_R.MulTV(d), ThetaCurr);
1111 
1113  }
1114 
1115  F = MultRV(GetF(), R1h);
1116 
1117  Vec3 dCrossF(d.Cross(F.GetVec1()));
1118 
1119  WorkVec.Add(1, F.GetVec1());
1120  WorkVec.Add(4, d1.Cross(F.GetVec1()) + hat_I*dCrossF + F.GetVec2());
1121  WorkVec.Sub(6 + 1, F.GetVec1());
1122  WorkVec.Sub(6 + 4, d2.Cross(F.GetVec1()) - hat_I.MulTV(dCrossF) + F.GetVec2());
1123 }
1124 
1125 void
1127  VectorHandler& /* XP */ )
1128 {
1129  /* Calcola le deformazioni, aggiorna il legame costitutivo
1130  * e crea la FDE */
1131 
1132  /* Recupera i dati */
1133  R1h = pNode1->GetRRef()*tilde_R1h;
1134 
1135  /* Calcola la deformazione corrente nel sistema locale (nodo a) */
1137 
1138  /* Calcola l'inversa di Gamma di ThetaRef */
1139  Mat3x3 GammaRefm1 = RotManip::DRot_I(ThetaRef);
1140 
1141  Mat3x3 hat_R(R1h*RotManip::Rot(ThetaRef/2.));
1142 
1143  d1 = pNode1->GetRCurr()*tilde_f1;
1144  d2 = pNode2->GetRRef()*tilde_f2;
1145  Vec3 d = pNode2->GetXCurr() + d2 - pNode1->GetXCurr() - d1;
1146 
1147  tilde_k = Vec6(hat_R.MulTV(d), ThetaRef);
1148 
1149  /* Aggiorna il legame costitutivo */
1151 
1152  /* Chiede la matrice tangente di riferimento e la porta
1153  * nel sistema globale */
1155  MultRMRtGammam1(FDE, R1h, GammaRefm1);
1156 
1157  bFirstRes = true;
1158 }
1159 
1160 
1161 /* Contributo allo jacobiano durante l'assemblaggio iniziale */
1164  const VectorHandler& /* XCurr */ )
1165 {
1166  FullSubMatrixHandler& WM = WorkMat.SetFull();
1167 
1168  /* Dimensiona e resetta la matrice di lavoro */
1169  integer iNumRows = 0;
1170  integer iNumCols = 0;
1171  InitialWorkSpaceDim(&iNumRows, &iNumCols);
1172  WM.ResizeReset(iNumRows, iNumCols);
1173 
1174  /* Recupera gli indici */
1175  integer iNode1FirstPosIndex = pNode1->iGetFirstPositionIndex();
1176  integer iNode2FirstPosIndex = pNode2->iGetFirstPositionIndex();
1177 
1178  /* Setta gli indici della matrice */
1179  for (int iCnt = 1; iCnt <= 6; iCnt++) {
1180  WM.PutRowIndex(iCnt, iNode1FirstPosIndex + iCnt);
1181  WM.PutColIndex(iCnt, iNode1FirstPosIndex + iCnt);
1182  WM.PutRowIndex(6 + iCnt, iNode2FirstPosIndex + iCnt);
1183  WM.PutColIndex(6 + iCnt, iNode2FirstPosIndex + iCnt);
1184  }
1185 
1186  AssMats(WM, WM, 1.);
1187 
1188  return WorkMat;
1189 }
1190 
1191 /* ElasticJointInv - end */
1192 
1193 
1194 /* ViscousJoint - begin */
1195 
1197  const DofOwner* pDO,
1198  const ConstitutiveLaw6D* pCL,
1199  const StructNode* pN1,
1200  const StructNode* pN2,
1201  const Vec3& tilde_f1,
1202  const Vec3& tilde_f2,
1203  const Mat3x3& tilde_R1h,
1204  const Mat3x3& tilde_R2h,
1205  const OrientationDescription& od,
1206  flag fOut)
1207 : Elem(uL, fOut),
1208 DeformableJoint(uL, pDO, pCL, pN1, pN2, tilde_f1, tilde_f2, tilde_R1h, tilde_R2h, od, fOut)
1209 {
1210  /*
1211  * Chiede la matrice tangente di riferimento
1212  * e la porta nel sistema globale
1213  */
1215 }
1216 
1218 {
1219  NO_OP;
1220 }
1221 
1222 void
1224  const VectorHandler& XP)
1225 {
1227 }
1228 
1229 void
1231  FullSubMatrixHandler& WMB, doublereal dCoef)
1232 {
1233  AssMatCommon(WMA, dCoef);
1234  AssMatViscous(WMA, WMB, dCoef, FDEPrime);
1235 }
1236 
1237 void
1239 {
1240  if (bFirstRes) {
1241  bFirstRes = false;
1242 
1243  } else {
1245 
1246  d2 = pNode2->GetRCurr()*tilde_f2;
1247  d1 = pNode2->GetXCurr() + d2 - pNode1->GetXCurr();
1248 
1249  d2Prime = pNode2->GetWCurr().Cross(d2);
1251 
1253  R1h.MulTV(pNode2->GetWCurr() - pNode1->GetWCurr()));
1254 
1256  }
1257 
1259 
1260  WorkVec.Add(1, F.GetVec1());
1261  WorkVec.Add(4, d1.Cross(F.GetVec1()) + F.GetVec2());
1262  WorkVec.Sub(6 + 1, F.GetVec1());
1263  WorkVec.Sub(6 + 4, d2.Cross(F.GetVec1()) + F.GetVec2());
1264 }
1265 
1266 void
1268  VectorHandler& /* XP */ )
1269 {
1270  /* Calcola le deformazioni, aggiorna il legame costitutivo
1271  * e crea la FDE */
1272 
1273  /* Recupera i dati */
1274  R1h = pNode1->GetRRef()*tilde_R1h;
1275 
1276  d2 = pNode2->GetRCurr()*tilde_f2;
1277  d1 = pNode2->GetXCurr() + d2 - pNode1->GetXCurr();
1278 
1279  d2Prime = pNode2->GetWCurr().Cross(d2);
1281 
1283  R1h.MulTV(pNode2->GetWCurr() - pNode1->GetWCurr()));
1284 
1286 
1287  /* FIXME: we need to be able to regenerate FDE
1288  * if the constitutive law throws ChangedEquationStructure */
1290 
1291  bFirstRes = true;
1292 }
1293 
1294 
1295 /* Contributo allo jacobiano durante l'assemblaggio iniziale */
1298  const VectorHandler& /* XCurr */ )
1299 {
1300  FullSubMatrixHandler& WM = WorkMat.SetFull();
1301 
1302  /* Dimensiona e resetta la matrice di lavoro */
1303  integer iNumRows = 0;
1304  integer iNumCols = 0;
1305  InitialWorkSpaceDim(&iNumRows, &iNumCols);
1306  WM.ResizeReset(iNumRows, iNumCols);
1307 
1308  /* Recupera gli indici */
1309  integer iNode1FirstPosIndex = pNode1->iGetFirstPositionIndex();
1310  integer iNode2FirstPosIndex = pNode2->iGetFirstPositionIndex();
1311 
1312  /* Setta gli indici della matrice */
1313  for (int iCnt = 1; iCnt <= 6; iCnt++) {
1314  WM.PutRowIndex(iCnt, iNode1FirstPosIndex + iCnt);
1315  WM.PutColIndex(iCnt, iNode1FirstPosIndex + iCnt);
1316  WM.PutRowIndex(6 + iCnt, iNode2FirstPosIndex + iCnt);
1317  WM.PutColIndex(6 + iCnt, iNode2FirstPosIndex + iCnt);
1318  }
1319 
1320  AssMats(WM, WM, 1.);
1321 
1322  return WorkMat;
1323 }
1324 
1325 
1326 /* ViscousJoint - end */
1327 
1328 
1329 /* ViscoElasticJoint - begin */
1330 
1332  const DofOwner* pDO,
1333  const ConstitutiveLaw6D* pCL,
1334  const StructNode* pN1,
1335  const StructNode* pN2,
1336  const Vec3& tilde_f1,
1337  const Vec3& tilde_f2,
1338  const Mat3x3& tilde_R1h,
1339  const Mat3x3& tilde_R2h,
1340  const OrientationDescription& od,
1341  flag fOut)
1342 : Elem(uL, fOut),
1343 DeformableJoint(uL, pDO, pCL, pN1, pN2, tilde_f1, tilde_f2, tilde_R1h, tilde_R2h, od, fOut),
1344 ThetaRef(Zero3)
1345 {
1346  /*
1347  * Chiede la matrice tangente di riferimento
1348  * e la porta nel sistema globale
1349  */
1352 }
1353 
1355 {
1356  NO_OP;
1357 }
1358 
1359 void
1361  const VectorHandler& XP)
1362 {
1364 }
1365 
1366 void
1368  FullSubMatrixHandler& WMB, doublereal dCoef)
1369 {
1370  AssMatCommon(WMA, dCoef);
1371  AssMatElastic(WMA, dCoef, FDE);
1372  AssMatViscous(WMA, WMB, dCoef, FDEPrime);
1373 }
1374 
1375 void
1377 {
1378  if (bFirstRes) {
1379  bFirstRes = false;
1380 
1381  } else {
1383 
1384  d2 = pNode2->GetRCurr()*tilde_f2;
1385  d1 = pNode2->GetXCurr() + d2 - pNode1->GetXCurr();
1386 
1387  d2Prime = pNode2->GetWCurr().Cross(d2);
1389 
1390  Mat3x3 R2h(pNode2->GetRCurr()*tilde_R2h);
1391  Vec3 f1(pNode1->GetRCurr()*tilde_f1);
1392 
1393  tilde_k = Vec6(R1h.MulTV(d1 - f1), RotManip::VecRot(R1h.MulTM(R2h)));
1394 
1396  R1h.MulTV(pNode2->GetWCurr() - pNode1->GetWCurr()));
1397 
1399  }
1400 
1402 
1403  WorkVec.Add(1, F.GetVec1());
1404  WorkVec.Add(4, d1.Cross(F.GetVec1()) + F.GetVec2());
1405  WorkVec.Sub(6 + 1, F.GetVec1());
1406  WorkVec.Sub(6 + 4, d2.Cross(F.GetVec1()) + F.GetVec2());
1407 }
1408 
1409 void
1411  VectorHandler& /* XP */ )
1412 {
1413  /* Calcola le deformazioni, aggiorna il legame costitutivo
1414  * e crea la FDE */
1415 
1416  /* Recupera i dati */
1417  R1h = pNode1->GetRRef()*tilde_R1h;
1418 
1419  d2 = pNode2->GetRCurr()*tilde_f2;
1420  d1 = pNode2->GetXCurr() + d2 - pNode1->GetXCurr();
1421 
1422  d2Prime = pNode2->GetWCurr().Cross(d2);
1424 
1425  /* Calcola la deformazione corrente nel sistema locale (nodo a) */
1427 
1428  /* Calcola l'inversa di Gamma di ThetaRef */
1429  Mat3x3 GammaRefm1 = RotManip::DRot_I(ThetaRef);
1430 
1431  Vec3 f1(pNode1->GetRRef()*tilde_f1);
1432 
1433  tilde_k = Vec6(R1h.MulTV(d1 - f1), ThetaRef);
1434 
1436  R1h.MulTV(pNode2->GetWCurr() - pNode1->GetWCurr()));
1437 
1438  ConstitutiveLaw6DOwner::Update(tilde_k, tilde_kPrime);
1439 
1440  /* Chiede la matrice tangente di riferimento e la porta
1441  * nel sistema globale */
1443  MultRMRtGammam1(FDE, R1h, GammaRefm1);
1444 
1445  /* FIXME: we need to be able to regenerate FDE
1446  * if the constitutive law throws ChangedEquationStructure */
1448 
1449  bFirstRes = true;
1450 }
1451 
1452 
1453 /* Contributo allo jacobiano durante l'assemblaggio iniziale */
1456  const VectorHandler& /* XCurr */ )
1457 {
1458  FullSubMatrixHandler& WM = WorkMat.SetFull();
1459 
1460  /* Dimensiona e resetta la matrice di lavoro */
1461  integer iNumRows = 0;
1462  integer iNumCols = 0;
1463  InitialWorkSpaceDim(&iNumRows, &iNumCols);
1464  WM.ResizeReset(iNumRows, iNumCols);
1465 
1466  /* Recupera gli indici */
1467  integer iNode1FirstPosIndex = pNode1->iGetFirstPositionIndex();
1468  integer iNode2FirstPosIndex = pNode2->iGetFirstPositionIndex();
1469 
1470  /* Setta gli indici della matrice */
1471  for (int iCnt = 1; iCnt <= 6; iCnt++) {
1472  WM.PutRowIndex(iCnt, iNode1FirstPosIndex + iCnt);
1473  WM.PutColIndex(iCnt, iNode1FirstPosIndex + iCnt);
1474  WM.PutRowIndex(6 + iCnt, iNode2FirstPosIndex + iCnt);
1475  WM.PutColIndex(6 + iCnt, iNode2FirstPosIndex + iCnt);
1476  }
1477 
1478  AssMats(WM, WM, 1.);
1479 
1480  return WorkMat;
1481 }
1482 
1483 
1484 /* ViscoElasticJoint - end */
1485 
Definition: hint.h:38
void OutputPrepare(OutputHandler &OH)
Definition: vehj3.cc:119
void AssMatElastic(FullSubMatrixHandler &WM, doublereal dCoef, const Mat6x6 &FDE)
Definition: vehj3.cc:490
VariableSubMatrixHandler & AssJac(VariableSubMatrixHandler &WorkMat, const VectorHandler &XCurr)
Definition: vehj3.cc:890
virtual bool bInverseDynamics(void) const
Definition: vehj3.cc:278
Mat3x3 MultRMRt(const Mat3x3 &m, const Mat3x3 &R)
Definition: matvec3.cc:1162
void PutColIndex(integer iSubCol, integer iCol)
Definition: submat.h:325
Vec3 d2Prime
Definition: vehj3.h:73
const Vec3 Zero3(0., 0., 0.)
Vec3 Cross(const Vec3 &v) const
Definition: matvec3.h:218
virtual std::ostream & Restart(std::ostream &out) const
Definition: vehj3.cc:104
std::ostream & Write(std::ostream &out, const char *sFill=" ") const
Definition: matvec3.cc:738
Vec3 MultRV(const Vec3 &v, const Mat3x3 &R)
Definition: matvec3.cc:1144
virtual void AssVec(SubVectorHandler &WorkVec)=0
long int flag
Definition: mbdyn.h:43
virtual const Mat3x3 & GetRRef(void) const
Definition: strnode.h:1006
virtual bool bToBeOutput(void) const
Definition: output.cc:890
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
Mat3x3 GetMat12(void)
Definition: matvec6.h:328
void AssMatViscous(FullSubMatrixHandler &WMA, FullSubMatrixHandler &WMB, doublereal dCoef, const Mat6x6 &FDEPrime)
Definition: vehj3.cc:543
ConstitutiveLaw< T, Tder > * pGetConstLaw(void) const
Definition: constltp.h:278
Definition: matvec3.h:98
OrientationDescription od
Definition: vehj3.h:53
virtual void ResizeReset(integer)
Definition: vh.cc:55
Vec6 tilde_k
Definition: vehj3.h:56
virtual SubVectorHandler & InitialAssRes(SubVectorHandler &WorkVec, const VectorHandler &XCurr)
Definition: vehj3.cc:783
const Vec3 & GetVec2(void) const
Definition: matvec6.h:76
const MatCross_Manip MatCross
Definition: matvec3.cc:639
bool UseNetCDF(int out) const
Definition: output.cc:491
void AssMats(FullSubMatrixHandler &WMA, FullSubMatrixHandler &WMB, doublereal dCoef)
Definition: vehj3.cc:1230
FullSubMatrixHandler & SetFull(void)
Definition: submat.h:1168
virtual const Mat3x3 & GetRCurr(void) const
Definition: strnode.h:1012
virtual Node::Type GetNodeType(void) const
Definition: strnode.cc:145
Mat3x3 tilde_R2h
Definition: vehj3.h:51
bool bIsErgonomy(void) const
Definition: elem.cc:83
void Add(integer iRow, integer iCol, const Vec3 &v)
Definition: submat.cc:209
void AssVec(SubVectorHandler &WorkVec)
Definition: vehj3.cc:1091
virtual unsigned int iGetPrivDataIdx(const char *s) const
Definition: constltp.h:361
virtual void InitialWorkSpaceDim(integer *piNumRows, integer *piNumCols) const
Definition: vehj3.h:444
virtual Hint * ParseHint(DataManager *pDM, const char *s) const
Definition: simentity.cc:76
std::ostream & Write(std::ostream &out, const FullMatrixHandler &m, const char *s, const char *s2)
Definition: fullmh.cc:376
Mat6x6 FDE
Definition: vehj3.h:321
Mat3x3 GetMat21(void)
Definition: matvec6.h:324
virtual void AfterConvergence(const VectorHandler &X, const VectorHandler &XP)
Definition: vehj3.cc:840
virtual void Sub(integer iRow, const Vec3 &v)
Definition: vh.cc:78
~ViscoElasticJoint(void)
Definition: vehj3.cc:1354
const Tder & GetFDE(void) const
Definition: constltp.h:298
const Mat3x3 Eye3(1., 0., 0., 0., 1., 0., 0., 0., 1.)
OrientationDescription
Definition: matvec3.h:1597
Vec3 tilde_f2
Definition: vehj3.h:49
virtual void WorkSpaceDim(integer *piNumRows, integer *piNumCols) const
Definition: vehj3.h:151
virtual unsigned int iGetNumPrivData(void) const
Definition: constltp.h:352
#define NO_OP
Definition: myassert.h:74
Mat3x3 GetMat22(void)
Definition: matvec6.h:332
virtual SubVectorHandler & AssRes(SubVectorHandler &WorkVec, doublereal dCoef, const VectorHandler &XCurr, const VectorHandler &XPrimeCurr)
Definition: vehj3.cc:721
virtual void InitialWorkSpaceDim(integer *piNumRows, integer *piNumCols) const =0
std::vector< Hint * > Hints
Definition: simentity.h:89
ElasticJointInv(unsigned int uL, const DofOwner *pDO, const ConstitutiveLaw6D *pCL, const StructNode *pN1, const StructNode *pN2, const Vec3 &tilde_f1, const Vec3 &tilde_f2, const Mat3x3 &tilde_R1h, const Mat3x3 &tilde_R2h, const OrientationDescription &od, flag fOut)
Definition: vehj3.cc:1014
Vec3 GetVec(unsigned short int i) const
Definition: matvec3.h:893
Mat6x6 FDE
Definition: vehj3.h:219
Mat3x3 R1h
Definition: vehj3.h:74
void PutMat22(const Mat3x3 &x)
Definition: matvec6.h:376
Vec3 VecRot(const Mat3x3 &Phi)
Definition: Rot.cc:136
virtual VariableSubMatrixHandler & InitialAssJac(VariableSubMatrixHandler &WorkMat, const VectorHandler &XCurr)
Definition: vehj3.cc:1163
Vec3 MulTV(const Vec3 &v) const
Definition: matvec3.cc:482
Mat3x3 GetMat11(void)
Definition: matvec6.h:320
virtual void AfterPredict(VectorHandler &X, VectorHandler &XP)
Definition: vehj3.cc:946
virtual void PutRowIndex(integer iSubRow, integer iRow)=0
~ElasticJointInv(void)
Definition: vehj3.cc:1036
Mat3x3 tilde_R1h
Definition: vehj3.h:50
Vec3 d1Prime
Definition: vehj3.h:73
virtual doublereal dGetPrivData(unsigned int i) const
Definition: vehj3.cc:393
Definition: matvec6.h:37
void AssVec(SubVectorHandler &WorkVec)
Definition: vehj3.cc:918
virtual unsigned int iGetNumPrivData(void) const
Definition: vehj3.cc:322
void AssMats(FullSubMatrixHandler &WMA, FullSubMatrixHandler &WMB, doublereal dCoef)
Definition: vehj3.cc:1082
const Vec3 & GetVec1(void) const
Definition: matvec6.h:72
void PutMat11(const Mat3x3 &x)
Definition: matvec6.h:364
Vec3 MatR2EulerAngles313(const Mat3x3 &R)
Definition: matvec3.cc:927
virtual void AfterConvergence(const VectorHandler &X, const VectorHandler &XP)
Definition: vehj3.cc:1042
Matrix< T, 2, 2 > Inv(const Matrix< T, 2, 2 > &A)
Definition: matvec.h:3282
void SetNullMatrix(void)
Definition: submat.h:1159
ViscoElasticJoint(unsigned int uL, const DofOwner *pDO, const ConstitutiveLaw6D *pCL, const StructNode *pN1, const StructNode *pN2, const Vec3 &tilde_f1, const Vec3 &tilde_f2, const Mat3x3 &tilde_R1, const Mat3x3 &tilde_R2, const OrientationDescription &od, flag fOut)
Definition: vehj3.cc:1331
Definition: mbdyn.h:76
const Vec6 Zero6(0., 0., 0., 0., 0., 0.)
virtual VariableSubMatrixHandler & InitialAssJac(VariableSubMatrixHandler &WorkMat, const VectorHandler &XCurr)
Definition: vehj3.cc:1297
long GetCurrentStep(void) const
Definition: output.h:116
void Update(const VectorHandler &XCurr, InverseDynamics::Order iOrder=InverseDynamics::INVERSE_DYNAMICS)
Definition: vehj3.cc:904
ViscousJoint(unsigned int uL, const DofOwner *pDO, const ConstitutiveLaw6D *pCL, const StructNode *pN1, const StructNode *pN2, const Vec3 &tilde_f1, const Vec3 &tilde_f2, const Mat3x3 &tilde_R1, const Mat3x3 &tilde_R2, const OrientationDescription &od, flag fOut)
Definition: vehj3.cc:1196
~ViscousJoint(void)
Definition: vehj3.cc:1217
Vec3 MatR2EulerAngles123(const Mat3x3 &R)
Definition: matvec3.cc:893
virtual ConstLawType::Type GetConstLawType(void) const =0
void AssVec(SubVectorHandler &WorkVec)
Definition: vehj3.cc:1376
Mat3x3 Rot(const Vec3 &phi)
Definition: Rot.cc:62
virtual integer iGetFirstMomentumIndex(void) const =0
virtual integer iGetFirstPositionIndex(void) const
Definition: strnode.h:452
virtual const Vec3 & GetWCurr(void) const
Definition: strnode.h:1030
virtual void AfterPredict(VectorHandler &X, VectorHandler &XP)
Definition: vehj3.cc:1126
const doublereal dRaDegr
Definition: matvec3.cc:884
virtual std::ostream & Restart(std::ostream &out) const
Definition: joint.h:195
std::ostream & Joints(void) const
Definition: output.h:443
Vec3 ThetaRef
Definition: vehj3.h:217
void AfterConvergence(const T &Eps, const T &EpsPrime=mb_zero< T >())
Definition: constltp.h:288
void PutMat12(const Mat3x3 &x)
Definition: matvec6.h:372
#define ASSERT(expression)
Definition: colamd.c:977
Mat3x3 MulTM(const Mat3x3 &m) const
Definition: matvec3.cc:500
VectorExpression< VectorCrossExpr< VectorLhsExpr, VectorRhsExpr >, 3 > Cross(const VectorExpression< VectorLhsExpr, 3 > &u, const VectorExpression< VectorRhsExpr, 3 > &v)
Definition: matvec.h:3248
Vec3 tilde_f1
Definition: vehj3.h:48
virtual void OutputPrepare_int(const std::string &type, OutputHandler &OH, std::string &name)
Definition: joint.cc:107
virtual const Vec3 & GetXCurr(void) const
Definition: strnode.h:310
virtual void Add(integer iRow, const Vec3 &v)
Definition: vh.cc:63
virtual void ResizeReset(integer, integer)
Definition: submat.cc:182
virtual VariableSubMatrixHandler & AssJac(VariableSubMatrixHandler &WorkMat, doublereal dCoef, const VectorHandler &XCurr, const VectorHandler &XPrimeCurr)
Definition: vehj3.cc:648
virtual Hint * ParseHint(DataManager *pDM, const char *s) const
Definition: vehj3.cc:284
static void MultRMRtGammam1(Mat6x6 &M, const Mat3x3 &R, const Mat3x3 &Gammam1)
Definition: vehj3.cc:44
Vec3 ThetaRef
Definition: vehj3.h:319
virtual doublereal dGetPrivData(unsigned int i) const
Definition: constltp.h:369
virtual void InitialWorkSpaceDim(integer *piNumRows, integer *piNumCols) const
Definition: vehj3.h:272
Mat3x3 Transpose(void) const
Definition: matvec3.h:816
const MatCrossCross_Manip MatCrossCross
Definition: matvec3.cc:640
virtual void InitialWorkSpaceDim(integer *piNumRows, integer *piNumCols) const
Definition: vehj3.h:362
const doublereal * pGetMat(void) const
Definition: matvec3.h:743
#define STRLENOF(s)
Definition: mbdyn.h:166
void SetValue(DataManager *pDM, VectorHandler &X, VectorHandler &XP, SimulationEntity::Hints *ph=0)
Definition: vehj3.cc:236
virtual VariableSubMatrixHandler & InitialAssJac(VariableSubMatrixHandler &WorkMat, const VectorHandler &XCurr)
Definition: vehj3.cc:981
Definition: elem.h:75
virtual ~DeformableJoint(void)
Definition: vehj3.cc:96
virtual unsigned int iGetPrivDataIdx(const char *s) const
Definition: vehj3.cc:328
void PutRowIndex(integer iSubRow, integer iRow)
Definition: submat.h:311
const T & GetF(void) const
Definition: constltp.h:293
const doublereal * pGetVec(void) const
Definition: matvec3.h:192
Vec3 MatR2EulerAngles321(const Mat3x3 &R)
Definition: matvec3.cc:941
virtual void WorkSpaceDim(integer *piNumRows, integer *piNumCols) const =0
virtual void AfterConvergence(const VectorHandler &X, const VectorHandler &XP)
Definition: vehj3.cc:1223
void AssMats(FullSubMatrixHandler &WorkMatA, FullSubMatrixHandler &WorkMatB, doublereal dCoef)
Definition: vehj3.cc:1367
Mat6x6 FDEPrime
Definition: vehj3.h:409
virtual void AfterConvergence(const VectorHandler &X, const VectorHandler &XP)
Definition: vehj3.cc:1360
virtual const Vec3 & GetVCurr(void) const
Definition: strnode.h:322
void Sub(integer iRow, integer iCol, const Vec3 &v)
Definition: submat.cc:215
virtual void InitialWorkSpaceDim(integer *piNumRows, integer *piNumCols) const
Definition: vehj3.h:531
Definition: joint.h:50
Mat3x3 MulMT(const Mat3x3 &m) const
Definition: matvec3.cc:444
virtual void SetValue(DataManager *pDM, VectorHandler &X, VectorHandler &XP, SimulationEntity::Hints *h=0)
Definition: simentity.cc:63
virtual void Output(OutputHandler &OH) const
Definition: vehj3.cc:143
Mat6x6 FDEPrime
Definition: vehj3.h:496
double doublereal
Definition: colamd.c:52
Mat6x6 FDE
Definition: vehj3.h:495
const Tder & GetFDEPrime(void) const
Definition: constltp.h:303
const StructNode * pNode2
Definition: vehj3.h:47
std::ostream & Output(std::ostream &out, const char *sJointName, unsigned int uLabel, const Vec3 &FLocal, const Vec3 &MLocal, const Vec3 &FGlobal, const Vec3 &MGlobal) const
Definition: joint.cc:138
long int integer
Definition: colamd.c:51
void Update(const T &Eps, const T &EpsPrime=mb_zero< T >())
Definition: constltp.h:283
void AssMatCommon(FullSubMatrixHandler &WM, doublereal dCoef)
Definition: vehj3.cc:461
virtual void AfterPredict(VectorHandler &X, VectorHandler &XP)
Definition: vehj3.cc:1267
void AssVec(SubVectorHandler &WorkVec)
Definition: vehj3.cc:1238
unsigned int GetLabel(void) const
Definition: withlab.cc:62
const StructNode * pNode1
Definition: vehj3.h:46
void AssMats(FullSubMatrixHandler &WMA, FullSubMatrixHandler &WMB, doublereal dCoef)
Definition: vehj3.cc:880
DeformableJoint(unsigned int uL, const DofOwner *pDO, const ConstitutiveLaw6D *pCL, const StructNode *pN1, const StructNode *pN2, const Vec3 &tilde_f1, const Vec3 &tilde_f2, const Mat3x3 &tilde_R1h, const Mat3x3 &tilde_R2h, const OrientationDescription &od, flag fOut)
Definition: vehj3.cc:57
virtual VariableSubMatrixHandler & InitialAssJac(VariableSubMatrixHandler &WorkMat, const VectorHandler &XCurr)
Definition: vehj3.cc:1455
~ElasticJoint(void)
Definition: vehj3.cc:834
ElasticJoint(unsigned int uL, const DofOwner *pDO, const ConstitutiveLaw6D *pCL, const StructNode *pN1, const StructNode *pN2, const Vec3 &tilde_f1, const Vec3 &tilde_f2, const Mat3x3 &tilde_R1h, const Mat3x3 &tilde_R2h, const OrientationDescription &od, flag fOut)
Definition: vehj3.cc:812
bool bFirstRes
Definition: vehj3.h:70
Mat3x3 DRot_I(const Vec3 &phi)
Definition: Rot.cc:111
Mat3x3 R
bool UseText(int out) const
Definition: output.cc:446
virtual void AfterPredict(VectorHandler &X, VectorHandler &XP)
Definition: vehj3.cc:1410
void PutMat21(const Mat3x3 &x)
Definition: matvec6.h:368
virtual void AssMats(FullSubMatrixHandler &WMA, FullSubMatrixHandler &WMB, doublereal dCoef)=0
Vec6 tilde_kPrime
Definition: vehj3.h:59