MBDyn-1.7.3
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Groups
spherj.cc
Go to the documentation of this file.
1 /* $Header: /var/cvs/mbdyn/mbdyn/mbdyn-1.0/mbdyn/struct/spherj.cc,v 1.47 2017/01/12 14:46:44 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 /* Giunti sferici */
33 
34 #include "mbconfig.h" /* This goes first in every *.c,*.cc file */
35 
36 #include "spherj.h"
37 #include "Rot.hh"
38 
39 
40 /* SphericalHingeJoint - begin */
41 
42 /* Costruttore non banale */
44  const StructNode* pN1,
45  const StructNode* pN2,
46  const Vec3& dTmp1, const Mat3x3& RTmp1h,
47  const Vec3& dTmp2, const Mat3x3& RTmp2h,
48  const OrientationDescription& od,
49  flag fOut)
50 : Elem(uL, fOut),
51 Joint(uL, pDO, fOut),
52 pNode1(pN1), pNode2(pN2),
53 #ifdef USE_NETCDF
54 Var_Phi(0),
55 #endif // USE_NETCDF
56 d1(dTmp1), R1h(RTmp1h),
57 d2(dTmp2), R2h(RTmp2h),
58 F(Zero3),
59 od(od)
60 {
61  NO_OP;
62 }
63 
64 
65 /* Distruttore banale */
67 {
68  NO_OP;
69 };
70 
71 
72 /* Contributo al file di restart */
73 std::ostream& SphericalHingeJoint::Restart(std::ostream& out) const
74 {
75  Joint::Restart(out) << ", spherical hinge, "
76  << pNode1->GetLabel() << ", reference, node, ",
77  d1.Write(out, ", ") << ", hinge, reference, node, 1, ", (R1h.GetVec(1)).Write(out, ", ")
78  << ", 2, ", (R1h.GetVec(2)).Write(out, ", ") << ", "
79  << pNode2->GetLabel() << ", reference, node, ",
80  d2.Write(out, ", ") << ", hinge, reference, node, 1, ", (R2h.GetVec(1)).Write(out, ", ")
81  << ", 2, ", (R2h.GetVec(2)).Write(out, ", ") << ';' << std::endl;
82 
83  return out;
84 }
85 
86 
87 /* Assemblaggio jacobiano */
90  doublereal dCoef,
91  const VectorHandler& /* XCurr */ ,
92  const VectorHandler& /* XPrimeCurr */ )
93 {
94  DEBUGCOUT("Entering SphericalHingeJoint::AssJac()" << std::endl);
95 
96  SparseSubMatrixHandler& WM = WorkMat.SetSparse();
97  WM.ResizeReset(54, 1);
98 
99  integer iNode1FirstPosIndex = pNode1->iGetFirstPositionIndex();
100  integer iNode1FirstMomIndex = pNode1->iGetFirstMomentumIndex();
101  integer iNode2FirstPosIndex = pNode2->iGetFirstPositionIndex();
102  integer iNode2FirstMomIndex = pNode2->iGetFirstMomentumIndex();
103  integer iFirstReactionIndex = iGetFirstIndex();
104 
105  Vec3 dTmp1(pNode1->GetRRef()*d1);
106  Vec3 dTmp2(pNode2->GetRRef()*d2);
107 
108 
109  /*
110  * L'equazione di vincolo afferma che il punto in cui si trova la
111  * cerniera deve essere consistente con la posizione dei due nodi:
112  * x2 + d2 = x1 + d1
113  *
114  * con: d2 = R2 * d2_0
115  * d1 = R1 * d1_0
116  *
117  * La forza e' data dalla reazione vincolare F, nel sistema globale
118  * La coppia dovuta all'eccentricita' e' data rispettivamente da:
119  * -d1 /\ F per il nodo 1,
120  * d2 /\ F per il nodo 2
121  *
122  *
123  * x1 g1 x2 g2 F
124  * Q1 | 0 0 0 0 I | | x1 | | -F |
125  * G1 | 0 cF/\d1/\ 0 0 d1/\ | | g1 | | -d1/\F |
126  * Q2 | 0 0 0 -cF/\d2/\ -I | | x2 | = | F |
127  * G2 | 0 0 0 0 -d2/\ | | g2 | | d2/\F |
128  * F | -c*I c*d1/\ c*I -c*d2/\ 0 | | F | | x1+d1-x2-d2 |
129  *
130  * con d1 = R1*d01, d2 = R2*d02, c = dCoef
131  */
132 
133  /* Moltiplico la forza per il coefficiente del metodo.
134  * Nota: F, la reazione vincolare, e' stata aggiornata da AssRes */
135  Vec3 FTmp = F*dCoef;
136 
137  /* termini di reazione sul nodo 1 */
138  WM.PutDiag(1, iNode1FirstMomIndex, iFirstReactionIndex, 1.);
139  WM.PutCross(4, iNode1FirstMomIndex+3, iFirstReactionIndex, dTmp1);
140 
141  WM.PutMat3x3(10, iNode1FirstMomIndex+3,
142  iNode1FirstPosIndex+3, Mat3x3(MatCrossCross, FTmp, dTmp1));
143 
144  /* termini di reazione sul nodo 2 */
145  WM.PutDiag(19, iNode2FirstMomIndex, iFirstReactionIndex, -1.);
146  WM.PutCross(22, iNode2FirstMomIndex+3, iFirstReactionIndex, -dTmp2);
147 
148  WM.PutMat3x3(28, iNode2FirstMomIndex+3,
149  iNode2FirstPosIndex+3, Mat3x3(MatCrossCross, FTmp, -dTmp2));
150 
151  /* Modifica: divido le equazioni di vincolo per dCoef */
152 
153  /* termini di vincolo dovuti al nodo 1 */
154  WM.PutDiag(37, iFirstReactionIndex, iNode1FirstPosIndex, -1.);
155  WM.PutCross(40, iFirstReactionIndex, iNode1FirstPosIndex+3, dTmp1);
156 
157  /* termini di vincolo dovuti al nodo 1 */
158  WM.PutDiag(46, iFirstReactionIndex, iNode2FirstPosIndex, 1.);
159  WM.PutCross(49, iFirstReactionIndex, iNode2FirstPosIndex+3, -dTmp2);
160 
161  return WorkMat;
162 }
163 
164 
165 /* Assemblaggio residuo */
167  doublereal dCoef,
168  const VectorHandler& XCurr,
169  const VectorHandler& /* XPrimeCurr */ )
170 {
171  DEBUGCOUT("Entering SphericalHingeJoint::AssRes()" << std::endl);
172 
173  /* Dimensiona e resetta la matrice di lavoro */
174  integer iNumRows = 0;
175  integer iNumCols = 0;
176  WorkSpaceDim(&iNumRows, &iNumCols);
177  WorkVec.ResizeReset(iNumRows);
178 
179  integer iNode1FirstMomIndex = pNode1->iGetFirstMomentumIndex();
180  integer iNode2FirstMomIndex = pNode2->iGetFirstMomentumIndex();
181  integer iFirstReactionIndex = iGetFirstIndex();
182 
183  /* Indici dei nodi */
184  for (int iCnt = 1; iCnt <= 6; iCnt++) {
185  WorkVec.PutRowIndex(iCnt, iNode1FirstMomIndex+iCnt);
186  WorkVec.PutRowIndex(6+iCnt, iNode2FirstMomIndex+iCnt);
187  }
188 
189  /* Indici del vincolo */
190  for (int iCnt = 1; iCnt <= 3; iCnt++) {
191  WorkVec.PutRowIndex(12+iCnt, iFirstReactionIndex+iCnt);
192  }
193 
194  F = Vec3(XCurr, iFirstReactionIndex+1);
195 
196  const Vec3& x1(pNode1->GetXCurr());
197  const Vec3& x2(pNode2->GetXCurr());
198 
199  Vec3 dTmp1(pNode1->GetRCurr()*d1);
200  Vec3 dTmp2(pNode2->GetRCurr()*d2);
201 
202  WorkVec.Sub(1, F);
203  WorkVec.Sub(4, dTmp1.Cross(F));
204  WorkVec.Add(7, F);
205  WorkVec.Add(10, dTmp2.Cross(F));
206 
207  /* Modifica: divido le equazioni di vincolo per dCoef */
208  ASSERT(dCoef != 0.);
209  WorkVec.Add(13, (x1+dTmp1-x2-dTmp2)/dCoef);
210 
211  return WorkVec;
212 }
213 
214 
216  ASSERTMSGBREAK(i >=0 and i < iGetNumDof(),
217  "INDEX ERROR in SphericalHingeJoint::GetEqType");
218  return DofOrder::ALGEBRAIC;
219 }
220 
221 void
223 {
224  if (bToBeOutput()) {
225 #ifdef USE_NETCDF
227  std::string name;
228  OutputPrepare_int("spherical hinge", OH, name);
229 
230  Var_Phi = OH.CreateRotationVar(name, "", od, "global");
231 
232  }
233 #endif // USE_NETCDF
234  }
235 }
236 
237 /* Output (da mettere a punto) */
239 {
240  if (bToBeOutput()) {
241  Mat3x3 R1Tmp(pNode1->GetRCurr()*R1h);
242  Mat3x3 RTmp(R1Tmp.MulTM(pNode2->GetRCurr()*R2h));
243  Vec3 E;
244  switch (od) {
245  case EULER_123:
246  E = MatR2EulerAngles123(RTmp)*dRaDegr;
247  break;
248 
249  case EULER_313:
250  E = MatR2EulerAngles313(RTmp)*dRaDegr;
251  break;
252 
253  case EULER_321:
254  E = MatR2EulerAngles321(RTmp)*dRaDegr;
255  break;
256 
257  case ORIENTATION_VECTOR:
258  E = RotManip::VecRot(RTmp);
259  break;
260 
261  case ORIENTATION_MATRIX:
262  break;
263 
264  default:
265  /* impossible */
266  break;
267  }
268 
269 #ifdef USE_NETCDF
271  Var_F_local->put_rec((R1Tmp.MulTV(F)).pGetVec(), OH.GetCurrentStep());
272  Var_M_local->put_rec(Zero3.pGetVec(), OH.GetCurrentStep());
273  Var_F_global->put_rec(F.pGetVec(), OH.GetCurrentStep());
274  Var_M_global->put_rec(Zero3.pGetVec(), OH.GetCurrentStep());
275  switch (od) {
276  case EULER_123:
277  case EULER_313:
278  case EULER_321:
279  case ORIENTATION_VECTOR:
280  Var_Phi->put_rec(E.pGetVec(), OH.GetCurrentStep());
281  break;
282 
283  case ORIENTATION_MATRIX:
284  Var_Phi->put_rec(RTmp.pGetMat(), OH.GetCurrentStep());
285  break;
286 
287  default:
288  /* impossible */
289  break;
290  }
291  }
292 #endif // USE_NETCDF
293  if (OH.UseText(OutputHandler::JOINTS)) {
294  Joint::Output(OH.Joints(), "SphericalHinge", GetLabel(),
295  R1Tmp.MulTV(F), Zero3, F, Zero3)
296  << " ";
297 
298  switch (od) {
299  case EULER_123:
300  case EULER_313:
301  case EULER_321:
302  case ORIENTATION_VECTOR:
303  OH.Joints() << E;
304  break;
305 
306  case ORIENTATION_MATRIX:
307  OH.Joints() << RTmp;
308  break;
309 
310  default:
311  /* impossible */
312  break;
313  }
314 
315  OH.Joints() << std::endl;
316  }
317  }
318 }
319 
320 void
324 {
325  if (ph) {
326  for (unsigned i = 0; i < ph->size(); i++) {
327  Joint::JointHint *pjh = dynamic_cast<Joint::JointHint *>((*ph)[i]);
328 
329  if (pjh == 0) {
330  continue;
331  }
332 
333  if (dynamic_cast<Joint::OffsetHint<1> *>(pjh)) {
334  const Mat3x3& R1(pNode1->GetRCurr());
335  Vec3 dTmp2(pNode2->GetRCurr()*d2);
336 
337  d1 = R1.MulTV(pNode2->GetXCurr() + dTmp2 - pNode1->GetXCurr());
338 
339  } else if (dynamic_cast<Joint::OffsetHint<2> *>(pjh)) {
340  const Mat3x3& R2(pNode2->GetRCurr());
341  Vec3 dTmp1(pNode1->GetRCurr()*d1);
342 
343  d2 = R2.MulTV(pNode1->GetXCurr() + dTmp1 - pNode2->GetXCurr());
344 
345  } else if (dynamic_cast<Joint::ReactionsHint *>(pjh)) {
346  /* TODO */
347  }
348  }
349  }
350 }
351 
352 Hint *
353 SphericalHingeJoint::ParseHint(DataManager *pDM, const char *s) const
354 {
355  if (strncasecmp(s, "offset{" /*}*/, STRLENOF("offset{" /*}*/)) == 0) {
356  s += STRLENOF("offset{" /*}*/);
357 
358  if (strcmp(&s[1], /*{*/ "}") != 0) {
359  return 0;
360  }
361 
362  switch (s[0]) {
363  case '1':
364  return new Joint::OffsetHint<1>;
365 
366  case '2':
367  return new Joint::OffsetHint<2>;
368  }
369  }
370 
371  return 0;
372 }
373 
374 /* Contributo allo jacobiano durante l'assemblaggio iniziale */
377  const VectorHandler& XCurr)
378 {
379  DEBUGCOUT("Entering SphericalHingeJoint::InitialAssJac()" << std::endl);
380 
381  FullSubMatrixHandler& WM = WorkMat.SetFull();
382 
383  /* Dimensiona e resetta la matrice di lavoro */
384  integer iNumRows = 0;
385  integer iNumCols = 0;
386  InitialWorkSpaceDim(&iNumRows, &iNumCols);
387  WM.ResizeReset(iNumRows, iNumCols);
388 
389  /* Equazioni: vedi joints.dvi */
390 
391  /* Indici */
392  integer iNode1FirstPosIndex = pNode1->iGetFirstPositionIndex();
393  integer iNode1FirstVelIndex = iNode1FirstPosIndex+6;
394  integer iNode2FirstPosIndex = pNode2->iGetFirstPositionIndex();
395  integer iNode2FirstVelIndex = iNode2FirstPosIndex+6;
396  integer iFirstReactionIndex = iGetFirstIndex();
397  integer iReactionPrimeIndex = iFirstReactionIndex+3;
398 
399  /* Setto gli indici */
400  for (int iCnt = 1; iCnt <= 6; iCnt++) {
401  WM.PutRowIndex(iCnt, iNode1FirstPosIndex+iCnt);
402  WM.PutColIndex(iCnt, iNode1FirstPosIndex+iCnt);
403  WM.PutRowIndex(6+iCnt, iNode1FirstVelIndex+iCnt);
404  WM.PutColIndex(6+iCnt, iNode1FirstVelIndex+iCnt);
405  WM.PutRowIndex(12+iCnt, iNode2FirstPosIndex+iCnt);
406  WM.PutColIndex(12+iCnt, iNode2FirstPosIndex+iCnt);
407  WM.PutRowIndex(18+iCnt, iNode2FirstVelIndex+iCnt);
408  WM.PutColIndex(18+iCnt, iNode2FirstVelIndex+iCnt);
409  WM.PutRowIndex(24+iCnt, iFirstReactionIndex+iCnt);
410  WM.PutColIndex(24+iCnt, iFirstReactionIndex+iCnt);
411  }
412 
413  /* Matrici identita' */
414 
415  for (int iCnt = 1; iCnt <= 3; iCnt++) {
416  /* Contributo di forza all'equazione della forza, nodo 1 */
417  WM.PutCoef(iCnt, 24+iCnt, 1.);
418 
419  /* Contrib. di der. di forza all'eq. della der. della forza, nodo 1 */
420  WM.PutCoef(6+iCnt, 27+iCnt, 1.);
421 
422  /* Contributo di forza all'equazione della forza, nodo 2 */
423  WM.PutCoef(12+iCnt, 24+iCnt, -1.);
424 
425  /* Contrib. di der. di forza all'eq. della der. della forza, nodo 2 */
426  WM.PutCoef(18+iCnt, 27+iCnt, -1.);
427 
428  /* Equazione di vincolo, nodo 1 */
429  WM.PutCoef(24+iCnt, iCnt, -1.);
430 
431  /* Derivata dell'equazione di vincolo, nodo 1 */
432  WM.PutCoef(27+iCnt, 6+iCnt, -1.);
433 
434  /* Equazione di vincolo, nodo 2 */
435  WM.PutCoef(24+iCnt, 12+iCnt, 1.);
436 
437  /* Derivata dell'equazione di vincolo, nodo 2 */
438  WM.PutCoef(27+iCnt, 18+iCnt, 1.);
439  }
440 
441  /* Recupera i dati */
442  const Mat3x3& R1(pNode1->GetRRef());
443  const Mat3x3& R2(pNode2->GetRRef());
444  const Vec3& Omega1(pNode1->GetWRef());
445  const Vec3& Omega2(pNode2->GetWRef());
446  /* F e' stata aggiornata da InitialAssRes */
447  Vec3 FPrime(XCurr, iReactionPrimeIndex+1);
448 
449  /* Distanza nel sistema globale */
450  Vec3 d1Tmp(R1*d1);
451  Vec3 d2Tmp(R2*d2);
452 
453  /* Matrici F/\d1/\, -F/\d2/\ */
454  Mat3x3 FWedged1Wedge(MatCrossCross, F, d1Tmp);
455  Mat3x3 FWedged2Wedge(MatCrossCross, F, -d2Tmp);
456 
457  /* Matrici (omega1/\d1)/\, -(omega2/\d2)/\ */
458  Mat3x3 O1Wedged1Wedge(MatCross, Omega1.Cross(d1Tmp));
459  Mat3x3 O2Wedged2Wedge(MatCross, d2Tmp.Cross(Omega2));
460 
461  /* Equazione di momento, nodo 1 */
462  WM.Add(4, 4, FWedged1Wedge);
463  WM.Add(4, 25, Mat3x3(MatCross, d1Tmp));
464 
465  /* Equazione di momento, nodo 2 */
466  WM.Add(16, 16, FWedged2Wedge);
467  WM.Sub(16, 25, Mat3x3(MatCross, d2Tmp));
468 
469  /* Derivata dell'equazione di momento, nodo 1 */
470  WM.Add(10, 4, (Mat3x3(MatCross, FPrime) + Mat3x3(MatCrossCross, F, Omega1))*Mat3x3(MatCross, d1Tmp));
471  WM.Add(10, 10, FWedged1Wedge);
472  WM.Add(10, 25, O1Wedged1Wedge);
473  WM.Add(10, 28, Mat3x3(MatCross, d1Tmp));
474 
475  /* Derivata dell'equazione di momento, nodo 2 */
476  WM.Sub(22, 16, (Mat3x3(MatCross, FPrime) + Mat3x3(MatCrossCross, F, Omega2))*Mat3x3(MatCross, d2Tmp));
477  WM.Add(22, 22, FWedged2Wedge);
478  WM.Add(22, 25, O2Wedged2Wedge);
479  WM.Sub(22, 28, Mat3x3(MatCross, d2Tmp));
480 
481  /* Equazione di vincolo */
482  WM.Add(25, 4, Mat3x3(MatCross, d1Tmp));
483  WM.Sub(25, 16, Mat3x3(MatCross, d2Tmp));
484 
485  /* Derivata dell'equazione di vincolo */
486  WM.Add(28, 4, O1Wedged1Wedge);
487  WM.Add(28, 10, Mat3x3(MatCross, d1Tmp));
488  WM.Add(28, 16, O2Wedged2Wedge);
489  WM.Sub(28, 22, Mat3x3(MatCross, d2Tmp));
490 
491  return WorkMat;
492 }
493 
494 
495 /* Contributo al residuo durante l'assemblaggio iniziale */
498  const VectorHandler& XCurr)
499 {
500  DEBUGCOUT("Entering SphericalHingeJoint::InitialAssRes()" << std::endl);
501 
502  /* Dimensiona e resetta la matrice di lavoro */
503  integer iNumRows = 0;
504  integer iNumCols = 0;
505  InitialWorkSpaceDim(&iNumRows, &iNumCols);
506  WorkVec.ResizeReset(iNumRows);
507 
508  /* Indici */
509  integer iNode1FirstPosIndex = pNode1->iGetFirstPositionIndex();
510  integer iNode1FirstVelIndex = iNode1FirstPosIndex+6;
511  integer iNode2FirstPosIndex = pNode2->iGetFirstPositionIndex();
512  integer iNode2FirstVelIndex = iNode2FirstPosIndex+6;
513  integer iFirstReactionIndex = iGetFirstIndex();
514  integer iReactionPrimeIndex = iFirstReactionIndex+3;
515 
516  /* Setta gli indici */
517  for (int iCnt = 1; iCnt <= 6; iCnt++) {
518  WorkVec.PutRowIndex(iCnt, iNode1FirstPosIndex+iCnt);
519  WorkVec.PutRowIndex(6+iCnt, iNode1FirstVelIndex+iCnt);
520  WorkVec.PutRowIndex(12+iCnt, iNode2FirstPosIndex+iCnt);
521  WorkVec.PutRowIndex(18+iCnt, iNode2FirstVelIndex+iCnt);
522  WorkVec.PutRowIndex(24+iCnt, iFirstReactionIndex+iCnt);
523  }
524 
525  /* Recupera i dati */
526  const Vec3& x1(pNode1->GetXCurr());
527  const Vec3& x2(pNode2->GetXCurr());
528  const Vec3& v1(pNode1->GetVCurr());
529  const Vec3& v2(pNode2->GetVCurr());
530  const Mat3x3& R1(pNode1->GetRCurr());
531  const Mat3x3& R2(pNode2->GetRCurr());
532  const Vec3& Omega1(pNode1->GetWCurr());
533  const Vec3& Omega2(pNode2->GetWCurr());
534  F = Vec3(XCurr, iFirstReactionIndex+1);
535  Vec3 FPrime(XCurr, iReactionPrimeIndex+1);
536 
537  /* Distanza nel sistema globale */
538  Vec3 d1Tmp(R1*d1);
539  Vec3 d2Tmp(R2*d2);
540 
541  /* Vettori omega1/\d1, -omega2/\d2 */
542  Vec3 O1Wedged1(Omega1.Cross(d1Tmp));
543  Vec3 O2Wedged2(Omega2.Cross(d2Tmp));
544 
545  /* Equazioni di equilibrio, nodo 1 */
546  WorkVec.Sub(1, F);
547  WorkVec.Add(4, F.Cross(d1Tmp)); /* Sfrutto il fatto che F/\d = -d/\F */
548 
549  /* Derivate delle equazioni di equilibrio, nodo 1 */
550  WorkVec.Sub(7, FPrime);
551  WorkVec.Add(10, FPrime.Cross(d1Tmp)-O1Wedged1.Cross(F));
552 
553  /* Equazioni di equilibrio, nodo 2 */
554  WorkVec.Add(13, F);
555  WorkVec.Add(16, d2Tmp.Cross(F));
556 
557  /* Derivate delle equazioni di equilibrio, nodo 2 */
558  WorkVec.Add(19, FPrime);
559  WorkVec.Add(22, d2Tmp.Cross(FPrime)+O2Wedged2.Cross(F));
560 
561  /* Equazione di vincolo */
562  WorkVec.Add(25, x1+d1Tmp-x2-d2Tmp);
563 
564  /* Deivata dell'equazione di vincolo */
565  WorkVec.Add(28, v1+O1Wedged1-v2-O2Wedged2);
566 
567  return WorkVec;
568 }
569 
570 /* SphericalHingeJoint - end */
571 
572 
573 /* PinJoint - begin */
574 /* Costruttore non banale */
575 PinJoint::PinJoint(unsigned int uL, const DofOwner* pDO,
576  const StructNode* pN,
577  const Vec3& X0Tmp, const Vec3& dTmp, flag fOut)
578 : Elem(uL, fOut),
579 Joint(uL, pDO, fOut), pNode(pN), X0(X0Tmp), d(dTmp), F(Zero3)
580 {
581  NO_OP;
582 }
583 
584 
585 /* Distruttore banale */
587 {
588  NO_OP;
589 };
590 
591 
592 /* Contributo al file di restart */
593 std::ostream& PinJoint::Restart(std::ostream& out) const
594 {
595  Joint::Restart(out) << ", pin, "
596  << pNode->GetLabel() << ", reference, node, ",
597  d.Write(out, ", ") << ", reference, global, ",
598  X0.Write(out, ", ") << ';' << std::endl;
599 
600  return out;
601 }
602 
603 
604 /* Assemblaggio jacobiano */
607  doublereal dCoef,
608  const VectorHandler& /* XCurr */ ,
609  const VectorHandler& /* XPrimeCurr */ )
610 {
611  DEBUGCOUT("Entering PinJoint::AssJac()" << std::endl);
612 
613  SparseSubMatrixHandler& WM = WorkMat.SetSparse();
614  WM.ResizeReset(27, 0);
615 
616  integer iFirstPositionIndex = pNode->iGetFirstPositionIndex();
617  integer iFirstMomentumIndex = pNode->iGetFirstMomentumIndex();
618  integer iFirstReactionIndex = iGetFirstIndex();
619 
620  const Mat3x3& R(pNode->GetRRef());
621  Vec3 dTmp(R*d);
622 
623  /*
624  * L'equazione di vincolo afferma che il punto in cui si trova la
625  * cerniera deve essere fissato:
626  * x + d = x0
627  *
628  * con: d = R * d_0
629  *
630  * La forza e' data dalla reazione vincolare F, nel sistema globale
631  * La coppia dovuta all'eccentricita' e' data rispettivamente da:
632  * d /\ F
633  *
634  *
635  * x g F
636  * Q1 | 0 0 I | | x | | -F |
637  * G1 | 0 cF/\d1/\ d/\ | | g | | -d/\F |
638  * F | I d/\ 0 | | F | | (x+d-x0)/c |
639  *
640  * con d = R*d_0, c = dCoef
641  */
642 
643  /* termini di reazione sul nodo */
644  for (int iCnt = 1; iCnt <= 3; iCnt++) {
645  WM.PutItem(iCnt, iFirstMomentumIndex+iCnt,
646  iFirstReactionIndex+iCnt, 1.);
647  }
648  WM.PutCross(4, iFirstMomentumIndex+3,
649  iFirstReactionIndex, dTmp);
650 
651  /* Nota: F, la reazione vincolare, e' stata aggiornata da AssRes */
652 
653  /* Termini diagonali del tipo: c*F/\d/\Delta_g
654  * nota: la forza e' gia' moltiplicata per dCoef */
655  WM.PutMat3x3(10, iFirstMomentumIndex+3,
656  iFirstPositionIndex+3, Mat3x3(MatCrossCross, F*dCoef, dTmp));
657 
658  /* Modifica: divido le equazioni di vincolo per dCoef */
659 
660  /* termini di vincolo dovuti al nodo 1 */
661  for (int iCnt = 1; iCnt <= 3; iCnt++) {
662  WM.PutItem(18+iCnt, iFirstReactionIndex+iCnt,
663  iFirstPositionIndex+iCnt, -1.);
664  }
665  WM.PutCross(22, iFirstReactionIndex,
666  iFirstPositionIndex+3, dTmp);
667 
668  return WorkMat;
669 }
670 
671 
672 /* Assemblaggio residuo */
674  doublereal dCoef,
675  const VectorHandler& XCurr,
676  const VectorHandler& /* XPrimeCurr */ )
677 {
678  DEBUGCOUT("Entering PinJoint::AssRes()" << std::endl);
679 
680  /* Dimensiona e resetta la matrice di lavoro */
681  integer iNumRows = 0;
682  integer iNumCols = 0;
683  WorkSpaceDim(&iNumRows, &iNumCols);
684  WorkVec.ResizeReset(iNumRows);
685 
686  integer iFirstMomentumIndex = pNode->iGetFirstMomentumIndex();
687  integer iFirstReactionIndex = iGetFirstIndex();
688 
689  /* Indici dei nodi */
690  for (int iCnt = 1; iCnt <= 6; iCnt++) {
691  WorkVec.PutRowIndex(iCnt, iFirstMomentumIndex+iCnt);
692  }
693 
694 
695  /* Indici del vincolo */
696  for(int iCnt = 1; iCnt <= 3; iCnt++) {
697  WorkVec.PutRowIndex(6+iCnt, iFirstReactionIndex+iCnt);
698  }
699 
700  F = Vec3(XCurr, iFirstReactionIndex+1);
701 
702  const Vec3& x(pNode->GetXCurr());
703  const Mat3x3& R(pNode->GetRCurr());
704 
705  Vec3 dTmp(R*d);
706 
707  WorkVec.Sub(1, F);
708  WorkVec.Add(4, F.Cross(dTmp)); /* Sfrutto il fatto che F/\d = -d/\F */
709 
710  /* Modifica: divido le equazioni di vincolo per dCoef */
711  ASSERT(dCoef != 0.);
712  WorkVec.Add(7, (x+dTmp-X0)/dCoef);
713 
714  return WorkVec;
715 }
716 
717 DofOrder::Order PinJoint::GetEqType(unsigned int i) const {
718  ASSERTMSGBREAK(i >=0 and i < iGetNumDof(),
719  "INDEX ERROR in PinJoint::GetEqType");
720  return DofOrder::ALGEBRAIC;
721 }
722 
723 /* Output (da mettere a punto) */
725 {
726  if (bToBeOutput()) {
727  Joint::Output(OH.Joints(), "Pin", GetLabel(), F, Zero3, F, Zero3)
728  << " " << MatR2EulerAngles(pNode->GetRCurr())*dRaDegr << std::endl;
729  }
730 }
731 
732 
733 /* Contributo allo jacobiano durante l'assemblaggio iniziale */
736  const VectorHandler& XCurr)
737 {
738  DEBUGCOUT("Entering PinJoint::InitialAssJac()" << std::endl);
739 
740  FullSubMatrixHandler& WM = WorkMat.SetFull();
741 
742  /* Dimensiona e resetta la matrice di lavoro */
743  integer iNumRows = 0;
744  integer iNumCols = 0;
745  InitialWorkSpaceDim(&iNumRows, &iNumCols);
746  WM.ResizeReset(iNumRows, iNumCols);
747 
748  /* Equazioni: vedi joints.dvi */
749 
750  /* Indici */
751  integer iFirstPositionIndex = pNode->iGetFirstPositionIndex();
752  integer iFirstVelocityIndex = iFirstPositionIndex+6;
753  integer iFirstReactionIndex = iGetFirstIndex();
754  integer iReactionPrimeIndex = iFirstReactionIndex+3;
755 
756  /* Setto gli indici */
757  for (int iCnt = 1; iCnt <= 6; iCnt++) {
758  WM.PutRowIndex(iCnt, iFirstPositionIndex+iCnt);
759  WM.PutColIndex(iCnt, iFirstPositionIndex+iCnt);
760  WM.PutRowIndex(6+iCnt, iFirstVelocityIndex+iCnt);
761  WM.PutColIndex(6+iCnt, iFirstVelocityIndex+iCnt);
762  WM.PutRowIndex(12+iCnt, iFirstReactionIndex+iCnt);
763  WM.PutColIndex(12+iCnt, iFirstReactionIndex+iCnt);
764  }
765 
766  /* Matrici identita' */
767  for (int iCnt = 1; iCnt <= 3; iCnt++) {
768  /* Contributo di forza all'equazione della forza */
769  WM.PutCoef(iCnt, 12+iCnt, 1.);
770 
771  /* Contrib. di der. di forza all'eq. della der. della forza */
772  WM.PutCoef(6+iCnt, 15+iCnt, 1.);
773 
774  /* Equazione di vincolo */
775  WM.PutCoef(12+iCnt, iCnt, -1.);
776 
777  /* Derivata dell'equazione di vincolo */
778  WM.PutCoef(15+iCnt, 6+iCnt, -1.);
779  }
780 
781  /* Recupera i dati */
782  const Mat3x3& R(pNode->GetRRef());
783  const Vec3& Omega(pNode->GetWRef());
784  /* F e' stata aggiornata da InitialAssRes */
785  Vec3 FPrime(XCurr, iReactionPrimeIndex+1);
786 
787  /* Distanza nel sistema globale */
788  Vec3 dTmp(R*d);
789 
790  /* Matrici F/\d/\ */
791  Mat3x3 FWedgedWedge(MatCrossCross, F, dTmp);
792 
793  /* Matrici (omega/\d)/\ */
794  Mat3x3 OWedgedWedge(MatCross, Omega.Cross(dTmp));
795 
796  /* Equazione di momento */
797  WM.Add(4, 4, FWedgedWedge);
798  WM.Add(4, 13, Mat3x3(MatCross, dTmp));
799 
800  /* Derivata dell'equazione di momento */
801  WM.Add(10, 4, (Mat3x3(MatCross, FPrime) + Mat3x3(MatCrossCross, F, Omega))*Mat3x3(MatCross, dTmp));
802  WM.Add(10, 10, FWedgedWedge);
803  WM.Add(10, 13, OWedgedWedge);
804  WM.Add(10, 16, Mat3x3(MatCross, dTmp));
805 
806  /* Equazione di vincolo */
807  WM.Add(13, 4, Mat3x3(MatCross, dTmp));
808 
809  /* Derivata dell'equazione di vincolo */
810  WM.Add(16, 4, OWedgedWedge);
811  WM.Add(16, 10, Mat3x3(MatCross, dTmp));
812 
813  return WorkMat;
814 }
815 
816 
817 /* Contributo al residuo durante l'assemblaggio iniziale */
820  const VectorHandler& XCurr)
821 {
822  DEBUGCOUT("Entering PinJoint::InitialAssRes()" << std::endl);
823 
824  /* Dimensiona e resetta la matrice di lavoro */
825  integer iNumRows = 0;
826  integer iNumCols = 0;
827  InitialWorkSpaceDim(&iNumRows, &iNumCols);
828  WorkVec.ResizeReset(iNumRows);
829 
830  /* Indici */
831  integer iFirstPositionIndex = pNode->iGetFirstPositionIndex();
832  integer iFirstVelocityIndex = iFirstPositionIndex+6;
833  integer iFirstReactionIndex = iGetFirstIndex();
834  integer iReactionPrimeIndex = iFirstReactionIndex+3;
835 
836  /* Setta gli indici */
837  for (int iCnt = 1; iCnt <= 6; iCnt++) {
838  WorkVec.PutRowIndex(iCnt, iFirstPositionIndex+iCnt);
839  WorkVec.PutRowIndex(6+iCnt, iFirstVelocityIndex+iCnt);
840  WorkVec.PutRowIndex(12+iCnt, iFirstReactionIndex+iCnt);
841  }
842 
843  /* Recupera i dati */
844  const Vec3& x(pNode->GetXCurr());
845  const Vec3& v(pNode->GetVCurr());
846  const Mat3x3& R(pNode->GetRCurr());
847  const Vec3& Omega(pNode->GetWCurr());
848  F = Vec3(XCurr, iFirstReactionIndex+1);
849  Vec3 FPrime(XCurr, iReactionPrimeIndex+1);
850 
851  /* Distanza nel sistema globale */
852  Vec3 dTmp(R*d);
853 
854  /* Vettori omega/\d */
855  Vec3 OWedged(Omega.Cross(dTmp));
856 
857  /* Equazioni di equilibrio */
858  WorkVec.Sub(1, F);
859  WorkVec.Add(4, F.Cross(dTmp)); /* Sfrutto il fatto che F/\d = -d/\F */
860 
861  /* Derivate delle equazioni di equilibrio */
862  WorkVec.Sub(7, FPrime);
863  WorkVec.Add(10, FPrime.Cross(dTmp)-OWedged.Cross(F));
864 
865  /* Equazione di vincolo */
866  WorkVec.Add(13, x+dTmp-X0);
867 
868  /* Derivata dell'equazione di vincolo */
869  WorkVec.Add(16, v+OWedged);
870 
871  return WorkVec;
872 }
873 
874 /* PinJoint - end */
Definition: hint.h:38
virtual void SetValue(DataManager *pDM, VectorHandler &X, VectorHandler &XP, SimulationEntity::Hints *ph=0)
Definition: spherj.cc:321
void PutColIndex(integer iSubCol, integer iCol)
Definition: submat.h:325
void PutMat3x3(integer iSubIt, integer iFirstRow, integer iFirstCol, const Mat3x3 &m)
Definition: submat.cc:1331
const Vec3 Zero3(0., 0., 0.)
Vec3 Cross(const Vec3 &v) const
Definition: matvec3.h:218
#define ASSERTMSGBREAK(expr, msg)
Definition: myassert.h:222
std::ostream & Write(std::ostream &out, const char *sFill=" ") const
Definition: matvec3.cc:738
long int flag
Definition: mbdyn.h:43
virtual const Mat3x3 & GetRRef(void) const
Definition: strnode.h:1006
virtual bool bToBeOutput(void) const
Definition: output.cc:890
Vec3 X0
Definition: spherj.h:158
~PinJoint(void)
Definition: spherj.cc:586
virtual void InitialWorkSpaceDim(integer *piNumRows, integer *piNumCols) const
Definition: spherj.h:211
Definition: matvec3.h:98
virtual void ResizeReset(integer)
Definition: vh.cc:55
const MatCross_Manip MatCross
Definition: matvec3.cc:639
bool UseNetCDF(int out) const
Definition: output.cc:491
FullSubMatrixHandler & SetFull(void)
Definition: submat.h:1168
virtual const Mat3x3 & GetRCurr(void) const
Definition: strnode.h:1012
const StructNode * pNode1
Definition: spherj.h:44
void Add(integer iRow, integer iCol, const Vec3 &v)
Definition: submat.cc:209
VariableSubMatrixHandler & InitialAssJac(VariableSubMatrixHandler &WorkMat, const VectorHandler &XCurr)
Definition: spherj.cc:376
virtual void WorkSpaceDim(integer *piNumRows, integer *piNumCols) const
Definition: spherj.h:86
std::ostream & Write(std::ostream &out, const FullMatrixHandler &m, const char *s, const char *s2)
Definition: fullmh.cc:376
VariableSubMatrixHandler & InitialAssJac(VariableSubMatrixHandler &WorkMat, const VectorHandler &XCurr)
Definition: spherj.cc:735
const StructNode * pNode
Definition: spherj.h:157
virtual void Sub(integer iRow, const Vec3 &v)
Definition: vh.cc:78
virtual void InitialWorkSpaceDim(integer *piNumRows, integer *piNumCols) const
Definition: spherj.h:117
virtual void Output(OutputHandler &OH) const
Definition: spherj.cc:238
OrientationDescription
Definition: matvec3.h:1597
virtual std::ostream & Restart(std::ostream &out) const
Definition: spherj.cc:593
void PutCoef(integer iRow, integer iCol, const doublereal &dCoef)
Definition: submat.h:672
void ResizeReset(integer iNewRow, integer iNewCol)
Definition: submat.cc:1084
virtual unsigned int iGetNumDof(void) const
Definition: spherj.h:77
DofOrder::Order GetEqType(unsigned int i) const
Definition: spherj.cc:215
virtual const Vec3 & GetWRef(void) const
Definition: strnode.h:1024
#define NO_OP
Definition: myassert.h:74
void PutCross(integer iSubIt, integer iFirstRow, integer iFirstCol, const Vec3 &v)
Definition: submat.cc:1236
std::vector< Hint * > Hints
Definition: simentity.h:89
Vec3 GetVec(unsigned short int i) const
Definition: matvec3.h:893
virtual void WorkSpaceDim(integer *piNumRows, integer *piNumCols) const
Definition: spherj.h:187
Vec3 VecRot(const Mat3x3 &Phi)
Definition: Rot.cc:136
Vec3 d
Definition: spherj.h:159
OrientationDescription od
Definition: spherj.h:56
SubVectorHandler & InitialAssRes(SubVectorHandler &WorkVec, const VectorHandler &XCurr)
Definition: spherj.cc:497
virtual void PutRowIndex(integer iSubRow, integer iRow)=0
const StructNode * pNode2
Definition: spherj.h:45
void PutItem(integer iSubIt, integer iRow, integer iCol, const doublereal &dCoef)
Definition: submat.h:997
Vec3 MatR2EulerAngles313(const Mat3x3 &R)
Definition: matvec3.cc:927
SubVectorHandler & AssRes(SubVectorHandler &WorkVec, doublereal dCoef, const VectorHandler &XCurr, const VectorHandler &XPrimeCurr)
Definition: spherj.cc:166
long GetCurrentStep(void) const
Definition: output.h:116
SubVectorHandler & AssRes(SubVectorHandler &WorkVec, doublereal dCoef, const VectorHandler &XCurr, const VectorHandler &XPrimeCurr)
Definition: spherj.cc:673
#define DEBUGCOUT(msg)
Definition: myassert.h:232
virtual std::ostream & Restart(std::ostream &out) const
Definition: spherj.cc:73
Vec3 MatR2EulerAngles123(const Mat3x3 &R)
Definition: matvec3.cc:893
SphericalHingeJoint(unsigned int uL, const DofOwner *pDO, const StructNode *pN1, const StructNode *pN2, const Vec3 &dTmp1, const Mat3x3 &RTmp1h, const Vec3 &dTmp2, const Mat3x3 &RTmp2h, const OrientationDescription &od, flag fOut)
Definition: spherj.cc:43
PinJoint(unsigned int uL, const DofOwner *pDO, const StructNode *pN, const Vec3 &X0Tmp, const Vec3 &dTmp, flag fOut)
Definition: spherj.cc:575
virtual integer iGetFirstMomentumIndex(void) const =0
virtual integer iGetFirstPositionIndex(void) const
Definition: strnode.h:452
virtual const Vec3 & GetWCurr(void) const
Definition: strnode.h:1030
const doublereal dRaDegr
Definition: matvec3.cc:884
virtual std::ostream & Restart(std::ostream &out) const
Definition: joint.h:195
std::ostream & Joints(void) const
Definition: output.h:443
VariableSubMatrixHandler & AssJac(VariableSubMatrixHandler &WorkMat, doublereal dCoef, const VectorHandler &XCurr, const VectorHandler &XPrimeCurr)
Definition: spherj.cc:89
#define ASSERT(expression)
Definition: colamd.c:977
virtual void OutputPrepare_int(const std::string &type, OutputHandler &OH, std::string &name)
Definition: joint.cc:107
Vec3 MatR2EulerAngles(const Mat3x3 &R)
Definition: matvec3.cc:887
virtual const Vec3 & GetXCurr(void) const
Definition: strnode.h:310
virtual void Add(integer iRow, const Vec3 &v)
Definition: vh.cc:63
virtual void ResizeReset(integer, integer)
Definition: submat.cc:182
virtual void Output(OutputHandler &OH) const
Definition: spherj.cc:724
const MatCrossCross_Manip MatCrossCross
Definition: matvec3.cc:640
#define STRLENOF(s)
Definition: mbdyn.h:166
Definition: elem.h:75
void PutRowIndex(integer iSubRow, integer iRow)
Definition: submat.h:311
void PutDiag(integer iSubIt, integer iFirstRow, integer iFirstCol, const Vec3 &v)
Definition: submat.cc:1125
const doublereal * pGetVec(void) const
Definition: matvec3.h:192
Vec3 MatR2EulerAngles321(const Mat3x3 &R)
Definition: matvec3.cc:941
DofOrder::Order GetEqType(unsigned int i) const
Definition: spherj.cc:717
~SphericalHingeJoint(void)
Definition: spherj.cc:66
virtual const Vec3 & GetVCurr(void) const
Definition: strnode.h:322
void Sub(integer iRow, integer iCol, const Vec3 &v)
Definition: submat.cc:215
Definition: joint.h:50
SparseSubMatrixHandler & SetSparse(void)
Definition: submat.h:1178
virtual integer iGetFirstIndex(void) const
Definition: dofown.h:127
VariableSubMatrixHandler & AssJac(VariableSubMatrixHandler &WorkMat, doublereal dCoef, const VectorHandler &XCurr, const VectorHandler &XPrimeCurr)
Definition: spherj.cc:606
double doublereal
Definition: colamd.c:52
std::ostream & Output(std::ostream &out, const char *sJointName, unsigned int uLabel, const Vec3 &FLocal, const Vec3 &MLocal, const Vec3 &FGlobal, const Vec3 &MGlobal) const
Definition: joint.cc:138
long int integer
Definition: colamd.c:51
Vec3 F
Definition: spherj.h:160
unsigned int GetLabel(void) const
Definition: withlab.cc:62
SubVectorHandler & InitialAssRes(SubVectorHandler &WorkVec, const VectorHandler &XCurr)
Definition: spherj.cc:819
void OutputPrepare(OutputHandler &OH)
Definition: spherj.cc:222
Mat3x3 R
bool UseText(int out) const
Definition: output.cc:446
virtual Hint * ParseHint(DataManager *pDM, const char *s) const
Definition: spherj.cc:353
virtual unsigned int iGetNumDof(void) const
Definition: spherj.h:178