MBDyn-1.7.3
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Groups
univj.cc
Go to the documentation of this file.
1 /* $Header: /var/cvs/mbdyn/mbdyn/mbdyn-1.0/mbdyn/struct/univj.cc,v 1.49 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 universali */
33 
34 #include "mbconfig.h" /* This goes first in every *.c,*.cc file */
35 
36 #include <limits>
37 
38 #include "univj.h"
39 #include "Rot.hh"
40 
41 /* UniversalHingeJoint - begin */
42 
43 /* Costruttore non banale */
44 UniversalHingeJoint::UniversalHingeJoint(unsigned int uL,
45  const DofOwner* pDO,
46  const StructNode* pN1,
47  const StructNode* pN2,
48  const Vec3& dTmp1,
49  const Vec3& dTmp2,
50  const Mat3x3& R1hTmp,
51  const Mat3x3& R2hTmp,
52  flag fOut)
53 : Elem(uL, fOut),
54 Joint(uL, pDO, fOut),
55 pNode1(pN1), pNode2(pN2),
56 d1(dTmp1), R1h(R1hTmp), d2(dTmp2), R2h(R2hTmp), F(Zero3), dM(0.)
57 {
58  NO_OP;
59 }
60 
61 
62 /* Distruttore banale */
63 UniversalHingeJoint::~UniversalHingeJoint(void)
64 {
65  NO_OP;
66 }
67 
68 
69 /* Contributo al file di restart */
70 std::ostream&
71 UniversalHingeJoint::Restart(std::ostream& out) const
72 {
73  Joint::Restart(out) << ", universal hinge, "
74  << pNode1->GetLabel() << ", "
75  "reference, node, ", d1.Write(out, ", ") << ", "
76  "hinge, reference, node, "
77  "1, ", (R1h.GetVec(1)).Write(out, ", ") << ", "
78  "2, ", (R1h.GetVec(2)).Write(out, ", ") << ", "
79  << pNode2->GetLabel() << ", "
80  "reference, node, ", d2.Write(out, ", ") << ", "
81  "hinge, reference, node, "
82  "1, ", (R2h.GetVec(1)).Write(out, ", ") << ", "
83  "2, ", (R2h.GetVec(2)).Write(out, ", ") << ";"
84  << std::endl;
85 
86  return out;
87 }
88 
89 
90 /* Assemblaggio jacobiano */
91 VariableSubMatrixHandler&
92 UniversalHingeJoint::AssJac(VariableSubMatrixHandler& WorkMat,
93  doublereal dCoef,
94  const VectorHandler& /* XCurr */ ,
95  const VectorHandler& /* XPrimeCurr */ )
96 {
97  DEBUGCOUT("Entering UniversalHingeJoint::AssJac()" << std::endl);
98 
99  FullSubMatrixHandler& WM = WorkMat.SetFull();
100 
101  /* Ridimensiona la sottomatrice in base alle esigenze */
102  integer iNumRows = 0;
103  integer iNumCols = 0;
104  WorkSpaceDim(&iNumRows, &iNumCols);
105  WM.ResizeReset(iNumRows, iNumCols);
106 
107  /* Recupera gli indici delle varie incognite */
108  integer iNode1FirstPosIndex = pNode1->iGetFirstPositionIndex();
109  integer iNode1FirstMomIndex = pNode1->iGetFirstMomentumIndex();
110  integer iNode2FirstPosIndex = pNode2->iGetFirstPositionIndex();
111  integer iNode2FirstMomIndex = pNode2->iGetFirstMomentumIndex();
112  integer iFirstReactionIndex = iGetFirstIndex();
113 
114  /* Recupera i dati che servono */
115  const Mat3x3& R1(pNode1->GetRRef());
116  const Mat3x3& R2(pNode2->GetRRef());
117  Vec3 d1Tmp(R1*d1);
118  Vec3 d2Tmp(R2*d2);
119 
120  /* Suppongo che le reazioni F, M siano gia' state aggiornate da AssRes;
121  * ricordo che la forza F e' nel sistema globale, mentre la coppia M
122  * e' nel sistema locale ed il terzo termine, M(3), e' nullo in quanto
123  * diretto come l'asse attorno al quale la rotazione e' consentita */
124 
125  /* Setta gli indici delle equazioni */
126  for (int iCnt = 1; iCnt <= 6; iCnt++) {
127  WM.PutRowIndex(iCnt, iNode1FirstMomIndex + iCnt);
128  WM.PutColIndex(iCnt, iNode1FirstPosIndex + iCnt);
129  WM.PutRowIndex(6 + iCnt, iNode2FirstMomIndex + iCnt);
130  WM.PutColIndex(6 + iCnt, iNode2FirstPosIndex + iCnt);
131  }
132 
133  for (int iCnt = 1; iCnt <= 4; iCnt++) {
134  WM.PutRowIndex(12 + iCnt, iFirstReactionIndex + iCnt);
135  WM.PutColIndex(12 + iCnt, iFirstReactionIndex + iCnt);
136  }
137 
138  /* Contributo della forza alle equazioni di equilibrio dei due nodi */
139  for (int iCnt = 1; iCnt <= 3; iCnt++) {
140  WM.PutCoef(iCnt, 12 + iCnt, 1.);
141  WM.PutCoef(6 + iCnt, 12 + iCnt, -1.);
142  }
143 
144  WM.Add(4, 13, Mat3x3(MatCross, d1Tmp));
145  WM.Sub(10, 13, Mat3x3(MatCross, d2Tmp));
146 
147  /* Moltiplica la forza ed il momento per il coefficiente del metodo */
148  Vec3 FTmp(F*dCoef);
149 
150  Vec3 e3a(R1*R1h.GetVec(3));
151  Vec3 e2b(R2*R2h.GetVec(2));
152  Vec3 MTmp = e2b*(dM*dCoef);
153 
154  Mat3x3 MWedgee3aWedge(MatCrossCross, MTmp, e3a);
155  Mat3x3 e3aWedgeMWedge(MatCrossCross, e3a, MTmp);
156 
157  WM.Add(4, 4, Mat3x3(MatCrossCross, FTmp, d1Tmp) - MWedgee3aWedge);
158  WM.Add(4, 10, e3aWedgeMWedge);
159 
160  WM.Add(10, 4, MWedgee3aWedge);
161  WM.Sub(10, 10, Mat3x3(MatCrossCross, FTmp, d2Tmp) + e3aWedgeMWedge);
162 
163  /* Contributo del momento alle equazioni di equilibrio dei nodi */
164  Vec3 Tmp(e2b.Cross(e3a));
165 
166  for (int iCnt = 1; iCnt <= 3; iCnt++) {
167  doublereal d = Tmp(iCnt);
168  WM.PutCoef(3 + iCnt, 16, d);
169  WM.PutCoef(9 + iCnt, 16, -d);
170  }
171 
172  /* Modifica: divido le equazioni di vincolo per dCoef */
173 
174  /* Equazioni di vincolo degli spostamenti */
175  for (int iCnt = 1; iCnt <= 3; iCnt++) {
176  WM.PutCoef(12 + iCnt, iCnt, 1.);
177  WM.PutCoef(12 + iCnt, 6 + iCnt, -1.);
178  }
179 
180  WM.Sub(13, 4, Mat3x3(MatCross, d1Tmp));
181  WM.Add(13, 10, Mat3x3(MatCross, d2Tmp));
182 
183  /* Equazione di vincolo del momento
184  *
185  * Attenzione: bisogna scrivere il vettore trasposto
186  * (Sb[1]^T*(Sa[3]/\))*dCoef
187  * Questo pero' e' uguale a:
188  * (-Sa[3]/\*Sb[1])^T*dCoef,
189  * che puo' essere ulteriormente semplificato:
190  * (Sa[3].Cross(Sb[1])*(-dCoef))^T;
191  */
192 
193  for (int iCnt = 1; iCnt <= 3; iCnt++) {
194  doublereal d = Tmp(iCnt);
195  WM.PutCoef(16, 3 + iCnt, d);
196  WM.PutCoef(16, 9 + iCnt, -d);
197  }
198 
199  return WorkMat;
200 }
201 
202 
203 /* Assemblaggio residuo */
204 SubVectorHandler&
205 UniversalHingeJoint::AssRes(SubVectorHandler& WorkVec,
206  doublereal dCoef,
207  const VectorHandler& XCurr,
208  const VectorHandler& /* XPrimeCurr */ )
209 {
210  DEBUGCOUT("Entering UniversalHingeJoint::AssRes()" << std::endl);
211 
212  /* Dimensiona e resetta la matrice di lavoro */
213  integer iNumRows = 0;
214  integer iNumCols = 0;
215  WorkSpaceDim(&iNumRows, &iNumCols);
216  WorkVec.ResizeReset(iNumRows);
217 
218  /* Indici */
219  integer iNode1FirstMomIndex = pNode1->iGetFirstMomentumIndex();
220  integer iNode2FirstMomIndex = pNode2->iGetFirstMomentumIndex();
221  integer iFirstReactionIndex = iGetFirstIndex();
222 
223  /* Indici dei nodi */
224  for (int iCnt = 1; iCnt <= 6; iCnt++) {
225  WorkVec.PutRowIndex(iCnt, iNode1FirstMomIndex + iCnt);
226  WorkVec.PutRowIndex(6 + iCnt, iNode2FirstMomIndex + iCnt);
227  }
228 
229  /* Indici del vincolo */
230  for (int iCnt = 1; iCnt <= 4; iCnt++) {
231  WorkVec.PutRowIndex(12 + iCnt, iFirstReactionIndex + iCnt);
232  }
233 
234  /* Aggiorna i dati propri */
235  F = Vec3(XCurr, iFirstReactionIndex + 1);
236  dM = XCurr(iFirstReactionIndex + 4);
237 
238  /* Recupera i dati */
239  const Vec3& x1(pNode1->GetXCurr());
240  const Vec3& x2(pNode2->GetXCurr());
241  const Mat3x3& R1(pNode1->GetRCurr());
242  const Mat3x3& R2(pNode2->GetRCurr());
243 
244  /* Costruisce i dati propri nella configurazione corrente */
245  Vec3 dTmp1(R1*d1);
246  Vec3 dTmp2(R2*d2);
247 
248  Vec3 e3a(R1*R1h.GetVec(3));
249  Vec3 e2b(R2*R2h.GetVec(2));
250 
251  Vec3 MTmp(e2b.Cross(e3a)*dM);
252 
253  /* Equazioni di equilibrio, nodo 1 */
254  WorkVec.Sub(1, F);
255  WorkVec.Sub(4, dTmp1.Cross(F) + MTmp);
256 
257  /* Equazioni di equilibrio, nodo 2 */
258  WorkVec.Add(7, F);
259  WorkVec.Add(10, dTmp2.Cross(F) + MTmp);
260 
261  /* Modifica: divido le equazioni di vincolo per dCoef */
262  ASSERT(dCoef != 0.);
263 
264  /* Equazione di vincolo di posizione */
265  WorkVec.Add(13, (x2 + dTmp2 - x1 - dTmp1)/dCoef);
266 
267  /* Equazione di vincolo di rotazione */
268  WorkVec.PutCoef(16, (e3a*e2b)/dCoef);
269 
270  return WorkVec;
271 }
272 
273 /* Output (da mettere a punto) */
274 void
275 UniversalHingeJoint::Output(OutputHandler& OH) const
276 {
277  if (bToBeOutput()) {
278  Mat3x3 R1Tmp(pNode1->GetRCurr()*R1h);
279  Mat3x3 R2Tmp(pNode2->GetRCurr()*R2h);
280 
281  Vec3 vTmp(R2Tmp.GetVec(2).Cross(R1Tmp.GetVec(3)));
282 
283  Joint::Output(OH.Joints(), "CardanoHinge", GetLabel(),
284  R1Tmp.Transpose()*F, Vec3(dM, 0., 0.), F, vTmp*dM)
285  << " " << MatR2EulerAngles(R2Tmp.MulTM(R1Tmp))*dRaDegr
286  << std::endl;
287  }
288 }
289 
290 
291 /* Contributo allo jacobiano durante l'assemblaggio iniziale */
292 VariableSubMatrixHandler&
293 UniversalHingeJoint::InitialAssJac(VariableSubMatrixHandler& WorkMat,
294  const VectorHandler& XCurr)
295 {
296  DEBUGCOUT("Entering UniversalHingeJoint::InitialAssJac()" << std::endl);
297 
298  /* Per ora usa la matrice piena; eventualmente si puo'
299  * passare a quella sparsa quando si ottimizza */
300  FullSubMatrixHandler& WM = WorkMat.SetFull();
301 
302  /* Dimensiona e resetta la matrice di lavoro */
303  integer iNumRows = 0;
304  integer iNumCols = 0;
305  InitialWorkSpaceDim(&iNumRows, &iNumCols);
306  WM.ResizeReset(iNumRows, iNumCols);
307 
308  /* Equazioni: vedi joints.dvi */
309 
310  /* Indici */
311  integer iNode1FirstPosIndex = pNode1->iGetFirstPositionIndex();
312  integer iNode1FirstVelIndex = iNode1FirstPosIndex + 6;
313  integer iNode2FirstPosIndex = pNode2->iGetFirstPositionIndex();
314  integer iNode2FirstVelIndex = iNode2FirstPosIndex + 6;
315  integer iFirstReactionIndex = iGetFirstIndex();
316  integer iReactionPrimeIndex = iFirstReactionIndex + 4;
317 
318  /* Nota: le reazioni vincolari sono:
319  * Forza, 3 incognite, riferimento globale,
320  * Momento, 1 incognita, riferimento locale
321  */
322 
323  /* Setta gli indici dei nodi */
324  for (int iCnt = 1; iCnt <= 6; iCnt++) {
325  WM.PutRowIndex(iCnt, iNode1FirstPosIndex + iCnt);
326  WM.PutColIndex(iCnt, iNode1FirstPosIndex + iCnt);
327  WM.PutRowIndex(6 + iCnt, iNode1FirstVelIndex + iCnt);
328  WM.PutColIndex(6 + iCnt, iNode1FirstVelIndex + iCnt);
329  WM.PutRowIndex(12 + iCnt, iNode2FirstPosIndex + iCnt);
330  WM.PutColIndex(12 + iCnt, iNode2FirstPosIndex + iCnt);
331  WM.PutRowIndex(18 + iCnt, iNode2FirstVelIndex + iCnt);
332  WM.PutColIndex(18 + iCnt, iNode2FirstVelIndex + iCnt);
333  }
334 
335  /* Setta gli indici delle reazioni */
336  for (int iCnt = 1; iCnt <= 8; iCnt++) {
337  WM.PutRowIndex(24 + iCnt, iFirstReactionIndex + iCnt);
338  WM.PutColIndex(24 + iCnt, iFirstReactionIndex + iCnt);
339  }
340 
341  /* Matrici identita' */
342  for (int iCnt = 1; iCnt <= 3; iCnt++) {
343  /* Contributo di forza all'equazione della forza, nodo 1 */
344  WM.IncCoef(iCnt, 24 + iCnt, 1.);
345 
346  /* Contrib. di der. di forza all'eq. della der. della forza, nodo 1 */
347  WM.IncCoef(6 + iCnt, 28 + iCnt, 1.);
348 
349  /* Contributo di forza all'equazione della forza, nodo 2 */
350  WM.DecCoef(12 + iCnt, 24 + iCnt, 1.);
351 
352  /* Contrib. di der. di forza all'eq. della der. della forza, nodo 2 */
353  WM.DecCoef(18 + iCnt, 28 + iCnt, 1.);
354 
355  /* Equazione di vincolo, nodo 1 */
356  WM.DecCoef(24 + iCnt, iCnt, 1.);
357 
358  /* Derivata dell'equazione di vincolo, nodo 1 */
359  WM.DecCoef(28 + iCnt, 6 + iCnt, 1.);
360 
361  /* Equazione di vincolo, nodo 2 */
362  WM.IncCoef(24 + iCnt, 12 + iCnt, 1.);
363 
364  /* Derivata dell'equazione di vincolo, nodo 2 */
365  WM.IncCoef(28 + iCnt, 18 + iCnt, 1.);
366  }
367 
368  /* Recupera i dati */
369  const Mat3x3& R1(pNode1->GetRRef());
370  const Mat3x3& R2(pNode2->GetRRef());
371  const Vec3& Omega1(pNode1->GetWRef());
372  const Vec3& Omega2(pNode2->GetWRef());
373  /* F ed M sono gia' state aggiornate da InitialAssRes */
374  Vec3 FPrime(XCurr, iReactionPrimeIndex+1);
375  doublereal dMPrime(XCurr(iReactionPrimeIndex+4));
376 
377  /* Distanze e matrici di rotazione dai nodi alla cerniera
378  * nel sistema globale */
379  Vec3 d1Tmp(R1*d1);
380  Vec3 d2Tmp(R2*d2);
381 
382  Vec3 e3a(R1*R1h.GetVec(3));
383  Vec3 e2b(R2*R2h.GetVec(2));
384 
385  /* Ruota il momento e la sua derivata con le matrici della cerniera
386  * rispetto ai nodi */
387  Vec3 MTmp(e2b*dM);
388  Vec3 MPrimeTmp(e2b*dMPrime);
389 
390  /* Matrici F/\d1/\, -F/\d2/\ */
391  Mat3x3 FWedged1Wedge(MatCrossCross, F, d1Tmp);
392  Mat3x3 FWedged2Wedge(MatCrossCross, F, -d2Tmp);
393 
394  /* Matrici (omega1/\d1)/\, -(omega2/\d2)/\ */
395  Mat3x3 O1Wedged1Wedge(MatCross, Omega1.Cross(d1Tmp));
396  Mat3x3 O2Wedged2Wedge(MatCross, d2Tmp.Cross(Omega2));
397 
398  Mat3x3 MDeltag1((Mat3x3(MatCross, Omega2.Cross(MTmp) + MPrimeTmp)
399  + Mat3x3(MatCrossCross, MTmp, Omega1))*Mat3x3(MatCross, e3a));
400  Mat3x3 MDeltag2(Mat3x3(MatCrossCross, Omega1.Cross(e3a), MTmp)
401  + Mat3x3(MatCrossCross, e3a, MPrimeTmp)
402  + e3a.Cross(Mat3x3(MatCrossCross, Omega2, MTmp)));
403 
404  /* Vettori temporanei */
405  Vec3 Tmp(e2b.Cross(e3a));
406 
407  /* Prodotto vettore tra il versore 3 della cerniera secondo il nodo 1
408  * ed il versore 1 della cerniera secondo il nodo 2. A convergenza
409  * devono essere ortogonali, quindi il loro prodotto vettore deve essere
410  * unitario */
411 
412  /* Error handling: il programma si ferma, pero' segnala dov'e' l'errore */
413  if (Tmp.Dot() < std::numeric_limits<doublereal>::epsilon()) {
414  silent_cerr("CardanoHingeJoint(" << GetLabel() << "): "
415  "first and second node hinge axes are (nearly) orthogonal"
416  << std::endl);
417  throw ErrNullNorm(MBDYN_EXCEPT_ARGS);
418  }
419 
420  Vec3 TmpPrime(e2b.Cross(Omega1.Cross(e3a)) - e3a.Cross(Omega2.Cross(e2b)));
421 
422  /* Equazione di momento, nodo 1 */
423  WM.Add(4, 4, FWedged1Wedge - Mat3x3(MatCrossCross, MTmp, e3a));
424  WM.Add(4, 16, Mat3x3(MatCrossCross, e3a, MTmp));
425  WM.Add(4, 25, Mat3x3(MatCross, d1Tmp));
426  for (int iCnt = 1; iCnt <= 3; iCnt++) {
427  WM.PutCoef(3 + iCnt, 28, Tmp(iCnt));
428  }
429 
430  /* Equazione di momento, nodo 2 */
431  WM.Add(16, 4, Mat3x3(MatCrossCross, MTmp, e3a));
432  WM.Add(16, 16, FWedged2Wedge - Mat3x3(MatCrossCross, e3a, MTmp));
433  WM.Sub(16, 25, Mat3x3(MatCross, d2Tmp));
434  for (int iCnt = 1; iCnt <= 3; iCnt++) {
435  WM.DecCoef(15 + iCnt, 28, Tmp(iCnt));
436  }
437 
438  /* Derivata dell'equazione di momento, nodo 1 */
439  WM.Add(10, 4, (Mat3x3(MatCross, FPrime) + Mat3x3(MatCrossCross, F, Omega1))*Mat3x3(MatCross, d1Tmp) - MDeltag1);
440  WM.Add(10, 10, FWedged1Wedge - Mat3x3(MatCrossCross, MTmp, e3a));
441  WM.Add(10, 16, MDeltag2);
442  WM.Add(10, 22, Mat3x3(MatCrossCross, e3a, MTmp));
443  WM.Add(10, 25, O1Wedged1Wedge);
444  WM.Add(10, 29, Mat3x3(MatCross, d1Tmp));
445 
446  for (int iCnt = 1; iCnt <= 3; iCnt++) {
447  WM.PutCoef(9 + iCnt, 28, TmpPrime(iCnt));
448  WM.PutCoef(9 + iCnt, 32, Tmp(iCnt));
449  }
450 
451  /* Derivata dell'equazione di momento, nodo 2 */
452  WM.Add(22, 4, MDeltag1);
453  WM.Add(22, 10, Mat3x3(MatCrossCross, MTmp, e3a));
454  WM.Sub(22, 16, (Mat3x3(MatCross, FPrime) + Mat3x3(MatCrossCross, F, Omega2))*Mat3x3(MatCross, d2Tmp) + MDeltag2);
455  WM.Add(22, 22, FWedged2Wedge - Mat3x3(MatCrossCross, e3a, MTmp));
456  WM.Add(22, 25, O2Wedged2Wedge);
457  WM.Sub(22, 29, Mat3x3(MatCross, d2Tmp));
458 
459  for (int iCnt = 1; iCnt <= 3; iCnt++) {
460  WM.DecCoef(21 + iCnt, 28, TmpPrime(iCnt));
461  WM.DecCoef(21 + iCnt, 32, Tmp(iCnt));
462  }
463 
464  /* Equazione di vincolo di posizione */
465  WM.Add(25, 4, Mat3x3(MatCross, d1Tmp));
466  WM.Sub(25, 16, Mat3x3(MatCross, d2Tmp));
467 
468  /* Derivata dell'equazione di vincolo di posizione */
469  WM.Add(29, 4, O1Wedged1Wedge);
470  WM.Add(29, 10, Mat3x3(MatCross, d1Tmp));
471  WM.Add(29, 16, O2Wedged2Wedge);
472  WM.Sub(29, 22, Mat3x3(MatCross, d2Tmp));
473 
474  /* Equazioni di vincolo di rotazione: e2b~e3a */
475 
476  for (int iCnt = 1; iCnt <= 3; iCnt++) {
477  doublereal d = Tmp(iCnt);
478  WM.IncCoef(28, 3 + iCnt, d);
479  WM.DecCoef(28, 15 + iCnt, d);
480 
481  /* Queste sono per la derivata dell'equazione, sono qui solo per
482  * ottimizzazione */
483  WM.IncCoef(32, 9 + iCnt, d);
484  WM.DecCoef(32, 21 + iCnt, d);
485  }
486 
487  /* Derivate delle equazioni di vincolo di rotazione: e2b~e3a */
488  Vec3 O1mO2(Omega1 - Omega2);
489  TmpPrime = e3a.Cross(O1mO2.Cross(e2b));
490  Vec3 TmpPrime2 = e2b.Cross(e3a.Cross(O1mO2));
491  for (int iCnt = 1; iCnt <= 3; iCnt++) {
492  WM.PutCoef(32, 3 + iCnt, TmpPrime(iCnt));
493  WM.PutCoef(32, 15 + iCnt, TmpPrime2(iCnt));
494  }
495 
496  return WorkMat;
497 }
498 
499 
500 /* Contributo al residuo durante l'assemblaggio iniziale */
501 SubVectorHandler&
502 UniversalHingeJoint::InitialAssRes(SubVectorHandler& WorkVec,
503  const VectorHandler& XCurr)
504 {
505  DEBUGCOUT("Entering UniversalHingeJoint::InitialAssRes()" << std::endl);
506 
507  /* Dimensiona e resetta la matrice di lavoro */
508  integer iNumRows = 0;
509  integer iNumCols = 0;
510  InitialWorkSpaceDim(&iNumRows, &iNumCols);
511  WorkVec.ResizeReset(iNumRows);
512 
513  /* Indici */
514  integer iNode1FirstPosIndex = pNode1->iGetFirstPositionIndex();
515  integer iNode1FirstVelIndex = iNode1FirstPosIndex + 6;
516  integer iNode2FirstPosIndex = pNode2->iGetFirstPositionIndex();
517  integer iNode2FirstVelIndex = iNode2FirstPosIndex + 6;
518  integer iFirstReactionIndex = iGetFirstIndex();
519  integer iReactionPrimeIndex = iFirstReactionIndex + 4;
520 
521  /* Setta gli indici */
522  for (int iCnt = 1; iCnt <= 6; iCnt++) {
523  WorkVec.PutRowIndex(iCnt, iNode1FirstPosIndex + iCnt);
524  WorkVec.PutRowIndex(6 + iCnt, iNode1FirstVelIndex + iCnt);
525  WorkVec.PutRowIndex(12 + iCnt, iNode2FirstPosIndex + iCnt);
526  WorkVec.PutRowIndex(18 + iCnt, iNode2FirstVelIndex + iCnt);
527  }
528 
529  for (int iCnt = 1; iCnt <= 8; iCnt++) {
530  WorkVec.PutRowIndex(24 + iCnt, iFirstReactionIndex+iCnt);
531  }
532 
533  /* Recupera i dati */
534  const Vec3& x1(pNode1->GetXCurr());
535  const Vec3& x2(pNode2->GetXCurr());
536  const Vec3& v1(pNode1->GetVCurr());
537  const Vec3& v2(pNode2->GetVCurr());
538  const Mat3x3& R1(pNode1->GetRCurr());
539  const Mat3x3& R2(pNode2->GetRCurr());
540  const Vec3& Omega1(pNode1->GetWCurr());
541  const Vec3& Omega2(pNode2->GetWCurr());
542 
543  /* Aggiorna F ed M, che restano anche per InitialAssJac */
544  F = Vec3(XCurr, iFirstReactionIndex + 1);
545  dM = XCurr(iFirstReactionIndex + 4);
546  Vec3 FPrime(XCurr, iReactionPrimeIndex + 1);
547  doublereal dMPrime(XCurr(iReactionPrimeIndex + 4));
548 
549  /* Distanza nel sistema globale */
550  Vec3 d1Tmp(R1*d1);
551  Vec3 d2Tmp(R2*d2);
552 
553  /* Vettori omega1/\d1, -omega2/\d2 */
554  Vec3 O1Wedged1(Omega1.Cross(d1Tmp));
555  Vec3 O2Wedged2(Omega2.Cross(d2Tmp));
556 
557  Vec3 e3a(R1*R1h.GetVec(3));
558  Vec3 e2b(R2*R2h.GetVec(2));
559 
560  /* Ruota il momento e la sua derivata con le matrici della cerniera
561  * rispetto ai nodi */
562  Vec3 MTmp(e2b*dM);
563  Vec3 MPrimeTmp(e3a.Cross(MTmp.Cross(Omega2)) + e2b.Cross(e3a)*dMPrime);
564 
565  /* Equazioni di equilibrio, nodo 1 */
566  WorkVec.Sub(1, F);
567  WorkVec.Sub(4, d1Tmp.Cross(F) + MTmp.Cross(e3a));
568 
569  /* Derivate delle equazioni di equilibrio, nodo 1 */
570  WorkVec.Sub(7, FPrime);
571  WorkVec.Sub(10, d1Tmp.Cross(FPrime) - O1Wedged1.Cross(F) - MPrimeTmp);
572 
573  /* Equazioni di equilibrio, nodo 2 */
574  WorkVec.Add(13, F);
575  WorkVec.Add(16, d2Tmp.Cross(F) + MTmp.Cross(e3a));
576 
577  /* Derivate delle equazioni di equilibrio, nodo 2 */
578  WorkVec.Add(19, FPrime);
579  WorkVec.Add(22, d2Tmp.Cross(FPrime) + O2Wedged2.Cross(F) + MPrimeTmp);
580 
581  /* Equazione di vincolo di posizione */
582  WorkVec.Add(25, x1 + d1Tmp - x2 - d2Tmp);
583 
584  /* Derivata dell'equazione di vincolo di posizione */
585  WorkVec.Add(29, v1 + O1Wedged1 - v2 - O2Wedged2);
586 
587  /* Equazioni di vincolo di rotazione */
588  WorkVec.PutCoef(28, e2b*e3a);
589 
590  /* Derivate delle equazioni di vincolo di rotazione: e2b~e3a */
591  Vec3 Tmp((Omega1 - Omega2).Cross(e3a));
592  WorkVec.PutCoef(32, e2b*Tmp);
593 
594  return WorkVec;
595 }
596 
597 /* UniversalHingeJoint - end */
598 
599 
600 /* UniversalRotationJoint - begin */
601 
602 /* Costruttore non banale */
603 UniversalRotationJoint::UniversalRotationJoint(unsigned int uL,
604  const DofOwner* pDO,
605  const StructNode* pN1,
606  const StructNode* pN2,
607  const Mat3x3& R1hTmp,
608  const Mat3x3& R2hTmp,
609  const OrientationDescription& od,
610  flag fOut)
611 : Elem(uL, fOut),
612 Joint(uL, pDO, fOut),
613 pNode1(pN1), pNode2(pN2),
614 #ifdef USE_NETCDF
615 Var_Phi(0),
616 #endif // USE_NETCDF
617 R1h(R1hTmp), R2h(R2hTmp), dM(0.), od(od)
618 {
619  NO_OP;
620 }
621 
622 
623 /* Distruttore banale */
624 UniversalRotationJoint::~UniversalRotationJoint(void)
625 {
626  NO_OP;
627 }
628 
629 
630 /* Contributo al file di restart */
631 std::ostream&
632 UniversalRotationJoint::Restart(std::ostream& out) const
633 {
634  Joint::Restart(out) << ", universal rotation, "
635  << pNode1->GetLabel() << ", hinge, reference, node, "
636  "1, ", (R1h.GetVec(1)).Write(out, ", ") << ", "
637  "2, ", (R1h.GetVec(2)).Write(out, ", ") << ", "
638  << pNode2->GetLabel() << ", hinge, reference, node, "
639  "1, ", (R2h.GetVec(1)).Write(out, ", ") << ", "
640  "2, ", (R2h.GetVec(2)).Write(out, ", ") << ";"
641  << std::endl;
642 
643  return out;
644 }
645 
646 
647 /* Assemblaggio jacobiano */
648 VariableSubMatrixHandler&
649 UniversalRotationJoint::AssJac(VariableSubMatrixHandler& WorkMat,
650  doublereal dCoef,
651  const VectorHandler& /* XCurr */ ,
652  const VectorHandler& /* XPrimeCurr */ )
653 {
654  DEBUGCOUT("Entering UniversalRotationJoint::AssJac()" << std::endl);
655 
656  /* Setta la sottomatrice come piena (e' un po' dispersivo, ma lo jacobiano
657  * e' complicato */
658  FullSubMatrixHandler& WM = WorkMat.SetFull();
659 
660  /* Ridimensiona la sottomatrice in base alle esigenze */
661  integer iNumRows = 0;
662  integer iNumCols = 0;
663  WorkSpaceDim(&iNumRows, &iNumCols);
664  WM.ResizeReset(iNumRows, iNumCols);
665 
666  /* Recupera gli indici delle varie incognite */
667  integer iNode1FirstPosIndex = pNode1->iGetFirstPositionIndex() + 3;
668  integer iNode1FirstMomIndex = pNode1->iGetFirstMomentumIndex() + 3;
669  integer iNode2FirstPosIndex = pNode2->iGetFirstPositionIndex() + 3;
670  integer iNode2FirstMomIndex = pNode2->iGetFirstMomentumIndex() + 3;
671  integer iFirstReactionIndex = iGetFirstIndex();
672 
673  /* Setta gli indici delle equazioni */
674  for (int iCnt = 1; iCnt <= 3; iCnt++) {
675  WM.PutRowIndex(iCnt, iNode1FirstMomIndex + iCnt);
676  WM.PutColIndex(iCnt, iNode1FirstPosIndex + iCnt);
677  WM.PutRowIndex(3 + iCnt, iNode2FirstMomIndex + iCnt);
678  WM.PutColIndex(3 + iCnt, iNode2FirstPosIndex + iCnt);
679  }
680 
681  WM.PutRowIndex(6 + 1, iFirstReactionIndex + 1);
682  WM.PutColIndex(6 + 1, iFirstReactionIndex + 1);
683 
684  /* Recupera i dati che servono */
685  const Mat3x3& R1(pNode1->GetRRef());
686  const Mat3x3& R2(pNode2->GetRRef());
687 
688  /* Suppongo che le reazioni F, M siano gia' state aggiornate da AssRes;
689  * ricordo che la forza F e' nel sistema globale, mentre la coppia M
690  * e' nel sistema locale ed il terzo termine, M(3), e' nullo in quanto
691  * diretto come l'asse attorno al quale la rotazione e' consentita */
692 
693  Vec3 e3a(R1*R1h.GetVec(3));
694  Vec3 e2b(R2*R2h.GetVec(2));
695  Vec3 MTmp = e2b*(dM*dCoef);
696 
697  Mat3x3 MWedgee3aWedge(MatCrossCross, MTmp, e3a);
698  Mat3x3 e3aWedgeMWedge(MatCrossCross, e3a, MTmp);
699 
700  WM.Sub(0 + 1, 0 + 1, MWedgee3aWedge);
701  WM.Add(0 + 1, 3 + 1, e3aWedgeMWedge);
702 
703  WM.Add(3 + 1, 0 + 1, MWedgee3aWedge);
704  WM.Sub(3 + 1, 3 + 1, e3aWedgeMWedge);
705 
706  /* Contributo del momento alle equazioni di equilibrio dei nodi */
707  Vec3 Tmp(e2b.Cross(e3a));
708 
709  for (int iCnt = 1; iCnt <= 3; iCnt++) {
710  doublereal d = Tmp(iCnt);
711  WM.IncCoef(iCnt, 6 + 1, d);
712  WM.DecCoef(3 + iCnt, 6 + 1, d);
713 
714  /* Modifica: divido le equazioni di vincolo per dCoef */
715  WM.IncCoef(6 + 1, iCnt, d);
716  WM.DecCoef(6 + 1, 3 + iCnt, d);
717  }
718 
719  return WorkMat;
720 }
721 
722 
723 /* Assemblaggio residuo */
724 SubVectorHandler&
725 UniversalRotationJoint::AssRes(SubVectorHandler& WorkVec,
726  doublereal dCoef,
727  const VectorHandler& XCurr,
728  const VectorHandler& /* XPrimeCurr */ )
729 {
730  DEBUGCOUT("Entering UniversalRotationJoint::AssRes()" << std::endl);
731 
732  /* Dimensiona e resetta la matrice di lavoro */
733  integer iNumRows = 0;
734  integer iNumCols = 0;
735  WorkSpaceDim(&iNumRows, &iNumCols);
736  WorkVec.ResizeReset(iNumRows);
737 
738  /* Indici */
739  integer iNode1FirstMomIndex = pNode1->iGetFirstMomentumIndex()+3;
740  integer iNode2FirstMomIndex = pNode2->iGetFirstMomentumIndex()+3;
741  integer iFirstReactionIndex = iGetFirstIndex();
742 
743  /* Indici dei nodi */
744  for (int iCnt = 1; iCnt <= 3; iCnt++) {
745  WorkVec.PutRowIndex(iCnt, iNode1FirstMomIndex + iCnt);
746  WorkVec.PutRowIndex(3 + iCnt, iNode2FirstMomIndex + iCnt);
747  }
748 
749  /* Indici del vincolo */
750  WorkVec.PutRowIndex(6 + 1, iFirstReactionIndex + 1);
751 
752  /* Aggiorna i dati propri */
753  dM = XCurr(iFirstReactionIndex+1);
754 
755  /* Recupera i dati */
756  const Mat3x3& R1(pNode1->GetRCurr());
757  const Mat3x3& R2(pNode2->GetRCurr());
758 
759  /* Costruisce i dati propri nella configurazione corrente */
760  Vec3 e3a(R1*R1h.GetVec(3));
761  Vec3 e2b(R2*R2h.GetVec(2));
762 
763  Vec3 MTmp(e2b.Cross(e3a)*dM);
764 
765  /* Equazioni di equilibrio, nodo 1 */
766  WorkVec.Sub(1, MTmp); /* Sfrutto F/\d = -d/\F */
767 
768  /* Equazioni di equilibrio, nodo 2 */
769  WorkVec.Add(4, MTmp);
770 
771  /* Modifica: divido le equazioni di vincolo per dCoef */
772  ASSERT(dCoef != 0.);
773  /* Equazione di vincolo di rotazione */
774  WorkVec.PutCoef(6 + 1, (e3a*e2b)/dCoef);
775 
776  return WorkVec;
777 }
778 
779 void
780 UniversalRotationJoint::OutputPrepare(OutputHandler& OH)
781 {
782  if (bToBeOutput()) {
783 #ifdef USE_NETCDF
784  if (OH.UseNetCDF(OutputHandler::JOINTS)) {
785  std::string name;
786  OutputPrepare_int("cardano rotation", OH, name);
787 
788  Var_Phi = OH.CreateRotationVar(name, "", od, "global");
789 
790  }
791 #endif // USE_NETCDF
792  }
793 }
794 
795 
796 /* Output (da mettere a punto) */
797 void
798 UniversalRotationJoint::Output(OutputHandler& OH) const
799 {
800  if (bToBeOutput()) {
801  Mat3x3 R1Tmp(pNode1->GetRCurr()*R1h);
802  Mat3x3 R2Tmp(pNode2->GetRCurr()*R2h);
803  Vec3 vTmp(R2Tmp.GetVec(2).Cross(R1Tmp.GetVec(3)));
804  Mat3x3 RTmp(R2Tmp.Transpose()*R1Tmp);
805  Vec3 E;
806  switch (od) {
807  case EULER_123:
808  E = MatR2EulerAngles123(RTmp)*dRaDegr;
809  break;
810 
811  case EULER_313:
812  E = MatR2EulerAngles313(RTmp)*dRaDegr;
813  break;
814 
815  case EULER_321:
816  E = MatR2EulerAngles321(RTmp)*dRaDegr;
817  break;
818 
819  case ORIENTATION_VECTOR:
820  E = RotManip::VecRot(RTmp);
821  break;
822 
823  case ORIENTATION_MATRIX:
824  break;
825 
826  default:
827  /* impossible */
828  break;
829  }
830 
831 
832 #ifdef USE_NETCDF
833  if (OH.UseNetCDF(OutputHandler::JOINTS)) {
834  Var_F_local->put_rec(Zero3.pGetVec(), OH.GetCurrentStep());
835  Var_M_local->put_rec((Vec3(dM, 0., 0.)).pGetVec(), OH.GetCurrentStep());
836  Var_F_global->put_rec(Zero3.pGetVec(), OH.GetCurrentStep());
837  Var_M_global->put_rec((vTmp*dM).pGetVec(), OH.GetCurrentStep());
838  switch (od) {
839  case EULER_123:
840  case EULER_313:
841  case EULER_321:
842  case ORIENTATION_VECTOR:
843  Var_Phi->put_rec(E.pGetVec(), OH.GetCurrentStep());
844  break;
845 
846  case ORIENTATION_MATRIX:
847  Var_Phi->put_rec(RTmp.pGetMat(), OH.GetCurrentStep());
848  break;
849 
850  default:
851  /* impossible */
852  break;
853  }
854  }
855 #endif // USE_NETCDF
856  if (OH.UseText(OutputHandler::JOINTS)) {
857  Joint::Output(OH.Joints(), "CardanoHinge", GetLabel(),
858  Zero3, Vec3(dM, 0., 0.), Zero3, vTmp*dM)
859  << " ";
860  switch (od) {
861  case EULER_123:
862  case EULER_313:
863  case EULER_321:
864  case ORIENTATION_VECTOR:
865  OH.Joints() << E;
866  break;
867 
868  case ORIENTATION_MATRIX:
869  OH.Joints() << RTmp;
870  break;
871 
872  default:
873  /* impossible */
874  break;
875  }
876 
877  OH.Joints() << std::endl;
878  }
879  }
880 }
881 
882 
883 /* Contributo allo jacobiano durante l'assemblaggio iniziale */
884 VariableSubMatrixHandler&
885 UniversalRotationJoint::InitialAssJac(VariableSubMatrixHandler& WorkMat,
886  const VectorHandler& XCurr)
887 {
888  DEBUGCOUT("Entering UniversalRotationJoint::InitialAssJac()" << std::endl);
889 
890  /* Per ora usa la matrice piena; eventualmente si puo'
891  * passare a quella sparsa quando si ottimizza */
892  FullSubMatrixHandler& WM = WorkMat.SetFull();
893 
894  /* Dimensiona e resetta la matrice di lavoro */
895  integer iNumRows = 0;
896  integer iNumCols = 0;
897  InitialWorkSpaceDim(&iNumRows, &iNumCols);
898  WM.ResizeReset(iNumRows, iNumCols);
899 
900  /* Equazioni: vedi joints.dvi */
901 
902  /* Indici */
903  integer iNode1FirstPosIndex = pNode1->iGetFirstPositionIndex() + 3;
904  integer iNode1FirstVelIndex = iNode1FirstPosIndex + 6;
905  integer iNode2FirstPosIndex = pNode2->iGetFirstPositionIndex() + 3;
906  integer iNode2FirstVelIndex = iNode2FirstPosIndex + 6;
907  integer iFirstReactionIndex = iGetFirstIndex();
908  integer iReactionPrimeIndex = iFirstReactionIndex + 1;
909 
910  /* Nota: le reazioni vincolari sono:
911  * Forza, 3 incognite, riferimento globale,
912  * Momento, 1 incognita, riferimento locale
913  */
914 
915  /* Setta gli indici dei nodi */
916  for (int iCnt = 1; iCnt <= 3; iCnt++) {
917  WM.PutRowIndex(iCnt, iNode1FirstPosIndex + iCnt);
918  WM.PutColIndex(iCnt, iNode1FirstPosIndex + iCnt);
919  WM.PutRowIndex(3 + iCnt, iNode1FirstVelIndex + iCnt);
920  WM.PutColIndex(3 + iCnt, iNode1FirstVelIndex + iCnt);
921  WM.PutRowIndex(6 + iCnt, iNode2FirstPosIndex + iCnt);
922  WM.PutColIndex(6 + iCnt, iNode2FirstPosIndex + iCnt);
923  WM.PutRowIndex(9 + iCnt, iNode2FirstVelIndex + iCnt);
924  WM.PutColIndex(9 + iCnt, iNode2FirstVelIndex + iCnt);
925  }
926 
927  /* Setta gli indici delle reazioni */
928  for (int iCnt = 1; iCnt <= 2; iCnt++) {
929  WM.PutRowIndex(12 + iCnt, iFirstReactionIndex + iCnt);
930  WM.PutColIndex(12 + iCnt, iFirstReactionIndex + iCnt);
931  }
932 
933  /* Recupera i dati */
934  const Mat3x3& R1(pNode1->GetRRef());
935  const Mat3x3& R2(pNode2->GetRRef());
936  const Vec3& Omega1(pNode1->GetWRef());
937  const Vec3& Omega2(pNode2->GetWRef());
938  /* F ed M sono gia' state aggiornate da InitialAssRes */
939  doublereal dMPrime(XCurr(iReactionPrimeIndex+1));
940 
941  /* Matrici di rotazione dai nodi alla cerniera
942  * nel sistema globale */
943  Vec3 e3a(R1*R1h.GetVec(3));
944  Vec3 e2b(R2*R2h.GetVec(2));
945 
946  /* Ruota il momento e la sua derivata con le matrici della cerniera
947  * rispetto ai nodi */
948  Vec3 MTmp(e2b*dM);
949  Vec3 MPrimeTmp(e2b*dMPrime);
950 
951  /* Matrici (omega1/\d1)/\, -(omega2/\d2)/\ */
952  Mat3x3 MDeltag1((Mat3x3(MatCross, Omega2.Cross(MTmp) + MPrimeTmp)
953  + Mat3x3(MatCrossCross, MTmp, Omega1))*Mat3x3(MatCross, e3a));
954  Mat3x3 MDeltag2(Mat3x3(MatCrossCross, Omega1.Cross(e3a), MTmp)
955  + Mat3x3(MatCrossCross, e3a, MPrimeTmp)
956  + e3a.Cross(Mat3x3(MatCrossCross, Omega2, MTmp)));
957 
958  /* Vettori temporanei */
959  Vec3 Tmp(e2b.Cross(e3a));
960 
961  /* Prodotto vettore tra il versore 3 della cerniera secondo il nodo 1
962  * ed il versore 1 della cerniera secondo il nodo 2. A convergenza
963  * devono essere ortogonali, quindi il loro prodotto vettore deve essere
964  * unitario */
965 
966  /* Error handling: il programma si ferma, pero' segnala dov'e' l'errore */
967  if (Tmp.Dot() < std::numeric_limits<doublereal>::epsilon()) {
968  silent_cerr("CardanoRotationJoint(" << GetLabel() << "): "
969  "first and second node hinge axes are (nearly) orthogonal"
970  << std::endl);
971  throw ErrNullNorm(MBDYN_EXCEPT_ARGS);
972  }
973 
974  Vec3 TmpPrime(e2b.Cross(Omega1.Cross(e3a)) - e3a.Cross(Omega2.Cross(e2b)));
975 
976  /* Equazione di momento, nodo 1 */
977  WM.Sub(1, 1, Mat3x3(MatCrossCross, MTmp, e3a));
978  WM.Add(4, 7, Mat3x3(MatCrossCross, e3a, MTmp));
979  for (int iCnt = 1; iCnt <= 3; iCnt++) {
980  WM.PutCoef(iCnt, 13, Tmp.dGet(iCnt));
981  }
982 
983  /* Equazione di momento, nodo 2 */
984  WM.Add(7, 1, Mat3x3(MatCrossCross, MTmp, e3a));
985  WM.Sub(7, 7, Mat3x3(MatCrossCross, e3a, MTmp));
986  for (int iCnt = 1; iCnt <= 3; iCnt++) {
987  WM.DecCoef(6 + iCnt, 13, Tmp(iCnt));
988  }
989 
990  /* Derivata dell'equazione di momento, nodo 1 */
991  WM.Sub(4, 1, MDeltag1);
992  WM.Sub(4, 4, Mat3x3(MatCrossCross, MTmp, e3a));
993  WM.Add(4, 7, MDeltag2);
994  WM.Add(4, 10, Mat3x3(MatCrossCross, e3a, MTmp));
995 
996  for (int iCnt = 1; iCnt <= 3; iCnt++) {
997  WM.PutCoef(9 + iCnt, 13, TmpPrime(iCnt));
998  WM.PutCoef(9 + iCnt, 14, Tmp(iCnt));
999  }
1000 
1001  /* Derivata dell'equazione di momento, nodo 2 */
1002  WM.Add(4, 1, Mat3x3(MatCrossCross, MTmp, e3a));
1003  WM.Sub(4, 4, Mat3x3(MatCrossCross, e3a, MTmp));
1004 
1005  for (int iCnt = 1; iCnt <= 3; iCnt++) {
1006  WM.DecCoef(3 + iCnt, 13, TmpPrime(iCnt));
1007  WM.DecCoef(3 + iCnt, 14, Tmp(iCnt));
1008  }
1009 
1010  /* Equazioni di vincolo di rotazione: e2b~e3a */
1011 
1012  for (int iCnt = 1; iCnt <= 3; iCnt++) {
1013  doublereal d = Tmp(iCnt);
1014  WM.IncCoef(13, iCnt, d);
1015  WM.DecCoef(13, 6 + iCnt, d);
1016 
1017  /* Queste sono per la derivata dell'equazione, sono qui solo per
1018  * ottimizzazione */
1019  WM.IncCoef(14, 3 + iCnt, d);
1020  WM.DecCoef(14, 9 + iCnt, d);
1021  }
1022 
1023  /* Derivate delle equazioni di vincolo di rotazione: e2b~e3a */
1024  Vec3 O1mO2(Omega1 - Omega2);
1025  TmpPrime = e3a.Cross(O1mO2.Cross(e2b));
1026  Vec3 TmpPrime2 = e2b.Cross(e3a.Cross(O1mO2));
1027  for (int iCnt = 1; iCnt <= 3; iCnt++) {
1028  WM.PutCoef(14, iCnt, TmpPrime(iCnt));
1029  WM.PutCoef(14, 6 + iCnt, TmpPrime2(iCnt));
1030  }
1031 
1032  return WorkMat;
1033 }
1034 
1035 
1036 /* Contributo al residuo durante l'assemblaggio iniziale */
1037 SubVectorHandler&
1038 UniversalRotationJoint::InitialAssRes(SubVectorHandler& WorkVec,
1039  const VectorHandler& XCurr)
1040 {
1041  DEBUGCOUT("Entering UniversalRotationJoint::InitialAssRes()" << std::endl);
1042 
1043  /* Dimensiona e resetta la matrice di lavoro */
1044  integer iNumRows = 0;
1045  integer iNumCols = 0;
1046  InitialWorkSpaceDim(&iNumRows, &iNumCols);
1047  WorkVec.ResizeReset(iNumRows);
1048 
1049  /* Indici */
1050  integer iNode1FirstPosIndex = pNode1->iGetFirstPositionIndex() + 3;
1051  integer iNode1FirstVelIndex = iNode1FirstPosIndex + 6;
1052  integer iNode2FirstPosIndex = pNode2->iGetFirstPositionIndex() + 3;
1053  integer iNode2FirstVelIndex = iNode2FirstPosIndex + 6;
1054  integer iFirstReactionIndex = iGetFirstIndex();
1055  integer iReactionPrimeIndex = iFirstReactionIndex + 1;
1056 
1057  /* Setta gli indici */
1058  for (int iCnt = 1; iCnt <= 3; iCnt++) {
1059  WorkVec.PutRowIndex(iCnt, iNode1FirstPosIndex + iCnt);
1060  WorkVec.PutRowIndex(3 + iCnt, iNode1FirstVelIndex + iCnt);
1061  WorkVec.PutRowIndex(6 + iCnt, iNode2FirstPosIndex + iCnt);
1062  WorkVec.PutRowIndex(9 + iCnt, iNode2FirstVelIndex + iCnt);
1063  }
1064 
1065  for (int iCnt = 1; iCnt <= 2; iCnt++) {
1066  WorkVec.PutRowIndex(12 + iCnt, iFirstReactionIndex + iCnt);
1067  }
1068 
1069  /* Recupera i dati */
1070  const Mat3x3& R1(pNode1->GetRCurr());
1071  const Mat3x3& R2(pNode2->GetRCurr());
1072  const Vec3& Omega1(pNode1->GetWCurr());
1073  const Vec3& Omega2(pNode2->GetWCurr());
1074 
1075  /* Aggiorna F ed M, che restano anche per InitialAssJac */
1076  dM = XCurr(iFirstReactionIndex + 1);
1077  doublereal dMPrime(XCurr(iReactionPrimeIndex + 1));
1078 
1079  /* orientazione nel sistema globale */
1080  Vec3 e3a(R1*R1h.GetVec(3));
1081  Vec3 e2b(R2*R2h.GetVec(2));
1082 
1083  /* Ruota il momento e la sua derivata con le matrici della cerniera
1084  * rispetto ai nodi */
1085  Vec3 MTmp(e2b*dM);
1086  Vec3 MPrimeTmp(e3a.Cross(MTmp.Cross(Omega2)) + e2b.Cross(e3a)*dMPrime);
1087 
1088  /* Equazioni di equilibrio, nodo 1 */
1089  WorkVec.Sub(1, MTmp.Cross(e3a));
1090 
1091  /* Derivate delle equazioni di equilibrio, nodo 1 */
1092  WorkVec.Sub(4, MPrimeTmp);
1093 
1094  /* Equazioni di equilibrio, nodo 2 */
1095  WorkVec.Add(7, MTmp.Cross(e3a));
1096 
1097  /* Derivate delle equazioni di equilibrio, nodo 2 */
1098  WorkVec.Add(10, MPrimeTmp);
1099 
1100  /* Equazioni di vincolo di rotazione */
1101  WorkVec.PutCoef(13, e2b*e3a);
1102 
1103  /* Derivate delle equazioni di vincolo di rotazione: e2b~e3a */
1104  Vec3 Tmp((Omega1 - Omega2).Cross(e3a));
1105  WorkVec.PutCoef(14, e2b*Tmp);
1106 
1107  return WorkVec;
1108 }
1109 
1110 /* UniversalRotationJoint - end */
1111 
1112 
1113 /* UniversalPinJoint - begin */
1114 
1115 /* Costruttore non banale */
1116 UniversalPinJoint::UniversalPinJoint(unsigned int uL, const DofOwner* pDO,
1117  const StructNode* pN,
1118  const Vec3& X0Tmp, const Mat3x3& R0Tmp,
1119  const Vec3& dTmp, const Mat3x3& RhTmp,
1120  flag fOut)
1121 : Elem(uL, fOut),
1122 Joint(uL, pDO, fOut),
1123 pNode(pN),
1124 X0(X0Tmp), R0(R0Tmp), d(dTmp), Rh(RhTmp), F(Zero3), dM(0.)
1125 {
1126  NO_OP;
1127 }
1128 
1129 
1130 /* Distruttore banale */
1131 UniversalPinJoint::~UniversalPinJoint(void)
1132 {
1133  NO_OP;
1134 }
1135 
1136 
1137 /* Contributo al file di restart */
1138 std::ostream&
1139 UniversalPinJoint::Restart(std::ostream& out) const
1140 {
1141  Joint::Restart(out) << ", universal pin, "
1142  << pNode->GetLabel() << ", "
1143  "reference, node, ", d.Write(out, ", ") << ", "
1144  "hinge, reference, node, "
1145  "1, ", (Rh.GetVec(1)).Write(out, ", ") << ", "
1146  "2, ", (Rh.GetVec(2)).Write(out, ", ") << ", "
1147  "reference, global, ", X0.Write(out, ", ") << ", "
1148  "reference, global, "
1149  "1, ", (R0.GetVec(1)).Write(out, ", ") << ", "
1150  "2, ", (R0.GetVec(2)).Write(out, ", ") << ';'
1151  << std::endl;
1152 
1153  return out;
1154 }
1155 
1156 
1157 /* Assemblaggio jacobiano */
1158 VariableSubMatrixHandler&
1159 UniversalPinJoint::AssJac(VariableSubMatrixHandler& WorkMat,
1160  doublereal dCoef,
1161  const VectorHandler& /* XCurr */ ,
1162  const VectorHandler& /* XPrimeCurr */ )
1163 {
1164  DEBUGCOUT("Entering UniversalPinJoint::AssJac()" << std::endl);
1165 
1166  FullSubMatrixHandler& WM = WorkMat.SetFull();
1167 
1168  /* Dimensiona e resetta la matrice di lavoro */
1169  integer iNumRows = 0;
1170  integer iNumCols = 0;
1171  WorkSpaceDim(&iNumRows, &iNumCols);
1172  WM.ResizeReset(iNumRows, iNumCols);
1173 
1174  integer iFirstPositionIndex = pNode->iGetFirstPositionIndex();
1175  integer iFirstMomentumIndex = pNode->iGetFirstMomentumIndex();
1176  integer iFirstReactionIndex = iGetFirstIndex();
1177 
1178  for (int iCnt = 1; iCnt <= 6; iCnt++) {
1179  WM.PutRowIndex(iCnt, iFirstMomentumIndex + iCnt);
1180  WM.PutColIndex(iCnt, iFirstPositionIndex + iCnt);
1181  }
1182 
1183  for (int iCnt = 1; iCnt <= 4; iCnt++) {
1184  WM.PutRowIndex(6 + iCnt, iFirstReactionIndex + iCnt);
1185  WM.PutColIndex(6 + iCnt, iFirstReactionIndex + iCnt);
1186  }
1187 
1188  const Mat3x3& R(pNode->GetRRef());
1189  Vec3 dTmp(R*d);
1190 
1191  /* termini di reazione sul nodo (forza e momento) */
1192  for (int iCnt = 1; iCnt <= 3; iCnt++) {
1193  WM.PutCoef(iCnt, 6 + iCnt, -1.);
1194  }
1195 
1196  WM.Sub(4, 7, Mat3x3(MatCross, dTmp));
1197 
1198  /* Note: F and dM updated by AssRes */
1199  Vec3 e3(R0.GetVec(3));
1200  Vec3 e2(R*Rh.GetVec(2));
1201  Vec3 MTmp(e2*(dM*dCoef));
1202  Vec3 FTmp(F*dCoef);
1203 
1204  Mat3x3 e3aWedgeMWedge(MatCrossCross, e3, MTmp);
1205 
1206  WM.Sub(4, 4, Mat3x3(MatCrossCross, FTmp, dTmp) + e3aWedgeMWedge);
1207 
1208  Vec3 Tmp(e2.Cross(e3));
1209 
1210  for (int iCnt = 1; iCnt <= 3; iCnt++) {
1211  doublereal d = Tmp(iCnt);
1212  WM.PutCoef(3 + iCnt, 10, -d);
1213  }
1214 
1215  /* Modifica: divido le equazioni di vincolo per dCoef */
1216 
1217  /* termini di vincolo dovuti al nodo 1 */
1218  for (int iCnt = 1; iCnt <= 3; iCnt++) {
1219  // position, delta x
1220  WM.PutCoef(6 + iCnt, iCnt, -1.);
1221 
1222  // orientation, delta g
1223  doublereal d = Tmp(iCnt);
1224  WM.PutCoef(6 + 3 + 1, 3 + iCnt, -d);
1225  }
1226 
1227  // position, delta g
1228  WM.Add(6 + 1, 3 + 1, Mat3x3(MatCross, dTmp));
1229 
1230  return WorkMat;
1231 }
1232 
1233 
1234 /* Assemblaggio residuo */
1235 SubVectorHandler& UniversalPinJoint::AssRes(SubVectorHandler& WorkVec,
1236  doublereal dCoef,
1237  const VectorHandler& XCurr,
1238  const VectorHandler& /* XPrimeCurr */ )
1239 {
1240  DEBUGCOUT("Entering UniversalPinJoint::AssRes()" << std::endl);
1241 
1242  /* Dimensiona e resetta la matrice di lavoro */
1243  integer iNumRows = 0;
1244  integer iNumCols = 0;
1245  WorkSpaceDim(&iNumRows, &iNumCols);
1246  WorkVec.ResizeReset(iNumRows);
1247 
1248  integer iFirstMomentumIndex = pNode->iGetFirstMomentumIndex();
1249  integer iFirstReactionIndex = iGetFirstIndex();
1250 
1251  /* Indici dei nodi */
1252  for (int iCnt = 1; iCnt <= 6; iCnt++) {
1253  WorkVec.PutRowIndex(iCnt, iFirstMomentumIndex + iCnt);
1254  }
1255 
1256  /* Indici del vincolo */
1257  for (int iCnt = 1; iCnt <= 4; iCnt++) {
1258  WorkVec.PutRowIndex(6 + iCnt, iFirstReactionIndex + iCnt);
1259  }
1260 
1261  F = Vec3(XCurr, iFirstReactionIndex + 1);
1262  dM = XCurr(iFirstReactionIndex + 4);
1263 
1264  const Vec3& x(pNode->GetXCurr());
1265  const Mat3x3& R(pNode->GetRCurr());
1266 
1267  Vec3 dTmp(R*d);
1268 
1269  Vec3 e3(R0.GetVec(3));
1270  Vec3 e2(R*Rh.GetVec(2));
1271 
1272  WorkVec.Add(1, F);
1273  WorkVec.Add(4, dTmp.Cross(F) + e2.Cross(e3)*dM);
1274 
1275  /* Modifica: divido le equazioni di vincolo per dCoef */
1276  ASSERT(dCoef != 0.);
1277  WorkVec.Add(7, (x + dTmp - X0)/dCoef);
1278 
1279  WorkVec.PutCoef(10, e3.Dot(e2)/dCoef);
1280 
1281  return WorkVec;
1282 }
1283 
1284 /* Output (da mettere a punto) */
1285 void
1286 UniversalPinJoint::Output(OutputHandler& OH) const
1287 {
1288  if (bToBeOutput()) {
1289  Mat3x3 RTmp(pNode->GetRCurr()*Rh);
1290  Vec3 vTmp(RTmp.GetVec(2).Cross(R0.GetVec(3)));
1291 
1292  Joint::Output(OH.Joints(), "CardanoPin", GetLabel(),
1293  RTmp.MulTV(F), Vec3(dM, 0., 0.), F, vTmp*dM)
1294  << " " << MatR2EulerAngles(R0.Transpose()*RTmp)*dRaDegr
1295  << std::endl;
1296  }
1297 }
1298 
1299 
1300 /* Contributo allo jacobiano durante l'assemblaggio iniziale */
1301 VariableSubMatrixHandler&
1302 UniversalPinJoint::InitialAssJac(VariableSubMatrixHandler& WorkMat,
1303  const VectorHandler& XCurr)
1304 {
1305  DEBUGCOUT("Entering UniversalPinJoint::InitialAssJac()" << std::endl);
1306 
1307  FullSubMatrixHandler& WM = WorkMat.SetFull();
1308 
1309  /* Dimensiona e resetta la matrice di lavoro */
1310  integer iNumRows = 0;
1311  integer iNumCols = 0;
1312  InitialWorkSpaceDim(&iNumRows, &iNumCols);
1313  WM.ResizeReset(iNumRows, iNumCols);
1314 
1315  /* Equazioni: vedi joints.dvi */
1316 
1317  /* Indici */
1318  integer iFirstPositionIndex = pNode->iGetFirstPositionIndex();
1319  integer iFirstVelocityIndex = iFirstPositionIndex + 6;
1320  integer iFirstReactionIndex = iGetFirstIndex();
1321  integer iReactionPrimeIndex = iFirstReactionIndex + 4;
1322 
1323  /* Setto gli indici */
1324  for (int iCnt = 1; iCnt <= 6; iCnt++) {
1325  WM.PutRowIndex(iCnt, iFirstPositionIndex + iCnt);
1326  WM.PutColIndex(iCnt, iFirstPositionIndex + iCnt);
1327  WM.PutRowIndex(6 + iCnt, iFirstVelocityIndex + iCnt);
1328  WM.PutColIndex(6 + iCnt, iFirstVelocityIndex + iCnt);
1329  }
1330 
1331  for (int iCnt = 1; iCnt <= 8; iCnt++) {
1332  WM.PutRowIndex(12 + iCnt, iFirstReactionIndex + iCnt);
1333  WM.PutColIndex(12 + iCnt, iFirstReactionIndex + iCnt);
1334  }
1335 
1336  /* Recupera i dati */
1337  const Mat3x3& R(pNode->GetRRef());
1338  const Vec3& Omega(pNode->GetWRef());
1339  /* F, M sono state aggiornate da InitialAssRes */
1340  Vec3 FPrime(XCurr, iReactionPrimeIndex + 1);
1341  doublereal dMPrime(XCurr(iReactionPrimeIndex + 4));
1342 
1343  /* Matrici identita' */
1344  for (int iCnt = 1; iCnt <= 3; iCnt++) {
1345  /* Contributo di forza all'equazione della forza */
1346  WM.PutCoef(iCnt, 12 + iCnt, 1.);
1347 
1348  /* Contrib. di der. di forza all'eq. della der. della forza */
1349  WM.PutCoef(6 + iCnt, 16 + iCnt, 1.);
1350 
1351  /* Equazione di vincolo */
1352  WM.PutCoef(12 + iCnt, iCnt, -1.);
1353 
1354  /* Derivata dell'equazione di vincolo */
1355  WM.PutCoef(16 + iCnt, 6 + iCnt, -1.);
1356  }
1357 
1358  /* Distanza nel sistema globale */
1359  Vec3 dTmp(R*d);
1360 
1361  Vec3 e3(R0.GetVec(3));
1362  Vec3 e2(R*Rh.GetVec(2));
1363 
1364  /* Vettori temporanei */
1365  Vec3 Tmp(e2.Cross(e3));
1366 
1367  /* Prodotto vettore tra il versore 3 della cerniera secondo il nodo 1
1368  * ed il versore 1 della cerniera secondo il nodo 2. A convergenza
1369  * devono essere ortogonali, quindi il loro prodotto vettore deve essere
1370  * unitario */
1371 
1372  /* Error handling: il programma si ferma, pero' segnala dov'e' l'errore */
1373  if (Tmp.Dot() < std::numeric_limits<doublereal>::epsilon()) {
1374  silent_cerr("CardanoPinJoint(" << GetLabel() << "): "
1375  "node and fixed point hinge axes are (nearly) orthogonal"
1376  << std::endl);
1377  throw ErrNullNorm(MBDYN_EXCEPT_ARGS);
1378  }
1379 
1380  Vec3 TmpPrime(e3.Cross(e2.Cross(Omega)));
1381 
1382  /* Ruota il momento e la sua derivata con le matrici della cerniera
1383  * rispetto ai nodi */
1384  Vec3 MTmp(e2*dM);
1385  Vec3 MPrimeTmp(e2*dMPrime);
1386 
1387  Mat3x3 MDeltag(Mat3x3(MatCrossCross, e3, MPrimeTmp) + e3.Cross(Mat3x3(MatCrossCross, Omega, MTmp)));
1388 
1389  /* Matrici F/\d/\ */
1390  Mat3x3 FWedgedWedge(MatCrossCross, F, dTmp);
1391 
1392  /* Matrici (omega/\d)/\ */
1393  Mat3x3 OWedgedWedge(MatCross, Omega.Cross(dTmp));
1394 
1395  /* Equazione di momento */
1396  WM.Add(4, 4, FWedgedWedge + Mat3x3(MatCrossCross, e3, MTmp));
1397  WM.Add(4, 13, Mat3x3(MatCross, dTmp));
1398 
1399  /* Derivata dell'equazione di momento */
1400  WM.Add(10, 4, (Mat3x3(MatCross, FPrime) + Mat3x3(MatCrossCross, F, Omega))*Mat3x3(MatCross, dTmp) + MDeltag);
1401  WM.Add(10, 10, FWedgedWedge + Mat3x3(MatCrossCross, e3, MTmp));
1402  WM.Add(10, 13, OWedgedWedge);
1403  WM.Add(10, 17, Mat3x3(MatCross, dTmp));
1404 
1405  for (int iCnt = 1; iCnt <= 3; iCnt++) {
1406  doublereal d = Tmp(iCnt);
1407  WM.PutCoef(3 + iCnt, 16, d);
1408  WM.PutCoef(9 + iCnt, 20, d);
1409 
1410  WM.PutCoef(9 + iCnt, 16, TmpPrime(iCnt));
1411  }
1412 
1413  /* Equazione di vincolo */
1414  WM.Add(13, 4, Mat3x3(MatCross, dTmp));
1415 
1416  /* Derivata dell'equazione di vincolo */
1417  WM.Add(17, 4, OWedgedWedge);
1418  WM.Add(17, 10, Mat3x3(MatCross, dTmp));
1419 
1420  /* Equazioni di vincolo di rotazione: e2b~e3a */
1421  for (int iCnt = 1; iCnt <= 3; iCnt++) {
1422  doublereal d = -Tmp(iCnt);
1423  WM.PutCoef(16, 3 + iCnt, d);
1424 
1425  /* Queste sono per la derivata dell'equazione, sono qui solo per
1426  * ottimizzazione */
1427  WM.PutCoef(20, 9 + iCnt, d);
1428  }
1429 
1430  /* Derivate delle equazioni di vincolo di rotazione: e2b~e3a */
1431  TmpPrime = e2.Cross(Omega.Cross(e3));
1432  for (int iCnt = 1; iCnt <= 3; iCnt++) {
1433  WM.PutCoef(20, 3 + iCnt, TmpPrime(iCnt));
1434  }
1435 
1436  return WorkMat;
1437 }
1438 
1439 
1440 /* Contributo al residuo durante l'assemblaggio iniziale */
1441 SubVectorHandler&
1442 UniversalPinJoint::InitialAssRes(SubVectorHandler& WorkVec,
1443  const VectorHandler& XCurr)
1444 {
1445  DEBUGCOUT("Entering UniversalPinJoint::InitialAssRes()" << std::endl);
1446 
1447  /* Dimensiona e resetta la matrice di lavoro */
1448  integer iNumRows = 0;
1449  integer iNumCols = 0;
1450  InitialWorkSpaceDim(&iNumRows, &iNumCols);
1451  WorkVec.ResizeReset(iNumRows);
1452 
1453  /* Indici */
1454  integer iFirstPositionIndex = pNode->iGetFirstPositionIndex();
1455  integer iFirstVelocityIndex = iFirstPositionIndex + 6;
1456  integer iFirstReactionIndex = iGetFirstIndex();
1457  integer iReactionPrimeIndex = iFirstReactionIndex + 4;
1458 
1459  /* Setta gli indici */
1460  for (int iCnt = 1; iCnt <= 6; iCnt++) {
1461  WorkVec.PutRowIndex(iCnt, iFirstPositionIndex + iCnt);
1462  WorkVec.PutRowIndex(6 + iCnt, iFirstVelocityIndex + iCnt);
1463  }
1464 
1465  for (int iCnt = 1; iCnt <= 8; iCnt++) {
1466  WorkVec.PutRowIndex(12 + iCnt, iFirstReactionIndex + iCnt);
1467  }
1468 
1469  /* Recupera i dati */
1470  const Vec3& x(pNode->GetXCurr());
1471  const Vec3& v(pNode->GetVCurr());
1472  const Mat3x3& R(pNode->GetRCurr());
1473  const Vec3& Omega(pNode->GetWCurr());
1474 
1475  F = Vec3(XCurr, iFirstReactionIndex + 1);
1476  dM = XCurr(iFirstReactionIndex + 4);
1477  Vec3 FPrime(XCurr, iReactionPrimeIndex + 1);
1478  doublereal dMPrime(XCurr(iReactionPrimeIndex + 4));
1479 
1480  /* Versori delle cerniere */
1481  Vec3 e3(R0.GetVec(3));
1482  Vec3 e2(R*Rh.GetVec(2));
1483 
1484  /* Vettori temporanei */
1485  Vec3 Tmp(e2.Cross(e3));
1486 
1487  Vec3 TmpPrime(e3.Cross(e2.Cross(Omega)));
1488 
1489  /* Distanza nel sistema globale */
1490  Vec3 dTmp(R*d);
1491 
1492  /* Vettori omega/\d */
1493  Vec3 OWedged(Omega.Cross(dTmp));
1494 
1495  /* Ruota il momento e la sua derivata con le matrici della cerniera
1496  * rispetto ai nodi */
1497  Vec3 MTmp(e2*dM);
1498  Vec3 MPrimeTmp(e3.Cross(MTmp.Cross(Omega)) + e2.Cross(e3)*dMPrime);
1499 
1500  /* Equazioni di equilibrio */
1501  WorkVec.Sub(1, F);
1502  WorkVec.Sub(4, dTmp.Cross(F) - MTmp.Cross(e3));
1503 
1504  /* Derivate delle equazioni di equilibrio, nodo 1 */
1505  WorkVec.Sub(7, FPrime);
1506  WorkVec.Sub(10, dTmp.Cross(FPrime) + OWedged.Cross(F) + MPrimeTmp);
1507 
1508  /* Equazione di vincolo di posizione */
1509  WorkVec.Add(13, x + dTmp - X0);
1510 
1511  /* Equazioni di vincolo di rotazione */
1512  WorkVec.PutCoef(16, e2*e3);
1513 
1514  /* Derivata dell'equazione di vincolo di posizione */
1515  WorkVec.Add(17, v + OWedged);
1516 
1517  /* Derivate delle equazioni di vincolo di rotazione: e2b~e3a */
1518  Tmp = e3.Cross(Omega);
1519  WorkVec.PutCoef(20, e2*Tmp);
1520 
1521  return WorkVec;
1522 }
1523 
1524 /* UniversalPinJoint - end */
UniversalHingeJoint::~UniversalHingeJoint
~UniversalHingeJoint(void)
Definition: univj.cc:63
UniversalPinJoint::AssJac
VariableSubMatrixHandler & AssJac(VariableSubMatrixHandler &WorkMat, doublereal dCoef, const VectorHandler &XCurr, const VectorHandler &XPrimeCurr)
Definition: univj.cc:1159
FullSubMatrixHandler::PutColIndex
void PutColIndex(integer iSubCol, integer iCol)
Definition: submat.h:325
Zero3
const Vec3 Zero3(0., 0., 0.)
Vec3::Cross
Vec3 Cross(const Vec3 &v) const
Definition: matvec3.h:218
Vec3::Write
std::ostream & Write(std::ostream &out, const char *sFill=" ") const
Definition: matvec3.cc:738
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
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
MatCross
const MatCross_Manip MatCross
Definition: matvec3.cc:639
OutputHandler::UseNetCDF
bool UseNetCDF(int out) const
Definition: output.cc:491
UniversalRotationJoint::WorkSpaceDim
void WorkSpaceDim(integer *piNumRows, integer *piNumCols) const
Definition: univj.h:183
VariableSubMatrixHandler::SetFull
FullSubMatrixHandler & SetFull(void)
Definition: submat.h:1168
StructNode::GetRCurr
virtual const Mat3x3 & GetRCurr(void) const
Definition: strnode.h:1012
FullSubMatrixHandler::Add
void Add(integer iRow, integer iCol, const Vec3 &v)
Definition: submat.cc:209
UniversalRotationJoint::Output
void Output(OutputHandler &OH) const
Definition: univj.cc:798
Write
std::ostream & Write(std::ostream &out, const FullMatrixHandler &m, const char *s, const char *s2)
Definition: fullmh.cc:376
VectorHandler::Sub
virtual void Sub(integer iRow, const Vec3 &v)
Definition: vh.cc:78
OrientationDescription
OrientationDescription
Definition: matvec3.h:1597
FullSubMatrixHandler::PutCoef
void PutCoef(integer iRow, integer iCol, const doublereal &dCoef)
Definition: submat.h:672
UniversalHingeJoint::pNode2
const StructNode * pNode2
Definition: univj.h:49
StructNode::GetWRef
virtual const Vec3 & GetWRef(void) const
Definition: strnode.h:1024
NO_OP
#define NO_OP
Definition: myassert.h:74
UniversalPinJoint::Output
virtual void Output(OutputHandler &OH) const
Definition: univj.cc:1286
FullSubMatrixHandler::IncCoef
void IncCoef(integer iRow, integer iCol, const doublereal &dCoef)
Definition: submat.h:683
Mat3x3::GetVec
Vec3 GetVec(unsigned short int i) const
Definition: matvec3.h:893
UniversalHingeJoint::pNode1
const StructNode * pNode1
Definition: univj.h:48
UniversalRotationJoint::AssJac
VariableSubMatrixHandler & AssJac(VariableSubMatrixHandler &WorkMat, doublereal dCoef, const VectorHandler &XCurr, const VectorHandler &XPrimeCurr)
Definition: univj.cc:649
RotManip::VecRot
Vec3 VecRot(const Mat3x3 &Phi)
Definition: Rot.cc:136
FullSubMatrixHandler::DecCoef
void DecCoef(integer iRow, integer iCol, const doublereal &dCoef)
Definition: submat.h:694
UniversalRotationJoint::~UniversalRotationJoint
~UniversalRotationJoint(void)
Definition: univj.cc:624
UniversalRotationJoint::Restart
virtual std::ostream & Restart(std::ostream &out) const
Definition: univj.cc:632
SubVectorHandler::PutRowIndex
virtual void PutRowIndex(integer iSubRow, integer iRow)=0
UniversalPinJoint::~UniversalPinJoint
~UniversalPinJoint(void)
Definition: univj.cc:1131
UniversalHingeJoint::Restart
virtual std::ostream & Restart(std::ostream &out) const
Definition: univj.cc:71
MatR2EulerAngles313
Vec3 MatR2EulerAngles313(const Mat3x3 &R)
Definition: matvec3.cc:927
UniversalHingeJoint::InitialWorkSpaceDim
virtual void InitialWorkSpaceDim(integer *piNumRows, integer *piNumCols) const
Definition: univj.h:106
UniversalHingeJoint::InitialAssRes
SubVectorHandler & InitialAssRes(SubVectorHandler &WorkVec, const VectorHandler &XCurr)
Definition: univj.cc:502
UniversalHingeJoint::dM
doublereal dM
Definition: univj.h:55
UniversalRotationJoint::UniversalRotationJoint
UniversalRotationJoint(unsigned int uL, const DofOwner *pDO, const StructNode *pN1, const StructNode *pN2, const Mat3x3 &R1hTmp, const Mat3x3 &R2hTmp, const OrientationDescription &od, flag fOut)
Definition: univj.cc:603
UniversalHingeJoint::AssRes
SubVectorHandler & AssRes(SubVectorHandler &WorkVec, doublereal dCoef, const VectorHandler &XCurr, const VectorHandler &XPrimeCurr)
Definition: univj.cc:205
UniversalPinJoint::InitialAssJac
VariableSubMatrixHandler & InitialAssJac(VariableSubMatrixHandler &WorkMat, const VectorHandler &XCurr)
Definition: univj.cc:1302
UniversalPinJoint::Restart
virtual std::ostream & Restart(std::ostream &out) const
Definition: univj.cc:1139
OutputHandler::GetCurrentStep
long GetCurrentStep(void) const
Definition: output.h:116
UniversalHingeJoint::AssJac
VariableSubMatrixHandler & AssJac(VariableSubMatrixHandler &WorkMat, doublereal dCoef, const VectorHandler &XCurr, const VectorHandler &XPrimeCurr)
Definition: univj.cc:92
DEBUGCOUT
#define DEBUGCOUT(msg)
Definition: myassert.h:232
UniversalRotationJoint::InitialAssJac
VariableSubMatrixHandler & InitialAssJac(VariableSubMatrixHandler &WorkMat, const VectorHandler &XCurr)
Definition: univj.cc:885
MatR2EulerAngles123
Vec3 MatR2EulerAngles123(const Mat3x3 &R)
Definition: matvec3.cc:893
StructDispNode::iGetFirstMomentumIndex
virtual integer iGetFirstMomentumIndex(void) const =0
StructDispNode::iGetFirstPositionIndex
virtual integer iGetFirstPositionIndex(void) const
Definition: strnode.h:452
UniversalPinJoint::InitialWorkSpaceDim
virtual void InitialWorkSpaceDim(integer *piNumRows, integer *piNumCols) const
Definition: univj.h:304
StructNode::GetWCurr
virtual const Vec3 & GetWCurr(void) const
Definition: strnode.h:1030
UniversalRotationJoint::InitialAssRes
SubVectorHandler & InitialAssRes(SubVectorHandler &WorkVec, const VectorHandler &XCurr)
Definition: univj.cc:1038
UniversalHingeJoint::Output
void Output(OutputHandler &OH) const
Definition: univj.cc:275
dRaDegr
const doublereal dRaDegr
Definition: matvec3.cc:884
Joint::Restart
virtual std::ostream & Restart(std::ostream &out) const
Definition: joint.h:195
UniversalHingeJoint::UniversalHingeJoint
UniversalHingeJoint(unsigned int uL, const DofOwner *pDO, const StructNode *pN1, const StructNode *pN2, const Vec3 &dTmp1, const Vec3 &dTmp2, const Mat3x3 &R1hTmp, const Mat3x3 &R2hTmp, flag fOut)
Definition: univj.cc:44
OutputHandler::Joints
std::ostream & Joints(void) const
Definition: output.h:443
ASSERT
#define ASSERT(expression)
Definition: colamd.c:977
UniversalRotationJoint::pNode2
const StructNode * pNode2
Definition: univj.h:144
UniversalPinJoint::WorkSpaceDim
virtual void WorkSpaceDim(integer *piNumRows, integer *piNumCols) const
Definition: univj.h:280
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
UniversalPinJoint::InitialAssRes
SubVectorHandler & InitialAssRes(SubVectorHandler &WorkVec, const VectorHandler &XCurr)
Definition: univj.cc:1442
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
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
FullSubMatrixHandler::ResizeReset
virtual void ResizeReset(integer, integer)
Definition: submat.cc:182
UniversalHingeJoint::WorkSpaceDim
void WorkSpaceDim(integer *piNumRows, integer *piNumCols) const
Definition: univj.h:84
Mat3x3::Transpose
Mat3x3 Transpose(void) const
Definition: matvec3.h:816
UniversalRotationJoint::AssRes
SubVectorHandler & AssRes(SubVectorHandler &WorkVec, doublereal dCoef, const VectorHandler &XCurr, const VectorHandler &XPrimeCurr)
Definition: univj.cc:725
MatCrossCross
const MatCrossCross_Manip MatCrossCross
Definition: matvec3.cc:640
Elem
Definition: elem.h:75
FullSubMatrixHandler::PutRowIndex
void PutRowIndex(integer iSubRow, integer iRow)
Definition: submat.h:311
UniversalRotationJoint::OutputPrepare
void OutputPrepare(OutputHandler &OH)
Definition: univj.cc:780
UniversalPinJoint::pNode
const StructNode * pNode
Definition: univj.h:244
Vec3::pGetVec
const doublereal * pGetVec(void) const
Definition: matvec3.h:192
MatR2EulerAngles321
Vec3 MatR2EulerAngles321(const Mat3x3 &R)
Definition: matvec3.cc:941
UniversalRotationJoint::pNode1
const StructNode * pNode1
Definition: univj.h:143
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
UniversalRotationJoint::od
OrientationDescription od
Definition: univj.h:153
Joint
Definition: joint.h:50
UniversalPinJoint::dM
doublereal dM
Definition: univj.h:250
DofOwnerOwner::iGetFirstIndex
virtual integer iGetFirstIndex(void) const
Definition: dofown.h:127
doublereal
double doublereal
Definition: colamd.c:52
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
UniversalRotationJoint::dM
doublereal dM
Definition: univj.h:150
integer
long int integer
Definition: colamd.c:51
WithLabel::GetLabel
unsigned int GetLabel(void) const
Definition: withlab.cc:62
UniversalPinJoint::UniversalPinJoint
UniversalPinJoint(unsigned int uL, const DofOwner *pDO, const StructNode *pN, const Vec3 &X0Tmp, const Mat3x3 &R0Tmp, const Vec3 &dTmp, const Mat3x3 &RhTmp, flag fOut)
Definition: univj.cc:1116
UniversalPinJoint::AssRes
SubVectorHandler & AssRes(SubVectorHandler &WorkVec, doublereal dCoef, const VectorHandler &XCurr, const VectorHandler &XPrimeCurr)
Definition: univj.cc:1235
UniversalRotationJoint::InitialWorkSpaceDim
virtual void InitialWorkSpaceDim(integer *piNumRows, integer *piNumCols) const
Definition: univj.h:208
R
Mat3x3 R
Definition: mbdyn_util_oct.cc:74
OutputHandler::UseText
bool UseText(int out) const
Definition: output.cc:446
UniversalHingeJoint::InitialAssJac
VariableSubMatrixHandler & InitialAssJac(VariableSubMatrixHandler &WorkMat, const VectorHandler &XCurr)
Definition: univj.cc:293