MBDyn-1.7.3
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Groups
planej.cc
Go to the documentation of this file.
1 /* $Header: /var/cvs/mbdyn/mbdyn/mbdyn-1.0/mbdyn/struct/planej.cc,v 1.105 2017/01/12 14:46:43 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 /* Continuano i vincoli di rotazione piani */
33 
34 #include "mbconfig.h" /* This goes first in every *.c,*.cc file */
35 
36 #include <iostream>
37 #include <limits>
38 
39 #include "planej.h"
40 #include "Rot.hh"
41 #include "hint_impl.h"
42 
43 /* PlaneHingeJoint - begin */
44 
45 const unsigned int PlaneHingeJoint::NumSelfDof(5);
46 const unsigned int PlaneHingeJoint::NumDof(17);
47 
48 /* Costruttore non banale */
49 PlaneHingeJoint::PlaneHingeJoint(unsigned int uL, const DofOwner* pDO,
50  const StructNode* pN1, const StructNode* pN2,
51  const Vec3& dTmp1, const Vec3& dTmp2,
52  const Mat3x3& R1hTmp, const Mat3x3& R2hTmp,
53  const OrientationDescription& od,
54  flag fOut,
55  const bool _calcInitdTheta,
56  const doublereal initDTheta,
57  const doublereal rr,
58  const doublereal pref,
59  BasicShapeCoefficient *const sh,
60  BasicFriction *const f)
61 : Elem(uL, fOut),
62 Joint(uL, pDO, fOut),
63 pNode1(pN1), pNode2(pN2),
64 d1(dTmp1), R1h(R1hTmp), d2(dTmp2), R2h(R2hTmp), F(Zero3), M(Zero3),
65 #ifdef USE_NETCDF
66 Var_Phi(0),
67 Var_Omega(0),
68 //Var_MFR(0),
69 //Var_MU(0),
70 #endif // USE_NETCDF
71 calcInitdTheta(_calcInitdTheta), NTheta(0), dTheta(initDTheta), dThetaWrapped(initDTheta),
72 Sh_c(sh), fc(f), preF(pref), r(rr),
73 od(od)
74 {
75  NO_OP;
76  char * fname = NULL;
77  int n = (uL > 0 ? 1 + (int)log10(uL) : 1);
78  int len = STRLENOF("hinge") + n + STRLENOF(".out") + 1;
79  SAFENEWARR(fname, char, len);
80  snprintf(fname, len, "hinge%.*d.out", n, uL);
81  SAFEDELETEARR(fname);
82 }
83 
84 
85 /* Distruttore banale */
86 PlaneHingeJoint::~PlaneHingeJoint(void)
87 {
88  if (Sh_c) {
89  delete Sh_c;
90  }
91 
92  if (fc) {
93  delete fc;
94  }
95 }
96 
97 std::ostream&
98 PlaneHingeJoint::DescribeDof(std::ostream& out, const char *prefix, bool bInitial) const
99 {
100  integer iIndex = iGetFirstIndex();
101 
102  out
103  << prefix << iIndex + 1 << "->" << iIndex + 3 << ": "
104  "reaction forces [Fx,Fy,Fz]" << std::endl
105  << prefix << iIndex + 4 << "->" << iIndex + 5 << ": "
106  "reaction couples [mx,my]" << std::endl;
107 
108  if (bInitial) {
109  iIndex += NumSelfDof;
110  out
111  << prefix << iIndex + 1 << "->" << iIndex + 3 << ": "
112  "reaction force derivatives [FPx,FPy,FPz]" << std::endl
113  << prefix << iIndex + 4 << "->" << iIndex + 5 << ": "
114  "reaction couple derivatives [mPx,mPy]" << std::endl;
115  }
116 
117  iIndex += NumSelfDof;
118  if (fc) {
119  integer iFCDofs = fc->iGetNumDof();
120  if (iFCDofs > 0) {
121  out << prefix << iIndex + 1;
122  if (iFCDofs > 1) {
123  out << "->" << iIndex + iFCDofs;
124  }
125  out << ": friction dof(s)" << std::endl
126  << " ", fc->DescribeDof(out, prefix, bInitial);
127  }
128  }
129 
130  return out;
131 }
132 
133 static const char xyz[] = "xyz";
134 
135 void
136 PlaneHingeJoint::DescribeDof(std::vector<std::string>& desc, bool bInitial, int i) const
137 {
138  std::ostringstream os;
139  os << "PlaneHingeJoint(" << GetLabel() << ")";
140 
141  unsigned short nself = NumSelfDof;
142  if (bInitial) {
143  nself *= 2;
144  }
145  if (fc && (i == -1 || i >= nself)) {
146  fc->DescribeDof(desc, bInitial, i - nself);
147  if (i != -1) {
148  desc[0] = os.str() + ": " + desc[0];
149  return;
150  }
151  }
152 
153  if (i == -1) {
154  // move fc desc to the end
155  unsigned short nfc = 0;
156  if (fc) {
157  nfc = desc.size();
158  }
159  desc.resize(nfc + nself);
160  for (unsigned i = nfc; i-- > 0; ) {
161  desc[nself + i] = os.str() + ": " + desc[nfc];
162  }
163 
164  std::string name = os.str();
165 
166  for (unsigned i = 0; i < 3; i++) {
167  os.str(name);
168  os.seekp(0, std::ios_base::end);
169  os << ": reaction force f" << xyz[i];
170  desc[i] = os.str();
171  }
172 
173  for (unsigned i = 0; i < 2; i++) {
174  os.str(name);
175  os.seekp(0, std::ios_base::end);
176  os << ": reaction couple m" << xyz[i];
177  desc[3 + i] = os.str();
178  }
179 
180  if (bInitial) {
181  for (unsigned i = 0; i < 3; i++) {
182  os.str(name);
183  os.seekp(0, std::ios_base::end);
184  os << ": reaction force derivative fP" << xyz[i];
185  desc[3 + 2 + i] = os.str();
186  }
187 
188  for (unsigned i = 0; i < 2; i++) {
189  os.str(name);
190  os.seekp(0, std::ios_base::end);
191  os << ": reaction couple derivative mP" << xyz[i];
192  desc[3 + 2 + 3 + i] = os.str();
193  }
194  }
195 
196  } else {
197  if (i < -1) {
198  // error
199  throw ErrGeneric(MBDYN_EXCEPT_ARGS);
200  }
201 
202  if (i >= nself) {
203  // error
204  throw ErrGeneric(MBDYN_EXCEPT_ARGS);
205  }
206 
207  desc.resize(1);
208 
209  switch (i) {
210  case 0:
211  case 1:
212  case 2:
213  os << ": reaction force f" << xyz[i];
214  break;
215 
216  case 3:
217  case 4:
218  os << ": reaction couple m" << xyz[i - 3];
219  break;
220 
221  case 5:
222  case 6:
223  case 7:
224  os << ": reaction force derivative fP" << xyz[i - 3 - 2];
225  break;
226 
227  case 8:
228  case 9:
229  os << ": reaction couple derivative mP" << xyz[i - 3 - 2 - 3];
230  break;
231  }
232  desc[0] = os.str();
233  }
234 }
235 
236 std::ostream&
237 PlaneHingeJoint::DescribeEq(std::ostream& out, const char *prefix, bool bInitial) const
238 {
239  integer iIndex = iGetFirstIndex();
240 
241  out
242  << prefix << iIndex + 1 << "->" << iIndex + 3 << ": "
243  "position constraints [Px1=Px2,Py1=Py2,Pz1=Pz2]" << std::endl
244  << prefix << iIndex + 4 << "->" << iIndex + 5 << ": "
245  "orientation constraints [gx1=gx2,gy1=gy2]" << std::endl;
246 
247  if (bInitial) {
248  iIndex += NumSelfDof;
249  out
250  << prefix << iIndex + 1 << "->" << iIndex + 3 << ": "
251  "velocity constraints [vx1=vx2,vy1=vy2,vz1=vz2]" << std::endl
252  << prefix << iIndex + 4 << "->" << iIndex + 5 << ": "
253  "angular velocity constraints [wx1=wx2,wy1=wy2]" << std::endl;
254  }
255 
256  iIndex += NumSelfDof;
257  if (fc) {
258  integer iFCDofs = fc->iGetNumDof();
259  if (iFCDofs > 0) {
260  out << prefix << iIndex + 1;
261  if (iFCDofs > 1) {
262  out << "->" << iIndex + iFCDofs;
263  }
264  out << ": friction equation(s)" << std::endl
265  << " ", fc->DescribeEq(out, prefix, bInitial);
266  }
267  }
268 
269  return out;
270 }
271 
272 void
273 PlaneHingeJoint::DescribeEq(std::vector<std::string>& desc, bool bInitial, int i) const
274 {
275  std::ostringstream os;
276  os << "PlaneHingeJoint(" << GetLabel() << ")";
277 
278  unsigned short nself = NumSelfDof;
279  if (bInitial) {
280  nself *= 2;
281  }
282  if (fc && (i == -1 || i >= nself)) {
283  fc->DescribeEq(desc, bInitial, i - nself);
284  if (i != -1) {
285  desc[0] = os.str() + ": " + desc[0];
286  return;
287  }
288  }
289 
290  if (i == -1) {
291  // move fc desc to the end
292  unsigned short nfc = 0;
293  if (fc) {
294  nfc = desc.size();
295  }
296  desc.resize(nfc + nself);
297  for (unsigned i = nfc; i-- > 0; ) {
298  desc[nself + i] = os.str() + ": " + desc[nfc];
299  }
300 
301  std::string name = os.str();
302 
303  for (unsigned i = 0; i < 3; i++) {
304  os.str(name);
305  os.seekp(0, std::ios_base::end);
306  os << ": position constraint P" << xyz[i];
307  desc[i] = os.str();
308  }
309 
310  for (unsigned i = 0; i < 2; i++) {
311  os.str(name);
312  os.seekp(0, std::ios_base::end);
313  os << ": orientation constraint g" << xyz[i];
314  desc[3 + i] = os.str();
315  }
316 
317  if (bInitial) {
318  for (unsigned i = 0; i < 3; i++) {
319  os.str(name);
320  os.seekp(0, std::ios_base::end);
321  os << ": position constraint derivative v" << xyz[i];
322  desc[3 + 2 + i] = os.str();
323  }
324 
325  for (unsigned i = 0; i < 2; i++) {
326  os.str(name);
327  os.seekp(0, std::ios_base::end);
328  os << ": orientation constraint derivative w" << xyz[i];
329  desc[3 + 2 + 3 + i] = os.str();
330  }
331  }
332 
333  } else {
334  if (i < -1) {
335  // error
336  throw ErrGeneric(MBDYN_EXCEPT_ARGS);
337  }
338 
339  if (i >= nself) {
340  // error
341  throw ErrGeneric(MBDYN_EXCEPT_ARGS);
342  }
343 
344  desc.resize(1);
345 
346  switch (i) {
347  case 0:
348  case 1:
349  case 2:
350  os << ": position constraint P" << xyz[i];
351  break;
352 
353  case 3:
354  case 4:
355  os << ": orientation constraint g" << xyz[i - 3];
356  break;
357 
358  case 5:
359  case 6:
360  case 7:
361  os << ": position constraint derivative v" << xyz[i - 3 - 2];
362  break;
363 
364  case 8:
365  case 9:
366  os << ": orientation constraint derivative w" << xyz[i - 3 - 2 - 3];
367  break;
368  }
369  desc[0] = os.str();
370  }
371 }
372 
373 void
374 PlaneHingeJoint::SetValue(DataManager *pDM,
375  VectorHandler& X, VectorHandler& XP,
376  SimulationEntity::Hints *ph)
377 {
378  if (ph) {
379  for (unsigned i = 0; i < ph->size(); i++) {
380  Joint::JointHint *pjh = dynamic_cast<Joint::JointHint *>((*ph)[i]);
381 
382  if (pjh == 0) {
383  continue;
384  }
385 
386  if (dynamic_cast<Joint::OffsetHint<1> *>(pjh)) {
387  const Mat3x3& R1(pNode1->GetRCurr());
388  Vec3 dTmp2(pNode2->GetRCurr()*d2);
389 
390  d1 = R1.MulTV(pNode2->GetXCurr() + dTmp2 - pNode1->GetXCurr());
391 
392  } else if (dynamic_cast<Joint::OffsetHint<2> *>(pjh)) {
393  const Mat3x3& R2(pNode2->GetRCurr());
394  Vec3 dTmp1(pNode1->GetRCurr()*d1);
395 
396  d2 = R2.MulTV(pNode1->GetXCurr() + dTmp1 - pNode2->GetXCurr());
397 
398  } else if (dynamic_cast<Joint::HingeHint<1> *>(pjh)) {
399  R1h = pNode1->GetRCurr().MulTM(pNode2->GetRCurr()*R2h);
400 
401  } else if (dynamic_cast<Joint::HingeHint<2> *>(pjh)) {
402  R2h = pNode2->GetRCurr().MulTM(pNode1->GetRCurr()*R1h);
403 
404  } else if (dynamic_cast<Joint::ReactionsHint *>(pjh)) {
405  /* TODO */
406  }
407  }
408  }
409 
410  if (calcInitdTheta) {
411  Vec3 v(RotManip::VecRot((pNode1->GetRCurr()*R1h).MulTM(pNode2->GetRCurr()*R2h)));
412 
413  dThetaWrapped = dTheta = v.dGet(3);
414  }
415 
416 #if 0
417  std::cerr << "F: " << F << std::endl;
418  std::cerr << "M: " << M << std::endl;
419 #endif
420 
421  integer iFirstReactionIndex = iGetFirstIndex();
422  X.Put(iFirstReactionIndex + 1, F);
423  X.PutCoef(iFirstReactionIndex + 4, M.dGet(1));
424  X.PutCoef(iFirstReactionIndex + 5, M.dGet(2));
425 
426  if (fc) {
427  fc->SetValue(pDM, X, XP, ph, iGetFirstIndex() + NumSelfDof);
428  }
429 }
430 
431 Hint *
432 PlaneHingeJoint::ParseHint(DataManager *pDM, const char *s) const
433 {
434  if (strncasecmp(s, "offset{" /*}*/ , STRLENOF("offset{" /*}*/ )) == 0)
435  {
436  s += STRLENOF("offset{" /*}*/ );
437 
438  if (strcmp(&s[1], /*{*/ "}") != 0) {
439  return 0;
440  }
441 
442  switch (s[0]) {
443  case '1':
444  return new Joint::OffsetHint<1>;
445 
446  case '2':
447  return new Joint::OffsetHint<2>;
448  }
449 
450  } else if (strncasecmp(s, "hinge{" /*}*/, STRLENOF("hinge{" /*}*/)) == 0) {
451  s += STRLENOF("hinge{" /*}*/);
452 
453  if (strcmp(&s[1], /* { */ "}") != 0) {
454  return 0;
455  }
456 
457  switch (s[0]) {
458  case '1':
459  return new Joint::HingeHint<1>;
460 
461  case '2':
462  return new Joint::HingeHint<2>;
463  }
464 
465  } else if (fc) {
466  return fc->ParseHint(pDM, s);
467  }
468 
469  return 0;
470 }
471 
472 void
473 PlaneHingeJoint::AfterConvergence(const VectorHandler& X,
474  const VectorHandler& XP)
475 {
476  Vec3 v(RotManip::VecRot((pNode1->GetRCurr()*R1h).MulTM(pNode2->GetRCurr()*R2h)));
477  doublereal dThetaTmp(v(3));
478 
479  // unwrap
480  if (dThetaTmp - dThetaWrapped < -M_PI) {
481  NTheta++;
482  }
483 
484  if (dThetaTmp - dThetaWrapped > M_PI) {
485  NTheta--;
486  }
487 
488  // save new wrapped angle
489  dThetaWrapped = dThetaTmp;
490 
491  // compute new unwrapped angle
492  dTheta = 2*M_PI*NTheta + dThetaWrapped;
493 
494  if (fc) {
495  Mat3x3 R1(pNode1->GetRCurr());
496  Mat3x3 R1hTmp(R1*R1h);
497  Vec3 e3a(R1hTmp.GetVec(3));
498  Vec3 Omega1(pNode1->GetWCurr());
499  Vec3 Omega2(pNode2->GetWCurr());
500  //relative velocity
501  doublereal v = (Omega1-Omega2).Dot(e3a)*r;
502  //reaction norm
503  doublereal modF = std::max(F.Norm(), preF);;
504  fc->AfterConvergence(modF,v,X,XP,iGetFirstIndex()+NumSelfDof);
505  }
506 }
507 
508 /* Funzione che legge lo stato iniziale dal file di input */
509 void
510 PlaneHingeJoint::ReadInitialState(MBDynParser& HP)
511 {
512  F = HP.GetVec3();
513  M = Vec3(HP.GetReal(), HP.GetReal(), 0.);
514 }
515 
516 
517 /* Contributo al file di restart */
518 std::ostream&
519 PlaneHingeJoint::Restart(std::ostream& out) const
520 {
521  Joint::Restart(out) << ", revolute hinge, "
522  << pNode1->GetLabel() << ", reference, node, ",
523  d1.Write(out, ", ")
524  << ", hinge, reference, node, 1, ", (R1h.GetVec(1)).Write(out, ", ")
525  << ", 2, ", (R1h.GetVec(2)).Write(out, ", ") << ", "
526  << pNode2->GetLabel() << ", reference, node, ",
527  d2.Write(out, ", ")
528  << ", hinge, reference, node, 1, ", (R2h.GetVec(1)).Write(out, ", ")
529  << ", 2, ", (R2h.GetVec(2)).Write(out, ", ") << ", "
530  << "initial theta, " << dTheta << ", "
531  << "initial state, ", F.Write(out, ", ")
532  << ", " << M.dGet(1) << ", " << M.dGet(2) << ';' << std::endl;
533 
534  return out;
535 }
536 
537 
538 /* Assemblaggio jacobiano */
539 VariableSubMatrixHandler&
540 PlaneHingeJoint::AssJac(VariableSubMatrixHandler& WorkMat,
541  doublereal dCoef,
542  const VectorHandler& XCurr,
543  const VectorHandler& XPrimeCurr)
544 {
545  DEBUGCOUT("Entering PlaneHingeJoint::AssJac()" << std::endl);
546 
547  /* Setta la sottomatrice come piena (e' un po' dispersivo, ma lo jacobiano
548  * e' complicato */
549  FullSubMatrixHandler& WM = WorkMat.SetFull();
550 
551  /* Ridimensiona la sottomatrice in base alle esigenze */
552  integer iNumRows = 0;
553  integer iNumCols = 0;
554  WorkSpaceDim(&iNumRows, &iNumCols);
555  WM.ResizeReset(iNumRows, iNumCols);
556 
557  /* Recupera gli indici delle varie incognite */
558  integer iNode1FirstPosIndex = pNode1->iGetFirstPositionIndex();
559  integer iNode1FirstMomIndex = pNode1->iGetFirstMomentumIndex();
560  integer iNode2FirstPosIndex = pNode2->iGetFirstPositionIndex();
561  integer iNode2FirstMomIndex = pNode2->iGetFirstMomentumIndex();
562  integer iFirstReactionIndex = iGetFirstIndex();
563 
564  /* Setta gli indici delle equazioni */
565  for (unsigned int iCnt = 1; iCnt <= 6; iCnt++) {
566  WM.PutRowIndex(iCnt, iNode1FirstMomIndex+iCnt);
567  WM.PutColIndex(iCnt, iNode1FirstPosIndex+iCnt);
568  WM.PutRowIndex(6+iCnt, iNode2FirstMomIndex+iCnt);
569  WM.PutColIndex(6+iCnt, iNode2FirstPosIndex+iCnt);
570  }
571 
572  for (unsigned int iCnt = 1; iCnt <= iGetNumDof(); iCnt++) {
573  WM.PutRowIndex(12+iCnt, iFirstReactionIndex+iCnt);
574  WM.PutColIndex(12+iCnt, iFirstReactionIndex+iCnt);
575  }
576 
577  /* Recupera i dati che servono */
578  const Mat3x3& R1(pNode1->GetRRef());
579  const Mat3x3& R2(pNode2->GetRRef());
580  Vec3 d1Tmp(R1*d1);
581  Vec3 d2Tmp(R2*d2);
582  Mat3x3 R1hTmp(R1*R1h);
583  Mat3x3 R2hTmp(R2*R2h);
584 
585  /* Suppongo che le reazioni F, M siano gia' state aggiornate da AssRes;
586  * ricordo che la forza F e' nel sistema globale, mentre la coppia M
587  * e' nel sistema locale ed il terzo termine, M(3), e' nullo in quanto
588  * diretto come l'asse attorno al quale la rotazione e' consentita */
589 
590 
591  /*
592  * La cerniera piana ha le prime 3 equazioni uguali alla cerniera sferica;
593  * inoltre ha due equazioni che affermano la coincidenza dell'asse 3 del
594  * riferimento solidale con la cerniera visto dai due nodi.
595  *
596  * (R1 * R1h * e1)^T * (R2 * R2h * e3) = 0
597  * (R1 * R1h * e2)^T * (R2 * R2h * e3) = 0
598  *
599  * A queste equazioni corrisponde una reazione di coppia agente attorno
600  * agli assi 1 e 2 del riferimento della cerniera. La coppia attorno
601  * all'asse 3 e' nulla per definizione. Quindi la coppia nel sistema
602  * globale e':
603  * -R1 * R1h * M per il nodo 1,
604  * R2 * R2h * M per il nodo 2
605  *
606  *
607  * xa ga xb gb F M
608  * Qa | 0 0 0 0 I 0 | | xa | | -F |
609  * Ga | 0 c*(F/\da/\-(Sa*M)/\) 0 0 da/\ Sa | | ga | | -da/\F-Sa*M |
610  * Qb | 0 0 0 0 -I 0 | | xb | = | F |
611  * Gb | 0 0 0 -c*(F/\db/\-(Sb*M)/\) -db/\ -Sb | | gb | | db/\F+Sb*M |
612  * F | -c*I c*da/\ c*I -c*db/\ 0 0 | | F | | xa+da-xb-db |
613  * M1 | 0 c*Tb1^T*Ta3/\ 0 c*Ta3^T*Tb1/\ 0 0 | | M | | Sb^T*Ta3 |
614  * M2 | 0 c*Tb2^T*Ta3/\ 0 c*Ta3^T*Tb2/\ 0 0 |
615  *
616  * con da = R1*d01, db = R2*d02, c = dCoef,
617  * Sa = R1*R1h*[e1,e2], Sb = R2*R2h*[e1, e2],
618  * Ta3 = R1*R1h*e3, Tb1 = R2*R2h*e1, Tb2 = R2*R2h*e2
619  */
620 
621  /* Contributo della forza alle equazioni di equilibrio dei due nodi */
622  for (int iCnt = 1; iCnt <= 3; iCnt++) {
623  WM.PutCoef(iCnt, 12+iCnt, 1.);
624  WM.PutCoef(6+iCnt, 12+iCnt, -1.);
625  }
626 
627  WM.Add(4, 13, Mat3x3(MatCross, d1Tmp));
628  WM.Sub(10, 13, Mat3x3(MatCross, d2Tmp));
629 
630  /* Moltiplica la forza ed il momento per il coefficiente
631  * del metodo */
632  Vec3 FTmp = F*dCoef;
633  Vec3 MTmp = M*dCoef;
634 
635  Vec3 e3a(R1hTmp.GetVec(3));
636  Vec3 e1b(R2hTmp.GetVec(1));
637  Vec3 e2b(R2hTmp.GetVec(2));
638  MTmp = e2b*MTmp.dGet(1)-e1b*MTmp.dGet(2);
639 
640  Mat3x3 MWedgee3aWedge(MatCrossCross, MTmp, e3a);
641  Mat3x3 e3aWedgeMWedge(MatCrossCross, e3a, MTmp);
642 
643  WM.Add(4, 4, Mat3x3(MatCrossCross, FTmp, d1Tmp) - MWedgee3aWedge);
644  WM.Add(4, 10, e3aWedgeMWedge);
645 
646  WM.Add(10, 4, MWedgee3aWedge);
647  WM.Sub(10, 10, Mat3x3(MatCrossCross, FTmp, d2Tmp) + e3aWedgeMWedge);
648 
649  /* Contributo del momento alle equazioni di equilibrio dei nodi */
650  Vec3 Tmp1(e2b.Cross(e3a));
651  Vec3 Tmp2(e3a.Cross(e1b));
652 
653  for (int iCnt = 1; iCnt <= 3; iCnt++) {
654  doublereal d = Tmp1.dGet(iCnt);
655  WM.PutCoef(3+iCnt, 16, d);
656  WM.PutCoef(9+iCnt, 16, -d);
657  d = Tmp2.dGet(iCnt);
658  WM.PutCoef(3+iCnt, 17, d);
659  WM.PutCoef(9+iCnt, 17, -d);
660  }
661 
662  /* Modifica: divido le equazioni di vincolo per dCoef */
663 
664  /* Equazioni di vincolo degli spostamenti */
665  for (int iCnt = 1; iCnt <= 3; iCnt++) {
666  WM.PutCoef(12+iCnt, iCnt, -1.);
667  WM.PutCoef(12+iCnt, 6+iCnt, 1.);
668  }
669 
670  WM.Add(13, 4, Mat3x3(MatCross, d1Tmp));
671  WM.Sub(13, 10, Mat3x3(MatCross, d2Tmp));
672 
673  /* Equazione di vincolo del momento
674  *
675  * Attenzione: bisogna scrivere il vettore trasposto
676  * (Sb[1]^T*(Sa[3]/\))*dCoef
677  * Questo pero' e' uguale a:
678  * (-Sa[3]/\*Sb[1])^T*dCoef,
679  * che puo' essere ulteriormente semplificato:
680  * (Sa[3].Cross(Sb[1])*(-dCoef))^T;
681  */
682 
683  for (int iCnt = 1; iCnt <= 3; iCnt++) {
684  doublereal d = Tmp1.dGet(iCnt);
685  WM.PutCoef(16, 3+iCnt, d);
686  WM.PutCoef(16, 9+iCnt, -d);
687  d = Tmp2.dGet(iCnt);
688  WM.PutCoef(17, 3+iCnt, -d);
689  WM.PutCoef(17, 9+iCnt, d);
690  }
691 
692  if (fc) {
693  //retrive
694  //friction coef
695  doublereal f = fc->fc();
696  //shape function
697  doublereal shc = Sh_c->Sh_c();
698  //omega and omega rif
699  const Vec3& Omega1(pNode1->GetWCurr());
700  const Vec3& Omega2(pNode2->GetWCurr());
701  // const Vec3& Omega1r(pNode1->GetWRef());
702  // const Vec3& Omega2r(pNode2->GetWRef());
703  //compute
704  //relative velocity
705  doublereal v = (Omega1-Omega2).Dot(e3a)*r;
706  //reaction norm
707  doublereal modF = std::max(F.Norm(), preF);
708  //reaction moment
709  //doublereal M3 = shc*modF*r;
710 
711  ExpandableRowVector dfc;
712  ExpandableRowVector dF;
713  ExpandableRowVector dv;
714  //variation of reaction force
715  dF.ReDim(3);
716  if ((modF == 0.) or (F.Norm() < preF)) {
717  dF.Set(Vec3(Zero3),1,12+1);
718  } else {
719  dF.Set(F/modF,1,12+1);
720  }
721  //variation of relative velocity
722  dv.ReDim(6);
723 
724 /* old (wrong?) relative velocity linearization */
725 
726 // dv.Set((e3a.dGet(1)*1.-( e3a.dGet(2)*Omega1r.dGet(3)-e3a.dGet(3)*Omega1r.dGet(2))*dCoef)*r,1,0+4);
727 // dv.Set((e3a.dGet(2)*1.-(-e3a.dGet(1)*Omega1r.dGet(3)+e3a.dGet(3)*Omega1r.dGet(1))*dCoef)*r,2,0+5);
728 // dv.Set((e3a.dGet(3)*1.-( e3a.dGet(1)*Omega1r.dGet(2)-e3a.dGet(2)*Omega1r.dGet(1))*dCoef)*r,3,0+6);
729 //
730 // dv.Set(-(e3a.dGet(1)*1.-( e3a.dGet(2)*Omega2r.dGet(3)-e3a.dGet(3)*Omega2r.dGet(2))*dCoef)*r,4,6+4);
731 // dv.Set(-(e3a.dGet(2)*1.-(-e3a.dGet(1)*Omega2r.dGet(3)+e3a.dGet(3)*Omega2r.dGet(1))*dCoef)*r,5,6+5);
732 // dv.Set(-(e3a.dGet(3)*1.-( e3a.dGet(1)*Omega2r.dGet(2)-e3a.dGet(2)*Omega2r.dGet(1))*dCoef)*r,6,6+6);
733 
734 /* new (exact?) relative velocity linearization */
735 //
736 // ExpandableRowVector domega11, domega12, domega13;
737 // ExpandableRowVector domega21, domega22, domega23;
738 // domega11.ReDim(3); domega12.ReDim(3); domega13.ReDim(3);
739 // domega21.ReDim(3); domega22.ReDim(3); domega23.ReDim(3);
740 //
741 // domega11.Set(1., 1, 0+4);
742 // domega11.Set( Omega1r.dGet(3)*dCoef, 2, 0+5);
743 // domega11.Set(-Omega1r.dGet(2)*dCoef, 3, 0+6);
744 // domega21.Set(1., 1, 6+4);
745 // domega21.Set( Omega2r.dGet(3)*dCoef, 2, 6+5);
746 // domega21.Set(-Omega2r.dGet(2)*dCoef, 3, 6+6);
747 // domega12.Set(1., 1, 0+5);
748 // domega12.Set(-Omega1r.dGet(3)*dCoef, 2, 0+4);
749 // domega12.Set( Omega1r.dGet(1)*dCoef, 3, 0+6);
750 // domega22.Set(1., 1, 6+5);
751 // domega22.Set(-Omega2r.dGet(3)*dCoef, 2, 6+4);
752 // domega22.Set( Omega2r.dGet(1)*dCoef, 3, 6+6);
753 // domega13.Set(1., 1, 0+6);
754 // domega13.Set( Omega1r.dGet(2)*dCoef, 2, 0+4);
755 // domega13.Set(-Omega1r.dGet(1)*dCoef, 3, 0+5);
756 // domega23.Set(1., 1, 6+6);
757 // domega23.Set( Omega2r.dGet(2)*dCoef, 2, 6+4);
758 // domega23.Set(-Omega2r.dGet(1)*dCoef, 3, 6+5);
759 //
760 // Vec3 domega = Omega1-Omega2;
761 // dv.Set((e3a.dGet(1)*1.-(
762 // e3a.dGet(2)*(Omega1.dGet(3)-Omega2.dGet(3))-
763 // e3a.dGet(3)*(domega.dGet(2)))*dCoef)*r,1);
764 // dv.Link(1, &domega11);
765 // dv.Set((e3a.dGet(2)*1.-(
766 // -e3a.dGet(1)*(Omega1.dGet(3)-Omega2.dGet(3))+
767 // e3a.dGet(3)*(domega.dGet(1)))*dCoef)*r,2);
768 // dv.Link(2, &domega12);
769 // dv.Set((e3a.dGet(3)*1.-(
770 // e3a.dGet(1)*(Omega1.dGet(2)-Omega2.dGet(2))-
771 // e3a.dGet(2)*(domega.dGet(1)))*dCoef)*r,3);
772 // dv.Link(3, &domega13);
773 //
774 // dv.Set(-(e3a.dGet(1)*1.)*r,4,6+4); dv.Link(4, &domega21);
775 // dv.Set(-(e3a.dGet(2)*1.)*r,5,6+5); dv.Link(5, &domega22);
776 // dv.Set(-(e3a.dGet(3)*1.)*r,6,6+6); dv.Link(6, &domega23);
777 
778 
779 /* new (approximate: assume constant triads orientations)
780  * relative velocity linearization
781 */
782 
783  dv.Set(e3a*r,1, 0+4);
784  dv.Set(-e3a*r,4, 6+4);
785 
786  //assemble friction states
787  fc->AssJac(WM,dfc,12+NumSelfDof,iFirstReactionIndex+NumSelfDof,dCoef,modF,v,
788  XCurr,XPrimeCurr,dF,dv);
789  ExpandableMatrix dM3;
790  ExpandableRowVector dShc;
791  //compute
792  //variation of shape function
793  Sh_c->dSh_c(dShc,f,modF,v,dfc,dF,dv);
794  //variation of moment component
795  dM3.ReDim(3,2);
796  dM3.SetBlockDim(1,1);
797  dM3.SetBlockDim(2,1);
798  dM3.Set(e3a*shc*r,1,1); dM3.Link(1,&dF);
799  dM3.Set(e3a*modF*r,1,2); dM3.Link(2,&dShc);
800  //assemble first node
801  //variation of moment component
802  dM3.Add(WM, 4, 1.);
803  //assemble second node
804  //variation of moment component
805  dM3.Sub(WM, 6+4, 1.);
806  }
807 
808  return WorkMat;
809 }
810 
811 
812 /* Assemblaggio residuo */
813 SubVectorHandler& PlaneHingeJoint::AssRes(SubVectorHandler& WorkVec,
814  doublereal dCoef,
815  const VectorHandler& XCurr,
816  const VectorHandler& XPrimeCurr)
817 {
818  DEBUGCOUT("Entering PlaneHingeJoint::AssRes()" << std::endl);
819 
820  /* Dimensiona e resetta la matrice di lavoro */
821  integer iNumRows = 0;
822  integer iNumCols = 0;
823  WorkSpaceDim(&iNumRows, &iNumCols);
824  WorkVec.ResizeReset(iNumRows);
825 
826  /* Indici */
827  integer iNode1FirstMomIndex = pNode1->iGetFirstMomentumIndex();
828  integer iNode2FirstMomIndex = pNode2->iGetFirstMomentumIndex();
829  integer iFirstReactionIndex = iGetFirstIndex();
830 
831  /* Indici dei nodi */
832  for (int iCnt = 1; iCnt <= 6; iCnt++) {
833  WorkVec.PutRowIndex(iCnt, iNode1FirstMomIndex+iCnt);
834  WorkVec.PutRowIndex(6+iCnt, iNode2FirstMomIndex+iCnt);
835  }
836 
837  /* Indici del vincolo */
838  for (unsigned int iCnt = 1; iCnt <= iGetNumDof(); iCnt++) {
839  WorkVec.PutRowIndex(12+iCnt, iFirstReactionIndex+iCnt);
840  }
841 
842  /* Aggiorna i dati propri */
843  F = Vec3(XCurr, iFirstReactionIndex+1);
844  M = Vec3(XCurr(iFirstReactionIndex+4),
845  XCurr(iFirstReactionIndex+5),
846  0.);
847 
848  /*
849  * FIXME: provare a mettere "modificatori" di forza/momento sui gdl
850  * residui: attrito, rigidezze e smorzamenti, ecc.
851  */
852 
853  /* Recupera i dati */
854  const Vec3& x1(pNode1->GetXCurr());
855  const Vec3& x2(pNode2->GetXCurr());
856  const Mat3x3& R1(pNode1->GetRCurr());
857  const Mat3x3& R2(pNode2->GetRCurr());
858 
859  /* Costruisce i dati propri nella configurazione corrente */
860  Vec3 dTmp1(R1*d1);
861  Vec3 dTmp2(R2*d2);
862  Mat3x3 R1hTmp(R1*R1h);
863  Mat3x3 R2hTmp(R2*R2h);
864 
865  Vec3 e3a(R1hTmp.GetVec(3));
866  Vec3 e1b(R2hTmp.GetVec(1));
867  Vec3 e2b(R2hTmp.GetVec(2));
868 
869  Vec3 MTmp(e2b.Cross(e3a)*M.dGet(1)+e3a.Cross(e1b)*M.dGet(2));
870 
871  /* Equazioni di equilibrio, nodo 1 */
872  WorkVec.Sub(1, F);
873  WorkVec.Add(4, F.Cross(dTmp1)-MTmp); /* Sfrutto F/\d = -d/\F */
874 
875  /* Equazioni di equilibrio, nodo 2 */
876  WorkVec.Add(7, F);
877  WorkVec.Add(10, dTmp2.Cross(F)+MTmp);
878 
879  /* Modifica: divido le equazioni di vincolo per dCoef */
880  ASSERT(dCoef != 0.);
881 
882  /* Equazione di vincolo di posizione */
883  WorkVec.Add(13, (x1+dTmp1-x2-dTmp2)/dCoef);
884 
885  /* Equazioni di vincolo di rotazione */
886  Vec3 Tmp = R1hTmp.GetVec(3);
887  WorkVec.PutCoef(16, Tmp.Dot(R2hTmp.GetVec(2))/dCoef);
888  WorkVec.PutCoef(17, Tmp.Dot(R2hTmp.GetVec(1))/dCoef);
889 
890  if (fc) {
891  bool ChangeJac(false);
892  const Vec3& Omega1(pNode1->GetWCurr());
893  const Vec3& Omega2(pNode2->GetWCurr());
894  doublereal v = (Omega1-Omega2).Dot(e3a)*r;
895  doublereal modF = std::max(F.Norm(), preF);
896  try {
897  fc->AssRes(WorkVec,12+NumSelfDof,iFirstReactionIndex+NumSelfDof,modF,v,XCurr,XPrimeCurr);
898  }
899  catch (Elem::ChangedEquationStructure) {
900  ChangeJac = true;
901  }
902  doublereal f = fc->fc();
903  doublereal shc = Sh_c->Sh_c(f,modF,v);
904  M3 = shc*modF*r;
905  WorkVec.Sub(4,e3a*M3);
906  WorkVec.Add(10,e3a*M3);
908 // M += e3a*M3;
909  if (ChangeJac) {
910  throw Elem::ChangedEquationStructure(MBDYN_EXCEPT_ARGS);
911  }
912  }
913 
914  return WorkVec;
915 }
916 
917 unsigned int PlaneHingeJoint::iGetNumDof(void) const {
918  unsigned int i = NumSelfDof;
919  if (fc) {
920  i+=fc->iGetNumDof();
921  }
922  return i;
923 };
924 
925 
926 DofOrder::Order
927 PlaneHingeJoint::GetDofType(unsigned int i) const {
928  ASSERT(i >= 0 && i < iGetNumDof());
929  if (i<NumSelfDof) {
930  return DofOrder::ALGEBRAIC;
931  } else {
932  return fc->GetDofType(i-NumSelfDof);
933  }
934 };
935 
936 DofOrder::Order
937 PlaneHingeJoint::GetEqType(unsigned int i) const
938 {
939  ASSERTMSGBREAK(i < iGetNumDof(),
940  "INDEX ERROR in PlaneHingeJoint::GetEqType");
941  if (i<NumSelfDof) {
942  return DofOrder::ALGEBRAIC;
943  } else {
944  return fc->GetEqType(i-NumSelfDof);
945  }
946 }
947 
948 void
949 PlaneHingeJoint::OutputPrepare(OutputHandler& OH)
950 {
951  if (bToBeOutput()) {
952 #ifdef USE_NETCDF
953  if (OH.UseNetCDF(OutputHandler::JOINTS)) {
954  std::string name;
955  OutputPrepare_int("revolute hinge", OH, name);
956 
957  Var_Phi = OH.CreateRotationVar(name, "", od, "global");
958 
959  Var_Omega = OH.CreateVar<Vec3>(name + "Omega", "radian/s",
960  "local relative angular velocity (x, y, z)");
961 
962 /* TODO
963  Var_MFR = OH.CreateVar<doublereal>(name + "MFR", "Nm",
964  "friciton moment ");
965 
966  Var_MU = OH.CreateVar<doublereal>(name + "MU", "--",
967  "friction model specific data: friction coefficient?");
968 */
969  }
970 #endif // USE_NETCDF
971  }
972 }
973 
974 /* Output (da mettere a punto) */
975 void PlaneHingeJoint::Output(OutputHandler& OH) const
976 {
977  if (bToBeOutput()) {
978  Mat3x3 R2Tmp(pNode2->GetRCurr()*R2h);
979  Mat3x3 RTmp((pNode1->GetRCurr()*R1h).MulTM(R2Tmp));
980  Vec3 OmegaTmp(R2Tmp.MulTV(pNode2->GetWCurr()-pNode1->GetWCurr()));
981  Vec3 E;
982  switch (od) {
983  case EULER_123:
984  E = MatR2EulerAngles123(RTmp)*dRaDegr;
985  break;
986 
987  case EULER_313:
988  E = MatR2EulerAngles313(RTmp)*dRaDegr;
989  break;
990 
991  case EULER_321:
992  E = MatR2EulerAngles321(RTmp)*dRaDegr;
993  break;
994 
995  case ORIENTATION_VECTOR:
996  E = RotManip::VecRot(RTmp);
997  break;
998 
999  case ORIENTATION_MATRIX:
1000  break;
1001 
1002  default:
1003  /* impossible */
1004  break;
1005  }
1006 
1007 #ifdef USE_NETCDF
1008  if (OH.UseNetCDF(OutputHandler::JOINTS)) {
1009  Var_F_local->put_rec((R2Tmp.MulTV(F)).pGetVec(), OH.GetCurrentStep());
1010  Var_M_local->put_rec(M.pGetVec(), OH.GetCurrentStep());
1011  Var_F_global->put_rec(F.pGetVec(), OH.GetCurrentStep());
1012  Var_M_global->put_rec((R2Tmp*M).pGetVec(), OH.GetCurrentStep());
1013 
1014  switch (od) {
1015  case EULER_123:
1016  case EULER_313:
1017  case EULER_321:
1018  case ORIENTATION_VECTOR:
1019  Var_Phi->put_rec(E.pGetVec(), OH.GetCurrentStep());
1020  break;
1021 
1022  case ORIENTATION_MATRIX:
1023  Var_Phi->put_rec(RTmp.pGetMat(), OH.GetCurrentStep());
1024  break;
1025 
1026  default:
1027  /* impossible */
1028  break;
1029  }
1030 
1031  Var_Omega->put_rec(OmegaTmp.pGetVec(), OH.GetCurrentStep());
1032 /*
1033  if (fc) {
1034  Var_MFR->put_rec(&M3, OH.GetCurrentStep());
1035  Var_MU->put_rec(fc->fc(), OH.GetCurrentStep());
1036  }
1037  else
1038  {
1039  Var_MFR->put_rec(0, OH.GetCurrentStep());
1040  Var_MU->put_rec(0, OH.GetCurrentStep());
1041  }
1042 */
1043  }
1044 #endif // USE_NETCDF
1045  if (OH.UseText(OutputHandler::JOINTS)) {
1046  std::ostream &of = Joint::Output(OH.Joints(), "PlaneHinge", GetLabel(),
1047  R2Tmp.MulTV(F), M, F, R2Tmp*M)
1048  << " ";
1049 
1050  switch (od) {
1051  case EULER_123:
1052  case EULER_313:
1053  case EULER_321:
1054  case ORIENTATION_VECTOR:
1055  of << E;
1056  break;
1057 
1058  case ORIENTATION_MATRIX:
1059  of << RTmp;
1060  break;
1061 
1062  default:
1063  /* impossible */
1064  break;
1065  }
1066 
1067  of << " " << R2Tmp.MulTV(pNode2->GetWCurr()-pNode1->GetWCurr());
1068  if (fc) {
1069  of << " " << M3 << " " << fc->fc();
1070  }
1071  of << std::endl;
1072  }
1073  }
1074 }
1075 
1076 
1077 /* Contributo allo jacobiano durante l'assemblaggio iniziale */
1078 VariableSubMatrixHandler&
1079 PlaneHingeJoint::InitialAssJac(VariableSubMatrixHandler& WorkMat,
1080  const VectorHandler& XCurr)
1081 {
1082  /* Per ora usa la matrice piena; eventualmente si puo'
1083  * passare a quella sparsa quando si ottimizza */
1084  FullSubMatrixHandler& WM = WorkMat.SetFull();
1085 
1086  /* Dimensiona e resetta la matrice di lavoro */
1087  integer iNumRows = 0;
1088  integer iNumCols = 0;
1089  InitialWorkSpaceDim(&iNumRows, &iNumCols);
1090  WM.ResizeReset(iNumRows, iNumCols);
1091 
1092  /* Equazioni: vedi joints.dvi */
1093 
1094  /* equazioni ed incognite
1095  * F1 Delta_x1 0+1 = 1
1096  * M1 Delta_g1 3+1 = 4
1097  * FP1 Delta_xP1 6+1 = 7
1098  * MP1 Delta_w1 9+1 = 10
1099  * F2 Delta_x2 12+1 = 13
1100  * M2 Delta_g2 15+1 = 16
1101  * FP2 Delta_xP2 18+1 = 19
1102  * MP2 Delta_w2 21+1 = 22
1103  * vincolo spostamento Delta_F 24+1 = 25
1104  * vincolo rotazione Delta_M 27+1 = 28
1105  * derivata vincolo spostamento Delta_FP 29+1 = 30
1106  * derivata vincolo rotazione Delta_MP 32+1 = 33
1107  */
1108 
1109 
1110  /* Indici */
1111  integer iNode1FirstPosIndex = pNode1->iGetFirstPositionIndex();
1112  integer iNode1FirstVelIndex = iNode1FirstPosIndex+6;
1113  integer iNode2FirstPosIndex = pNode2->iGetFirstPositionIndex();
1114  integer iNode2FirstVelIndex = iNode2FirstPosIndex+6;
1115  integer iFirstReactionIndex = iGetFirstIndex();
1116  integer iReactionPrimeIndex = iFirstReactionIndex+5;
1117 
1118  /* Nota: le reazioni vincolari sono:
1119  * Forza, 3 incognite, riferimento globale,
1120  * Momento, 2 incognite, riferimento locale
1121  */
1122 
1123  /* Setta gli indici dei nodi */
1124  for (int iCnt = 1; iCnt <= 6; iCnt++) {
1125  WM.PutRowIndex(iCnt, iNode1FirstPosIndex+iCnt);
1126  WM.PutColIndex(iCnt, iNode1FirstPosIndex+iCnt);
1127  WM.PutRowIndex(6+iCnt, iNode1FirstVelIndex+iCnt);
1128  WM.PutColIndex(6+iCnt, iNode1FirstVelIndex+iCnt);
1129  WM.PutRowIndex(12+iCnt, iNode2FirstPosIndex+iCnt);
1130  WM.PutColIndex(12+iCnt, iNode2FirstPosIndex+iCnt);
1131  WM.PutRowIndex(18+iCnt, iNode2FirstVelIndex+iCnt);
1132  WM.PutColIndex(18+iCnt, iNode2FirstVelIndex+iCnt);
1133  }
1134 
1135  /* Setta gli indici delle reazioni */
1136  for (int iCnt = 1; iCnt <= 10; iCnt++) {
1137  WM.PutRowIndex(24+iCnt, iFirstReactionIndex+iCnt);
1138  WM.PutColIndex(24+iCnt, iFirstReactionIndex+iCnt);
1139  }
1140 
1141 
1142  /* Recupera i dati */
1143  const Mat3x3& R1(pNode1->GetRRef());
1144  const Mat3x3& R2(pNode2->GetRRef());
1145  const Vec3& Omega1(pNode1->GetWRef());
1146  const Vec3& Omega2(pNode2->GetWRef());
1147 
1148  /* F ed M sono gia' state aggiornate da InitialAssRes */
1149  Vec3 FPrime(XCurr, iReactionPrimeIndex+1);
1150  Vec3 MPrime(XCurr(iReactionPrimeIndex+4),
1151  XCurr(iReactionPrimeIndex+5),
1152  0.);
1153 
1154  /* Matrici identita' */
1155  for (int iCnt = 1; iCnt <= 3; iCnt++) {
1156  /* Contributo di forza all'equazione della forza, nodo 1 */
1157  WM.PutCoef(iCnt, 24+iCnt, 1.);
1158 
1159  /* Contrib. di der. di forza all'eq. della der. della forza, nodo 1 */
1160  WM.PutCoef(6+iCnt, 29+iCnt, 1.);
1161 
1162  /* Contributo di forza all'equazione della forza, nodo 2 */
1163  WM.PutCoef(12+iCnt, 24+iCnt, -1.);
1164 
1165  /* Contrib. di der. di forza all'eq. della der. della forza, nodo 2 */
1166  WM.PutCoef(18+iCnt, 29+iCnt, -1.);
1167 
1168  /* Equazione di vincolo, nodo 1 */
1169  WM.PutCoef(24+iCnt, iCnt, -1.);
1170 
1171  /* Derivata dell'equazione di vincolo, nodo 1 */
1172  WM.PutCoef(29+iCnt, 6+iCnt, -1.);
1173 
1174  /* Equazione di vincolo, nodo 2 */
1175  WM.PutCoef(24+iCnt, 12+iCnt, 1.);
1176 
1177  /* Derivata dell'equazione di vincolo, nodo 2 */
1178  WM.PutCoef(29+iCnt, 18+iCnt, 1.);
1179  }
1180 
1181  /* Distanze e matrici di rotazione dai nodi alla cerniera
1182  * nel sistema globale */
1183  Vec3 d1Tmp(R1*d1);
1184  Vec3 d2Tmp(R2*d2);
1185  Mat3x3 R1hTmp(R1*R1h);
1186  Mat3x3 R2hTmp(R2*R2h);
1187 
1188  Vec3 e3a(R1hTmp.GetVec(3));
1189  Vec3 e1b(R2hTmp.GetVec(1));
1190  Vec3 e2b(R2hTmp.GetVec(2));
1191 
1192  /* Ruota il momento e la sua derivata con le matrici della cerniera
1193  * rispetto ai nodi */
1194  Vec3 MTmp(e2b*M.dGet(1)-e1b*M.dGet(2));
1195  Vec3 MPrimeTmp(e2b*MPrime.dGet(1)-e1b*MPrime.dGet(2));
1196 
1197  /* Matrici F/\d1/\, -F/\d2/\ */
1198  Mat3x3 FWedged1Wedge(MatCrossCross, F, d1Tmp);
1199  Mat3x3 FWedged2Wedge(MatCrossCross, F, -d2Tmp);
1200 
1201  /* Matrici (omega1/\d1)/\, -(omega2/\d2)/\ */
1202  Mat3x3 O1Wedged1Wedge(MatCross, Omega1.Cross(d1Tmp));
1203  Mat3x3 O2Wedged2Wedge(MatCross, d2Tmp.Cross(Omega2));
1204 
1205  Mat3x3 MDeltag1((Mat3x3(MatCross, Omega2.Cross(MTmp) + MPrimeTmp)
1206  + Mat3x3(MatCrossCross, MTmp, Omega1))*Mat3x3(MatCross, e3a));
1207  Mat3x3 MDeltag2(Mat3x3(MatCrossCross, Omega1.Cross(e3a), MTmp)
1208  + Mat3x3(MatCrossCross, e3a, MPrimeTmp)
1209  + e3a.Cross(Mat3x3(MatCrossCross, Omega2, MTmp)));
1210 
1211  /* Vettori temporanei */
1212  Vec3 Tmp1(e2b.Cross(e3a));
1213  Vec3 Tmp2(e3a.Cross(e1b));
1214 
1215  /* Prodotto vettore tra il versore 3 della cerniera secondo il nodo 1
1216  * ed il versore 1 della cerniera secondo il nodo 2. A convergenza
1217  * devono essere ortogonali, quindi il loro prodotto vettore deve essere
1218  * unitario */
1219 
1220  /* Error handling: il programma si ferma, pero' segnala dov'e' l'errore */
1221  if (Tmp1.Dot() <= std::numeric_limits<doublereal>::epsilon() || Tmp2.Dot() <= std::numeric_limits<doublereal>::epsilon()) {
1222  silent_cerr("PlaneHingeJoint(" << GetLabel() << "): "
1223  "first and second node hinge axes are (nearly) orthogonal"
1224  << std::endl);
1225  throw ErrNullNorm(MBDYN_EXCEPT_ARGS);
1226  }
1227 
1228  Vec3 TmpPrime1(e2b.Cross(Omega1.Cross(e3a))-e3a.Cross(Omega2.Cross(e2b)));
1229  Vec3 TmpPrime2(e3a.Cross(Omega2.Cross(e1b))-e1b.Cross(Omega1.Cross(e3a)));
1230 
1231  /* Equazione di momento, nodo 1 */
1232  WM.Add(4, 4, FWedged1Wedge - Mat3x3(MatCrossCross, MTmp, e3a));
1233  WM.Add(4, 16, Mat3x3(MatCrossCross, e3a, MTmp));
1234  WM.Add(4, 25, Mat3x3(MatCross, d1Tmp));
1235  for (int iCnt = 1; iCnt <= 3; iCnt++) {
1236  WM.PutCoef(3+iCnt, 28, Tmp1(iCnt));
1237  WM.PutCoef(3+iCnt, 29, Tmp2(iCnt));
1238  }
1239 
1240  /* Equazione di momento, nodo 2 */
1241  WM.Add(16, 4, Mat3x3(MatCrossCross, MTmp, e3a));
1242  WM.Add(16, 16, FWedged2Wedge - Mat3x3(MatCrossCross, e3a, MTmp));
1243  WM.Sub(16, 25, Mat3x3(MatCross, d2Tmp));
1244  for (int iCnt = 1; iCnt <= 3; iCnt++) {
1245  WM.PutCoef(15+iCnt, 28, -Tmp1(iCnt));
1246  WM.PutCoef(15+iCnt, 29, -Tmp2(iCnt));
1247  }
1248 
1249  /* Derivata dell'equazione di momento, nodo 1 */
1250  WM.Add(10, 4, (Mat3x3(MatCross, FPrime) + Mat3x3(MatCrossCross, F, Omega1))*Mat3x3(MatCross, d1Tmp) - MDeltag1);
1251  WM.Add(10, 10, FWedged1Wedge - Mat3x3(MatCrossCross, MTmp, e3a));
1252  WM.Add(10, 16, MDeltag2);
1253  WM.Add(10, 22, Mat3x3(MatCrossCross, e3a, MTmp));
1254  WM.Add(10, 25, O1Wedged1Wedge);
1255  WM.Add(10, 30, Mat3x3(MatCross, d1Tmp));
1256 
1257  for (int iCnt = 1; iCnt <= 3; iCnt++) {
1258  WM.PutCoef(9+iCnt, 28, TmpPrime1(iCnt));
1259  WM.PutCoef(9+iCnt, 29, TmpPrime2(iCnt));
1260  WM.PutCoef(9+iCnt, 33, Tmp1(iCnt));
1261  WM.PutCoef(9+iCnt, 34, Tmp2(iCnt));
1262  }
1263 
1264  /* Derivata dell'equazione di momento, nodo 2 */
1265  WM.Add(22, 4, MDeltag1);
1266  WM.Add(22, 10, Mat3x3(MatCrossCross, MTmp, e3a));
1267  WM.Sub(22, 16, (Mat3x3(MatCross, FPrime) + Mat3x3(MatCrossCross, F, Omega2))*Mat3x3(MatCross, d2Tmp) + MDeltag2);
1268  WM.Add(22, 22, FWedged2Wedge - Mat3x3(MatCrossCross, e3a, MTmp));
1269  WM.Add(22, 25, O2Wedged2Wedge);
1270  WM.Sub(22, 30, Mat3x3(MatCross, d2Tmp));
1271 
1272  for (int iCnt = 1; iCnt <= 3; iCnt++) {
1273  WM.PutCoef(21+iCnt, 28, -TmpPrime1(iCnt));
1274  WM.PutCoef(21+iCnt, 29, -TmpPrime2(iCnt));
1275  WM.PutCoef(21+iCnt, 33, -Tmp1(iCnt));
1276  WM.PutCoef(21+iCnt, 34, -Tmp2(iCnt));
1277  }
1278 
1279  /* Equazione di vincolo di posizione */
1280  WM.Add(25, 4, Mat3x3(MatCross, d1Tmp));
1281  WM.Sub(25, 16, Mat3x3(MatCross, d2Tmp));
1282 
1283  /* Derivata dell'equazione di vincolo di posizione */
1284  WM.Add(30, 4, O1Wedged1Wedge);
1285  WM.Add(30, 10, Mat3x3(MatCross, d1Tmp));
1286  WM.Add(30, 16, O2Wedged2Wedge);
1287  WM.Sub(30, 22, Mat3x3(MatCross, d2Tmp));
1288 
1289  /* Equazioni di vincolo di rotazione: e1b~e3a, e2b~e3a */
1290 
1291  for (int iCnt = 1; iCnt <= 3; iCnt++) {
1292  doublereal d = Tmp1(iCnt);
1293  WM.PutCoef(28, 3+iCnt, d);
1294  WM.PutCoef(28, 15+iCnt, -d);
1295 
1296  /* Queste sono per la derivata dell'equazione, sono qui solo per
1297  * ottimizzazione */
1298  WM.PutCoef(33, 9+iCnt, d);
1299  WM.PutCoef(33, 21+iCnt, -d);
1300 
1301  d = Tmp2.dGet(iCnt);
1302  WM.PutCoef(29, 3+iCnt, -d);
1303  WM.PutCoef(29, 15+iCnt, d);
1304 
1305  /* Queste sono per la derivata dell'equazione, sono qui solo per
1306  * ottimizzazione */
1307  WM.PutCoef(34, 9+iCnt, -d);
1308  WM.PutCoef(34, 21+iCnt, d);
1309  }
1310 
1311  /* Derivate delle equazioni di vincolo di rotazione: e1b~e3a, e2b~e3a */
1312  Vec3 O1mO2(Omega1-Omega2);
1313  TmpPrime1 = e3a.Cross(O1mO2.Cross(e2b));
1314  TmpPrime2 = e2b.Cross(e3a.Cross(O1mO2));
1315  for (int iCnt = 1; iCnt <= 3; iCnt++) {
1316  WM.PutCoef(33, 3+iCnt, TmpPrime1.dGet(iCnt));
1317  WM.PutCoef(33, 15+iCnt, TmpPrime2.dGet(iCnt));
1318  }
1319 
1320  TmpPrime1 = e3a.Cross(O1mO2.Cross(e1b));
1321  TmpPrime2 = e1b.Cross(e3a.Cross(O1mO2));
1322  for (int iCnt = 1; iCnt <= 3; iCnt++) {
1323  WM.PutCoef(34, 3+iCnt, TmpPrime1.dGet(iCnt));
1324  WM.PutCoef(34, 15+iCnt, TmpPrime2.dGet(iCnt));
1325  }
1326 
1327  return WorkMat;
1328 }
1329 
1330 
1331 /* Contributo al residuo durante l'assemblaggio iniziale */
1332 SubVectorHandler&
1333 PlaneHingeJoint::InitialAssRes(SubVectorHandler& WorkVec,
1334  const VectorHandler& XCurr)
1335 {
1336  DEBUGCOUT("Entering PlaneHingeJoint::InitialAssRes()" << std::endl);
1337 
1338  /* Dimensiona e resetta la matrice di lavoro */
1339  integer iNumRows = 0;
1340  integer iNumCols = 0;
1341  InitialWorkSpaceDim(&iNumRows, &iNumCols);
1342  WorkVec.ResizeReset(iNumRows);
1343 
1344  /* Indici */
1345  integer iNode1FirstPosIndex = pNode1->iGetFirstPositionIndex();
1346  integer iNode1FirstVelIndex = iNode1FirstPosIndex+6;
1347  integer iNode2FirstPosIndex = pNode2->iGetFirstPositionIndex();
1348  integer iNode2FirstVelIndex = iNode2FirstPosIndex+6;
1349  integer iFirstReactionIndex = iGetFirstIndex();
1350  integer iReactionPrimeIndex = iFirstReactionIndex+5;
1351 
1352  /* Setta gli indici */
1353  for (int iCnt = 1; iCnt <= 6; iCnt++) {
1354  WorkVec.PutRowIndex(iCnt, iNode1FirstPosIndex+iCnt);
1355  WorkVec.PutRowIndex(6+iCnt, iNode1FirstVelIndex+iCnt);
1356  WorkVec.PutRowIndex(12+iCnt, iNode2FirstPosIndex+iCnt);
1357  WorkVec.PutRowIndex(18+iCnt, iNode2FirstVelIndex+iCnt);
1358  }
1359 
1360  for (int iCnt = 1; iCnt <= 10; iCnt++) {
1361  WorkVec.PutRowIndex(24+iCnt, iFirstReactionIndex+iCnt);
1362  }
1363 
1364  /* Recupera i dati */
1365  const Vec3& x1(pNode1->GetXCurr());
1366  const Vec3& x2(pNode2->GetXCurr());
1367  const Vec3& v1(pNode1->GetVCurr());
1368  const Vec3& v2(pNode2->GetVCurr());
1369  const Mat3x3& R1(pNode1->GetRCurr());
1370  const Mat3x3& R2(pNode2->GetRCurr());
1371  const Vec3& Omega1(pNode1->GetWCurr());
1372  const Vec3& Omega2(pNode2->GetWCurr());
1373 
1374  /* Aggiorna F ed M, che restano anche per InitialAssJac */
1375  F = Vec3(XCurr, iFirstReactionIndex+1);
1376  M = Vec3(XCurr(iFirstReactionIndex+4),
1377  XCurr(iFirstReactionIndex+5),
1378  0.);
1379  Vec3 FPrime(XCurr, iReactionPrimeIndex+1);
1380  Vec3 MPrime(XCurr(iReactionPrimeIndex+4),
1381  XCurr(iReactionPrimeIndex+5),
1382  0.);
1383 
1384  /* Distanza nel sistema globale */
1385  Vec3 d1Tmp(R1*d1);
1386  Vec3 d2Tmp(R2*d2);
1387  Mat3x3 R1hTmp(R1*R1h);
1388  Mat3x3 R2hTmp(R2*R2h);
1389 
1390  /* Vettori omega1/\d1, -omega2/\d2 */
1391  Vec3 O1Wedged1(Omega1.Cross(d1Tmp));
1392  Vec3 O2Wedged2(Omega2.Cross(d2Tmp));
1393 
1394  Vec3 e3a(R1hTmp.GetVec(3));
1395  Vec3 e1b(R2hTmp.GetVec(1));
1396  Vec3 e2b(R2hTmp.GetVec(2));
1397 
1398  /* Ruota il momento e la sua derivata con le matrici della cerniera
1399  * rispetto ai nodi */
1400  Vec3 MTmp(e2b*M.dGet(1)-e1b*M.dGet(2));
1401  Vec3 MPrimeTmp(e3a.Cross(MTmp.Cross(Omega2))+MTmp.Cross(Omega1.Cross(e3a))+
1402  e2b.Cross(e3a)*MPrime.dGet(1)+e3a.Cross(e1b)*MPrime.dGet(2));
1403 
1404  /* Equazioni di equilibrio, nodo 1 */
1405  WorkVec.Sub(1, F);
1406  WorkVec.Add(4, F.Cross(d1Tmp)-MTmp.Cross(e3a)); /* Sfrutto il fatto che F/\d = -d/\F */
1407 
1408  /* Derivate delle equazioni di equilibrio, nodo 1 */
1409  WorkVec.Sub(7, FPrime);
1410  WorkVec.Add(10, FPrime.Cross(d1Tmp)-O1Wedged1.Cross(F)-MPrimeTmp);
1411 
1412  /* Equazioni di equilibrio, nodo 2 */
1413  WorkVec.Add(13, F);
1414  WorkVec.Add(16, d2Tmp.Cross(F)+MTmp.Cross(e3a));
1415 
1416  /* Derivate delle equazioni di equilibrio, nodo 2 */
1417  WorkVec.Add(19, FPrime);
1418  WorkVec.Add(22, d2Tmp.Cross(FPrime)+O2Wedged2.Cross(F)+MPrimeTmp);
1419 
1420  /* Equazione di vincolo di posizione */
1421  WorkVec.Add(25, x1+d1Tmp-x2-d2Tmp);
1422 
1423  /* Derivata dell'equazione di vincolo di posizione */
1424  WorkVec.Add(30, v1+O1Wedged1-v2-O2Wedged2);
1425 
1426  /* Equazioni di vincolo di rotazione */
1427  WorkVec.PutCoef(28, e2b.Dot(e3a));
1428  WorkVec.PutCoef(29, e1b.Dot(e3a));
1429 
1430  /* Derivate delle equazioni di vincolo di rotazione: e1b~e3a, e2b~e3a */
1431  Vec3 Tmp((Omega1-Omega2).Cross(e3a));
1432  WorkVec.PutCoef(33, e2b.Dot(Tmp));
1433  WorkVec.PutCoef(34, e1b.Dot(Tmp));
1434 
1435  return WorkVec;
1436 }
1437 
1438 
1439 unsigned int
1440 PlaneHingeJoint::iGetNumPrivData(void) const
1441 {
1442  return 8;
1443 }
1444 
1445 unsigned int
1446 PlaneHingeJoint::iGetPrivDataIdx(const char *s) const
1447 {
1448  ASSERT(s != NULL);
1449 
1450  unsigned int idx = 0;
1451 
1452  switch (s[0]) {
1453  case 'w':
1454  idx++;
1455  case 'r':
1456  idx++;
1457  if (s[1] == 'z') {
1458  return idx;
1459  }
1460  break;
1461 
1462  case 'M':
1463  idx += 3;
1464  case 'F':
1465  idx += 2;
1466 
1467  switch (s[1]) {
1468  case 'x':
1469  return idx + 1;
1470 
1471  case 'y':
1472  return idx + 2;
1473 
1474  case 'z':
1475  return idx + 3;
1476  }
1477  }
1478 
1479  return 0;
1480 }
1481 
1482 doublereal PlaneHingeJoint::dGetPrivData(unsigned int i) const
1483 {
1484  ASSERT(i >= 1 && i <= iGetNumPrivData());
1485 
1486  switch (i) {
1487  case 1: {
1488  Vec3 v(RotManip::VecRot((pNode1->GetRCurr()*R1h).MulTM(pNode2->GetRCurr()*R2h)));
1489  doublereal dThetaTmp(v(3));
1490 
1491  int n = 0;
1492 
1493  if (dThetaTmp - dThetaWrapped < -M_PI) {
1494  n++;
1495  }
1496 
1497  if (dThetaTmp - dThetaWrapped > M_PI) {
1498  n--;
1499  }
1500 
1501  return 2*M_PI*(NTheta + n) + dThetaTmp;
1502  }
1503 
1504  case 2: {
1505  Mat3x3 R2Tmp((pNode2->GetRCurr()*R2h));
1506  Vec3 v(R2Tmp.MulTV(pNode2->GetWCurr()-pNode1->GetWCurr()));
1507 
1508  return v(3);
1509  }
1510 
1511  case 3:
1512  case 4:
1513  case 5:
1514  return F(i - 2);
1515 
1516  case 6:
1517  case 7:
1518  case 8:
1519  return M(i - 5);
1520  }
1521 
1522  silent_cerr("PlaneHingeJoint(" << GetLabel() << "): "
1523  "illegal private data " << i << std::endl);
1524  throw ErrGeneric(MBDYN_EXCEPT_ARGS);
1525 }
1526 
1527 /* PlaneHingeJoint - end */
1528 
1529 
1530 /* PlaneRotationJoint - begin */
1531 
1532 /* Costruttore non banale */
1533 PlaneRotationJoint::PlaneRotationJoint(unsigned int uL, const DofOwner* pDO,
1534  const StructNode* pN1, const StructNode* pN2,
1535  const Mat3x3& R1hTmp, const Mat3x3& R2hTmp,
1536  const OrientationDescription& od,
1537  flag fOut)
1538 : Elem(uL, fOut),
1539 Joint(uL, pDO, fOut),
1540 pNode1(pN1), pNode2(pN2),
1541 R1h(R1hTmp), R2h(R2hTmp), M(Zero3),
1542 NTheta(0), dTheta(0.),
1543 dThetaWrapped(0.),
1544 #ifdef USE_NETCDF
1545 Var_Phi(0),
1546 Var_Omega(0),
1547 //Var_MFR(0),
1548 //Var_MU(0),
1549 #endif // USE_NETCDF
1550 od(od)
1551 {
1552  NO_OP;
1553 }
1554 
1555 
1556 /* Distruttore banale */
1557 PlaneRotationJoint::~PlaneRotationJoint(void)
1558 {
1559  NO_OP;
1560 };
1561 
1562 
1563 std::ostream&
1564 PlaneRotationJoint::DescribeDof(std::ostream& out, const char *prefix, bool bInitial) const
1565 {
1566  integer iIndex = iGetFirstIndex();
1567 
1568  out
1569  << prefix << iIndex + 1 << "->" << iIndex + 2 << ": "
1570  "reaction couples [mx,my]" << std::endl;
1571 
1572  if (bInitial) {
1573  iIndex += 2;
1574  out
1575  << prefix << iIndex + 1 << "->" << iIndex + 2 << ": "
1576  "reaction couple derivatives [mPx,mPy]" << std::endl;
1577  }
1578 
1579  return out;
1580 }
1581 
1582 void
1583 PlaneRotationJoint::DescribeDof(std::vector<std::string>& desc, bool bInitial, int i) const
1584 {
1585  std::ostringstream os;
1586  os << "PlaneRotationJoint(" << GetLabel() << ")";
1587 
1588  unsigned short nself = 2;
1589  if (bInitial) {
1590  nself *= 2;
1591  }
1592 
1593  if (i == -1) {
1594  desc.resize(nself);
1595  std::string name = os.str();
1596 
1597  for (unsigned i = 0; i < 2; i++) {
1598  os.str(name);
1599  os.seekp(0, std::ios_base::end);
1600  os << ": reaction couple m" << xyz[i];
1601  desc[i] = os.str();
1602  }
1603 
1604  if (bInitial) {
1605  for (unsigned i = 0; i < 2; i++) {
1606  os.str(name);
1607  os.seekp(0, std::ios_base::end);
1608  os << ": reaction couple derivative mP" << xyz[i];
1609  desc[2 + i] = os.str();
1610  }
1611  }
1612 
1613  } else {
1614  if (i < -1) {
1615  // error
1616  throw ErrGeneric(MBDYN_EXCEPT_ARGS);
1617  }
1618 
1619  if (i >= nself) {
1620  // error
1621  throw ErrGeneric(MBDYN_EXCEPT_ARGS);
1622  }
1623 
1624  desc.resize(1);
1625 
1626  switch (i) {
1627  case 0:
1628  case 1:
1629  os << ": reaction couple m" << xyz[i];
1630  break;
1631 
1632  case 2:
1633  case 3:
1634  os << ": reaction couple derivative mP" << xyz[i - 2];
1635  break;
1636  }
1637  desc[0] = os.str();
1638  }
1639 }
1640 
1641 std::ostream&
1642 PlaneRotationJoint::DescribeEq(std::ostream& out, const char *prefix, bool bInitial) const
1643 {
1644  integer iIndex = iGetFirstIndex();
1645 
1646  out
1647  << prefix << iIndex + 1 << "->" << iIndex + 2 << ": "
1648  "orientation constraints [gx1=gx2,gy1=gy2]" << std::endl;
1649 
1650  if (bInitial) {
1651  iIndex += 2;
1652  out
1653  << prefix << iIndex + 1 << "->" << iIndex + 2 << ": "
1654  "angular velocity constraints [wx1=wx2,wy1=wy2]" << std::endl;
1655  }
1656 
1657  return out;
1658 }
1659 
1660 void
1661 PlaneRotationJoint::DescribeEq(std::vector<std::string>& desc, bool bInitial, int i) const
1662 {
1663  std::ostringstream os;
1664  os << "PlaneRotationJoint(" << GetLabel() << ")";
1665 
1666  unsigned short nself = 2;
1667  if (bInitial) {
1668  nself *= 2;
1669  }
1670 
1671  if (i == -1) {
1672  desc.resize(nself);
1673  std::string name = os.str();
1674 
1675  for (unsigned i = 0; i < 2; i++) {
1676  os.str(name);
1677  os.seekp(0, std::ios_base::end);
1678  os << ": orientation constraint g" << xyz[i];
1679  desc[i] = os.str();
1680  }
1681 
1682  if (bInitial) {
1683  for (unsigned i = 0; i < 2; i++) {
1684  os.str(name);
1685  os.seekp(0, std::ios_base::end);
1686  os << ": orientation constraint derivative w" << xyz[i];
1687  desc[2 + i] = os.str();
1688  }
1689  }
1690 
1691  } else {
1692  if (i < -1) {
1693  // error
1694  throw ErrGeneric(MBDYN_EXCEPT_ARGS);
1695  }
1696 
1697  if (i >= nself) {
1698  // error
1699  throw ErrGeneric(MBDYN_EXCEPT_ARGS);
1700  }
1701 
1702  desc.resize(1);
1703 
1704  switch (i) {
1705  case 0:
1706  case 1:
1707  os << ": orientation constraint g" << xyz[i];
1708  break;
1709 
1710  case 2:
1711  case 3:
1712  os << ": orientation constraint derivative w" << xyz[i - 2];
1713  break;
1714  }
1715  desc[0] = os.str();
1716  }
1717 }
1718 
1719 void
1720 PlaneRotationJoint::SetValue(DataManager *pDM,
1721  VectorHandler& X, VectorHandler& XP,
1722  SimulationEntity::Hints *ph)
1723 {
1724  if (ph) {
1725  for (unsigned i = 0; i < ph->size(); i++) {
1726  Joint::JointHint *pjh = dynamic_cast<Joint::JointHint *>((*ph)[i]);
1727 
1728  if (pjh == 0) {
1729  continue;
1730  }
1731 
1732  if (dynamic_cast<Joint::HingeHint<1> *>(pjh)) {
1733  R1h = pNode1->GetRCurr().MulTM(pNode2->GetRCurr()*R2h);
1734 
1735  } else if (dynamic_cast<Joint::HingeHint<2> *>(pjh)) {
1736  R2h = pNode2->GetRCurr().MulTM(pNode1->GetRCurr()*R1h);
1737 
1738  } else if (dynamic_cast<Joint::ReactionsHint *>(pjh)) {
1739  /* TODO */
1740  }
1741  }
1742  }
1743 
1744  Vec3 v(RotManip::VecRot((pNode1->GetRCurr()*R1h).MulTM(pNode2->GetRCurr()*R2h)));
1745 
1746  dThetaWrapped = dTheta = v.dGet(3);
1747 }
1748 
1749 Hint *
1750 PlaneRotationJoint::ParseHint(DataManager *pDM, const char *s) const
1751 {
1752  if (strncasecmp(s, "hinge{" /*}*/, STRLENOF("hinge{" /*}*/)) == 0) {
1753  s += STRLENOF("hinge{" /*}*/);
1754 
1755  if (strcmp(&s[1], /*{*/ "}") != 0) {
1756  return 0;
1757  }
1758 
1759  switch (s[0]) {
1760  case '1':
1761  return new Joint::HingeHint<1>;
1762 
1763  case '2':
1764  return new Joint::HingeHint<2>;
1765  }
1766  }
1767 
1768  return 0;
1769 }
1770 
1771 void
1772 PlaneRotationJoint::AfterConvergence(const VectorHandler& X,
1773  const VectorHandler& XP)
1774 {
1775  Vec3 v(RotManip::VecRot((pNode1->GetRCurr()*R1h).MulTM(pNode2->GetRCurr()*R2h)));
1776  doublereal dThetaTmp(v(3));
1777 
1778  // unwrap
1779  if (dThetaTmp - dThetaWrapped < -M_PI) {
1780  NTheta++;
1781  }
1782 
1783  if (dThetaTmp - dThetaWrapped > M_PI) {
1784  NTheta--;
1785  }
1786 
1787  // save new wrapped angle
1788  dThetaWrapped = dThetaTmp;
1789 
1790  // compute new unwrapped angle
1791  dTheta = 2*M_PI*NTheta + dThetaWrapped;
1792 }
1793 
1794 
1795 /* Contributo al file di restart */
1796 std::ostream& PlaneRotationJoint::Restart(std::ostream& out) const
1797 {
1798  Joint::Restart(out) << ", revolute hinge, "
1799  << pNode1->GetLabel()
1800  << ", hinge, reference, node, 1, ", (R1h.GetVec(1)).Write(out, ", ")
1801  << ", 2, ", (R1h.GetVec(2)).Write(out, ", ") << ", "
1802  << pNode2->GetLabel()
1803  << ", hinge, reference, node, 1, ", (R2h.GetVec(1)).Write(out, ", ")
1804  << ", 2, ", (R2h.GetVec(2)).Write(out, ", ") << ';' << std::endl;
1805 
1806  return out;
1807 }
1808 
1809 
1810 /* Assemblaggio jacobiano */
1811 VariableSubMatrixHandler&
1812 PlaneRotationJoint::AssJac(VariableSubMatrixHandler& WorkMat,
1813  doublereal dCoef,
1814  const VectorHandler& /* XCurr */ ,
1815  const VectorHandler& /* XPrimeCurr */ )
1816 {
1817  DEBUGCOUT("Entering PlaneRotationJoint::AssJac()" << std::endl);
1818 
1819  /* Setta la sottomatrice come piena (e' un po' dispersivo, ma lo jacobiano
1820  * e' complicato */
1821  FullSubMatrixHandler& WM = WorkMat.SetFull();
1822 
1823  /* Ridimensiona la sottomatrice in base alle esigenze */
1824  integer iNumRows = 0;
1825  integer iNumCols = 0;
1826  WorkSpaceDim(&iNumRows, &iNumCols);
1827  WM.ResizeReset(iNumRows, iNumCols);
1828 
1829  /* Recupera gli indici delle varie incognite */
1830  integer iNode1FirstPosIndex = pNode1->iGetFirstPositionIndex()+3;
1831  integer iNode1FirstMomIndex = pNode1->iGetFirstMomentumIndex()+3;
1832  integer iNode2FirstPosIndex = pNode2->iGetFirstPositionIndex()+3;
1833  integer iNode2FirstMomIndex = pNode2->iGetFirstMomentumIndex()+3;
1834  integer iFirstReactionIndex = iGetFirstIndex();
1835 
1836  /* Setta gli indici delle equazioni */
1837  for (int iCnt = 1; iCnt <= 3; iCnt++) {
1838  WM.PutRowIndex(iCnt, iNode1FirstMomIndex+iCnt);
1839  WM.PutColIndex(iCnt, iNode1FirstPosIndex+iCnt);
1840  WM.PutRowIndex(3+iCnt, iNode2FirstMomIndex+iCnt);
1841  WM.PutColIndex(3+iCnt, iNode2FirstPosIndex+iCnt);
1842  }
1843 
1844  for (int iCnt = 1; iCnt <= 2; iCnt++) {
1845  WM.PutRowIndex(6+iCnt, iFirstReactionIndex+iCnt);
1846  WM.PutColIndex(6+iCnt, iFirstReactionIndex+iCnt);
1847  }
1848 
1849  /* Recupera i dati che servono */
1850  const Mat3x3& R1(pNode1->GetRRef());
1851  const Mat3x3& R2(pNode2->GetRRef());
1852  Mat3x3 R1hTmp(R1*R1h);
1853  Mat3x3 R2hTmp(R2*R2h);
1854 
1855  /* Suppongo che le reazioni F, M siano gia' state aggiornate da AssRes;
1856  * ricordo che la forza F e' nel sistema globale, mentre la coppia M
1857  * e' nel sistema locale ed il terzo termine, M(3), e' nullo in quanto
1858  * diretto come l'asse attorno al quale la rotazione e' consentita */
1859 
1860 
1861  /*
1862  * La cerniera piana ha le prime 3 equazioni uguali alla cerniera sferica;
1863  * inoltre ha due equazioni che affermano la coincidenza dell'asse 3 del
1864  * riferimento solidale con la cerniera visto dai due nodi.
1865  *
1866  * (R1 * R1h * e1)^T * (R2 * R2h * e3) = 0
1867  * (R1 * R1h * e2)^T * (R2 * R2h * e3) = 0
1868  *
1869  * A queste equazioni corrisponde una reazione di coppia agente attorno
1870  * agli assi 1 e 2 del riferimento della cerniera. La coppia attorno
1871  * all'asse 3 e' nulla per definizione. Quindi la coppia nel sistema
1872  * globale e':
1873  * -R1 * R1h * M per il nodo 1,
1874  * R2 * R2h * M per il nodo 2
1875  *
1876  *
1877  * xa ga xb gb F M
1878  * Qa | 0 0 0 0 I 0 | | xa | | -F |
1879  * Ga | 0 c*(F/\da/\-(Sa*M)/\) 0 0 da/\ Sa | | ga | | -da/\F-Sa*M |
1880  * Qb | 0 0 0 0 -I 0 | | xb | = | F |
1881  * Gb | 0 0 0 -c*(F/\db/\-(Sb*M)/\) -db/\ -Sb | | gb | | db/\F+Sb*M |
1882  * F | -c*I c*da/\ c*I -c*db/\ 0 0 | | F | | xa+da-xb-db |
1883  * M1 | 0 c*Tb1^T*Ta3/\ 0 c*Ta3^T*Tb1/\ 0 0 | | M | | Sb^T*Ta3 |
1884  * M2 | 0 c*Tb2^T*Ta3/\ 0 c*Ta3^T*Tb2/\ 0 0 |
1885  *
1886  * con da = R1*d01, db = R2*d02, c = dCoef,
1887  * Sa = R1*R1h*[e1,e2], Sb = R2*R2h*[e1, e2],
1888  * Ta3 = R1*R1h*e3, Tb1 = R2*R2h*e1, Tb2 = R2*R2h*e2
1889  */
1890 
1891  /* Moltiplica il momento per il coefficiente del metodo */
1892  Vec3 MTmp = M*dCoef;
1893 
1894  Vec3 e3a(R1hTmp.GetVec(3));
1895  Vec3 e1b(R2hTmp.GetVec(1));
1896  Vec3 e2b(R2hTmp.GetVec(2));
1897  MTmp = e2b*MTmp.dGet(1)-e1b*MTmp.dGet(2);
1898 
1899  Mat3x3 MWedgee3aWedge(MatCrossCross, MTmp, e3a);
1900  Mat3x3 e3aWedgeMWedge(MatCrossCross, e3a, MTmp);
1901 
1902  WM.Sub(1, 1, MWedgee3aWedge);
1903  WM.Add(1, 4, e3aWedgeMWedge);
1904 
1905  WM.Add(4, 1, MWedgee3aWedge);
1906  WM.Sub(4, 4, e3aWedgeMWedge);
1907 
1908  /* Contributo del momento alle equazioni di equilibrio dei nodi */
1909  Vec3 Tmp1(e2b.Cross(e3a));
1910  Vec3 Tmp2(e3a.Cross(e1b));
1911 
1912  for (int iCnt = 1; iCnt <= 3; iCnt++) {
1913  doublereal d = Tmp1(iCnt);
1914  WM.PutCoef(iCnt, 7, d);
1915  WM.PutCoef(3+iCnt, 7, -d);
1916  d = Tmp2.dGet(iCnt);
1917  WM.PutCoef(iCnt, 8, d);
1918  WM.PutCoef(3+iCnt, 8, -d);
1919  }
1920 
1921  /* Modifica: divido le equazioni di vincolo per dCoef */
1922 
1923  /* Equazione di vincolo del momento
1924  *
1925  * Attenzione: bisogna scrivere il vettore trasposto
1926  * (Sb[1]^T*(Sa[3]/\))*dCoef
1927  * Questo pero' e' uguale a:
1928  * (-Sa[3]/\*Sb[1])^T*dCoef,
1929  * che puo' essere ulteriormente semplificato:
1930  * (Sa[3].Cross(Sb[1])*(-dCoef))^T;
1931  */
1932 
1933  for (int iCnt = 1; iCnt <= 3; iCnt++) {
1934  doublereal d = Tmp1.dGet(iCnt);
1935  WM.PutCoef(7, iCnt, d);
1936  WM.PutCoef(7, 3+iCnt, -d);
1937  d = Tmp2.dGet(iCnt);
1938  WM.PutCoef(8, iCnt, -d);
1939  WM.PutCoef(8, 3+iCnt, d);
1940  }
1941 
1942  return WorkMat;
1943 }
1944 
1945 
1946 /* Assemblaggio residuo */
1947 SubVectorHandler& PlaneRotationJoint::AssRes(SubVectorHandler& WorkVec,
1948  doublereal dCoef,
1949  const VectorHandler& XCurr,
1950  const VectorHandler& /* XPrimeCurr */ )
1951 {
1952  DEBUGCOUT("Entering PlaneRotationJoint::AssRes()" << std::endl);
1953 
1954  /* Dimensiona e resetta la matrice di lavoro */
1955  integer iNumRows = 0;
1956  integer iNumCols = 0;
1957  WorkSpaceDim(&iNumRows, &iNumCols);
1958  WorkVec.ResizeReset(iNumRows);
1959 
1960  /* Indici */
1961  integer iNode1FirstMomIndex = pNode1->iGetFirstMomentumIndex()+3;
1962  integer iNode2FirstMomIndex = pNode2->iGetFirstMomentumIndex()+3;
1963  integer iFirstReactionIndex = iGetFirstIndex();
1964 
1965  /* Indici dei nodi */
1966  for (int iCnt = 1; iCnt <= 3; iCnt++) {
1967  WorkVec.PutRowIndex(iCnt, iNode1FirstMomIndex+iCnt);
1968  WorkVec.PutRowIndex(3+iCnt, iNode2FirstMomIndex+iCnt);
1969  }
1970 
1971  /* Indici del vincolo */
1972  for (int iCnt = 1; iCnt <= 2; iCnt++) {
1973  WorkVec.PutRowIndex(6+iCnt, iFirstReactionIndex+iCnt);
1974  }
1975 
1976  /* Aggiorna i dati propri */
1977  M = Vec3(XCurr(iFirstReactionIndex+1),
1978  XCurr(iFirstReactionIndex+2),
1979  0.);
1980 
1981  /*
1982  * FIXME: provare a mettere "modificatori" di forza/momento sui gdl
1983  * residui: attrito, rigidezze e smorzamenti, ecc.
1984  */
1985 
1986  /* Recupera i dati */
1987  const Mat3x3& R1(pNode1->GetRCurr());
1988  const Mat3x3& R2(pNode2->GetRCurr());
1989 
1990  /* Costruisce i dati propri nella configurazione corrente */
1991  Mat3x3 R1hTmp(R1*R1h);
1992  Mat3x3 R2hTmp(R2*R2h);
1993 
1994  Vec3 e3a(R1hTmp.GetVec(3));
1995  Vec3 e1b(R2hTmp.GetVec(1));
1996  Vec3 e2b(R2hTmp.GetVec(2));
1997 
1998  Vec3 MTmp(e2b.Cross(e3a)*M.dGet(1)+e3a.Cross(e1b)*M.dGet(2));
1999 
2000  /* Equazioni di equilibrio, nodo 1 */
2001  WorkVec.Sub(1, MTmp); /* Sfrutto F/\d = -d/\F */
2002 
2003  /* Equazioni di equilibrio, nodo 2 */
2004  WorkVec.Add(4, MTmp);
2005 
2006  /* Modifica: divido le equazioni di vincolo per dCoef */
2007  ASSERT(dCoef != 0.);
2008 
2009  /* Equazioni di vincolo di rotazione */
2010  Vec3 Tmp = R1hTmp.GetVec(3);
2011  WorkVec.PutCoef(7, Tmp.Dot(R2hTmp.GetVec(2))/dCoef);
2012  WorkVec.PutCoef(8, Tmp.Dot(R2hTmp.GetVec(1))/dCoef);
2013 
2014  return WorkVec;
2015 }
2016 
2017 DofOrder::Order
2018 PlaneRotationJoint::GetEqType(unsigned int i) const
2019 {
2020  ASSERTMSGBREAK(i < iGetNumDof(),
2021  "INDEX ERROR in PlaneRotationJoint::GetEqType");
2022  return DofOrder::ALGEBRAIC;
2023 }
2024 
2025 
2026 void
2027 PlaneRotationJoint::OutputPrepare(OutputHandler& OH)
2028 {
2029  if (bToBeOutput()) {
2030 #ifdef USE_NETCDF
2031  if (OH.UseNetCDF(OutputHandler::JOINTS)) {
2032  std::string name;
2033  OutputPrepare_int("revolute rotation", OH, name);
2034 
2035  Var_Phi = OH.CreateRotationVar(name, "", od, "global");
2036 
2037  Var_Omega = OH.CreateVar<Vec3>(name + "Omega", "radian/s",
2038  "local relative angular velocity (x, y, z)");
2039  }
2040 #endif // USE_NETCDF
2041  }
2042 }
2043 
2044 /* Output (da mettere a punto) */
2045 void PlaneRotationJoint::Output(OutputHandler& OH) const
2046 {
2047  if (bToBeOutput()) {
2048  Mat3x3 R2Tmp(pNode2->GetRCurr()*R2h);
2049  Mat3x3 RTmp((pNode1->GetRCurr()*R1h).MulTM(R2Tmp));
2050  Vec3 E;
2051  switch (od) {
2052  case EULER_123:
2053  E = MatR2EulerAngles123(RTmp)*dRaDegr;
2054  break;
2055 
2056  case EULER_313:
2057  E = MatR2EulerAngles313(RTmp)*dRaDegr;
2058  break;
2059 
2060  case EULER_321:
2061  E = MatR2EulerAngles321(RTmp)*dRaDegr;
2062  break;
2063 
2064  case ORIENTATION_VECTOR:
2065  E = RotManip::VecRot(RTmp);
2066  break;
2067 
2068  case ORIENTATION_MATRIX:
2069  break;
2070 
2071  default:
2072  /* impossible */
2073  break;
2074  }
2075  Vec3 OmegaTmp(R2Tmp.MulTV(pNode2->GetWCurr()-pNode1->GetWCurr()));
2076 
2077 #ifdef USE_NETCDF
2078  if (OH.UseNetCDF(OutputHandler::JOINTS)) {
2079  Var_F_local->put_rec(Zero3.pGetVec(), OH.GetCurrentStep());
2080  Var_M_local->put_rec(M.pGetVec(), OH.GetCurrentStep());
2081  Var_F_global->put_rec(Zero3.pGetVec(), OH.GetCurrentStep());
2082  Var_M_global->put_rec((R2Tmp*M).pGetVec(), OH.GetCurrentStep());
2083  switch (od) {
2084  case EULER_123:
2085  case EULER_313:
2086  case EULER_321:
2087  case ORIENTATION_VECTOR:
2088  Var_Phi->put_rec(E.pGetVec(), OH.GetCurrentStep());
2089  break;
2090 
2091  case ORIENTATION_MATRIX:
2092  Var_Phi->put_rec(RTmp.pGetMat(), OH.GetCurrentStep());
2093  break;
2094 
2095  default:
2096  /* impossible */
2097  break;
2098  }
2099 
2100  Var_Omega->put_rec(OmegaTmp.pGetVec(), OH.GetCurrentStep());
2101  }
2102 #endif // USE_NETCDF
2103  if (OH.UseText(OutputHandler::JOINTS)) {
2104  Joint::Output(OH.Joints(), "PlaneRotation", GetLabel(),
2105  Zero3, M, Zero3, R2Tmp*M)
2106  << " ";
2107 
2108  switch (od) {
2109  case EULER_123:
2110  case EULER_313:
2111  case EULER_321:
2112  case ORIENTATION_VECTOR:
2113  OH.Joints() << E;
2114  break;
2115 
2116  case ORIENTATION_MATRIX:
2117  OH.Joints() << RTmp;
2118  break;
2119 
2120  default:
2121  /* impossible */
2122  break;
2123  }
2124 
2125  OH.Joints() << " " << OmegaTmp << std::endl;
2126  }
2127  }
2128 }
2129 
2130 
2131 /* Contributo allo jacobiano durante l'assemblaggio iniziale */
2132 VariableSubMatrixHandler&
2133 PlaneRotationJoint::InitialAssJac(VariableSubMatrixHandler& WorkMat,
2134  const VectorHandler& XCurr)
2135 {
2136  /* Per ora usa la matrice piena; eventualmente si puo'
2137  * passare a quella sparsa quando si ottimizza */
2138  FullSubMatrixHandler& WM = WorkMat.SetFull();
2139 
2140  /* Dimensiona e resetta la matrice di lavoro */
2141  integer iNumRows = 0;
2142  integer iNumCols = 0;
2143  InitialWorkSpaceDim(&iNumRows, &iNumCols);
2144  WM.ResizeReset(iNumRows, iNumCols);
2145 
2146  /* Equazioni: vedi joints.dvi */
2147 
2148  /* equazioni ed incognite
2149  * F1 Delta_x1 0+1 = 1
2150  * M1 Delta_g1 3+1 = 4
2151  * FP1 Delta_xP1 6+1 = 7
2152  * MP1 Delta_w1 9+1 = 10
2153  * F2 Delta_x2 12+1 = 13
2154  * M2 Delta_g2 15+1 = 16
2155  * FP2 Delta_xP2 18+1 = 19
2156  * MP2 Delta_w2 21+1 = 22
2157  * vincolo spostamento Delta_F 24+1 = 25
2158  * vincolo rotazione Delta_M 27+1 = 28
2159  * derivata vincolo spostamento Delta_FP 29+1 = 30
2160  * derivata vincolo rotazione Delta_MP 32+1 = 33
2161  */
2162 
2163 
2164  /* Indici */
2165  integer iNode1FirstPosIndex = pNode1->iGetFirstPositionIndex()+3;
2166  integer iNode1FirstVelIndex = iNode1FirstPosIndex+6+3;
2167  integer iNode2FirstPosIndex = pNode2->iGetFirstPositionIndex()+3;
2168  integer iNode2FirstVelIndex = iNode2FirstPosIndex+6+3;
2169  integer iFirstReactionIndex = iGetFirstIndex();
2170  integer iReactionPrimeIndex = iFirstReactionIndex+2;
2171 
2172  /* Nota: le reazioni vincolari sono:
2173  * Forza, 3 incognite, riferimento globale,
2174  * Momento, 2 incognite, riferimento locale
2175  */
2176 
2177  /* Setta gli indici dei nodi */
2178  for (int iCnt = 1; iCnt <= 3; iCnt++) {
2179  WM.PutRowIndex(iCnt, iNode1FirstPosIndex+iCnt);
2180  WM.PutColIndex(iCnt, iNode1FirstPosIndex+iCnt);
2181  WM.PutRowIndex(3+iCnt, iNode1FirstVelIndex+iCnt);
2182  WM.PutColIndex(3+iCnt, iNode1FirstVelIndex+iCnt);
2183  WM.PutRowIndex(6+iCnt, iNode2FirstPosIndex+iCnt);
2184  WM.PutColIndex(6+iCnt, iNode2FirstPosIndex+iCnt);
2185  WM.PutRowIndex(9+iCnt, iNode2FirstVelIndex+iCnt);
2186  WM.PutColIndex(9+iCnt, iNode2FirstVelIndex+iCnt);
2187  }
2188 
2189  /* Setta gli indici delle reazioni */
2190  for (int iCnt = 1; iCnt <= 4; iCnt++) {
2191  WM.PutRowIndex(12+iCnt, iFirstReactionIndex+iCnt);
2192  WM.PutColIndex(12+iCnt, iFirstReactionIndex+iCnt);
2193  }
2194 
2195 
2196  /* Recupera i dati */
2197  const Mat3x3& R1(pNode1->GetRRef());
2198  const Mat3x3& R2(pNode2->GetRRef());
2199  const Vec3& Omega1(pNode1->GetWRef());
2200  const Vec3& Omega2(pNode2->GetWRef());
2201 
2202  /* F ed M sono gia' state aggiornate da InitialAssRes */
2203  Vec3 MPrime(XCurr(iReactionPrimeIndex+1),
2204  XCurr(iReactionPrimeIndex+2),
2205  0.);
2206 
2207  /* Matrici identita' */
2208  for (int iCnt = 1; iCnt <= 3; iCnt++) {
2209  /* Contributo di forza all'equazione della forza, nodo 1 */
2210  WM.PutCoef(iCnt, 12+iCnt, 1.);
2211 
2212  /* Contrib. di der. di forza all'eq. della der. della forza, nodo 1 */
2213  WM.PutCoef(3+iCnt, 14+iCnt, 1.);
2214 
2215  /* Contributo di forza all'equazione della forza, nodo 2 */
2216  WM.PutCoef(6+iCnt, 12+iCnt, -1.);
2217 
2218  /* Contrib. di der. di forza all'eq. della der. della forza, nodo 2 */
2219  WM.PutCoef(9+iCnt, 14+iCnt, -1.);
2220  }
2221 
2222  /* Matrici di rotazione dai nodi alla cerniera nel sistema globale */
2223  Mat3x3 R1hTmp(R1*R1h);
2224  Mat3x3 R2hTmp(R2*R2h);
2225 
2226  Vec3 e3a(R1hTmp.GetVec(3));
2227  Vec3 e1b(R2hTmp.GetVec(1));
2228  Vec3 e2b(R2hTmp.GetVec(2));
2229 
2230  /* Ruota il momento e la sua derivata con le matrici della cerniera
2231  * rispetto ai nodi */
2232  Vec3 MTmp(e2b*M.dGet(1)-e1b*M.dGet(2));
2233  Vec3 MPrimeTmp(e2b*MPrime.dGet(1)-e1b*MPrime.dGet(2));
2234 
2235  Mat3x3 MDeltag1((Mat3x3(MatCross, Omega2.Cross(MTmp) + MPrimeTmp)
2236  + Mat3x3(MatCrossCross, MTmp, Omega1))*Mat3x3(MatCross, e3a));
2237  Mat3x3 MDeltag2(Mat3x3(MatCrossCross, Omega1.Cross(e3a), MTmp)
2238  + Mat3x3(MatCrossCross, e3a, MPrimeTmp)
2239  + e3a.Cross(Mat3x3(MatCrossCross, Omega2, MTmp)));
2240 
2241  /* Vettori temporanei */
2242  Vec3 Tmp1(e2b.Cross(e3a));
2243  Vec3 Tmp2(e3a.Cross(e1b));
2244 
2245  /* Prodotto vettore tra il versore 3 della cerniera secondo il nodo 1
2246  * ed il versore 1 della cerniera secondo il nodo 2. A convergenza
2247  * devono essere ortogonali, quindi il loro prodotto vettore deve essere
2248  * unitario */
2249 
2250  /* Error handling: il programma si ferma, pero' segnala dov'e' l'errore */
2251  if (Tmp1.Dot() <= std::numeric_limits<doublereal>::epsilon() || Tmp2.Dot() <= std::numeric_limits<doublereal>::epsilon()) {
2252  silent_cerr("PlaneRotationJoint(" << GetLabel() << "): "
2253  "first and second node hinge axes are (nearly) orthogonal"
2254  << std::endl);
2255  throw ErrNullNorm(MBDYN_EXCEPT_ARGS);
2256  }
2257 
2258  Vec3 TmpPrime1(e2b.Cross(Omega1.Cross(e3a))-e3a.Cross(Omega2.Cross(e2b)));
2259  Vec3 TmpPrime2(e3a.Cross(Omega2.Cross(e1b))-e1b.Cross(Omega1.Cross(e3a)));
2260 
2261  /* Equazione di momento, nodo 1 */
2262  WM.Sub(4, 4, Mat3x3(MatCrossCross, MTmp, e3a));
2263  WM.Add(4, 16, Mat3x3(MatCrossCross, e3a, MTmp));
2264  for (int iCnt = 1; iCnt <= 3; iCnt++) {
2265  WM.PutCoef(iCnt, 13, Tmp1.dGet(iCnt));
2266  WM.PutCoef(iCnt, 14, Tmp2.dGet(iCnt));
2267  }
2268 
2269  /* Equazione di momento, nodo 2 */
2270  WM.Add(7, 1, Mat3x3(MatCrossCross, MTmp, e3a));
2271  WM.Sub(7, 7, Mat3x3(MatCrossCross, e3a, MTmp));
2272  for (int iCnt = 1; iCnt <= 3; iCnt++) {
2273  WM.PutCoef(6+iCnt, 13, -Tmp1(iCnt));
2274  WM.PutCoef(6+iCnt, 14, -Tmp2(iCnt));
2275  }
2276 
2277  /* Derivata dell'equazione di momento, nodo 1 */
2278  WM.Sub(4, 1, MDeltag1);
2279  WM.Sub(4, 4, Mat3x3(MatCrossCross, MTmp, e3a));
2280  WM.Add(4, 7, MDeltag2);
2281  WM.Add(4, 10, Mat3x3(MatCrossCross, e3a, MTmp));
2282 
2283  for (int iCnt = 1; iCnt <= 3; iCnt++) {
2284  WM.PutCoef(3+iCnt, 13, TmpPrime1(iCnt));
2285  WM.PutCoef(3+iCnt, 14, TmpPrime2(iCnt));
2286  WM.PutCoef(3+iCnt, 15, Tmp1(iCnt));
2287  WM.PutCoef(3+iCnt, 16, Tmp2(iCnt));
2288  }
2289 
2290  /* Derivata dell'equazione di momento, nodo 2 */
2291  WM.Add(10, 1, MDeltag1);
2292  WM.Add(10, 4, Mat3x3(MatCrossCross, MTmp, e3a));
2293  WM.Sub(10, 7, MDeltag2);
2294  WM.Sub(10, 10, Mat3x3(MatCrossCross, e3a, MTmp));
2295 
2296  for (int iCnt = 1; iCnt <= 3; iCnt++) {
2297  WM.PutCoef(9+iCnt, 13, -TmpPrime1(iCnt));
2298  WM.PutCoef(9+iCnt, 14, -TmpPrime2(iCnt));
2299  WM.PutCoef(9+iCnt, 15, -Tmp1(iCnt));
2300  WM.PutCoef(9+iCnt, 16, -Tmp2(iCnt));
2301  }
2302 
2303  /* Equazioni di vincolo di rotazione: e1b~e3a, e2b~e3a */
2304 
2305  for (int iCnt = 1; iCnt <= 3; iCnt++) {
2306  doublereal d = Tmp1(iCnt);
2307  WM.PutCoef(13, iCnt, d);
2308  WM.PutCoef(13, 6+iCnt, -d);
2309 
2310  /* Queste sono per la derivata dell'equazione, sono qui solo per
2311  * ottimizzazione */
2312  WM.PutCoef(15, 3+iCnt, d);
2313  WM.PutCoef(15, 9+iCnt, -d);
2314 
2315  d = Tmp2(iCnt);
2316  WM.PutCoef(14, iCnt, -d);
2317  WM.PutCoef(14, 6+iCnt, d);
2318 
2319  /* Queste sono per la derivata dell'equazione, sono qui solo per
2320  * ottimizzazione */
2321  WM.PutCoef(16, 3+iCnt, -d);
2322  WM.PutCoef(16, 9+iCnt, d);
2323  }
2324 
2325  /* Derivate delle equazioni di vincolo di rotazione: e1b~e3a, e2b~e3a */
2326  Vec3 O1mO2(Omega1 - Omega2);
2327  TmpPrime1 = e3a.Cross(O1mO2.Cross(e2b));
2328  TmpPrime2 = e2b.Cross(e3a.Cross(O1mO2));
2329  for (int iCnt = 1; iCnt <= 3; iCnt++) {
2330  WM.PutCoef(15, iCnt, TmpPrime1(iCnt));
2331  WM.PutCoef(15, 6+iCnt, TmpPrime2(iCnt));
2332  }
2333 
2334  TmpPrime1 = e3a.Cross(O1mO2.Cross(e1b));
2335  TmpPrime2 = e1b.Cross(e3a.Cross(O1mO2));
2336  for (int iCnt = 1; iCnt <= 3; iCnt++) {
2337  WM.PutCoef(16, iCnt, TmpPrime1(iCnt));
2338  WM.PutCoef(16, 6+iCnt, TmpPrime2(iCnt));
2339  }
2340 
2341  return WorkMat;
2342 }
2343 
2344 
2345 /* Contributo al residuo durante l'assemblaggio iniziale */
2346 SubVectorHandler&
2347 PlaneRotationJoint::InitialAssRes(SubVectorHandler& WorkVec,
2348  const VectorHandler& XCurr)
2349 {
2350  DEBUGCOUT("Entering PlaneRotationJoint::InitialAssRes()" << std::endl);
2351 
2352  /* Dimensiona e resetta la matrice di lavoro */
2353  integer iNumRows = 0;
2354  integer iNumCols = 0;
2355  InitialWorkSpaceDim(&iNumRows, &iNumCols);
2356  WorkVec.ResizeReset(iNumRows);
2357 
2358  /* Indici */
2359  integer iNode1FirstPosIndex = pNode1->iGetFirstPositionIndex()+3;
2360  integer iNode1FirstVelIndex = iNode1FirstPosIndex+6+3;
2361  integer iNode2FirstPosIndex = pNode2->iGetFirstPositionIndex()+3;
2362  integer iNode2FirstVelIndex = iNode2FirstPosIndex+6+3;
2363  integer iFirstReactionIndex = iGetFirstIndex();
2364  integer iReactionPrimeIndex = iFirstReactionIndex+2;
2365 
2366  /* Setta gli indici */
2367  for (int iCnt = 1; iCnt <= 3; iCnt++) {
2368  WorkVec.PutRowIndex(iCnt, iNode1FirstPosIndex+iCnt);
2369  WorkVec.PutRowIndex(3+iCnt, iNode1FirstVelIndex+iCnt);
2370  WorkVec.PutRowIndex(6+iCnt, iNode2FirstPosIndex+iCnt);
2371  WorkVec.PutRowIndex(9+iCnt, iNode2FirstVelIndex+iCnt);
2372  }
2373 
2374  for (int iCnt = 1; iCnt <= 4; iCnt++) {
2375  WorkVec.PutRowIndex(12+iCnt, iFirstReactionIndex+iCnt);
2376  }
2377 
2378  /* Recupera i dati */
2379  const Mat3x3& R1(pNode1->GetRCurr());
2380  const Mat3x3& R2(pNode2->GetRCurr());
2381  const Vec3& Omega1(pNode1->GetWCurr());
2382  const Vec3& Omega2(pNode2->GetWCurr());
2383 
2384  /* Aggiorna F ed M, che restano anche per InitialAssJac */
2385  M = Vec3(XCurr(iFirstReactionIndex+1),
2386  XCurr(iFirstReactionIndex+2),
2387  0.);
2388  Vec3 MPrime(XCurr(iReactionPrimeIndex+1),
2389  XCurr(iReactionPrimeIndex+2),
2390  0.);
2391 
2392  /* Distanza nel sistema globale */
2393  Mat3x3 R1hTmp(R1*R1h);
2394  Mat3x3 R2hTmp(R2*R2h);
2395 
2396  Vec3 e3a(R1hTmp.GetVec(3));
2397  Vec3 e1b(R2hTmp.GetVec(1));
2398  Vec3 e2b(R2hTmp.GetVec(2));
2399 
2400  /* Ruota il momento e la sua derivata con le matrici della cerniera
2401  * rispetto ai nodi */
2402  Vec3 MTmp(e2b*M.dGet(1)-e1b*M.dGet(2));
2403  Vec3 MPrimeTmp(e3a.Cross(MTmp.Cross(Omega2))+MTmp.Cross(Omega1.Cross(e3a))+
2404  e2b.Cross(e3a)*MPrime.dGet(1)+e3a.Cross(e1b)*MPrime.dGet(2));
2405 
2406  /* Equazioni di equilibrio, nodo 1 */
2407  WorkVec.Sub(1, MTmp.Cross(e3a));
2408 
2409  /* Derivate delle equazioni di equilibrio, nodo 1 */
2410  WorkVec.Sub(4, MPrimeTmp);
2411 
2412  /* Equazioni di equilibrio, nodo 2 */
2413  WorkVec.Add(7, MTmp.Cross(e3a));
2414 
2415  /* Derivate delle equazioni di equilibrio, nodo 2 */
2416  WorkVec.Add(10, MPrimeTmp);
2417 
2418  /* Equazioni di vincolo di rotazione */
2419  WorkVec.PutCoef(13, e2b.Dot(e3a));
2420  WorkVec.PutCoef(14, e1b.Dot(e3a));
2421 
2422  /* Derivate delle equazioni di vincolo di rotazione: e1b~e3a, e2b~e3a */
2423  Vec3 Tmp((Omega1-Omega2).Cross(e3a));
2424  WorkVec.PutCoef(15, e2b.Dot(Tmp));
2425  WorkVec.PutCoef(16, e1b.Dot(Tmp));
2426 
2427  return WorkVec;
2428 }
2429 
2430 
2431 unsigned int
2432 PlaneRotationJoint::iGetNumPrivData(void) const
2433 {
2434  return 5;
2435 }
2436 
2437 unsigned int
2438 PlaneRotationJoint::iGetPrivDataIdx(const char *s) const
2439 {
2440  ASSERT(s != NULL);
2441 
2442  unsigned int idx = 0;
2443 
2444  switch (s[0]) {
2445  case 'w':
2446  idx++;
2447  case 'r':
2448  idx++;
2449  if (s[1] == 'z') {
2450  return idx;
2451  }
2452  break;
2453 
2454  case 'M':
2455  idx += 2;
2456 
2457  switch (s[1]) {
2458  case 'x':
2459  return idx + 1;
2460 
2461  case 'y':
2462  return idx + 2;
2463 
2464  case 'z':
2465  return idx + 3;
2466  }
2467  }
2468 
2469  return 0;
2470 }
2471 
2472 doublereal PlaneRotationJoint::dGetPrivData(unsigned int i) const
2473 {
2474  ASSERT(i >= 1 && i <= iGetNumPrivData());
2475 
2476  switch (i) {
2477  case 1: {
2478  Vec3 v(RotManip::VecRot((pNode1->GetRCurr()*R1h).MulTM(pNode2->GetRCurr()*R2h)));
2479  doublereal dThetaTmp(v(3));
2480 
2481  int n = 0;
2482 
2483  if (dThetaTmp - dThetaWrapped < -M_PI) {
2484  n++;
2485  }
2486 
2487  if (dThetaTmp - dThetaWrapped > M_PI) {
2488  n--;
2489  }
2490 
2491  return 2*M_PI*(NTheta + n) + dThetaTmp;
2492  }
2493 
2494  case 2: {
2495  Mat3x3 R2Tmp((pNode2->GetRCurr()*R2h));
2496  Vec3 v(R2Tmp.MulTV(pNode2->GetWCurr()-pNode1->GetWCurr()));
2497 
2498  return v(3);
2499  }
2500 
2501  case 3:
2502  case 4:
2503  case 5:
2504  return M(i - 2);
2505  }
2506 
2507  silent_cerr("PlaneRotationJoint(" << GetLabel() << "): "
2508  "illegal private data " << i << std::endl);
2509  throw ErrGeneric(MBDYN_EXCEPT_ARGS);
2510 }
2511 
2512 /* PlaneRotationJoint - end */
2513 
2514 
2515 /* AxialRotationJoint - begin */
2516 
2517 const unsigned int AxialRotationJoint::NumSelfDof(6);
2518 const unsigned int AxialRotationJoint::NumDof(18);
2519 
2520 /* Costruttore non banale */
2521 AxialRotationJoint::AxialRotationJoint(unsigned int uL, const DofOwner* pDO,
2522  const StructNode* pN1,
2523  const StructNode* pN2,
2524  const Vec3& dTmp1, const Vec3& dTmp2,
2525  const Mat3x3& R1hTmp,
2526  const Mat3x3& R2hTmp,
2527  const DriveCaller* pDC,
2528  const OrientationDescription& od,
2529  flag fOut,
2530  const doublereal rr,
2531  const doublereal pref,
2532  BasicShapeCoefficient *const sh,
2533  BasicFriction *const f)
2534 : Elem(uL, fOut),
2535 Joint(uL, pDO, fOut),
2536 DriveOwner(pDC),
2537 pNode1(pN1), pNode2(pN2),
2538 d1(dTmp1), R1h(R1hTmp), d2(dTmp2), R2h(R2hTmp), F(Zero3), M(Zero3),
2539 NTheta(0), dTheta(0.), dThetaWrapped(0.),
2540 #ifdef USE_NETCDF
2541 Var_Phi(0),
2542 Var_Omega(0),
2543 //Var_MFR(0),
2544 //Var_MU(0),
2545 #endif // USE_NETCDF
2546 Sh_c(sh), fc(f), preF(pref), r(rr),
2547 od(od)
2548 {
2549  NO_OP;
2550 }
2551 
2552 
2553 /* Distruttore banale */
2554 AxialRotationJoint::~AxialRotationJoint(void)
2555 {
2556  if (Sh_c) {
2557  delete Sh_c;
2558  }
2559 
2560  if (fc) {
2561  delete fc;
2562  }
2563 }
2564 
2565 
2566 std::ostream&
2567 AxialRotationJoint::DescribeDof(std::ostream& out, const char *prefix, bool bInitial) const
2568 {
2569  integer iIndex = iGetFirstIndex();
2570 
2571  out
2572  << prefix << iIndex + 1 << "->" << iIndex + 3 << ": "
2573  "reaction forces [Fx,Fy,Fz]" << std::endl
2574  << prefix << iIndex + 4 << "->" << iIndex + 6 << ": "
2575  "reaction couples [mx,my,mz]" << std::endl;
2576 
2577  if (bInitial) {
2578  iIndex += NumSelfDof;
2579  out
2580  << prefix << iIndex + 1 << "->" << iIndex + 3 << ": "
2581  "reaction force derivatives [FPx,FPy,FPz]" << std::endl
2582  << prefix << iIndex + 4 << "->" << iIndex + 5 << ": "
2583  "reaction couple derivatives [mPx,mPy]" << std::endl;
2584  }
2585 
2586  iIndex += NumSelfDof;
2587  if (fc) {
2588  integer iFCDofs = fc->iGetNumDof();
2589  if (iFCDofs > 0) {
2590  out << prefix << iIndex + 1;
2591  if (iFCDofs > 1) {
2592  out << "->" << iIndex + iFCDofs;
2593  }
2594  out << ": friction dof(s)" << std::endl
2595  << " ", fc->DescribeDof(out, prefix, bInitial);
2596  }
2597  }
2598 
2599  return out;
2600 }
2601 
2602 void
2603 AxialRotationJoint::DescribeDof(std::vector<std::string>& desc, bool bInitial, int i) const
2604 {
2605  std::ostringstream os;
2606  os << "AxialRotationJoint(" << GetLabel() << ")";
2607 
2608  unsigned short nself = NumSelfDof;
2609  if (bInitial) {
2610  nself += NumSelfDof - 1;
2611  }
2612  if (fc && (i == -1 || i >= nself)) {
2613  fc->DescribeDof(desc, bInitial, i - nself);
2614  if (i != -1) {
2615  desc[0] = os.str() + ": " + desc[0];
2616  return;
2617  }
2618  }
2619 
2620  if (i == -1) {
2621  // move fc desc to the end
2622  unsigned short nfc = 0;
2623  if (fc) {
2624  nfc = desc.size();
2625  }
2626  desc.resize(nfc + nself);
2627  for (unsigned i = nfc; i-- > 0; ) {
2628  desc[nself + i] = os.str() + ": " + desc[nfc];
2629  }
2630 
2631  std::string name = os.str();
2632 
2633  for (unsigned i = 0; i < 3; i++) {
2634  os.str(name);
2635  os.seekp(0, std::ios_base::end);
2636  os << ": reaction force f" << xyz[i];
2637  desc[i] = os.str();
2638  }
2639 
2640  for (unsigned i = 0; i < 3; i++) {
2641  os.str(name);
2642  os.seekp(0, std::ios_base::end);
2643  os << ": reaction couple m" << xyz[i];
2644  desc[3 + i] = os.str();
2645  }
2646 
2647  if (bInitial) {
2648  for (unsigned i = 0; i < 3; i++) {
2649  os.str(name);
2650  os.seekp(0, std::ios_base::end);
2651  os << ": reaction force derivative fP" << xyz[i];
2652  desc[6 + i] = os.str();
2653  }
2654 
2655  for (unsigned i = 0; i < 2; i++) {
2656  os.str(name);
2657  os.seekp(0, std::ios_base::end);
2658  os << ": reaction couple derivative mP" << xyz[i];
2659  desc[9 + i] = os.str();
2660  }
2661  }
2662 
2663  } else {
2664  if (i < -1) {
2665  // error
2666  throw ErrGeneric(MBDYN_EXCEPT_ARGS);
2667  }
2668 
2669  if (i >= nself) {
2670  // error
2671  throw ErrGeneric(MBDYN_EXCEPT_ARGS);
2672  }
2673 
2674  desc.resize(1);
2675 
2676  switch (i) {
2677  case 0:
2678  case 1:
2679  case 2:
2680  os << ": reaction force f" << xyz[i];
2681  break;
2682 
2683  case 3:
2684  case 4:
2685  case 5:
2686  os << ": reaction couple m" << xyz[i - 3];
2687  break;
2688 
2689  case 6:
2690  case 7:
2691  case 8:
2692  os << ": reaction force derivative fP" << xyz[i - 6];
2693  break;
2694 
2695  case 9:
2696  case 10:
2697  os << ": reaction couple derivative mP" << xyz[i - 9];
2698  break;
2699  }
2700  desc[0] = os.str();
2701  }
2702 }
2703 
2704 std::ostream&
2705 AxialRotationJoint::DescribeEq(std::ostream& out, const char *prefix, bool bInitial) const
2706 {
2707  integer iIndex = iGetFirstIndex();
2708 
2709  out
2710  << prefix << iIndex + 1 << "->" << iIndex + 3 << ": "
2711  "position constraints [Px1=Px2,Py1=Py2,Pz1=Pz2]" << std::endl
2712  << prefix << iIndex + 4 << "->" << iIndex + 5 << ": "
2713  "orientation constraints [gx1=gx2,gy1=gy2]" << std::endl
2714  << prefix << iIndex + 6 << ": "
2715  "angular velocity constraint wz2-wz1=Omega]" << std::endl;
2716 
2717  if (bInitial) {
2718  iIndex += NumSelfDof;
2719  out
2720  << prefix << iIndex + 1 << "->" << iIndex + 3 << ": "
2721  "velocity constraints [vx1=vx2,vy1=vy2,vz1=vz2]" << std::endl
2722  << prefix << iIndex + 4 << "->" << iIndex + 5 << ": "
2723  "reaction couple derivatives [wx1=wx2,wy1=wy2]" << std::endl;
2724  }
2725 
2726  iIndex += NumSelfDof;
2727  if (fc) {
2728  integer iFCDofs = fc->iGetNumDof();
2729  if (iFCDofs > 0) {
2730  out << prefix << iIndex + 1;
2731  if (iFCDofs > 1) {
2732  out << "->" << iIndex + iFCDofs;
2733  }
2734  out << ": friction equation(s)" << std::endl
2735  << " ", fc->DescribeEq(out, prefix, bInitial);
2736  }
2737  }
2738 
2739  return out;
2740 }
2741 
2742 void
2743 AxialRotationJoint::DescribeEq(std::vector<std::string>& desc, bool bInitial, int i) const
2744 {
2745  std::ostringstream os;
2746  os << "AxialRotationJoint(" << GetLabel() << ")";
2747 
2748  unsigned short nself = NumSelfDof;
2749  if (bInitial) {
2750  nself += NumSelfDof - 1;
2751  }
2752  if (fc && (i == -1 || i >= nself)) {
2753  fc->DescribeEq(desc, bInitial, i - nself);
2754  if (i != -1) {
2755  desc[0] = os.str() + ": " + desc[0];
2756  return;
2757  }
2758  }
2759 
2760  if (i == -1) {
2761  // move fc desc to the end
2762  unsigned short nfc = 0;
2763  if (fc) {
2764  nfc = desc.size();
2765  }
2766  desc.resize(nfc + nself);
2767  for (unsigned i = nfc; i-- > 0; ) {
2768  desc[nself + i] = os.str() + ": " + desc[nfc];
2769  }
2770 
2771  std::string name = os.str();
2772 
2773  for (unsigned i = 0; i < 3; i++) {
2774  os.str(name);
2775  os.seekp(0, std::ios_base::end);
2776  os << ": position constraint P" << xyz[i];
2777  desc[i] = os.str();
2778  }
2779 
2780  for (unsigned i = 0; i < 2; i++) {
2781  os.str(name);
2782  os.seekp(0, std::ios_base::end);
2783  os << ": orientation constraint g" << xyz[i];
2784  desc[3 + i] = os.str();
2785  }
2786 
2787  os.str(name);
2788  os.seekp(0, std::ios_base::end);
2789  os << ": angular velocity constraint wz";
2790  desc[5] = os.str();
2791 
2792  if (bInitial) {
2793  for (unsigned i = 0; i < 3; i++) {
2794  os.str(name);
2795  os.seekp(0, std::ios_base::end);
2796  os << ": position constraint derivative v" << xyz[i];
2797  desc[6 + i] = os.str();
2798  }
2799 
2800  for (unsigned i = 0; i < 2; i++) {
2801  os.str(name);
2802  os.seekp(0, std::ios_base::end);
2803  os << ": orientation constraint derivative w" << xyz[i];
2804  desc[9 + i] = os.str();
2805  }
2806  }
2807 
2808  } else {
2809  if (i < -1) {
2810  // error
2811  throw ErrGeneric(MBDYN_EXCEPT_ARGS);
2812  }
2813 
2814  if (i >= nself) {
2815  // error
2816  throw ErrGeneric(MBDYN_EXCEPT_ARGS);
2817  }
2818 
2819  desc.resize(1);
2820 
2821  switch (i) {
2822  case 0:
2823  case 1:
2824  case 2:
2825  os << ": position constraint P" << xyz[i];
2826  break;
2827 
2828  case 3:
2829  case 4:
2830  os << ": orientation constraint g" << xyz[i - 3];
2831  break;
2832 
2833  case 5:
2834  os << ": angular velocity constraint wz";
2835  break;
2836 
2837  case 6:
2838  case 7:
2839  case 8:
2840  os << ": position constraint derivative v" << xyz[i - 6];
2841  break;
2842 
2843  case 9:
2844  case 10:
2845  os << ": orientation constraint derivative w" << xyz[i - 9];
2846  break;
2847  }
2848  desc[0] = os.str();
2849  }
2850 }
2851 
2852 void
2853 AxialRotationJoint::SetValue(DataManager *pDM,
2854  VectorHandler& X, VectorHandler& XP,
2855  SimulationEntity::Hints *ph)
2856 {
2857  if (ph) {
2858  for (unsigned i = 0; i < ph->size(); i++) {
2859  Joint::JointHint *pjh = dynamic_cast<Joint::JointHint *>((*ph)[i]);
2860 
2861  if (pjh == 0) {
2862  DriveHint *pdh = dynamic_cast<DriveHint *>((*ph)[i]);
2863  if (pdh) {
2864  DriveCaller *pDC = pdh->pCreateDrive(pDM);
2865  if (pDC == 0) {
2866  silent_cerr("AxialRotationJoint::SetValue: "
2867  "unable to create drive after hint "
2868  "#" << i << std::endl);
2869  throw ErrGeneric(MBDYN_EXCEPT_ARGS);
2870  }
2871 
2872  DriveOwner::Set(pDC);
2873  }
2874  continue;
2875  }
2876 
2877  if (dynamic_cast<Joint::OffsetHint<1> *>(pjh)) {
2878  const Mat3x3& R1(pNode1->GetRCurr());
2879  Vec3 dTmp2(pNode2->GetRCurr()*d2);
2880 
2881  d1 = R1.MulTV(pNode2->GetXCurr() + dTmp2 - pNode1->GetXCurr());
2882 
2883  } else if (dynamic_cast<Joint::OffsetHint<2> *>(pjh)) {
2884  const Mat3x3& R2(pNode2->GetRCurr());
2885  Vec3 dTmp1(pNode1->GetRCurr()*d1);
2886 
2887  d2 = R2.MulTV(pNode1->GetXCurr() + dTmp1 - pNode2->GetXCurr());
2888 
2889  } else if (dynamic_cast<Joint::HingeHint<1> *>(pjh)) {
2890  R1h = pNode1->GetRCurr().MulTM(pNode2->GetRCurr()*R2h);
2891 
2892  } else if (dynamic_cast<Joint::HingeHint<2> *>(pjh)) {
2893  R2h = pNode2->GetRCurr().MulTM(pNode1->GetRCurr()*R1h);
2894 
2895  } else if (dynamic_cast<Joint::ReactionsHint *>(pjh)) {
2896  /* TODO */
2897  }
2898  }
2899  }
2900 
2901  Mat3x3 RTmp((pNode1->GetRCurr()*R1h).MulTM(pNode2->GetRCurr()*R2h));
2902  Vec3 v(MatR2EulerAngles(RTmp));
2903 
2904  dThetaWrapped = dTheta = v.dGet(3);
2905 
2906  if (fc) {
2907  fc->SetValue(pDM, X, XP, ph, iGetFirstIndex() + NumSelfDof);
2908  }
2909 }
2910 
2911 Hint *
2912 AxialRotationJoint::ParseHint(DataManager *pDM, const char *s) const
2913 {
2914  if (strncasecmp(s, "offset{" /*}*/, STRLENOF("offset{" /*}*/)) == 0) {
2915  s += STRLENOF("offset{" /*}*/);
2916 
2917  if (strcmp(&s[1], /*{*/ "}") != 0) {
2918  return 0;
2919  }
2920 
2921  switch (s[0]) {
2922  case '1':
2923  return new Joint::OffsetHint<1>;
2924 
2925  case '2':
2926  return new Joint::OffsetHint<2>;
2927  }
2928 
2929  } else if (strncasecmp(s, "hinge{" /*}*/, STRLENOF("hinge{" /*}*/)) == 0) {
2930  s += STRLENOF("hinge{" /*}*/);
2931 
2932  if (strcmp(&s[1], /*{*/ "}") != 0) {
2933  return 0;
2934  }
2935 
2936  switch (s[0]) {
2937  case '1':
2938  return new Joint::HingeHint<1>;
2939 
2940  case '2':
2941  return new Joint::HingeHint<2>;
2942  }
2943 
2944  } else if (fc) {
2945  return fc->ParseHint(pDM, s);
2946  }
2947 
2948  return 0;
2949 }
2950 
2951 void
2952 AxialRotationJoint::AfterConvergence(const VectorHandler& X,
2953  const VectorHandler& XP)
2954 {
2955  Vec3 v(RotManip::VecRot((pNode1->GetRCurr()*R1h).MulTM(pNode2->GetRCurr()*R2h)));
2956  doublereal dThetaTmp(v(3));
2957 
2958  // unwrap
2959  if (dThetaTmp - dThetaWrapped < -M_PI) {
2960  NTheta++;
2961  }
2962 
2963  if (dThetaTmp - dThetaWrapped > M_PI) {
2964  NTheta--;
2965  }
2966 
2967  // save new wrapped angle
2968  dThetaWrapped = dThetaTmp;
2969 
2970  // compute new unwrapped angle
2971  dTheta = 2*M_PI*NTheta + dThetaWrapped;
2972 
2973  if (fc) {
2974  const Mat3x3& R1(pNode1->GetRCurr());
2975  Mat3x3 R1hTmp(R1*R1h);
2976  Vec3 e3a(R1hTmp.GetVec(3));
2977  const Vec3& Omega1(pNode1->GetWCurr());
2978  const Vec3& Omega2(pNode2->GetWCurr());
2979  //relative velocity
2980  doublereal v = (Omega1-Omega2).Dot(e3a)*r;
2981  //reaction norm
2982  doublereal modF = std::max(F.Norm(), preF);;
2983  fc->AfterConvergence(modF,v,X,XP,iGetFirstIndex()+NumSelfDof);
2984  }
2985 }
2986 
2987 
2988 /* Contributo al file di restart */
2989 std::ostream& AxialRotationJoint::Restart(std::ostream& out) const
2990 {
2991  Joint::Restart(out) << ", axial rotation, "
2992  << pNode1->GetLabel()
2993  << ", reference, node, ", d1.Write(out, ", ")
2994  << ", hinge, reference, node, 1, ", (R1h.GetVec(1)).Write(out, ", ")
2995  << ", 2, ", (R1h.GetVec(2)).Write(out, ", ") << ", "
2996  << pNode2->GetLabel()
2997  << ", reference, node, ", d2.Write(out, ", ")
2998  << ", hinge, reference, node, 1, ", (R2h.GetVec(1)).Write(out, ", ")
2999  << ", 2, ", (R2h.GetVec(2)).Write(out, ", ") << ", ";
3000 
3001  return pGetDriveCaller()->Restart(out) << ';' << std::endl;
3002 }
3003 
3004 
3005 /* Assemblaggio jacobiano */
3006 VariableSubMatrixHandler&
3007 AxialRotationJoint::AssJac(VariableSubMatrixHandler& WorkMat,
3008  doublereal dCoef,
3009  const VectorHandler& XCurr,
3010  const VectorHandler& XPrimeCurr)
3011 {
3012  DEBUGCOUT("Entering AxialRotationJoint::AssJac()" << std::endl);
3013 
3014  /* Setta la sottomatrice come piena (e' un po' dispersivo, ma lo jacobiano
3015  * e' complicato */
3016  FullSubMatrixHandler& WM = WorkMat.SetFull();
3017 
3018  /* Ridimensiona la sottomatrice in base alle esigenze */
3019  integer iNumRows = 0;
3020  integer iNumCols = 0;
3021  WorkSpaceDim(&iNumRows, &iNumCols);
3022  WM.ResizeReset(iNumRows, iNumCols);
3023 
3024  /* Recupera gli indici delle varie incognite */
3025  integer iNode1FirstPosIndex = pNode1->iGetFirstPositionIndex();
3026  integer iNode1FirstMomIndex = pNode1->iGetFirstMomentumIndex();
3027  integer iNode2FirstPosIndex = pNode2->iGetFirstPositionIndex();
3028  integer iNode2FirstMomIndex = pNode2->iGetFirstMomentumIndex();
3029  integer iFirstReactionIndex = iGetFirstIndex();
3030 
3031  /* Recupera i dati che servono */
3032  const Mat3x3& R1(pNode1->GetRRef());
3033  const Mat3x3& R2(pNode2->GetRRef());
3034  const Vec3& Omega2(pNode2->GetWRef()); /* Serve dopo */
3035  Vec3 d1Tmp(R1*d1);
3036  Vec3 d2Tmp(R2*d2);
3037  Mat3x3 R1hTmp(R1*R1h);
3038  Mat3x3 R2hTmp(R2*R2h);
3039  /* Suppongo che le reazioni F, M siano gia' state aggiornate da AssRes;
3040  * ricordo che la forza F e' nel sistema globale, mentre la coppia M
3041  * e' nel sistema locale ed il terzo termine, M(3), e' nullo in quanto
3042  * diretto come l'asse attorno al quale la rotazione e' consentita */
3043 
3044 
3045  /*
3046  * La cerniera piana ha le prime 3 equazioni uguali alla cerniera sferica;
3047  * inoltre ha due equazioni che affermano la coincidenza dell'asse 3 del
3048  * riferimento solidale con la cerniera visto dai due nodi.
3049  *
3050  * (R1 * R1h * e1)^T * (R2 * R2h * e3) = 0
3051  * (R1 * R1h * e2)^T * (R2 * R2h * e3) = 0
3052  *
3053  * A queste equazioni corrisponde una reazione di coppia agente attorno
3054  * agli assi 1 e 2 del riferimento della cerniera. La coppia attorno
3055  * all'asse 3 e' nulla per definizione. Quindi la coppia nel sistema
3056  * globale e':
3057  * -R1 * R1h * M per il nodo 1,
3058  * R2 * R2h * M per il nodo 2
3059  *
3060  *
3061  * xa ga xb gb F M
3062  * Qa | 0 0 0 0 I 0 | | xa | | -F |
3063  * Ga | 0 c*(F/\da/\-(Sa*M)/\) 0 0 da/\ Sa | | ga | | -da/\F-Sa*M |
3064  * Qb | 0 0 0 0 -I 0 | | xb | = | F |
3065  * Gb | 0 0 0 -c*(F/\db/\-(Sb*M)/\) -db/\ -Sb | | gb | | db/\F+Sb*M |
3066  * F | -c*I c*da/\ c*I -c*db/\ 0 0 | | F | | xa+da-xb-db |
3067  * M1 | 0 c*Tb1^T*Ta3/\ 0 c*Ta3^T*Tb1/\ 0 0 | | M | | Sb^T*Ta3 |
3068  * M2 | 0 c*Tb2^T*Ta3/\ 0 c*Ta3^T*Tb2/\ 0 0 |
3069  *
3070  * con da = R1*d01, db = R2*d02, c = dCoef,
3071  * Sa = R1*R1h*[e1,e2], Sb = R2*R2h*[e1, e2],
3072  * Ta3 = R1*R1h*e3, Tb1 = R2*R2h*e1, Tb2 = R2*R2h*e2
3073  */
3074 
3075  /* Setta gli indici delle equazioni */
3076  for (int iCnt = 1; iCnt <= 6; iCnt++) {
3077  WM.PutRowIndex(iCnt, iNode1FirstMomIndex+iCnt);
3078  WM.PutColIndex(iCnt, iNode1FirstPosIndex+iCnt);
3079  WM.PutRowIndex(6+iCnt, iNode2FirstMomIndex+iCnt);
3080  WM.PutColIndex(6+iCnt, iNode2FirstPosIndex+iCnt);
3081  }
3082 
3083  for (unsigned int iCnt = 1; iCnt <= iGetNumDof(); iCnt++) {
3084  WM.PutRowIndex(12+iCnt, iFirstReactionIndex+iCnt);
3085  WM.PutColIndex(12+iCnt, iFirstReactionIndex+iCnt);
3086  }
3087 
3088  /* Contributo della forza alle equazioni di equilibrio dei due nodi */
3089  for (int iCnt = 1; iCnt <= 3; iCnt++) {
3090  WM.PutCoef(iCnt, 12+iCnt, 1.);
3091  WM.PutCoef(6+iCnt, 12+iCnt, -1.);
3092  }
3093 
3094  WM.Add(4, 13, Mat3x3(MatCross, d1Tmp));
3095  WM.Sub(10, 13, Mat3x3(MatCross, d2Tmp));
3096 
3097  /* Moltiplica la forza ed il momento per il coefficiente
3098  * del metodo */
3099  Vec3 FTmp = F*dCoef;
3100  Vec3 MTmp = M*dCoef;
3101 
3102  Vec3 e3a(R1hTmp.GetVec(3));
3103  Vec3 e1b(R2hTmp.GetVec(1));
3104  Vec3 e2b(R2hTmp.GetVec(2));
3105  MTmp = e2b*MTmp.dGet(1) - e1b*MTmp.dGet(2);
3106 
3107  Mat3x3 MWedgee3aWedge(MatCrossCross, MTmp, e3a);
3108  Mat3x3 e3aWedgeMWedge(MatCrossCross, e3a, MTmp);
3109 
3110  WM.Add(4, 4, Mat3x3(MatCrossCross, FTmp, d1Tmp) - MWedgee3aWedge - Mat3x3(MatCross, e3a*M(3)));
3111  WM.Add(4, 10, e3aWedgeMWedge);
3112 
3113  WM.Add(10, 4, MWedgee3aWedge);
3114  WM.Sub(10, 10, Mat3x3(MatCrossCross, FTmp, d2Tmp) + e3aWedgeMWedge
3115  - Mat3x3(MatCross, e3a*M(3)));
3116 
3117  /* Contributo del momento alle equazioni di equilibrio dei nodi */
3118  Vec3 Tmp1(e2b.Cross(e3a));
3119  Vec3 Tmp2(e3a.Cross(e1b));
3120 
3121  for (int iCnt = 1; iCnt <= 3; iCnt++) {
3122  doublereal d = Tmp1(iCnt);
3123  WM.PutCoef(3+iCnt, 16, d);
3124  WM.PutCoef(9+iCnt, 16, -d);
3125 
3126  d = Tmp2(iCnt);
3127  WM.PutCoef(3+iCnt, 17, d);
3128  WM.PutCoef(9+iCnt, 17, -d);
3129 
3130  d = e3a(iCnt);
3131  WM.PutCoef(3+iCnt, 18, d);
3132  WM.PutCoef(9+iCnt, 18, -d);
3133  }
3134 
3135  /* Modifica: divido le equazioni di vincolo per dCoef */
3136 
3137  /* Equazioni di vincolo degli spostamenti */
3138  for (int iCnt = 1; iCnt <= 3; iCnt++) {
3139  WM.PutCoef(12+iCnt, iCnt, -1.);
3140  WM.PutCoef(12+iCnt, 6+iCnt, 1.);
3141  }
3142 
3143  WM.Add(13, 4, Mat3x3(MatCross, d1Tmp));
3144  WM.Sub(13, 10, Mat3x3(MatCross, d2Tmp));
3145 
3146  /* Equazione di vincolo del momento
3147  *
3148  * Attenzione: bisogna scrivere il vettore trasposto
3149  * (Sb[1]^T*(Sa[3]/\))*dCoef
3150  * Questo pero' e' uguale a:
3151  * (-Sa[3]/\*Sb[1])^T*dCoef,
3152  * che puo' essere ulteriormente semplificato:
3153  * (Sa[3].Cross(Sb[1])*(-dCoef))^T;
3154  */
3155 
3156  for (int iCnt = 1; iCnt <= 3; iCnt++) {
3157  doublereal d = Tmp1(iCnt);
3158  WM.PutCoef(16, 3+iCnt, d);
3159  WM.PutCoef(16, 9+iCnt, -d);
3160 
3161  d = Tmp2.dGet(iCnt);
3162  WM.PutCoef(17, 3+iCnt, -d);
3163  WM.PutCoef(17, 9+iCnt, d);
3164  }
3165 
3166  /* Questa equazione non viene divisa per dCoef */
3167 
3168  /* Equazione di imposizione della velocita' di rotazione:
3169  * (e3a+c(wb/\e3a))^T*(Delta_gPb-Delta_gPa) */
3170  Vec3 Tmp = e3a + Omega2.Cross(e3a*dCoef);
3171  for (int iCnt = 1; iCnt <= 3; iCnt++) {
3172  doublereal d = Tmp(iCnt);
3173  WM.PutCoef(18, 3+iCnt, -d);
3174  WM.PutCoef(18, 9+iCnt, d);
3175  }
3176 
3177  if (fc) {
3178  //retrive
3179  //friction coef
3180  doublereal f = fc->fc();
3181  //shape function
3182  doublereal shc = Sh_c->Sh_c();
3183  //omega and omega rif
3184  const Vec3& Omega1(pNode1->GetWCurr());
3185  const Vec3& Omega2(pNode2->GetWCurr());
3186  // const Vec3& Omega1r(pNode1->GetWRef());
3187  // const Vec3& Omega2r(pNode2->GetWRef());
3188  //compute
3189  //relative velocity
3190  doublereal v = (Omega1-Omega2).Dot(e3a)*r;
3191  //reaction norm
3192  doublereal modF = std::max(F.Norm(), preF);
3193  //reaction moment
3194  //doublereal M3 = shc*modF*f*r;
3195 
3196  ExpandableRowVector dfc;
3197  ExpandableRowVector dF;
3198  ExpandableRowVector dv;
3199  //variation of reaction force
3200  dF.ReDim(3);
3201  if ((modF == 0.) or (F.Norm() > preF)) {
3202  dF.Set(0.,1,12+1);
3203  dF.Set(0.,2,12+2);
3204  dF.Set(0.,3,12+3);
3205  } else {
3206  dF.Set(F.dGet(1)/modF,1,12+1);
3207  dF.Set(F.dGet(2)/modF,2,12+2);
3208  dF.Set(F.dGet(3)/modF,3,12+3);
3209  }
3210  //variation of relative velocity
3211  dv.ReDim(6);
3212 
3213 /* (approximate: assume constant triads orientations)
3214  * relative velocity linearization
3215 */
3216  dv.Set(e3a*r,1, 0+4);
3217  dv.Set(-e3a*r,4, 6+4);
3218 
3219  //assemble friction states
3220  fc->AssJac(WM,dfc,12+NumSelfDof,iFirstReactionIndex+NumSelfDof,dCoef,modF,v,
3221  XCurr,XPrimeCurr,dF,dv);
3222  ExpandableRowVector dM3;
3223  ExpandableRowVector dShc;
3224  //compute
3225  //variation of shape function
3226  Sh_c->dSh_c(dShc,f,modF,v,dfc,dF,dv);
3227  //variation of moment component
3228  dM3.ReDim(2);
3229  dM3.Set(shc * r,1); dM3.Link(1,&dF);
3230  dM3.Set(modF * r,2); dM3.Link(2,&dShc);
3231  //assemble first node
3232  //variation of moment component
3233  dM3.Add(WM,0+4,e3a.dGet(1));
3234  dM3.Add(WM,0+5,e3a.dGet(2));
3235  dM3.Add(WM,0+6,e3a.dGet(3));
3236  //assemble second node
3237  //variation of moment component
3238  dM3.Sub(WM,6+4,e3a.dGet(1));
3239  dM3.Sub(WM,6+5,e3a.dGet(2));
3240  dM3.Sub(WM,6+6,e3a.dGet(3));
3241  }
3242 
3243  return WorkMat;
3244 }
3245 
3246 
3247 /* Assemblaggio residuo */
3248 SubVectorHandler& AxialRotationJoint::AssRes(SubVectorHandler& WorkVec,
3249  doublereal dCoef,
3250  const VectorHandler& XCurr,
3251  const VectorHandler& XPrimeCurr)
3252 {
3253  DEBUGCOUT("Entering AxialRotationJoint::AssRes()" << std::endl);
3254 
3255  /* Dimensiona e resetta la matrice di lavoro */
3256  integer iNumRows = 0;
3257  integer iNumCols = 0;
3258  WorkSpaceDim(&iNumRows, &iNumCols);
3259  WorkVec.ResizeReset(iNumRows);
3260 
3261  /* Indici */
3262  integer iNode1FirstMomIndex = pNode1->iGetFirstMomentumIndex();
3263  integer iNode2FirstMomIndex = pNode2->iGetFirstMomentumIndex();
3264  integer iFirstReactionIndex = iGetFirstIndex();
3265 
3266  /* Indici dei nodi */
3267  for (int iCnt = 1; iCnt <= 6; iCnt++) {
3268  WorkVec.PutRowIndex(iCnt, iNode1FirstMomIndex+iCnt);
3269  WorkVec.PutRowIndex(6+iCnt, iNode2FirstMomIndex+iCnt);
3270  }
3271  /* Indici del vincolo */
3272  for (unsigned int iCnt = 1; iCnt <= iGetNumDof(); iCnt++) {
3273  WorkVec.PutRowIndex(12+iCnt, iFirstReactionIndex+iCnt);
3274  }
3275 
3276  /* Aggiorna i dati propri */
3277  F = Vec3(XCurr, iFirstReactionIndex+1);
3278  M = Vec3(XCurr, iFirstReactionIndex+4);
3279 
3280  /* Recupera i dati */
3281  const Vec3& x1(pNode1->GetXCurr());
3282  const Vec3& x2(pNode2->GetXCurr());
3283  const Mat3x3& R1(pNode1->GetRCurr());
3284  const Mat3x3& R2(pNode2->GetRCurr());
3285 
3286  /* Costruisce i dati propri nella configurazione corrente */
3287  Vec3 dTmp1(R1*d1);
3288  Vec3 dTmp2(R2*d2);
3289  Mat3x3 R1hTmp(R1*R1h);
3290  Mat3x3 R2hTmp(R2*R2h);
3291 
3292  Vec3 e3a(R1hTmp.GetVec(3));
3293  Vec3 e1b(R2hTmp.GetVec(1));
3294  Vec3 e2b(R2hTmp.GetVec(2));
3295 
3296  Vec3 MTmp(e2b.Cross(e3a)*M.dGet(1)+e3a.Cross(e1b)*M.dGet(2));
3297 
3298  /* Equazioni di equilibrio, nodo 1 */
3299  WorkVec.Sub(1, F);
3300  WorkVec.Add(4, F.Cross(dTmp1)-MTmp-e3a*M.dGet(3)); /* Sfrutto F/\d = -d/\F */
3301 
3302  /* Equazioni di equilibrio, nodo 2 */
3303  WorkVec.Add(7, F);
3304  WorkVec.Add(10, dTmp2.Cross(F)+MTmp+e3a*M.dGet(3));
3305 
3306  /* Modifica: divido le equazioni di vincolo per dCoef */
3307  ASSERT(dCoef != 0.);
3308 
3309  /* Equazione di vincolo di posizione */
3310  WorkVec.Add(13, (x1+dTmp1-x2-dTmp2)/dCoef);
3311 
3312  /* Equazioni di vincolo di rotazione */
3313  Vec3 Tmp = Vec3(R1hTmp.GetVec(3));
3314  WorkVec.PutCoef(16, Tmp.Dot(R2hTmp.GetVec(2))/dCoef);
3315  WorkVec.PutCoef(17, Tmp.Dot(R2hTmp.GetVec(1))/dCoef);
3316 
3317  /* Questa equazione non viene divisa per dCoef */
3318 
3319  /* Equazione di vincolo di velocita' di rotazione */
3320  const Vec3& Omega1(pNode1->GetWCurr());
3321  const Vec3& Omega2(pNode2->GetWCurr());
3322  doublereal dOmega0 = pGetDriveCaller()->dGet();
3323  WorkVec.PutCoef(18, dOmega0-e3a.Dot(Omega2-Omega1));
3324 
3325  if (fc) {
3326  bool ChangeJac(false);
3327  doublereal v = (Omega1-Omega2).Dot(e3a)*r;
3328  doublereal modF = std::max(F.Norm(), preF);
3329  try {
3330  fc->AssRes(WorkVec,12+NumSelfDof,iFirstReactionIndex+NumSelfDof,modF,v,XCurr,XPrimeCurr);
3331  }
3332  catch (Elem::ChangedEquationStructure) {
3333  ChangeJac = true;
3334  }
3335  doublereal f = fc->fc();
3336  doublereal shc = Sh_c->Sh_c(f,modF,v);
3337  M3 = shc*modF*r;
3338  WorkVec.Sub(4,e3a*M3);
3339  WorkVec.Add(10,e3a*M3);
3341 // M += e3a*M3;
3342  if (ChangeJac) {
3343  throw Elem::ChangedEquationStructure(MBDYN_EXCEPT_ARGS);
3344  }
3345  }
3346 
3347  return WorkVec;
3348 }
3349 
3350 DofOrder::Order
3351 AxialRotationJoint::GetEqType(unsigned int i) const
3352 {
3353  ASSERTMSGBREAK(i < iGetNumDof(),
3354  "INDEX ERROR in AxialRotationJoint::GetEqType");
3355  if (i == 5) {
3356  return DofOrder::DIFFERENTIAL;
3357  } else if (i < NumSelfDof) {
3358  return DofOrder::ALGEBRAIC;
3359  } else {
3360  return fc->GetEqType(i-NumSelfDof);
3361  }
3362 }
3363 
3364 void
3365 AxialRotationJoint::OutputPrepare(OutputHandler& OH)
3366 {
3367  if (bToBeOutput()) {
3368 #ifdef USE_NETCDF
3369  if (OH.UseNetCDF(OutputHandler::JOINTS)) {
3370  std::string name;
3371  OutputPrepare_int("axial rotation", OH, name);
3372 
3373  Var_Phi = OH.CreateRotationVar(name, "", od, "global");
3374 
3375  Var_Omega = OH.CreateVar<Vec3>(name + "Omega", "radian/s",
3376  "local relative angular velocity (x, y, z)");
3377 
3378 /* TODO
3379  Var_MFR = OH.CreateVar<doublereal>(name + "MFR", "Nm",
3380  "friciton moment ");
3381 
3382  Var_MU = OH.CreateVar<doublereal>(name + "MU", "--",
3383  "friction model specific data: friction coefficient?");
3384 */
3385  }
3386 #endif // USE_NETCDF
3387  }
3388 }
3389 
3390 /* Output (da mettere a punto) */
3391 void AxialRotationJoint::Output(OutputHandler& OH) const
3392 {
3393  if (bToBeOutput()) {
3394  Mat3x3 R2Tmp(pNode2->GetRCurr()*R2h);
3395  Mat3x3 RTmp((pNode1->GetRCurr()*R1h).MulTM(R2Tmp));
3396  Vec3 E;
3397  switch (od) {
3398  case EULER_123:
3399  E = MatR2EulerAngles123(RTmp)*dRaDegr;
3400  break;
3401 
3402  case EULER_313:
3403  E = MatR2EulerAngles313(RTmp)*dRaDegr;
3404  break;
3405 
3406  case EULER_321:
3407  E = MatR2EulerAngles321(RTmp)*dRaDegr;
3408  break;
3409 
3410  case ORIENTATION_VECTOR:
3411  E = RotManip::VecRot(RTmp);
3412  break;
3413 
3414  case ORIENTATION_MATRIX:
3415  break;
3416 
3417  default:
3418  /* impossible */
3419  break;
3420  }
3421  Vec3 OmegaTmp(R2Tmp.MulTV(pNode2->GetWCurr()-pNode1->GetWCurr()));
3422 
3423 #ifdef USE_NETCDF
3424  if (OH.UseNetCDF(OutputHandler::JOINTS)) {
3425  Var_F_local->put_rec((R2Tmp.MulTV(F)).pGetVec(), OH.GetCurrentStep());
3426  Var_M_local->put_rec(M.pGetVec(), OH.GetCurrentStep());
3427  Var_F_global->put_rec(F.pGetVec(), OH.GetCurrentStep());
3428  Var_M_global->put_rec((R2Tmp*M).pGetVec(), OH.GetCurrentStep());
3429  switch (od) {
3430  case EULER_123:
3431  case EULER_313:
3432  case EULER_321:
3433  case ORIENTATION_VECTOR:
3434  Var_Phi->put_rec(E.pGetVec(), OH.GetCurrentStep());
3435  break;
3436 
3437  case ORIENTATION_MATRIX:
3438  Var_Phi->put_rec(RTmp.pGetMat(), OH.GetCurrentStep());
3439  break;
3440 
3441  default:
3442  /* impossible */
3443  break;
3444  }
3445  Var_Omega->put_rec(OmegaTmp.pGetVec(), OH.GetCurrentStep());
3446 /*
3447  if (fc) {
3448  Var_MFR->put_rec(&M3, OH.GetCurrentStep());
3449  Var_MU->put_rec(fc->fc(), OH.GetCurrentStep());
3450  }
3451  else
3452  {
3453  Var_MFR->put_rec(0, OH.GetCurrentStep());
3454  Var_MU->put_rec(0, OH.GetCurrentStep());
3455  }
3456 */
3457  }
3458 #endif // USE_NETCDF
3459  if (OH.UseText(OutputHandler::JOINTS)) {
3460  std::ostream &of = Joint::Output(OH.Joints(), "AxialRotation", GetLabel(),
3461  R2Tmp.MulTV(F), M, F, R2Tmp*M)
3462  << " ";
3463 
3464  switch (od) {
3465  case EULER_123:
3466  case EULER_313:
3467  case EULER_321:
3468  case ORIENTATION_VECTOR:
3469  OH.Joints() << E;
3470  break;
3471 
3472  case ORIENTATION_MATRIX:
3473  OH.Joints() << RTmp;
3474  break;
3475 
3476  default:
3477  /* impossible */
3478  break;
3479  }
3480 
3481  OH.Joints() << " " << dGet()
3482  << " " << OmegaTmp;
3483  if (fc) {
3484  of << " " << M3 << " " << fc->fc();
3485  }
3486  of << std::endl;
3487  }
3488  }
3489 }
3490 
3491 
3492 /* Contributo allo jacobiano durante l'assemblaggio iniziale */
3493 VariableSubMatrixHandler&
3494 AxialRotationJoint::InitialAssJac(VariableSubMatrixHandler& WorkMat,
3495  const VectorHandler& XCurr)
3496 {
3497  /* Per ora usa la matrice piena; eventualmente si puo'
3498  * passare a quella sparsa quando si ottimizza */
3499  FullSubMatrixHandler& WM = WorkMat.SetFull();
3500 
3501  /* Dimensiona e resetta la matrice di lavoro */
3502  integer iNumRows = 0;
3503  integer iNumCols = 0;
3504  InitialWorkSpaceDim(&iNumRows, &iNumCols);
3505  WM.ResizeReset(iNumRows, iNumCols);
3506 
3507  /* Equazioni: vedi joints.dvi */
3508 
3509  /* equazioni ed incognite
3510  * F1 Delta_x1 0+1 = 1
3511  * M1 Delta_g1 3+1 = 4
3512  * FP1 Delta_xP1 6+1 = 7
3513  * MP1 Delta_w1 9+1 = 10
3514  * F2 Delta_x2 12+1 = 13
3515  * M2 Delta_g2 15+1 = 16
3516  * FP2 Delta_xP2 18+1 = 19
3517  * MP2 Delta_w2 21+1 = 22
3518  * vincolo spostamento Delta_F 24+1 = 25
3519  * vincolo rotazione Delta_M 27+1 = 28
3520  * derivata vincolo spostamento Delta_FP 29+1 = 30
3521  * derivata vincolo rotazione Delta_MP 32+1 = 33
3522  * vincolo di velocita' di rotazione
3523  */
3524 
3525 
3526  /* Indici */
3527  integer iNode1FirstPosIndex = pNode1->iGetFirstPositionIndex();
3528  integer iNode1FirstVelIndex = iNode1FirstPosIndex+6;
3529  integer iNode2FirstPosIndex = pNode2->iGetFirstPositionIndex();
3530  integer iNode2FirstVelIndex = iNode2FirstPosIndex+6;
3531  integer iFirstReactionIndex = iGetFirstIndex();
3532  integer iReactionPrimeIndex = iFirstReactionIndex+5;
3533 
3534  /* Nota: le reazioni vincolari sono:
3535  * Forza, 3 incognite, riferimento globale,
3536  * Momento, 2 incognite, riferimento locale
3537  */
3538 
3539  /* Setta gli indici dei nodi */
3540  for (int iCnt = 1; iCnt <= 6; iCnt++) {
3541  WM.PutRowIndex(iCnt, iNode1FirstPosIndex+iCnt);
3542  WM.PutColIndex(iCnt, iNode1FirstPosIndex+iCnt);
3543  WM.PutRowIndex(6+iCnt, iNode1FirstVelIndex+iCnt);
3544  WM.PutColIndex(6+iCnt, iNode1FirstVelIndex+iCnt);
3545  WM.PutRowIndex(12+iCnt, iNode2FirstPosIndex+iCnt);
3546  WM.PutColIndex(12+iCnt, iNode2FirstPosIndex+iCnt);
3547  WM.PutRowIndex(18+iCnt, iNode2FirstVelIndex+iCnt);
3548  WM.PutColIndex(18+iCnt, iNode2FirstVelIndex+iCnt);
3549  }
3550 
3551  /* Setta gli indici delle reazioni */
3552  for (int iCnt = 1; iCnt <= 5; iCnt++) {
3553  WM.PutRowIndex(24+iCnt, iFirstReactionIndex+iCnt);
3554  WM.PutColIndex(24+iCnt, iFirstReactionIndex+iCnt);
3555  }
3556 
3557  for (int iCnt = 1; iCnt <= 6; iCnt++) {
3558  WM.PutRowIndex(29+iCnt, iReactionPrimeIndex+iCnt);
3559  WM.PutColIndex(29+iCnt, iReactionPrimeIndex+iCnt);
3560  }
3561 
3562  /* Recupera i dati */
3563  const Mat3x3& R1(pNode1->GetRRef());
3564  const Mat3x3& R2(pNode2->GetRRef());
3565  const Vec3& Omega1(pNode1->GetWRef());
3566  const Vec3& Omega2(pNode2->GetWRef());
3567  /* F ed M sono gia' state aggiornate da InitialAssRes */
3568  Vec3 FPrime(XCurr, iReactionPrimeIndex+1);
3569  Vec3 MPrime(XCurr, iReactionPrimeIndex+4);
3570 
3571  /* Matrici identita' */
3572  for (int iCnt = 1; iCnt <= 3; iCnt++) {
3573  /* Contributo di forza all'equazione della forza, nodo 1 */
3574  WM.PutCoef(iCnt, 24+iCnt, 1.);
3575 
3576  /* Contrib. di der. di forza all'eq. della der. della forza, nodo 1 */
3577  WM.PutCoef(6+iCnt, 29+iCnt, 1.);
3578 
3579  /* Contributo di forza all'equazione della forza, nodo 2 */
3580  WM.PutCoef(12+iCnt, 24+iCnt, -1.);
3581 
3582  /* Contrib. di der. di forza all'eq. della der. della forza, nodo 2 */
3583  WM.PutCoef(18+iCnt, 29+iCnt, -1.);
3584 
3585  /* Equazione di vincolo, nodo 1 */
3586  WM.PutCoef(24+iCnt, iCnt, -1.);
3587 
3588  /* Derivata dell'equazione di vincolo, nodo 1 */
3589  WM.PutCoef(29+iCnt, 6+iCnt, -1.);
3590 
3591  /* Equazione di vincolo, nodo 2 */
3592  WM.PutCoef(24+iCnt, 12+iCnt, 1.);
3593 
3594  /* Derivata dell'equazione di vincolo, nodo 2 */
3595  WM.PutCoef(29+iCnt, 18+iCnt, 1.);
3596  }
3597 
3598  /* Distanze e matrici di rotazione dai nodi alla cerniera
3599  * nel sistema globale */
3600  Vec3 d1Tmp(R1*d1);
3601  Vec3 d2Tmp(R2*d2);
3602  Mat3x3 R1hTmp(R1*R1h);
3603  Mat3x3 R2hTmp(R2*R2h);
3604 
3605  Vec3 e3a(R1hTmp.GetVec(3));
3606  Vec3 e1b(R2hTmp.GetVec(1));
3607  Vec3 e2b(R2hTmp.GetVec(2));
3608 
3609  /* Ruota il momento e la sua derivata con le matrici della cerniera
3610  * rispetto ai nodi */
3611  Vec3 MTmp(e2b*M.dGet(1)-e1b*M.dGet(2));
3612  Vec3 MPrimeTmp(e2b*MPrime.dGet(1)-e1b*MPrime.dGet(2));
3613 
3614  /* Matrici F/\d1/\, -F/\d2/\ */
3615  Mat3x3 FWedged1Wedge(MatCrossCross, F, d1Tmp);
3616  Mat3x3 FWedged2Wedge(MatCrossCross, F, -d2Tmp);
3617 
3618  /* Matrici (omega1/\d1)/\, -(omega2/\d2)/\ */
3619  Mat3x3 O1Wedged1Wedge(MatCross, Omega1.Cross(d1Tmp));
3620  Mat3x3 O2Wedged2Wedge(MatCross, d2Tmp.Cross(Omega2));
3621 
3622  Mat3x3 MDeltag1((Mat3x3(MatCross, Omega2.Cross(MTmp) + MPrimeTmp)
3623  + Mat3x3(MatCrossCross, MTmp, Omega1))*Mat3x3(MatCross, e3a));
3624  Mat3x3 MDeltag2(Mat3x3(MatCrossCross, Omega1.Cross(e3a), MTmp)
3625  + Mat3x3(MatCrossCross, e3a, MPrimeTmp)
3626  + e3a.Cross(Mat3x3(MatCrossCross, Omega2, MTmp)));
3627 
3628  /* Vettori temporanei */
3629  Vec3 Tmp1(e2b.Cross(e3a));
3630  Vec3 Tmp2(e3a.Cross(e1b));
3631 
3632  /* Prodotto vettore tra il versore 3 della cerniera secondo il nodo 1
3633  * ed il versore 1 della cerniera secondo il nodo 2. A convergenza
3634  * devono essere ortogonali, quindi il loro prodotto vettore deve essere
3635  * unitario */
3636 
3637  /* Error handling: il programma si ferma, pero' segnala dov'e' l'errore */
3638  if (Tmp1.Dot() <= std::numeric_limits<doublereal>::epsilon() || Tmp2.Dot() <= std::numeric_limits<doublereal>::epsilon()) {
3639  silent_cerr("AxialRotationJoint(" << GetLabel() << "): "
3640  "first and second node hinge axes are (nearly) orthogonal"
3641  << std::endl);
3642  throw ErrNullNorm(MBDYN_EXCEPT_ARGS);
3643  }
3644 
3645  Vec3 TmpPrime1(e2b.Cross(Omega1.Cross(e3a))-e3a.Cross(Omega2.Cross(e2b)));
3646  Vec3 TmpPrime2(e3a.Cross(Omega2.Cross(e1b))-e1b.Cross(Omega1.Cross(e3a)));
3647 
3648  /* Equazione di momento, nodo 1 */
3649  WM.Add(4, 4, FWedged1Wedge - Mat3x3(MatCrossCross, MTmp, e3a));
3650  WM.Add(4, 16, Mat3x3(MatCrossCross, e3a, MTmp));
3651  WM.Add(4, 25, Mat3x3(MatCross, d1Tmp));
3652  for (int iCnt = 1; iCnt <= 3; iCnt++) {
3653  WM.PutCoef(3+iCnt, 28, Tmp1(iCnt));
3654  WM.PutCoef(3+iCnt, 29, Tmp2(iCnt));
3655  }
3656 
3657  /* Equazione di momento, nodo 2 */
3658  WM.Add(4, 16, Mat3x3(MatCrossCross, MTmp, e3a));
3659  WM.Add(16, 16, FWedged2Wedge - Mat3x3(MatCrossCross, e3a, MTmp));
3660  WM.Sub(16, 25, Mat3x3(MatCross, d2Tmp));
3661  for (int iCnt = 1; iCnt <= 3; iCnt++) {
3662  WM.PutCoef(15+iCnt, 28, -Tmp1(iCnt));
3663  WM.PutCoef(15+iCnt, 29, -Tmp2(iCnt));
3664  }
3665 
3666  /* Derivata dell'equazione di momento, nodo 1 */
3667  WM.Add(10, 4, (Mat3x3(MatCross, FPrime) + Mat3x3(MatCrossCross, F, Omega1))*Mat3x3(MatCross, d1Tmp)
3668  - MDeltag1
3669  - Mat3x3(MatCross, e3a*MPrime(3)));
3670  WM.Add(10, 10, FWedged1Wedge
3671  - Mat3x3(MatCrossCross, MTmp, e3a)
3672  - Mat3x3(MatCross, e3a*MPrime(3)));
3673  WM.Add(10, 16, MDeltag2);
3674  WM.Add(10, 22, Mat3x3(MatCrossCross, e3a, MTmp));
3675  WM.Add(10, 25, O1Wedged1Wedge);
3676  WM.Add(10, 30, Mat3x3(MatCross, d1Tmp));
3677 
3678  for (int iCnt = 1; iCnt <= 3; iCnt++) {
3679  WM.PutCoef(9+iCnt, 28, TmpPrime1(iCnt));
3680  WM.PutCoef(9+iCnt, 29, TmpPrime2(iCnt));
3681  WM.PutCoef(9+iCnt, 33, Tmp1(iCnt));
3682  WM.PutCoef(9+iCnt, 34, Tmp2(iCnt));
3683 
3684  /* Contributo del momento attorno all'asse 3, dovuto alla velocita'
3685  * imposta */
3686  WM.PutCoef(9+iCnt, 35, e3a(iCnt));
3687  }
3688 
3689  /* Derivata dell'equazione di momento, nodo 2 */
3690  WM.Add(22, 4, MDeltag1 + Mat3x3(MatCross, e3a*MPrime(3)));
3691  WM.Add(22, 10, Mat3x3(MatCrossCross, MTmp, e3a) + Mat3x3(MatCross, e3a*MPrime(3)));
3692  WM.Sub(22, 16, (Mat3x3(MatCross, FPrime) + Mat3x3(MatCrossCross, F, Omega2))*Mat3x3(MatCross, d2Tmp) + MDeltag2);
3693  WM.Add(22, 22, FWedged2Wedge - Mat3x3(MatCrossCross, e3a, MTmp));
3694  WM.Add(22, 25, O2Wedged2Wedge);
3695  WM.Sub(22, 30, Mat3x3(MatCross, d2Tmp));
3696 
3697  for (int iCnt = 1; iCnt <= 3; iCnt++) {
3698  WM.PutCoef(21+iCnt, 28, -TmpPrime1(iCnt));
3699  WM.PutCoef(21+iCnt, 29, -TmpPrime2(iCnt));
3700  WM.PutCoef(21+iCnt, 33, -Tmp1(iCnt));
3701  WM.PutCoef(21+iCnt, 34, -Tmp2(iCnt));
3702 
3703  /* Contributo del momento attorno all'asse 3, dovuto alla velocita'
3704  * imposta */
3705  WM.PutCoef(21+iCnt, 35, -e3a(iCnt));
3706  }
3707 
3708  /* Equazione di vincolo di posizione */
3709  WM.Add(25, 4, Mat3x3(MatCross, d1Tmp));
3710  WM.Sub(25, 16, Mat3x3(MatCross, d2Tmp));
3711 
3712  /* Derivata dell'equazione di vincolo di posizione */
3713  WM.Add(30, 4, O1Wedged1Wedge);
3714  WM.Add(30, 10, Mat3x3(MatCross, d1Tmp));
3715  WM.Add(30, 16, O2Wedged2Wedge);
3716  WM.Sub(30, 22, Mat3x3(MatCross, d2Tmp));
3717 
3718  /* Equazioni di vincolo di rotazione: e1b~e3a, e2b~e3a */
3719 
3720  for (int iCnt = 1; iCnt <= 3; iCnt++) {
3721  doublereal d = Tmp1(iCnt);
3722  WM.PutCoef(28, 3+iCnt, d);
3723  WM.PutCoef(28, 15+iCnt, -d);
3724 
3725  /* Queste sono per la derivata dell'equazione, sono qui solo per
3726  * ottimizzazione */
3727  WM.PutCoef(33, 9+iCnt, d);
3728  WM.PutCoef(33, 21+iCnt, -d);
3729 
3730  d = Tmp2.dGet(iCnt);
3731  WM.PutCoef(29, 3+iCnt, -d);
3732  WM.PutCoef(29, 15+iCnt, d);
3733 
3734  /* Queste sono per la derivata dell'equazione, sono qui solo per
3735  * ottimizzazione */
3736  WM.PutCoef(34, 9+iCnt, -d);
3737  WM.PutCoef(34, 21+iCnt, d);
3738  }
3739 
3740  /* Derivate delle equazioni di vincolo di rotazione: e1b~e3a, e2b~e3a */
3741  Vec3 O1mO2(Omega1 - Omega2);
3742  TmpPrime1 = e3a.Cross(O1mO2.Cross(e2b));
3743  TmpPrime2 = e2b.Cross(e3a.Cross(O1mO2));
3744  for (int iCnt = 1; iCnt <= 3; iCnt++) {
3745  WM.PutCoef(33, 3+iCnt, TmpPrime1(iCnt));
3746  WM.PutCoef(33, 15+iCnt, TmpPrime2(iCnt));
3747  }
3748 
3749  TmpPrime1 = e3a.Cross(O1mO2.Cross(e1b));
3750  TmpPrime2 = e1b.Cross(e3a.Cross(O1mO2));
3751  for (int iCnt = 1; iCnt <= 3; iCnt++) {
3752  WM.PutCoef(34, 3+iCnt, TmpPrime1(iCnt));
3753  WM.PutCoef(34, 15+iCnt, TmpPrime2(iCnt));
3754  }
3755 
3756  /* Equazione di vincolo di velocita' di rotazione; viene posta qui perche'
3757  * a questo numero di equazione corrisponde il numero della
3758  * relativa incognita */
3759  Vec3 Tmp = O1mO2.Cross(e3a);
3760  for (int iCnt = 1; iCnt <= 3; iCnt++) {
3761  WM.PutCoef(35, 3+iCnt, Tmp(iCnt));
3762  doublereal d = e3a(iCnt);
3763  WM.PutCoef(35, 9+iCnt, -d);
3764  WM.PutCoef(35, 21+iCnt, d);
3765  }
3766 
3767  return WorkMat;
3768 }
3769 
3770 
3771 /* Contributo al residuo durante l'assemblaggio iniziale */
3772 SubVectorHandler&
3773 AxialRotationJoint::InitialAssRes(SubVectorHandler& WorkVec,
3774  const VectorHandler& XCurr)
3775 {
3776  DEBUGCOUT("Entering AxialRotationJoint::InitialAssRes()" << std::endl);
3777 
3778  /* Dimensiona e resetta la matrice di lavoro */
3779  integer iNumRows = 0;
3780  integer iNumCols = 0;
3781  InitialWorkSpaceDim(&iNumRows, &iNumCols);
3782  WorkVec.ResizeReset(iNumRows);
3783 
3784  /* Indici */
3785  integer iNode1FirstPosIndex = pNode1->iGetFirstPositionIndex();
3786  integer iNode1FirstVelIndex = iNode1FirstPosIndex+6;
3787  integer iNode2FirstPosIndex = pNode2->iGetFirstPositionIndex();
3788  integer iNode2FirstVelIndex = iNode2FirstPosIndex+6;
3789  integer iFirstReactionIndex = iGetFirstIndex();
3790  integer iReactionPrimeIndex = iFirstReactionIndex+5;
3791 
3792  /* Setta gli indici */
3793  for (int iCnt = 1; iCnt <= 6; iCnt++) {
3794  WorkVec.PutRowIndex(iCnt, iNode1FirstPosIndex+iCnt);
3795  WorkVec.PutRowIndex(6+iCnt, iNode1FirstVelIndex+iCnt);
3796  WorkVec.PutRowIndex(12+iCnt, iNode2FirstPosIndex+iCnt);
3797  WorkVec.PutRowIndex(18+iCnt, iNode2FirstVelIndex+iCnt);
3798  }
3799 
3800  for (int iCnt = 1; iCnt <= 5; iCnt++) {
3801  WorkVec.PutRowIndex(24+iCnt, iFirstReactionIndex+iCnt);
3802  }
3803 
3804  for (int iCnt = 1; iCnt <= 6; iCnt++) {
3805  WorkVec.PutRowIndex(29+iCnt, iReactionPrimeIndex+iCnt);
3806  }
3807 
3808  /* Recupera i dati */
3809  const Vec3& x1(pNode1->GetXCurr());
3810  const Vec3& x2(pNode2->GetXCurr());
3811  const Vec3& v1(pNode1->GetVCurr());
3812  const Vec3& v2(pNode2->GetVCurr());
3813  const Mat3x3& R1(pNode1->GetRCurr());
3814  const Mat3x3& R2(pNode2->GetRCurr());
3815  const Vec3& Omega1(pNode1->GetWCurr());
3816  const Vec3& Omega2(pNode2->GetWCurr());
3817 
3818  /* Aggiorna F ed M, che restano anche per InitialAssJac */
3819  F = Vec3(XCurr, iFirstReactionIndex+1);
3820  M = Vec3(XCurr(iFirstReactionIndex+4),
3821  XCurr(iFirstReactionIndex+5),
3822  0.);
3823  Vec3 FPrime(XCurr, iReactionPrimeIndex+1);
3824  Vec3 MPrime(XCurr, iReactionPrimeIndex+4);
3825 
3826  /* Distanza nel sistema globale */
3827  Vec3 d1Tmp(R1*d1);
3828  Vec3 d2Tmp(R2*d2);
3829  Mat3x3 R1hTmp(R1*R1h);
3830  Mat3x3 R2hTmp(R2*R2h);
3831 
3832  /* Vettori omega1/\d1, -omega2/\d2 */
3833  Vec3 O1Wedged1(Omega1.Cross(d1Tmp));
3834  Vec3 O2Wedged2(Omega2.Cross(d2Tmp));
3835 
3836  Vec3 e3a(R1hTmp.GetVec(3));
3837  Vec3 e1b(R2hTmp.GetVec(1));
3838  Vec3 e2b(R2hTmp.GetVec(2));
3839 
3840  /* Ruota il momento e la sua derivata con le matrici della cerniera
3841  * rispetto ai nodi */
3842  Vec3 MTmp(e2b*M.dGet(1)-e1b*M.dGet(2));
3843  Vec3 MPrimeTmp(e3a.Cross(MTmp.Cross(Omega2))+MTmp.Cross(Omega1.Cross(e3a))+
3844  e2b.Cross(e3a)*MPrime.dGet(1)+e3a.Cross(e1b)*MPrime.dGet(2));
3845 
3846  /* Equazioni di equilibrio, nodo 1 */
3847  WorkVec.Sub(1, F);
3848  WorkVec.Add(4, F.Cross(d1Tmp)-MTmp.Cross(e3a)); /* Sfrutto il fatto che F/\d = -d/\F */
3849 
3850  /* Derivate delle equazioni di equilibrio, nodo 1 */
3851  WorkVec.Sub(7, FPrime);
3852  WorkVec.Add(10, FPrime.Cross(d1Tmp)-O1Wedged1.Cross(F)-MPrimeTmp-
3853  e3a*MPrime.dGet(3));
3854 
3855  /* Equazioni di equilibrio, nodo 2 */
3856  WorkVec.Add(13, F);
3857  WorkVec.Add(16, d2Tmp.Cross(F)+MTmp.Cross(e3a));
3858 
3859  /* Derivate delle equazioni di equilibrio, nodo 2 */
3860  WorkVec.Add(19, FPrime);
3861  WorkVec.Add(22, d2Tmp.Cross(FPrime)+O2Wedged2.Cross(F)+MPrimeTmp+
3862  e3a*MPrime.dGet(3));
3863 
3864  /* Equazione di vincolo di posizione */
3865  WorkVec.Add(25, x1+d1Tmp-x2-d2Tmp);
3866 
3867  /* Derivata dell'equazione di vincolo di posizione */
3868  WorkVec.Add(30, v1+O1Wedged1-v2-O2Wedged2);
3869 
3870  /* Equazioni di vincolo di rotazione */
3871  WorkVec.PutCoef(28, e2b.Dot(e3a));
3872  WorkVec.PutCoef(29, e1b.Dot(e3a));
3873 
3874  /* Derivate delle equazioni di vincolo di rotazione: e1b~e3a, e2b~e3a */
3875  Vec3 Tmp((Omega1-Omega2).Cross(e3a));
3876  WorkVec.PutCoef(33, e2b.Dot(Tmp));
3877  WorkVec.PutCoef(34, e1b.Dot(Tmp));
3878 
3879  /* Equazione di vincolo di velocita' di rotazione */
3880  doublereal Omega0 = pGetDriveCaller()->dGet();
3881  WorkVec.PutCoef(35, Omega0-e3a.Dot(Omega2-Omega1));
3882 
3883  return WorkVec;
3884 }
3885 
3886 unsigned int
3887 AxialRotationJoint::iGetNumPrivData(void) const
3888 {
3889  return 8;
3890 }
3891 
3892 unsigned int
3893 AxialRotationJoint::iGetPrivDataIdx(const char *s) const
3894 {
3895  ASSERT(s != NULL);
3896 
3897  unsigned int idx = 0;
3898 
3899  switch (s[0]) {
3900  case 'w':
3901  idx++;
3902  case 'r':
3903  idx++;
3904  if (s[1] == 'z') {
3905  return idx;
3906  }
3907  break;
3908 
3909  case 'M':
3910  idx += 3;
3911  case 'F':
3912  idx += 2;
3913 
3914  switch (s[1]) {
3915  case 'x':
3916  return idx + 1;
3917 
3918  case 'y':
3919  return idx + 2;
3920 
3921  case 'z':
3922  return idx + 3;
3923  }
3924  }
3925 
3926  return 0;
3927 }
3928 
3929 doublereal
3930 AxialRotationJoint::dGetPrivData(unsigned int i) const
3931 {
3932  ASSERT(i >= 1 && i <= iGetNumPrivData());
3933 
3934  switch (i) {
3935  case 1: {
3936  Vec3 v(RotManip::VecRot((pNode1->GetRCurr()*R1h).MulTM(pNode2->GetRCurr()*R2h)));
3937  doublereal dThetaTmp(v(3));
3938 
3939  int n = 0;
3940 
3941  if (dThetaTmp - dThetaWrapped < -M_PI) {
3942  n++;
3943  }
3944 
3945  if (dThetaTmp - dThetaWrapped > M_PI) {
3946  n--;
3947  }
3948 
3949  return 2*M_PI*(NTheta + n) + dThetaTmp;
3950  }
3951 
3952  case 2:
3953  return dGet();
3954 
3955  case 3:
3956  case 4:
3957  case 5:
3958  return F(i - 2);
3959 
3960  case 6:
3961  case 7:
3962  case 8:
3963  return M(i - 5);
3964  }
3965 
3966  silent_cerr("AxialRotationJoint(" << GetLabel() << "): "
3967  "illegal private data " << i << std::endl);
3968  throw ErrGeneric(MBDYN_EXCEPT_ARGS);
3969 }
3970 
3971 /* AxialRotationJoint - end */
3972 
3973 
3974 /* PlanePinJoint - begin */
3975 
3976 /* Costruttore non banale */
3977 PlanePinJoint::PlanePinJoint(unsigned int uL, const DofOwner* pDO,
3978  const StructNode* pN,
3979  const Vec3& X0Tmp, const Mat3x3& R0Tmp,
3980  const Vec3& dTmp, const Mat3x3& RhTmp,
3981  flag fOut, const bool _calcInitdTheta,
3982  const doublereal initDTheta)
3983 : Elem(uL, fOut),
3984 Joint(uL, pDO, fOut),
3985 pNode(pN),
3986 X0(X0Tmp), R0(R0Tmp), d(dTmp), Rh(RhTmp),
3987 F(Zero3), M(Zero3),
3988 calcInitdTheta(_calcInitdTheta),
3989 NTheta(0), dTheta(initDTheta), dThetaWrapped(0.)
3990 {
3991  NO_OP;
3992 }
3993 
3994 
3995 /* Distruttore banale */
3996 PlanePinJoint::~PlanePinJoint(void)
3997 {
3998  NO_OP;
3999 };
4000 
4001 
4002 std::ostream&
4003 PlanePinJoint::DescribeDof(std::ostream& out, const char *prefix, bool bInitial) const
4004 {
4005  integer iIndex = iGetFirstIndex();
4006 
4007  out
4008  << prefix << iIndex + 1 << "->" << iIndex + 3 << ": "
4009  "reaction forces [Fx,Fy,Fz]" << std::endl
4010  << prefix << iIndex + 4 << "->" << iIndex + 5 << ": "
4011  "reaction couples [mx,my]" << std::endl;
4012 
4013  if (bInitial) {
4014  iIndex += 5;
4015  out
4016  << prefix << iIndex + 1 << "->" << iIndex + 3 << ": "
4017  "reaction force derivatives [FPx,FPy,FPz]" << std::endl
4018  << prefix << iIndex + 4 << "->" << iIndex + 5 << ": "
4019  "reaction couple derivatives [mPx,mPy]" << std::endl;
4020  }
4021 
4022  return out;
4023 }
4024 
4025 void
4026 PlanePinJoint::DescribeDof(std::vector<std::string>& desc, bool bInitial, int i) const
4027 {
4028  std::ostringstream os;
4029  os << "PlanePinJoint(" << GetLabel() << ")";
4030 
4031  unsigned short nself = 5;
4032  if (bInitial) {
4033  nself *= 2;
4034  }
4035 
4036  if (i == -1) {
4037  desc.resize(nself);
4038  std::string name = os.str();
4039 
4040  for (unsigned i = 0; i < 3; i++) {
4041  os.str(name);
4042  os.seekp(0, std::ios_base::end);
4043  os << ": reaction force f" << xyz[i];
4044  desc[i] = os.str();
4045  }
4046 
4047  for (unsigned i = 0; i < 2; i++) {
4048  os.str(name);
4049  os.seekp(0, std::ios_base::end);
4050  os << ": reaction couple m" << xyz[i];
4051  desc[3 + i] = os.str();
4052  }
4053 
4054  if (bInitial) {
4055  for (unsigned i = 0; i < 3; i++) {
4056  os.str(name);
4057  os.seekp(0, std::ios_base::end);
4058  os << ": reaction force derivative fP" << xyz[i];
4059  desc[5 + i] = os.str();
4060  }
4061 
4062  for (unsigned i = 0; i < 2; i++) {
4063  os.str(name);
4064  os.seekp(0, std::ios_base::end);
4065  os << ": reaction couple derivative mP" << xyz[i];
4066  desc[8 + i] = os.str();
4067  }
4068  }
4069 
4070  } else {
4071  if (i < -1) {
4072  // error
4073  throw ErrGeneric(MBDYN_EXCEPT_ARGS);
4074  }
4075 
4076  if (i >= nself) {
4077  // error
4078  throw ErrGeneric(MBDYN_EXCEPT_ARGS);
4079  }
4080 
4081  desc.resize(1);
4082 
4083  switch (i) {
4084  case 0:
4085  case 1:
4086  case 2:
4087  os << ": reaction force f" << xyz[i];
4088  break;
4089 
4090  case 3:
4091  case 4:
4092  os << ": reaction couple m" << xyz[i - 3];
4093  break;
4094 
4095  case 5:
4096  case 6:
4097  case 7:
4098  os << ": reaction force derivative fP" << xyz[i - 5];
4099  break;
4100 
4101  case 8:
4102  case 9:
4103  os << ": reaction couple derivative mP" << xyz[i - 8];
4104  break;
4105  }
4106  desc[0] = os.str();
4107  }
4108 }
4109 
4110 std::ostream&
4111 PlanePinJoint::DescribeEq(std::ostream& out, const char *prefix, bool bInitial) const
4112 {
4113  integer iIndex = iGetFirstIndex();
4114 
4115  out
4116  << prefix << iIndex + 1 << "->" << iIndex + 3 << ": "
4117  "position constraints [Px=Px0,Py=Py0,Pz=Pz0]" << std::endl
4118  << prefix << iIndex + 4 << "->" << iIndex + 5 << ": "
4119  "orientation constraints [gx=gx0,gy=gy0]" << std::endl;
4120 
4121  if (bInitial) {
4122  iIndex += 5;
4123  out
4124  << prefix << iIndex + 1 << "->" << iIndex + 3 << ": "
4125  "velocity constraints [vx=0,vy=0,vz=0]" << std::endl
4126  << prefix << iIndex + 4 << "->" << iIndex + 5 << ": "
4127  "angular velocity constraints [wx=0,wy=0]" << std::endl;
4128  }
4129 
4130  return out;
4131 }
4132 
4133 void
4134 PlanePinJoint::DescribeEq(std::vector<std::string>& desc, bool bInitial, int i) const
4135 {
4136  std::ostringstream os;
4137  os << "PlanePinJoint(" << GetLabel() << ")";
4138 
4139  unsigned short nself = 5;
4140  if (bInitial) {
4141  nself *= 2;
4142  }
4143 
4144  if (i == -1) {
4145  desc.resize(nself);
4146  std::string name = os.str();
4147 
4148  for (unsigned i = 0; i < 3; i++) {
4149  os.str(name);
4150  os.seekp(0, std::ios_base::end);
4151  os << ": position constraint P" << xyz[i];
4152  desc[i] = os.str();
4153  }
4154 
4155  for (unsigned i = 0; i < 2; i++) {
4156  os.str(name);
4157  os.seekp(0, std::ios_base::end);
4158  os << ": orientation constraint g" << xyz[i];
4159  desc[3 + i] = os.str();
4160  }
4161 
4162  if (bInitial) {
4163  for (unsigned i = 0; i < 3; i++) {
4164  os.str(name);
4165  os.seekp(0, std::ios_base::end);
4166  os << ": position constraint derivative v" << xyz[i];
4167  desc[5 + i] = os.str();
4168  }
4169 
4170  for (unsigned i = 0; i < 2; i++) {
4171  os.str(name);
4172  os.seekp(0, std::ios_base::end);
4173  os << ": orientation constraint derivative w" << xyz[i];
4174  desc[8 + i] = os.str();
4175  }
4176  }
4177 
4178  } else {
4179  if (i < -1) {
4180  // error
4181  throw ErrGeneric(MBDYN_EXCEPT_ARGS);
4182  }
4183 
4184  if (i >= nself) {
4185  // error
4186  throw ErrGeneric(MBDYN_EXCEPT_ARGS);
4187  }
4188 
4189  desc.resize(1);
4190 
4191  switch (i) {
4192  case 0:
4193  case 1:
4194  case 2:
4195  os << ": position constraint P" << xyz[i];
4196  break;
4197 
4198  case 3:
4199  case 4:
4200  os << ": orientation constraint g" << xyz[i - 3];
4201  break;
4202 
4203  case 5:
4204  case 6:
4205  case 7:
4206  os << ": position constraint derivative v" << xyz[i - 5];
4207  break;
4208 
4209  case 8:
4210  case 9:
4211  os << ": orientation constraint derivative w" << xyz[i - 8];
4212  break;
4213  }
4214  desc[0] = os.str();
4215  }
4216 }
4217 
4218 void
4219 PlanePinJoint::SetValue(DataManager *pDM,
4220  VectorHandler& X, VectorHandler& XP,
4221  SimulationEntity::Hints *ph)
4222 {
4223  if (ph) {
4224  for (unsigned i = 0; i < ph->size(); i++) {
4225  Joint::JointHint *pjh = dynamic_cast<Joint::JointHint *>((*ph)[i]);
4226 
4227  if (pjh == 0) {
4228  continue;
4229  }
4230 
4231  if (dynamic_cast<Joint::OffsetHint<1> *>(pjh)) {
4232  const Mat3x3& R(pNode->GetRCurr());
4233 
4234  d = R.MulTV(X0 - pNode->GetXCurr());
4235 
4236  } else if (dynamic_cast<Joint::OffsetHint<0> *>(pjh)) {
4237  Vec3 dTmp(pNode->GetRCurr()*d);
4238 
4239  X0 = R0.MulTV(pNode->GetXCurr() + dTmp);
4240 
4241  } else if (dynamic_cast<Joint::HingeHint<1> *>(pjh)) {
4242  Rh = pNode->GetRCurr().MulTM(R0);
4243 
4244  } else if (dynamic_cast<Joint::HingeHint<2> *>(pjh)) {
4245  R0 = pNode->GetRCurr()*Rh;
4246 
4247  } else if (dynamic_cast<Joint::ReactionsHint *>(pjh)) {
4248  /* TODO */
4249  }
4250  }
4251  }
4252 
4253  if (calcInitdTheta) {
4254  Vec3 v(RotManip::VecRot(R0.MulTM(pNode->GetRCurr()*Rh)));
4255  dThetaWrapped = dTheta = v.dGet(3);
4256  }
4257 }
4258 
4259 Hint *
4260 PlanePinJoint::ParseHint(DataManager *pDM, const char *s) const
4261 {
4262  if (strncasecmp(s, "offset{" /*}*/, STRLENOF("offset{" /*}*/)) == 0) {
4263  s += STRLENOF("offset{" /*}*/);
4264 
4265  if (strcmp(&s[1], /* { */ "}") != 0) {
4266  return 0;
4267  }
4268 
4269  switch (s[0]) {
4270  case '1':
4271  return new Joint::OffsetHint<1>;
4272 
4273  case '0':
4274  return new Joint::OffsetHint<0>;
4275  }
4276 
4277  } else if (strncasecmp(s, "hinge{" /*}*/, STRLENOF("hinge{" /*}*/)) == 0) {
4278  s += STRLENOF("hinge{" /*}*/);
4279 
4280  if (strcmp(&s[1], /*{*/ "}") != 0) {
4281  return 0;
4282  }
4283 
4284  switch (s[0]) {
4285  case '1':
4286  return new Joint::HingeHint<1>;
4287 
4288  case '0':
4289  return new Joint::HingeHint<0>;
4290  }
4291  }
4292 
4293  return 0;
4294 }
4295 
4296 DofOrder::Order
4297 PlanePinJoint::GetEqType(unsigned int i) const
4298 {
4299  ASSERTMSGBREAK(i < iGetNumDof(),
4300  "INDEX ERROR in PlanePinJoint::GetEqType");
4301  return DofOrder::ALGEBRAIC;
4302 }
4303 
4304 void
4305 PlanePinJoint::AfterConvergence(const VectorHandler& X,
4306  const VectorHandler& XP)
4307 {
4308  Vec3 v(RotManip::VecRot(R0.MulTM(pNode->GetRCurr()*Rh)));
4309  doublereal dThetaTmp(v(3));
4310 
4311  // unwrap
4312  if (dThetaTmp - dThetaWrapped < -M_PI) {
4313  NTheta++;
4314  }
4315 
4316  if (dThetaTmp - dThetaWrapped > M_PI) {
4317  NTheta--;
4318  }
4319 
4320  // save new wrapped angle
4321  dThetaWrapped = dThetaTmp;
4322 
4323  // compute new unwrapped angle
4324  dTheta = 2*M_PI*NTheta + dThetaWrapped;
4325 
4326 }
4327 
4328 void
4329 PlanePinJoint::ReadInitialState(MBDynParser& HP)
4330 {
4331  F = HP.GetVec3();
4332  M = Vec3(HP.GetReal(), HP.GetReal(), 0.);
4333 }
4334 
4335 /* Contributo al file di restart */
4336 std::ostream& PlanePinJoint::Restart(std::ostream& out) const
4337 {
4338  Joint::Restart(out) << ", revolute pin, "
4339  << pNode->GetLabel()
4340  << ", reference, node, ", d.Write(out, ", ")
4341  << ", hinge, reference, node, 1, ",
4342  (Rh.GetVec(1)).Write(out, ", ") << ", 2, ",
4343  (Rh.GetVec(2)).Write(out, ", ")
4344  << ", reference, global, ", X0.Write(out, ", ")
4345  << ",hinge, reference, global, 1, ",
4346  (R0.GetVec(1)).Write(out, ", ") << ", 2, ",
4347  (R0.GetVec(2)).Write(out, ", ") << ", "
4348  << "initial theta, " << dTheta << ", "
4349  << "initial state, ", F.Write(out, ", ")
4350  << ", " << M.dGet(1) << ", " << M.dGet(2) << ';' << std::endl;
4351 
4352  return out;
4353 }
4354 
4355 
4356 /* Assemblaggio jacobiano */
4357 VariableSubMatrixHandler&
4358 PlanePinJoint::AssJac(VariableSubMatrixHandler& WorkMat,
4359  doublereal dCoef,
4360  const VectorHandler& /* XCurr */ ,
4361  const VectorHandler& /* XPrimeCurr */ )
4362 {
4363  DEBUGCOUT("Entering PlanePinJoint::AssJac()" << std::endl);
4364 
4365  SparseSubMatrixHandler& WM = WorkMat.SetSparse();
4366  WM.ResizeReset(39, 0);
4367 
4368  integer iFirstPositionIndex = pNode->iGetFirstPositionIndex();
4369  integer iFirstMomentumIndex = pNode->iGetFirstMomentumIndex();
4370  integer iFirstReactionIndex = iGetFirstIndex();
4371 
4372  const Mat3x3& R(pNode->GetRRef());
4373  Vec3 dTmp(R*d);
4374  Mat3x3 RhTmp(R*Rh);
4375 
4376 
4377  /*
4378  * L'equazione di vincolo afferma che il punto in cui si trova la
4379  * cerniera deve essere fissato:
4380  * x + d = x0
4381  * e3_0^Te1 = 0
4382  * e3_0^Te2 = 0
4383  *
4384  * con: d = R * d_0
4385  *
4386  * La forza e' data dalla reazione vincolare F, nel sistema globale
4387  * La coppia dovuta all'eccentricita' e' data rispettivamente da:
4388  * d /\ F
4389  *
4390  *
4391  * x g F
4392  * Q1 | 0 0 I 0 | | x | | -F |
4393  * G1 | 0 cF/\d1/\-M/\ d/\ e1e2 | | g | | -d/\F-M |
4394  * F | I d/\ 0 0 | | F | | (x+d-x0)/c |
4395  * M | 0 e_0/\e1,e2 0 0 | | M | | e_0^Te1,e2 |
4396  *
4397  * con d = R*d_0, c = dCoef
4398  */
4399 
4400 
4401 
4402  /* Moltiplica la forza ed il momento per il coefficiente
4403  * del metodo */
4404 
4405  Vec3 e3(R0.GetVec(3));
4406  Vec3 e1(RhTmp.GetVec(1));
4407  Vec3 e2(RhTmp.GetVec(2));
4408  Vec3 MTmp(e2*M.dGet(1)-e1*M.dGet(2));
4409 
4410  Vec3 Tmp1((e2).Cross(e3));
4411  Vec3 Tmp2((e3).Cross(e1));
4412 
4413  /* termini di reazione sul nodo (forza e momento) */
4414  for (int iCnt = 1; iCnt <= 3; iCnt++) {
4415  WM.PutItem(iCnt, iFirstMomentumIndex+iCnt,
4416  iFirstReactionIndex+iCnt, 1.);
4417  WM.PutItem(3+iCnt, 3+iFirstMomentumIndex+iCnt,
4418  iFirstReactionIndex+4, Tmp1.dGet(iCnt));
4419  WM.PutItem(6+iCnt, 3+iFirstMomentumIndex+iCnt,
4420  iFirstReactionIndex+5, Tmp2.dGet(iCnt));
4421  }
4422 
4423  WM.PutCross(10, iFirstMomentumIndex+3,
4424  iFirstReactionIndex, dTmp);
4425 
4426 
4427  /* Nota: F ed M, le reazioni vincolari, sono state aggiornate da AssRes */
4428 
4429  /* Termini diagonali del tipo: c*F/\d/\Delta_g
4430  * nota: la forza e' gia' moltiplicata per dCoef */
4431  WM.PutMat3x3(16, iFirstMomentumIndex+3, iFirstPositionIndex+3,
4432  Mat3x3(MatCrossCross, F*dCoef, dTmp) + Mat3x3(MatCrossCross, e3, MTmp*dCoef));
4433 
4434  /* Modifica: divido le equazioni di vincolo per dCoef */
4435 
4436  /* termini di vincolo dovuti al nodo 1 */
4437  for (int iCnt = 1; iCnt <= 3; iCnt++) {
4438  WM.PutItem(24+iCnt, iFirstReactionIndex+iCnt,
4439  iFirstPositionIndex+iCnt, -1.);
4440  }
4441 
4442  WM.PutCross(28, iFirstReactionIndex,
4443  iFirstPositionIndex+3, dTmp);
4444 
4445  for (int iCnt = 1; iCnt <= 3; iCnt ++) {
4446  WM.PutItem(33+iCnt, iFirstReactionIndex+4,
4447  iFirstPositionIndex+3+iCnt, -Tmp1.dGet(iCnt));
4448  WM.PutItem(36+iCnt, iFirstReactionIndex+5,
4449  iFirstPositionIndex+3+iCnt, Tmp2.dGet(iCnt));
4450  }
4451 
4452  return WorkMat;
4453 }
4454 
4455 
4456 /* Assemblaggio residuo */
4457 SubVectorHandler& PlanePinJoint::AssRes(SubVectorHandler& WorkVec,
4458  doublereal dCoef,
4459  const VectorHandler& XCurr,
4460  const VectorHandler& /* XPrimeCurr */ )
4461 {
4462  DEBUGCOUT("Entering PlanePinJoint::AssRes()" << std::endl);
4463 
4464  /* Dimensiona e resetta la matrice di lavoro */
4465  integer iNumRows = 0;
4466  integer iNumCols = 0;
4467  WorkSpaceDim(&iNumRows, &iNumCols);
4468  WorkVec.ResizeReset(iNumRows);
4469 
4470  integer iFirstMomentumIndex = pNode->iGetFirstMomentumIndex();
4471  integer iFirstReactionIndex = iGetFirstIndex();
4472 
4473  /* Indici dei nodi */
4474  for (int iCnt = 1; iCnt <= 6; iCnt++) {
4475  WorkVec.PutRowIndex(iCnt, iFirstMomentumIndex+iCnt);
4476  }
4477 
4478 
4479  /* Indici del vincolo */
4480  for (int iCnt = 1; iCnt <= 5; iCnt++) {
4481  WorkVec.PutRowIndex(6+iCnt, iFirstReactionIndex+iCnt);
4482  }
4483 
4484  F = Vec3(XCurr, iFirstReactionIndex+1);
4485  M = Vec3(XCurr(iFirstReactionIndex+4),
4486  XCurr(iFirstReactionIndex+5),
4487  0.);
4488 
4489  const Vec3& x(pNode->GetXCurr());
4490  const Mat3x3& R(pNode->GetRCurr());
4491 
4492  Vec3 dTmp(R*d);
4493  Mat3x3 RhTmp(R*Rh);
4494 
4495  Vec3 e3(R0.GetVec(3));
4496  Vec3 e1(RhTmp.GetVec(1));
4497  Vec3 e2(RhTmp.GetVec(2));
4498 
4499  WorkVec.Sub(1, F);
4500  WorkVec.Add(4, F.Cross(dTmp)-(e2*M.dGet(1)-e1*M.dGet(2)).Cross(e3)); /* Sfrutto il fatto che F/\d = -d/\F */
4501 
4502  /* Modifica: divido le equazioni di vincolo per dCoef */
4503  ASSERT(dCoef != 0.);
4504  WorkVec.Add(7, (x+dTmp-X0)/dCoef);
4505 
4506  WorkVec.PutCoef(10, e3.Dot(e2)/dCoef);
4507  WorkVec.PutCoef(11, e3.Dot(e1)/dCoef);
4508 
4509  return WorkVec;
4510 }
4511 
4512 /* Output (da mettere a punto) */
4513 void PlanePinJoint::Output(OutputHandler& OH) const
4514 {
4515  if (bToBeOutput()) {
4516  Mat3x3 RTmp(pNode->GetRCurr()*Rh);
4517  Mat3x3 R0Tmp(R0.MulTM(RTmp));
4518 
4519  Joint::Output(OH.Joints(), "PlanePin", GetLabel(),
4520  RTmp.MulTV(F), M, F, RTmp*M)
4521  << " " << MatR2EulerAngles(R0Tmp)*dRaDegr
4522  << " " << RTmp.MulTV(pNode->GetWCurr()) << std::endl;
4523  }
4524 }
4525 
4526 
4527 /* Contributo allo jacobiano durante l'assemblaggio iniziale */
4528 VariableSubMatrixHandler&
4529 PlanePinJoint::InitialAssJac(VariableSubMatrixHandler& WorkMat,
4530  const VectorHandler& XCurr)
4531 {
4532  FullSubMatrixHandler& WM = WorkMat.SetFull();
4533 
4534  /* Dimensiona e resetta la matrice di lavoro */
4535  integer iNumRows = 0;
4536  integer iNumCols = 0;
4537  InitialWorkSpaceDim(&iNumRows, &iNumCols);
4538  WM.ResizeReset(iNumRows, iNumCols);
4539 
4540  /* Equazioni: vedi joints.dvi */
4541 
4542  /* Indici */
4543  integer iFirstPositionIndex = pNode->iGetFirstPositionIndex();
4544  integer iFirstVelocityIndex = iFirstPositionIndex+6;
4545  integer iFirstReactionIndex = iGetFirstIndex();
4546  integer iReactionPrimeIndex = iFirstReactionIndex+5;
4547 
4548  /* Setto gli indici */
4549  for (int iCnt = 1; iCnt <= 6; iCnt++) {
4550  WM.PutRowIndex(iCnt, iFirstPositionIndex+iCnt);
4551  WM.PutColIndex(iCnt, iFirstPositionIndex+iCnt);
4552  WM.PutRowIndex(6+iCnt, iFirstVelocityIndex+iCnt);
4553  WM.PutColIndex(6+iCnt, iFirstVelocityIndex+iCnt);
4554  }
4555 
4556  for (int iCnt = 1; iCnt <= 10; iCnt++) {
4557  WM.PutRowIndex(12+iCnt, iFirstReactionIndex+iCnt);
4558  WM.PutColIndex(12+iCnt, iFirstReactionIndex+iCnt);
4559  }
4560 
4561  /* Matrici identita' */
4562 
4563  for (int iCnt = 1; iCnt <= 3; iCnt++) {
4564  /* Contributo di forza all'equazione della forza */
4565  WM.PutCoef(iCnt, 12+iCnt, 1.);
4566 
4567  /* Contrib. di der. di forza all'eq. della der. della forza */
4568  WM.PutCoef(6+iCnt, 17+iCnt, 1.);
4569 
4570  /* Equazione di vincolo */
4571  WM.PutCoef(12+iCnt, iCnt, -1.);
4572 
4573  /* Derivata dell'equazione di vincolo */
4574  WM.PutCoef(17+iCnt, 6+iCnt, -1.);
4575  }
4576 
4577  /* Recupera i dati */
4578  const Mat3x3& R(pNode->GetRRef());
4579  const Vec3& Omega(pNode->GetWRef());
4580  /* F, M sono state aggiornate da InitialAssRes */
4581  Vec3 FPrime(XCurr, iReactionPrimeIndex+1);
4582  Vec3 MPrime(XCurr(iReactionPrimeIndex+4),
4583  XCurr(iReactionPrimeIndex+5),
4584  0.);
4585 
4586  /* Distanza nel sistema globale */
4587  Vec3 dTmp(R*d);
4588  Mat3x3 RhTmp(R*Rh);
4589 
4590  Vec3 e3(R0.GetVec(3));
4591  Vec3 e1(RhTmp.GetVec(1));
4592  Vec3 e2(RhTmp.GetVec(2));
4593 
4594  /* Vettori temporanei */
4595  Vec3 Tmp1(e2.Cross(e3));
4596  Vec3 Tmp2(e3.Cross(e1));
4597 
4598  /* Prodotto vettore tra il versore 3 della cerniera secondo il nodo 1
4599  * ed il versore 1 della cerniera secondo il nodo 2. A convergenza
4600  * devono essere ortogonali, quindi il loro prodotto vettore deve essere
4601  * unitario */
4602 
4603  /* Error handling: il programma si ferma, pero' segnala dov'e' l'errore */
4604  if (Tmp1.Dot() <= std::numeric_limits<doublereal>::epsilon() || Tmp2.Dot() <= std::numeric_limits<doublereal>::epsilon()) {
4605  silent_cerr("PlanePinJoint(" << GetLabel() << "): "
4606  "node and fixed point hinge axes are (nearly) orthogonal"
4607  << std::endl);
4608  throw ErrNullNorm(MBDYN_EXCEPT_ARGS);
4609  }
4610 
4611  Vec3 TmpPrime1(e3.Cross(e2.Cross(Omega)));
4612  Vec3 TmpPrime2(e3.Cross(Omega.Cross(e1)));
4613 
4614  /* Ruota il momento e la sua derivata con le matrici della cerniera
4615  * rispetto ai nodi */
4616  Vec3 MTmp(e2*M.dGet(1)-e1*M.dGet(2));
4617  Vec3 MPrimeTmp(e2*MPrime.dGet(1)-e1*MPrime.dGet(2));
4618 
4619  Mat3x3 MDeltag(Mat3x3(MatCrossCross, e3, MPrimeTmp) + e3.Cross(Mat3x3(MatCrossCross, Omega, MTmp)));
4620 
4621  /* Matrici F/\d/\ */
4622  Mat3x3 FWedgedWedge(MatCrossCross, F, dTmp);
4623 
4624  /* Matrici (omega/\d)/\ */
4625  Mat3x3 OWedgedWedge(MatCross, Omega.Cross(dTmp));
4626 
4627  /* Equazione di momento */
4628  WM.Add(4, 4, FWedgedWedge + Mat3x3(MatCrossCross, e3, MTmp));
4629  WM.Add(4, 13, Mat3x3(MatCross, dTmp));
4630 
4631  /* Derivata dell'equazione di momento */
4632  WM.Add(10, 4, (Mat3x3(MatCross, FPrime) + Mat3x3(MatCrossCross, F, Omega))*Mat3x3(MatCross, dTmp) + MDeltag);
4633  WM.Add(10, 10, FWedgedWedge + Mat3x3(MatCrossCross, e3, MTmp));
4634  WM.Add(10, 13, OWedgedWedge);
4635  WM.Add(10, 18, Mat3x3(MatCross, dTmp));
4636 
4637  for (int iCnt = 1; iCnt <= 3; iCnt++) {
4638  doublereal d = Tmp1(iCnt);
4639  WM.PutCoef(3+iCnt, 16, d);
4640  WM.PutCoef(9+iCnt, 21, d);
4641 
4642  d = Tmp2(iCnt);
4643  WM.PutCoef(3+iCnt, 17, d);
4644  WM.PutCoef(9+iCnt, 22, d);
4645 
4646  WM.PutCoef(9+iCnt, 16, TmpPrime1(iCnt));
4647  WM.PutCoef(9+iCnt, 17, TmpPrime2(iCnt));
4648  }
4649 
4650  /* Equazione di vincolo */
4651  WM.Add(13, 4, Mat3x3(MatCross, dTmp));
4652 
4653  /* Derivata dell'equazione di vincolo */
4654  WM.Add(18, 4, OWedgedWedge);
4655  WM.Add(18, 10, Mat3x3(MatCross, dTmp));
4656 
4657  /* Equazioni di vincolo di rotazione: e1b~e3a, e2b~e3a */
4658  for (int iCnt = 1; iCnt <= 3; iCnt++) {
4659  doublereal d = -Tmp1(iCnt);
4660  WM.PutCoef(16, 3+iCnt, d);
4661 
4662  /* Queste sono per la derivata dell'equazione, sono qui solo per
4663  * ottimizzazione */
4664  WM.PutCoef(21, 9+iCnt, d);
4665 
4666  d = Tmp2(iCnt);
4667  WM.PutCoef(17, 3+iCnt, d);
4668 
4669  /* Queste sono per la derivata dell'equazione, sono qui solo per
4670  * ottimizzazione */
4671  WM.PutCoef(22, 9+iCnt, d);
4672  }
4673 
4674  /* Derivate delle equazioni di vincolo di rotazione: e1b~e3a, e2b~e3a */
4675  TmpPrime2 = e2.Cross(Omega.Cross(e3));
4676  for (int iCnt = 1; iCnt <= 3; iCnt++) {
4677  WM.PutCoef(21, 3+iCnt, TmpPrime2(iCnt));
4678  }
4679 
4680  TmpPrime2 = e1.Cross(Omega.Cross(e3));
4681  for (int iCnt = 1; iCnt <= 3; iCnt++) {
4682  WM.PutCoef(22, 3+iCnt, TmpPrime2(iCnt));
4683  }
4684 
4685  return WorkMat;
4686 }
4687 
4688 
4689 /* Contributo al residuo durante l'assemblaggio iniziale */
4690 SubVectorHandler&
4691 PlanePinJoint::InitialAssRes(SubVectorHandler& WorkVec,
4692  const VectorHandler& XCurr)
4693 {
4694  DEBUGCOUT("Entering PlanePinJoint::InitialAssRes()" << std::endl);
4695 
4696  /* Dimensiona e resetta la matrice di lavoro */
4697  integer iNumRows = 0;
4698  integer iNumCols = 0;
4699  InitialWorkSpaceDim(&iNumRows, &iNumCols);
4700  WorkVec.ResizeReset(iNumRows);
4701 
4702  /* Indici */
4703  integer iFirstPositionIndex = pNode->iGetFirstPositionIndex();
4704  integer iFirstVelocityIndex = iFirstPositionIndex+6;
4705  integer iFirstReactionIndex = iGetFirstIndex();
4706  integer iReactionPrimeIndex = iFirstReactionIndex+5;
4707 
4708  /* Setta gli indici */
4709  for (int iCnt = 1; iCnt <= 6; iCnt++) {
4710  WorkVec.PutRowIndex(iCnt, iFirstPositionIndex+iCnt);
4711  WorkVec.PutRowIndex(6+iCnt, iFirstVelocityIndex+iCnt);
4712  }
4713 
4714  for (int iCnt = 1; iCnt <= 10; iCnt++) {
4715  WorkVec.PutRowIndex(12+iCnt, iFirstReactionIndex+iCnt);
4716  }
4717 
4718  /* Recupera i dati */
4719  const Vec3& x(pNode->GetXCurr());
4720  const Vec3& v(pNode->GetVCurr());
4721  const Mat3x3& R(pNode->GetRCurr());
4722  const Vec3& Omega(pNode->GetWCurr());
4723 
4724  Mat3x3 RhTmp(R*Rh);
4725 
4726  F = Vec3(XCurr, iFirstReactionIndex+1);
4727  M = Vec3(XCurr(iFirstReactionIndex+4),
4728  XCurr(iFirstReactionIndex+5),
4729  0.);
4730  Vec3 FPrime(XCurr, iReactionPrimeIndex+1);
4731  Vec3 MPrime(XCurr(iReactionPrimeIndex+4),
4732  XCurr(iReactionPrimeIndex+5),
4733  0.);
4734 
4735  /* Versori delle cerniere */
4736  Vec3 e3(R0.GetVec(3));
4737  Vec3 e1(RhTmp.GetVec(1));
4738  Vec3 e2(RhTmp.GetVec(2));
4739 
4740  /* Vettori temporanei */
4741  Vec3 Tmp1(e2.Cross(e3));
4742  Vec3 Tmp2(e3.Cross(e1));
4743 
4744  Vec3 TmpPrime1(e3.Cross(e2.Cross(Omega)));
4745  Vec3 TmpPrime2(e3.Cross(Omega.Cross(e1)));
4746 
4747  /* Distanza nel sistema globale */
4748  Vec3 dTmp(R*d);
4749 
4750  /* Vettori omega/\d */
4751  Vec3 OWedged(Omega.Cross(dTmp));
4752 
4753  /* Ruota il momento e la sua derivata con le matrici della cerniera
4754  * rispetto ai nodi */
4755  Vec3 MTmp(e2*M.dGet(1)-e1*M.dGet(2));
4756  Vec3 MPrimeTmp(e3.Cross(MTmp.Cross(Omega))+
4757  e2.Cross(e3)*MPrime.dGet(1)+e3.Cross(e1)*MPrime.dGet(2));
4758 
4759  /* Equazioni di equilibrio */
4760  WorkVec.Sub(1, F);
4761  WorkVec.Add(4, F.Cross(dTmp)-MTmp.Cross(e3)); /* Sfrutto il fatto che F/\d = -d/\F */
4762 
4763  /* Derivate delle equazioni di equilibrio, nodo 1 */
4764  WorkVec.Sub(7, FPrime);
4765  WorkVec.Add(10, FPrime.Cross(dTmp)-OWedged.Cross(F)-MPrimeTmp);
4766 
4767  /* Equazione di vincolo di posizione */
4768  WorkVec.Add(13, x+dTmp-X0);
4769 
4770  /* Equazioni di vincolo di rotazione */
4771  WorkVec.PutCoef(16, e2.Dot(e3));
4772  WorkVec.PutCoef(17, e1.Dot(e3));
4773 
4774  /* Derivata dell'equazione di vincolo di posizione */
4775  WorkVec.Add(18, v+OWedged);
4776 
4777  /* Derivate delle equazioni di vincolo di rotazione: e1b~e3a, e2b~e3a */
4778  Vec3 Tmp(e3.Cross(Omega));
4779  WorkVec.PutCoef(21, e2.Dot(Tmp));
4780  WorkVec.PutCoef(22, e1.Dot(Tmp));
4781 
4782  return WorkVec;
4783 }
4784 
4785 unsigned int
4786 PlanePinJoint::iGetNumPrivData(void) const
4787 {
4788  return 2;
4789 }
4790 
4791 unsigned int
4792 PlanePinJoint::iGetPrivDataIdx(const char *s) const
4793 {
4794  ASSERT(s != NULL);
4795 
4796  unsigned int idx = 0;
4797 
4798  switch (s[0]) {
4799  case 'w':
4800  idx++;
4801  case 'r':
4802  idx++;
4803  if (s[1] == 'z') {
4804  return idx;
4805  }
4806  break;
4807 
4808  case 'M':
4809  idx += 3;
4810  case 'F':
4811  idx += 2;
4812 
4813  switch (s[1]) {
4814  case 'x':
4815  return idx + 1;
4816 
4817  case 'y':
4818  return idx + 2;
4819 
4820  case 'z':
4821  return idx + 3;
4822  }
4823  }
4824 
4825  return 0;
4826 }
4827 
4828 doublereal
4829 PlanePinJoint::dGetPrivData(unsigned int i) const
4830 {
4831  ASSERT(i >= 1 && i <= iGetNumPrivData());
4832 
4833  switch (i) {
4834  case 1: {
4835  Vec3 v(RotManip::VecRot(R0.MulTM(pNode->GetRCurr()*Rh)));
4836  doublereal dThetaTmp(v(3));
4837 
4838  int n = 0;
4839 
4840  if (dThetaTmp - dThetaWrapped < -M_PI) {
4841  n++;
4842  }
4843 
4844  if (dThetaTmp - dThetaWrapped > M_PI) {
4845  n--;
4846  }
4847 
4848  return 2*M_PI*(NTheta + n) + dThetaTmp;
4849  }
4850 
4851  case 2: {
4852  Mat3x3 RTmp(pNode->GetRCurr()*Rh);
4853  Vec3 v(RTmp.MulTV(pNode->GetWCurr()));
4854 
4855  return v(3);
4856  }
4857 
4858  case 3:
4859  case 4:
4860  case 5:
4861  return F(i - 2);
4862 
4863  case 6:
4864  case 7:
4865  case 8:
4866  return M(i - 5);
4867  }
4868 
4869  silent_cerr("PlanePinJoint(" << GetLabel() << "): "
4870  "illegal private data " << i << std::endl);
4871  throw ErrGeneric(MBDYN_EXCEPT_ARGS);
4872 }
4873 
4874 /* PlanePinJoint - end */
Hint
Definition: hint.h:38
SimulationEntity::GetEqType
virtual DofOrder::Order GetEqType(unsigned int i) const
Definition: simentity.h:138
PlaneRotationJoint::iGetNumDof
virtual unsigned int iGetNumDof(void) const
Definition: planej.h:250
PlaneRotationJoint::WorkSpaceDim
void WorkSpaceDim(integer *piNumRows, integer *piNumCols) const
Definition: planej.h:283
PlaneHingeJoint::preF
const doublereal preF
Definition: planej.h:75
PlaneHingeJoint::ParseHint
virtual Hint * ParseHint(DataManager *pDM, const char *s) const
Definition: planej.cc:432
AxialRotationJoint::pNode2
const StructNode * pNode2
Definition: planej.h:358
PlaneRotationJoint::AssRes
SubVectorHandler & AssRes(SubVectorHandler &WorkVec, doublereal dCoef, const VectorHandler &XCurr, const VectorHandler &XPrimeCurr)
Definition: planej.cc:1947
PlanePinJoint::AssRes
SubVectorHandler & AssRes(SubVectorHandler &WorkVec, doublereal dCoef, const VectorHandler &XCurr, const VectorHandler &XPrimeCurr)
Definition: planej.cc:4457
FullSubMatrixHandler::PutColIndex
void PutColIndex(integer iSubCol, integer iCol)
Definition: submat.h:325
PlanePinJoint::AssJac
VariableSubMatrixHandler & AssJac(VariableSubMatrixHandler &WorkMat, doublereal dCoef, const VectorHandler &XCurr, const VectorHandler &XPrimeCurr)
Definition: planej.cc:4358
PlaneHingeJoint::M3
doublereal M3
Definition: planej.h:77
M_PI
#define M_PI
Definition: gradienttest.cc:67
SparseSubMatrixHandler::PutMat3x3
void PutMat3x3(integer iSubIt, integer iFirstRow, integer iFirstCol, const Mat3x3 &m)
Definition: submat.cc:1331
Zero3
const Vec3 Zero3(0., 0., 0.)
Vec3::Cross
Vec3 Cross(const Vec3 &v) const
Definition: matvec3.h:218
PlanePinJoint::DescribeEq
virtual std::ostream & DescribeEq(std::ostream &out, const char *prefix="", bool bInitial=false) const
Definition: planej.cc:4111
ASSERTMSGBREAK
#define ASSERTMSGBREAK(expr, msg)
Definition: myassert.h:222
ExpandableRowVector::Set
void Set(doublereal xx, integer i, integer iidx)
Definition: JacSubMatrix.cc:95
BasicFriction::AssRes
virtual void AssRes(SubVectorHandler &WorkVec, const unsigned int startdof, const unsigned int solution_startdof, const doublereal F, const doublereal v, const VectorHandler &X, const VectorHandler &XP)=0
PlaneHingeJoint::AssJac
VariableSubMatrixHandler & AssJac(VariableSubMatrixHandler &WorkMat, doublereal dCoef, const VectorHandler &XCurr, const VectorHandler &XPrimeCurr)
Definition: planej.cc:540
Vec3::Write
std::ostream & Write(std::ostream &out, const char *sFill=" ") const
Definition: matvec3.cc:738
sh
static void sh(int signum)
Definition: test_modalext.cc:48
flag
long int flag
Definition: mbdyn.h:43
StructNode::GetRRef
virtual const Mat3x3 & GetRRef(void) const
Definition: strnode.h:1006
ToBeOutput::bToBeOutput
virtual bool bToBeOutput(void) const
Definition: output.cc:890
PlaneRotationJoint::od
OrientationDescription od
Definition: planej.h:229
PlanePinJoint::ParseHint
virtual Hint * ParseHint(DataManager *pDM, const char *s) const
Definition: planej.cc:4260
PlanePinJoint::AfterConvergence
virtual void AfterConvergence(const VectorHandler &X, const VectorHandler &XP)
Definition: planej.cc:4305
MBDYN_EXCEPT_ARGS
#define MBDYN_EXCEPT_ARGS
Definition: except.h:63
Vec3
Definition: matvec3.h:98
VectorHandler::ResizeReset
virtual void ResizeReset(integer)
Definition: vh.cc:55
PlaneHingeJoint::OutputPrepare
void OutputPrepare(OutputHandler &OH)
Definition: planej.cc:949
MatCross
const MatCross_Manip MatCross
Definition: matvec3.cc:639
OutputHandler::UseNetCDF
bool UseNetCDF(int out) const
Definition: output.cc:491
VariableSubMatrixHandler::SetFull
FullSubMatrixHandler & SetFull(void)
Definition: submat.h:1168
StructNode::GetRCurr
virtual const Mat3x3 & GetRCurr(void) const
Definition: strnode.h:1012
PlanePinJoint::DescribeDof
virtual std::ostream & DescribeDof(std::ostream &out, const char *prefix="", bool bInitial=false) const
Definition: planej.cc:4003
Vec3::Norm
doublereal Norm(void) const
Definition: matvec3.h:263
PlaneHingeJoint::NumDof
static const unsigned int NumDof
Definition: planej.h:79
Vec3::Dot
doublereal Dot(const Vec3 &v) const
Definition: matvec3.h:243
AxialRotationJoint::Output
void Output(OutputHandler &OH) const
Definition: planej.cc:3391
FullSubMatrixHandler::Add
void Add(integer iRow, integer iCol, const Vec3 &v)
Definition: submat.cc:209
PlaneRotationJoint::PlaneRotationJoint
PlaneRotationJoint(unsigned int uL, const DofOwner *pDO, const StructNode *pN1, const StructNode *pN2, const Mat3x3 &R1hTmp, const Mat3x3 &R2hTmp, const OrientationDescription &od, flag fOut)
Definition: planej.cc:1533
PlaneHingeJoint::DescribeDof
virtual std::ostream & DescribeDof(std::ostream &out, const char *prefix="", bool bInitial=false) const
Definition: planej.cc:98
PlaneRotationJoint::Restart
virtual std::ostream & Restart(std::ostream &out) const
Definition: planej.cc:1796
SimulationEntity::ParseHint
virtual Hint * ParseHint(DataManager *pDM, const char *s) const
Definition: simentity.cc:76
Write
std::ostream & Write(std::ostream &out, const FullMatrixHandler &m, const char *s, const char *s2)
Definition: fullmh.cc:376
PlaneRotationJoint::AfterConvergence
virtual void AfterConvergence(const VectorHandler &X, const VectorHandler &XP)
Definition: planej.cc:1772
AxialRotationJoint::dTheta
doublereal dTheta
Definition: planej.h:366
PlaneHingeJoint::pNode2
const StructNode * pNode2
Definition: planej.h:54
PlaneHingeJoint::Sh_c
BasicShapeCoefficient *const Sh_c
Definition: planej.h:73
ExpandableRowVector::Add
void Add(doublereal xx, integer i)
Definition: JacSubMatrix.cc:119
PlaneRotationJoint::~PlaneRotationJoint
~PlaneRotationJoint(void)
Definition: planej.cc:1557
PlaneHingeJoint::r
const doublereal r
Definition: planej.h:76
BasicFriction::SetValue
void SetValue(DataManager *pDM, VectorHandler &X, VectorHandler &XP, SimulationEntity::Hints *ph=0)
Definition: friction.h:45
ExpandableMatrix::Set
void Set(doublereal xx, integer eq, integer block, integer block_col=1)
Definition: JacSubMatrix.cc:360
VectorHandler::Sub
virtual void Sub(integer iRow, const Vec3 &v)
Definition: vh.cc:78
SAFEDELETEARR
#define SAFEDELETEARR(pnt)
Definition: mynewmem.h:713
DriveOwner::dGet
doublereal dGet(void) const
Definition: drive.cc:671
PlaneHingeJoint::iGetNumDof
virtual unsigned int iGetNumDof(void) const
Definition: planej.cc:917
OrientationDescription
OrientationDescription
Definition: matvec3.h:1597
AxialRotationJoint::dThetaWrapped
doublereal dThetaWrapped
Definition: planej.h:366
FullSubMatrixHandler::PutCoef
void PutCoef(integer iRow, integer iCol, const doublereal &dCoef)
Definition: submat.h:672
PlanePinJoint::NTheta
int NTheta
Definition: planej.h:531
PlaneHingeJoint::AssRes
SubVectorHandler & AssRes(SubVectorHandler &WorkVec, doublereal dCoef, const VectorHandler &XCurr, const VectorHandler &XPrimeCurr)
Definition: planej.cc:813
PlaneHingeJoint::Output
void Output(OutputHandler &OH) const
Definition: planej.cc:975
ExpandableMatrix::Sub
void Sub(doublereal xx, integer eq, integer block, integer block_col=1)
Definition: JacSubMatrix.cc:414
SparseSubMatrixHandler::ResizeReset
void ResizeReset(integer iNewRow, integer iNewCol)
Definition: submat.cc:1084
BasicShapeCoefficient::Sh_c
virtual doublereal Sh_c(void) const =0
AxialRotationJoint::fc
BasicFriction *const fc
Definition: planej.h:377
xyz
static const char xyz[]
Definition: planej.cc:133
PlaneHingeJoint::GetEqType
DofOrder::Order GetEqType(unsigned int i) const
Definition: planej.cc:937
StructNode::GetWRef
virtual const Vec3 & GetWRef(void) const
Definition: strnode.h:1024
NO_OP
#define NO_OP
Definition: myassert.h:74
SparseSubMatrixHandler::PutCross
void PutCross(integer iSubIt, integer iFirstRow, integer iFirstCol, const Vec3 &v)
Definition: submat.cc:1236
PlaneHingeJoint::iGetNumPrivData
virtual unsigned int iGetNumPrivData(void) const
Definition: planej.cc:1440
SimulationEntity::Hints
std::vector< Hint * > Hints
Definition: simentity.h:89
AxialRotationJoint::OutputPrepare
void OutputPrepare(OutputHandler &OH)
Definition: planej.cc:3365
PlanePinJoint::WorkSpaceDim
virtual void WorkSpaceDim(integer *piNumRows, integer *piNumCols) const
Definition: planej.h:591
Mat3x3::GetVec
Vec3 GetVec(unsigned short int i) const
Definition: matvec3.h:893
AxialRotationJoint::AxialRotationJoint
AxialRotationJoint(unsigned int uL, const DofOwner *pDO, const StructNode *pN1, const StructNode *pN2, const Vec3 &dTmp1, const Vec3 &dTmp2, const Mat3x3 &R1hTmp, const Mat3x3 &R2hTmp, const DriveCaller *pDC, const OrientationDescription &od, flag fOut, const doublereal rr=0., const doublereal pref=0., BasicShapeCoefficient *const sh=0, BasicFriction *const f=0)
Definition: planej.cc:2521
PlaneHingeJoint::~PlaneHingeJoint
~PlaneHingeJoint(void)
Definition: planej.cc:86
PlaneRotationJoint::SetValue
virtual void SetValue(DataManager *pDM, VectorHandler &X, VectorHandler &XP, SimulationEntity::Hints *ph=0)
Definition: planej.cc:1720
PlaneHingeJoint::calcInitdTheta
bool calcInitdTheta
Definition: planej.h:68
AxialRotationJoint::WorkSpaceDim
void WorkSpaceDim(integer *piNumRows, integer *piNumCols) const
Definition: planej.h:454
PlaneHingeJoint::SetValue
virtual void SetValue(DataManager *pDM, VectorHandler &X, VectorHandler &XP, SimulationEntity::Hints *ph=0)
Definition: planej.cc:374
PlanePinJoint::InitialAssJac
VariableSubMatrixHandler & InitialAssJac(VariableSubMatrixHandler &WorkMat, const VectorHandler &XCurr)
Definition: planej.cc:4529
DriveCaller::Restart
virtual std::ostream & Restart(std::ostream &out) const =0
BasicFriction::fc
virtual doublereal fc(void) const =0
RotManip::VecRot
Vec3 VecRot(const Mat3x3 &Phi)
Definition: Rot.cc:136
PlanePinJoint::InitialWorkSpaceDim
virtual void InitialWorkSpaceDim(integer *piNumRows, integer *piNumCols) const
Definition: planej.h:613
Mat3x3::MulTV
Vec3 MulTV(const Vec3 &v) const
Definition: matvec3.cc:482
PlaneRotationJoint::dThetaWrapped
doublereal dThetaWrapped
Definition: planej.h:222
PlaneHingeJoint::GetDofType
DofOrder::Order GetDofType(unsigned int i) const
Definition: planej.cc:927
PlanePinJoint::pNode
const StructNode * pNode
Definition: planej.h:523
PlanePinJoint::Rh
Mat3x3 Rh
Definition: planej.h:527
PlaneHingeJoint::InitialAssRes
SubVectorHandler & InitialAssRes(SubVectorHandler &WorkVec, const VectorHandler &XCurr)
Definition: planej.cc:1333
SubVectorHandler::PutRowIndex
virtual void PutRowIndex(integer iSubRow, integer iRow)=0
PlaneHingeJoint::InitialAssJac
VariableSubMatrixHandler & InitialAssJac(VariableSubMatrixHandler &WorkMat, const VectorHandler &XCurr)
Definition: planej.cc:1079
AxialRotationJoint::preF
const doublereal preF
Definition: planej.h:378
AxialRotationJoint::SetValue
virtual void SetValue(DataManager *pDM, VectorHandler &X, VectorHandler &XP, SimulationEntity::Hints *ph=0)
Definition: planej.cc:2853
AxialRotationJoint::AssRes
SubVectorHandler & AssRes(SubVectorHandler &WorkVec, doublereal dCoef, const VectorHandler &XCurr, const VectorHandler &XPrimeCurr)
Definition: planej.cc:3248
SparseSubMatrixHandler::PutItem
void PutItem(integer iSubIt, integer iRow, integer iCol, const doublereal &dCoef)
Definition: submat.h:997
PlaneHingeJoint::InitialWorkSpaceDim
virtual void InitialWorkSpaceDim(integer *piNumRows, integer *piNumCols) const
Definition: planej.h:171
PlaneRotationJoint::OutputPrepare
void OutputPrepare(OutputHandler &OH)
Definition: planej.cc:2027
ExpandableRowVector::ReDim
void ReDim(const integer n)
Definition: JacSubMatrix.cc:50
PlaneRotationJoint::InitialAssRes
SubVectorHandler & InitialAssRes(SubVectorHandler &WorkVec, const VectorHandler &XCurr)
Definition: planej.cc:2347
BasicFriction::AfterConvergence
virtual void AfterConvergence(const doublereal F, const doublereal v, const VectorHandler &X, const VectorHandler &XP, const unsigned int solution_startdof)
Definition: friction.h:96
AxialRotationJoint::DescribeEq
virtual std::ostream & DescribeEq(std::ostream &out, const char *prefix="", bool bInitial=false) const
Definition: planej.cc:2705
MatR2EulerAngles313
Vec3 MatR2EulerAngles313(const Mat3x3 &R)
Definition: matvec3.cc:927
PlaneRotationJoint::iGetNumPrivData
virtual unsigned int iGetNumPrivData(void) const
Definition: planej.cc:2432
AxialRotationJoint::iGetNumPrivData
virtual unsigned int iGetNumPrivData(void) const
Definition: planej.cc:3887
AxialRotationJoint::InitialAssRes
SubVectorHandler & InitialAssRes(SubVectorHandler &WorkVec, const VectorHandler &XCurr)
Definition: planej.cc:3773
AxialRotationJoint::ParseHint
virtual Hint * ParseHint(DataManager *pDM, const char *s) const
Definition: planej.cc:2912
PlaneHingeJoint::WorkSpaceDim
void WorkSpaceDim(integer *piNumRows, integer *piNumCols) const
Definition: planej.h:141
AxialRotationJoint::iGetPrivDataIdx
virtual unsigned int iGetPrivDataIdx(const char *s) const
Definition: planej.cc:3893
AxialRotationJoint::NumDof
static const unsigned int NumDof
Definition: planej.h:382
OutputHandler::GetCurrentStep
long GetCurrentStep(void) const
Definition: output.h:116
Vec3::dGet
const doublereal & dGet(unsigned short int iRow) const
Definition: matvec3.h:285
hint_impl.h
PlaneHingeJoint::fc
BasicFriction *const fc
Definition: planej.h:74
PlanePinJoint::Output
virtual void Output(OutputHandler &OH) const
Definition: planej.cc:4513
PlaneHingeJoint::Restart
virtual std::ostream & Restart(std::ostream &out) const
Definition: planej.cc:519
PlanePinJoint::iGetNumPrivData
virtual unsigned int iGetNumPrivData(void) const
Definition: planej.cc:4786
DEBUGCOUT
#define DEBUGCOUT(msg)
Definition: myassert.h:232
SimulationEntity::DescribeDof
virtual std::ostream & DescribeDof(std::ostream &out, const char *prefix="", bool bInitial=false) const =0
PlanePinJoint::calcInitdTheta
bool calcInitdTheta
Definition: planej.h:530
MatR2EulerAngles123
Vec3 MatR2EulerAngles123(const Mat3x3 &R)
Definition: matvec3.cc:893
SimulationEntity::DescribeEq
virtual std::ostream & DescribeEq(std::ostream &out, const char *prefix="", bool bInitial=false) const =0
AxialRotationJoint::dGetPrivData
virtual doublereal dGetPrivData(unsigned int i) const
Definition: planej.cc:3930
PlaneRotationJoint::DescribeEq
virtual std::ostream & DescribeEq(std::ostream &out, const char *prefix="", bool bInitial=false) const
Definition: planej.cc:1642
PlanePinJoint::R0
Mat3x3 R0
Definition: planej.h:525
PlaneRotationJoint::ParseHint
virtual Hint * ParseHint(DataManager *pDM, const char *s) const
Definition: planej.cc:1750
PlaneHingeJoint::AfterConvergence
virtual void AfterConvergence(const VectorHandler &X, const VectorHandler &XP)
Definition: planej.cc:473
StructDispNode::iGetFirstMomentumIndex
virtual integer iGetFirstMomentumIndex(void) const =0
StructDispNode::iGetFirstPositionIndex
virtual integer iGetFirstPositionIndex(void) const
Definition: strnode.h:452
StructNode::GetWCurr
virtual const Vec3 & GetWCurr(void) const
Definition: strnode.h:1030
dRaDegr
const doublereal dRaDegr
Definition: matvec3.cc:884
PlanePinJoint::GetEqType
DofOrder::Order GetEqType(unsigned int i) const
Definition: planej.cc:4297
Joint::Restart
virtual std::ostream & Restart(std::ostream &out) const
Definition: joint.h:195
SimulationEntity::GetDofType
virtual DofOrder::Order GetDofType(unsigned int i) const =0
OutputHandler::Joints
std::ostream & Joints(void) const
Definition: output.h:443
DriveHint::pCreateDrive
DriveCaller * pCreateDrive(DataManager *pDM) const
Definition: hint_impl.cc:117
PlaneRotationJoint::dTheta
doublereal dTheta
Definition: planej.h:222
ExpandableMatrix::Add
void Add(doublereal xx, integer eq, integer block, integer block_col=1)
Definition: JacSubMatrix.cc:405
grad::Dot
DotTraits< VectorExprLhs, VectorExprRhs, N_rows, N_rows >::ExpressionType Dot(const VectorExpression< VectorExprLhs, N_rows > &u, const VectorExpression< VectorExprRhs, N_rows > &v)
Definition: matvec.h:3133
BasicShapeCoefficient::dSh_c
virtual void dSh_c(ExpandableRowVector &dShc, const doublereal f, const doublereal F, const doublereal v, const ExpandableRowVector &dfc, const ExpandableRowVector &dF, const ExpandableRowVector &dv) const =0
AxialRotationJoint::~AxialRotationJoint
~AxialRotationJoint(void)
Definition: planej.cc:2554
ASSERT
#define ASSERT(expression)
Definition: colamd.c:977
PlanePinJoint::PlanePinJoint
PlanePinJoint(unsigned int uL, const DofOwner *pDO, const StructNode *pN, const Vec3 &X0Tmp, const Mat3x3 &R0Tmp, const Vec3 &dTmp, const Mat3x3 &RhTmp, flag fOut, const bool _calcInitdTheta, const doublereal initDTheta)
Definition: planej.cc:3977
AxialRotationJoint::r
const doublereal r
Definition: planej.h:379
Mat3x3::MulTM
Mat3x3 MulTM(const Mat3x3 &m) const
Definition: matvec3.cc:500
VectorHandler::PutCoef
virtual void PutCoef(integer iRow, const doublereal &dCoef)=0
grad::Cross
VectorExpression< VectorCrossExpr< VectorLhsExpr, VectorRhsExpr >, 3 > Cross(const VectorExpression< VectorLhsExpr, 3 > &u, const VectorExpression< VectorRhsExpr, 3 > &v)
Definition: matvec.h:3248
Joint::OutputPrepare_int
virtual void OutputPrepare_int(const std::string &type, OutputHandler &OH, std::string &name)
Definition: joint.cc:107
MatR2EulerAngles
Vec3 MatR2EulerAngles(const Mat3x3 &R)
Definition: matvec3.cc:887
PlaneHingeJoint::od
OrientationDescription od
Definition: planej.h:83
DriveOwner::pGetDriveCaller
DriveCaller * pGetDriveCaller(void) const
Definition: drive.cc:658
StructDispNode::GetXCurr
virtual const Vec3 & GetXCurr(void) const
Definition: strnode.h:310
VectorHandler::Add
virtual void Add(integer iRow, const Vec3 &v)
Definition: vh.cc:63
ExpandableRowVector::Link
void Link(const integer i, const ExpandableRowVector *const xp, const integer rhs_block=1)
Definition: JacSubMatrix.cc:68
AxialRotationJoint::M3
doublereal M3
Definition: planej.h:380
ExpandableMatrix::ReDim
void ReDim(const integer n, const integer m)
Definition: JacSubMatrix.cc:287
FullSubMatrixHandler::ResizeReset
virtual void ResizeReset(integer, integer)
Definition: submat.cc:182
PlanePinJoint::dGetPrivData
virtual doublereal dGetPrivData(unsigned int i) const
Definition: planej.cc:4829
PlaneHingeJoint::ReadInitialState
virtual void ReadInitialState(MBDynParser &HP)
Definition: planej.cc:510
DriveCaller::dGet
virtual doublereal dGet(const doublereal &dVar) const =0
AxialRotationJoint::iGetNumDof
virtual unsigned int iGetNumDof(void) const
Definition: planej.h:413
BasicFriction::AssJac
virtual void AssJac(FullSubMatrixHandler &WorkMat, ExpandableRowVector &dfc, const unsigned int startdof, const unsigned int solution_startdof, const doublereal dCoef, const doublereal F, const doublereal v, const VectorHandler &X, const VectorHandler &XP, const ExpandableRowVector &dF, const ExpandableRowVector &dv) const =0
PlaneRotationJoint::AssJac
VariableSubMatrixHandler & AssJac(VariableSubMatrixHandler &WorkMat, doublereal dCoef, const VectorHandler &XCurr, const VectorHandler &XPrimeCurr)
Definition: planej.cc:1812
AxialRotationJoint::Sh_c
BasicShapeCoefficient *const Sh_c
Definition: planej.h:376
SimulationEntity::iGetNumDof
virtual unsigned int iGetNumDof(void) const =0
PlaneHingeJoint::dThetaWrapped
doublereal dThetaWrapped
Definition: planej.h:70
AxialRotationJoint::InitialWorkSpaceDim
virtual void InitialWorkSpaceDim(integer *piNumRows, integer *piNumCols) const
Definition: planej.h:484
AxialRotationJoint::od
OrientationDescription od
Definition: planej.h:386
MatCrossCross
const MatCrossCross_Manip MatCrossCross
Definition: matvec3.cc:640
AxialRotationJoint::AfterConvergence
virtual void AfterConvergence(const VectorHandler &X, const VectorHandler &XP)
Definition: planej.cc:2952
VectorHandler::Put
virtual void Put(integer iRow, const Vec3 &v)
Definition: vh.cc:93
PlanePinJoint::~PlanePinJoint
~PlanePinJoint(void)
Definition: planej.cc:3996
STRLENOF
#define STRLENOF(s)
Definition: mbdyn.h:166
PlaneRotationJoint::InitialWorkSpaceDim
virtual void InitialWorkSpaceDim(integer *piNumRows, integer *piNumCols) const
Definition: planej.h:309
PlanePinJoint::Restart
virtual std::ostream & Restart(std::ostream &out) const
Definition: planej.cc:4336
Elem
Definition: elem.h:75
PlanePinJoint::InitialAssRes
SubVectorHandler & InitialAssRes(SubVectorHandler &WorkVec, const VectorHandler &XCurr)
Definition: planej.cc:4691
PlanePinJoint::iGetNumDof
virtual unsigned int iGetNumDof(void) const
Definition: planej.h:556
PlaneHingeJoint::iGetPrivDataIdx
virtual unsigned int iGetPrivDataIdx(const char *s) const
Definition: planej.cc:1446
FullSubMatrixHandler::PutRowIndex
void PutRowIndex(integer iSubRow, integer iRow)
Definition: submat.h:311
PlaneHingeJoint::dGetPrivData
virtual doublereal dGetPrivData(unsigned int i) const
Definition: planej.cc:1482
Vec3::pGetVec
const doublereal * pGetVec(void) const
Definition: matvec3.h:192
MatR2EulerAngles321
Vec3 MatR2EulerAngles321(const Mat3x3 &R)
Definition: matvec3.cc:941
DriveOwner::Set
void Set(const DriveCaller *pDC)
Definition: drive.cc:647
PlanePinJoint::dThetaWrapped
doublereal dThetaWrapped
Definition: planej.h:532
PlaneHingeJoint::R2h
Mat3x3 R2h
Definition: planej.h:58
PlaneRotationJoint::Output
void Output(OutputHandler &OH) const
Definition: planej.cc:2045
AxialRotationJoint::NumSelfDof
static const unsigned int NumSelfDof
Definition: planej.h:381
ExpandableRowVector::Sub
void Sub(doublereal xx, integer i)
Definition: JacSubMatrix.cc:124
AxialRotationJoint::Restart
virtual std::ostream & Restart(std::ostream &out) const
Definition: planej.cc:2989
StructDispNode::GetVCurr
virtual const Vec3 & GetVCurr(void) const
Definition: strnode.h:322
FullSubMatrixHandler::Sub
void Sub(integer iRow, integer iCol, const Vec3 &v)
Definition: submat.cc:215
SAFENEWARR
#define SAFENEWARR(pnt, item, sz)
Definition: mynewmem.h:701
AxialRotationJoint::pNode1
const StructNode * pNode1
Definition: planej.h:357
PlaneHingeJoint::PlaneHingeJoint
PlaneHingeJoint(unsigned int uL, const DofOwner *pDO, const StructNode *pN1, const StructNode *pN2, const Vec3 &dTmp1, const Vec3 &dTmp2, const Mat3x3 &R1hTmp, const Mat3x3 &R2hTmp, const OrientationDescription &od, flag fOut, const bool _calcInitdTheta=true, const doublereal initDTheta=0., const doublereal rr=0., const doublereal pref=0., BasicShapeCoefficient *const sh=0, BasicFriction *const f=0)
Definition: planej.cc:49
Joint
Definition: joint.h:50
AxialRotationJoint::AssJac
VariableSubMatrixHandler & AssJac(VariableSubMatrixHandler &WorkMat, doublereal dCoef, const VectorHandler &XCurr, const VectorHandler &XPrimeCurr)
Definition: planej.cc:3007
PlaneRotationJoint::pNode2
const StructNode * pNode2
Definition: planej.h:217
VariableSubMatrixHandler::SetSparse
SparseSubMatrixHandler & SetSparse(void)
Definition: submat.h:1178
PlanePinJoint::ReadInitialState
virtual void ReadInitialState(MBDynParser &HP)
Definition: planej.cc:4329
DofOwnerOwner::iGetFirstIndex
virtual integer iGetFirstIndex(void) const
Definition: dofown.h:127
PlanePinJoint::dTheta
doublereal dTheta
Definition: planej.h:532
doublereal
double doublereal
Definition: colamd.c:52
AxialRotationJoint::GetEqType
DofOrder::Order GetEqType(unsigned int i) const
Definition: planej.cc:3351
Joint::Output
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
PlaneRotationJoint::pNode1
const StructNode * pNode1
Definition: planej.h:216
ExpandableMatrix::SetBlockDim
void SetBlockDim(const integer block, const integer ncols)
Definition: JacSubMatrix.cc:296
PlaneHingeJoint::R1h
Mat3x3 R1h
Definition: planej.h:56
integer
long int integer
Definition: colamd.c:51
PlaneRotationJoint::dGetPrivData
virtual doublereal dGetPrivData(unsigned int i) const
Definition: planej.cc:2472
ExpandableMatrix::Link
void Link(const integer block, const ExpandableMatrix *const xp)
Definition: JacSubMatrix.cc:340
PlaneRotationJoint::DescribeDof
virtual std::ostream & DescribeDof(std::ostream &out, const char *prefix="", bool bInitial=false) const
Definition: planej.cc:1564
PlaneHingeJoint::DescribeEq
virtual std::ostream & DescribeEq(std::ostream &out, const char *prefix="", bool bInitial=false) const
Definition: planej.cc:237
WithLabel::GetLabel
unsigned int GetLabel(void) const
Definition: withlab.cc:62
PlanePinJoint::iGetPrivDataIdx
virtual unsigned int iGetPrivDataIdx(const char *s) const
Definition: planej.cc:4792
AxialRotationJoint::InitialAssJac
VariableSubMatrixHandler & InitialAssJac(VariableSubMatrixHandler &WorkMat, const VectorHandler &XCurr)
Definition: planej.cc:3494
MBDynParser::GetVec3
virtual Vec3 GetVec3(void)
Definition: mbpar.cc:2220
PlaneHingeJoint::pNode1
const StructNode * pNode1
Definition: planej.h:53
PlaneRotationJoint::GetEqType
DofOrder::Order GetEqType(unsigned int i) const
Definition: planej.cc:2018
AxialRotationJoint::DescribeDof
virtual std::ostream & DescribeDof(std::ostream &out, const char *prefix="", bool bInitial=false) const
Definition: planej.cc:2567
R
Mat3x3 R
Definition: mbdyn_util_oct.cc:74
OutputHandler::UseText
bool UseText(int out) const
Definition: output.cc:446
PlaneRotationJoint::iGetPrivDataIdx
virtual unsigned int iGetPrivDataIdx(const char *s) const
Definition: planej.cc:2438
PlanePinJoint::SetValue
virtual void SetValue(DataManager *pDM, VectorHandler &X, VectorHandler &XP, SimulationEntity::Hints *ph=0)
Definition: planej.cc:4219
PlaneHingeJoint::dTheta
doublereal dTheta
Definition: planej.h:70
HighParser::GetReal
virtual doublereal GetReal(const doublereal &dDefval=0.0)
Definition: parser.cc:1056
PlaneHingeJoint::NumSelfDof
static const unsigned int NumSelfDof
Definition: planej.h:78
PlaneRotationJoint::InitialAssJac
VariableSubMatrixHandler & InitialAssJac(VariableSubMatrixHandler &WorkMat, const VectorHandler &XCurr)
Definition: planej.cc:2133