MBDyn-1.7.3
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Groups
prismj.cc
Go to the documentation of this file.
1 /* $Header: /var/cvs/mbdyn/mbdyn/mbdyn-1.0/mbdyn/struct/prismj.cc,v 1.48 2017/01/12 14:46:43 masarati Exp $ */
2 /*
3  * MBDyn (C) is a multibody analysis code.
4  * http://www.mbdyn.org
5  *
6  * Copyright (C) 1996-2017
7  *
8  * Pierangelo Masarati <masarati@aero.polimi.it>
9  * Paolo Mantegazza <mantegazza@aero.polimi.it>
10  *
11  * Dipartimento di Ingegneria Aerospaziale - Politecnico di Milano
12  * via La Masa, 34 - 20156 Milano, Italy
13  * http://www.aero.polimi.it
14  *
15  * Changing this copyright notice is forbidden.
16  *
17  * This program is free software; you can redistribute it and/or modify
18  * it under the terms of the GNU General Public License as published by
19  * the Free Software Foundation (version 2 of the License).
20  *
21  *
22  * This program is distributed in the hope that it will be useful,
23  * but WITHOUT ANY WARRANTY; without even the implied warranty of
24  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
25  * GNU General Public License for more details.
26  *
27  * You should have received a copy of the GNU General Public License
28  * along with this program; if not, write to the Free Software
29  * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
30  */
31 
32 /* Vincolo prismatico */
33 
34 #include "mbconfig.h" /* This goes first in every *.c,*.cc file */
35 
36 #include <limits>
37 
38 #include "prismj.h"
39 
40 /* PrismaticJoint - begin */
41 
42 /* Costruttore non banale */
43 PrismaticJoint::PrismaticJoint(unsigned int uL, const DofOwner* pDO,
44  const StructNode* pN1, const StructNode* pN2,
45  const Mat3x3& R1hTmp, const Mat3x3& R2hTmp,
46  flag fOut)
47 : Elem(uL, fOut),
48 Joint(uL, pDO, fOut),
49 pNode1(pN1), pNode2(pN2),
50 R1h(R1hTmp), R2h(R2hTmp), M(Zero3)
51 {
52  ASSERT(pNode1 != NULL);
54  ASSERT(pNode2 != NULL);
56 }
57 
58 
59 /* Distruttore banale */
61 {
62  NO_OP;
63 };
64 
65 
67 PrismaticJoint::GetEqType(unsigned int i) const
68 {
70  "INDEX ERROR in PrismaticJoint::GetEqType");
71  return DofOrder::ALGEBRAIC;
72 }
73 
74 
75 /* Contributo al file di restart */
76 std::ostream& PrismaticJoint::Restart(std::ostream& out) const
77 {
78  Joint::Restart(out) << ", prismatic, "
79  << pNode1->GetLabel()
80  << ", hinge, reference, node, 1, ", (R1h.GetVec(1)).Write(out, ", ")
81  << ", 2, ", (R1h.GetVec(2)).Write(out, ", ") << ", "
82  << pNode2->GetLabel()
83  << ", hinge, reference, node, 1, ", (R2h.GetVec(1)).Write(out, ", ")
84  << ", 2, ", (R2h.GetVec(2)).Write(out, ", ") << ';' << std::endl;
85 
86  return out;
87 }
88 
89 
90 /* Assemblaggio jacobiano */
93  doublereal dCoef,
94  const VectorHandler& /* XCurr */ ,
95  const VectorHandler& /* XPrimeCurr */ )
96 {
97  DEBUGCOUT("Entering PrismaticJoint::AssJac()" << std::endl);
98 
99  /* Setta la sottomatrice come piena (e' un po' dispersivo, ma lo jacobiano
100  * e' complicato */
101  FullSubMatrixHandler& WM = WorkMat.SetFull();
102 
103  /* Ridimensiona la sottomatrice in base alle esigenze */
104  integer iNumRows = 0;
105  integer iNumCols = 0;
106  this->WorkSpaceDim(&iNumRows, &iNumCols);
107  WM.ResizeReset(iNumRows, iNumCols);
108 
109  /* Recupera gli indici delle varie incognite */
110  integer iNode1FirstPosIndex = pNode1->iGetFirstPositionIndex()+3;
111  integer iNode1FirstMomIndex = pNode1->iGetFirstMomentumIndex()+3;
112  integer iNode2FirstPosIndex = pNode2->iGetFirstPositionIndex()+3;
113  integer iNode2FirstMomIndex = pNode2->iGetFirstMomentumIndex()+3;
114  integer iFirstReactionIndex = iGetFirstIndex();
115 
116  /* Recupera i dati che servono */
117  Mat3x3 R1hTmp(pNode1->GetRRef()*R1h);
118  Mat3x3 R2hTmp(pNode2->GetRRef()*R2h);
119  /* Suppongo che le reazioni M siano gia' state aggiornate da AssRes;
120  * ricordo che la coppia M e' nel sistema locale */
121 
122 
123  /*
124  * Il vincolo prismatico ha tre equazioni che affermano la coincidenza del
125  * riferimento solidale con il vincolo visto dai due nodi.
126  *
127  * (R1 * R1h * e2)^T * (R2 * R2h * e3) = 0
128  * (R1 * R1h * e3)^T * (R2 * R2h * e1) = 0
129  * (R1 * R1h * e1)^T * (R2 * R2h * e2) = 0
130  *
131  * A queste equazioni corrisponde una reazione di coppia agente attorno
132  * agli assi dati dall'errore di allineamento:
133  *
134  * [ (R1 * R1h * e2) /\ (R2 * R2h *e3), ]
135  * [ (R1 * R1h * e3) /\ (R2 * R2h *e1), ] M
136  * [ (R1 * R1h * e1) /\ (R2 * R2h *e2) ]
137  *
138  */
139 
140  /* Setta gli indici delle equazioni */
141  for (int iCnt = 1; iCnt <= 3; iCnt++) {
142  WM.PutRowIndex(0+iCnt, iNode1FirstMomIndex+iCnt);
143  WM.PutColIndex(0+iCnt, iNode1FirstPosIndex+iCnt);
144  WM.PutRowIndex(3+iCnt, iNode2FirstMomIndex+iCnt);
145  WM.PutColIndex(3+iCnt, iNode2FirstPosIndex+iCnt);
146 
147  WM.PutRowIndex(6+iCnt, iFirstReactionIndex+iCnt);
148  WM.PutColIndex(6+iCnt, iFirstReactionIndex+iCnt);
149  }
150 
151  Vec3 MTmp = M*dCoef; /* M e' stato aggiornato da AssRes */
152 
153  Vec3 e1a(R1hTmp.GetVec(1));
154  Vec3 e2a(R1hTmp.GetVec(2));
155  Vec3 e3a(R1hTmp.GetVec(3));
156  Vec3 e1b(R2hTmp.GetVec(1));
157  Vec3 e2b(R2hTmp.GetVec(2));
158  Vec3 e3b(R2hTmp.GetVec(3));
159 
160  Mat3x3 MWedge(Mat3x3(MatCrossCross, e3b, e2a*MTmp(1))
161  +Mat3x3(MatCrossCross, e1b, e3a*MTmp(2))
162  +Mat3x3(MatCrossCross, e2b, e1a*MTmp(3)));
163  Mat3x3 MWedgeT(MWedge.Transpose());
164 
165  WM.Add(1, 1, MWedge);
166  WM.Sub(1, 4, MWedgeT);
167 
168  WM.Add(4, 1, MWedgeT);
169  WM.Sub(4, 4, MWedge);
170 
171  Vec3 v1(e2a.Cross(e3b));
172  Vec3 v2(e3a.Cross(e1b));
173  Vec3 v3(e1a.Cross(e2b));
174 
175  MWedge = Mat3x3(v1, v2, v3);
176 
177  WM.Add(1, 7, MWedge);
178  WM.Sub(4, 7, MWedge);
179 
180  /* Modifica: divido le equazioni di vincolo per dCoef */
181 
182  /* Equazione di vincolo del momento
183  *
184  * Attenzione: bisogna scrivere il vettore trasposto
185  * (Sb[1]^T*(Sa[3]/\))*dCoef
186  * Questo pero' e' uguale a:
187  * (-Sa[3]/\*Sb[1])^T*dCoef,
188  * che puo' essere ulteriormente semplificato:
189  * (Sa[3].Cross(Sb[1])*(-dCoef))^T;
190  */
191 
192  MWedge = MWedge.Transpose();
193 
194  WM.Add(7, 1, MWedge);
195  WM.Sub(7, 4, MWedge);
196 
197  return WorkMat;
198 }
199 
200 
201 /* Assemblaggio residuo */
203  doublereal dCoef,
204  const VectorHandler& XCurr,
205  const VectorHandler& /* XPrimeCurr */ )
206 {
207  DEBUGCOUT("Entering PrismaticJoint::AssRes()" << std::endl);
208 
209  /* Dimensiona e resetta la matrice di lavoro */
210  integer iNumRows = 0;
211  integer iNumCols = 0;
212  this->WorkSpaceDim(&iNumRows, &iNumCols);
213  WorkVec.ResizeReset(iNumRows);
214 
215  /* Indici */
216  integer iNode1FirstMomIndex = pNode1->iGetFirstMomentumIndex()+3;
217  integer iNode2FirstMomIndex = pNode2->iGetFirstMomentumIndex()+3;
218  integer iFirstReactionIndex = iGetFirstIndex();
219 
220  /* Indici dei nodi */
221  for (int iCnt = 1; iCnt <= 3; iCnt++) {
222  WorkVec.PutRowIndex(0+iCnt, iNode1FirstMomIndex+iCnt);
223  WorkVec.PutRowIndex(3+iCnt, iNode2FirstMomIndex+iCnt);
224 
225  WorkVec.PutRowIndex(6+iCnt, iFirstReactionIndex+iCnt);
226  }
227 
228  /* Aggiorna i dati propri */
229  M = Vec3(XCurr, iFirstReactionIndex+1);
230 
231  /* Costruisce i dati propri nella configurazione corrente */
232  Mat3x3 R1hTmp(pNode1->GetRCurr()*R1h);
233  Mat3x3 R2hTmp(pNode2->GetRCurr()*R2h);
234 
235  Vec3 e1a(R1hTmp.GetVec(1));
236  Vec3 e2a(R1hTmp.GetVec(2));
237  Vec3 e3a(R1hTmp.GetVec(3));
238  Vec3 e1b(R2hTmp.GetVec(1));
239  Vec3 e2b(R2hTmp.GetVec(2));
240  Vec3 e3b(R2hTmp.GetVec(3));
241 
242  Vec3 MTmp(Mat3x3(e2a.Cross(e3b), e3a.Cross(e1b), e1a.Cross(e2b))*M);
243 
244  /* Equazioni di equilibrio, nodo 1 */
245  WorkVec.Sub(1, MTmp);
246 
247  /* Equazioni di equilibrio, nodo 2 */
248  WorkVec.Add(4, MTmp);
249 
250  /* Modifica: divido le equazioni di vincolo per dCoef */
251  ASSERT(dCoef != 0.);
252 
253  /* Equazioni di vincolo di rotazione */
254  WorkVec.PutCoef(7, -(e3b.Dot(e2a)/dCoef));
255  WorkVec.PutCoef(8, -(e1b.Dot(e3a)/dCoef));
256  WorkVec.PutCoef(9, -(e2b.Dot(e1a)/dCoef));
257 
258  return WorkVec;
259 }
260 
261 void
263 {
264  if (bToBeOutput()) {
265 #ifdef USE_NETCDF
267  std::string name;
268  OutputPrepare_int("prismatic", OH, name);
269  }
270 #endif // USE_NETCDF
271  }
272 }
273 
274 /* Output (da mettere a punto) */
276 {
277  if (bToBeOutput()) {
278  Mat3x3 R1Tmp(pNode1->GetRCurr()*R1h);
279 
280 #ifdef USE_NETCDF
282  Var_F_local->put_rec(Zero3.pGetVec(), OH.GetCurrentStep());
283  Var_M_local->put_rec(M.pGetVec(), OH.GetCurrentStep());
284  Var_F_global->put_rec(Zero3.pGetVec(), OH.GetCurrentStep());
285  Var_M_global->put_rec((R1Tmp*M).pGetVec(), OH.GetCurrentStep());
286  }
287 #endif // USE_NETCDF
288 
289 
290  if (OH.UseText(OutputHandler::JOINTS)) {
291  Joint::Output(OH.Joints(), "PlaneHinge", GetLabel(),
292  Zero3, M, Zero3, R1Tmp*M) << std::endl;
293  }
294  }
295 }
296 
297 void
301 {
302  if (ph) {
303  for (unsigned i = 0; i < ph->size(); i++) {
304  Joint::JointHint *pjh = dynamic_cast<Joint::JointHint *>((*ph)[i]);
305 
306  if (pjh == 0) {
307  continue;
308  }
309 
310  if (dynamic_cast<Joint::HingeHint<1> *>(pjh)) {
312 
313  } else if (dynamic_cast<Joint::HingeHint<2> *>(pjh)) {
315 
316  } else if (dynamic_cast<Joint::ReactionsHint *>(pjh)) {
317  /* TODO */
318  }
319  }
320  }
321 }
322 
323 Hint *
324 PrismaticJoint::ParseHint(DataManager *pDM, const char *s) const
325 {
326  if (strncasecmp(s, "hinge{" /*}*/, STRLENOF("hinge{" /*}*/)) == 0) {
327  s += STRLENOF("hinge{" /*}*/);
328 
329  if (strcmp(&s[1], /*{*/ "}") != 0) {
330  return 0;
331  }
332 
333  switch (s[0]) {
334  case '1':
335  return new Joint::HingeHint<1>;
336 
337  case '2':
338  return new Joint::HingeHint<2>;
339  }
340  }
341 
342  return 0;
343 }
344 
345 /* Contributo allo jacobiano durante l'assemblaggio iniziale */
348  const VectorHandler& XCurr)
349 {
350  DEBUGCOUT("Entering PrismaticJoint::InitialAssJac()" << std::endl);
351 
352  /* Per ora usa la matrice piena; eventualmente si puo'
353  * passare a quella sparsa quando si ottimizza */
354  FullSubMatrixHandler& WM = WorkMat.SetFull();
355 
356  /* Dimensiona e resetta la matrice di lavoro */
357  integer iNumRows = 0;
358  integer iNumCols = 0;
359  this->InitialWorkSpaceDim(&iNumRows, &iNumCols);
360  WM.ResizeReset(iNumRows, iNumCols);
361 
362 
363  /* Indici */
364  integer iNode1FirstPosIndex = pNode1->iGetFirstPositionIndex()+3;
365  integer iNode1FirstVelIndex = iNode1FirstPosIndex+6;
366  integer iNode2FirstPosIndex = pNode2->iGetFirstPositionIndex()+3;
367  integer iNode2FirstVelIndex = iNode2FirstPosIndex+6;
368  integer iFirstReactionIndex = iGetFirstIndex();
369  integer iReactionPrimeIndex = iFirstReactionIndex+3;
370 
371  /* Nota: le reazioni vincolari sono:
372  * Momento, 3 incognite, riferimento locale
373  */
374 
375  /* Setta gli indici dei nodi */
376  for (int iCnt = 1; iCnt <= 3; iCnt++) {
377  WM.PutRowIndex(0+iCnt, iNode1FirstPosIndex+iCnt);
378  WM.PutColIndex(0+iCnt, iNode1FirstPosIndex+iCnt);
379  WM.PutRowIndex(3+iCnt, iNode1FirstVelIndex+iCnt);
380  WM.PutColIndex(3+iCnt, iNode1FirstVelIndex+iCnt);
381  WM.PutRowIndex(6+iCnt, iNode2FirstPosIndex+iCnt);
382  WM.PutColIndex(6+iCnt, iNode2FirstPosIndex+iCnt);
383  WM.PutRowIndex(9+iCnt, iNode2FirstVelIndex+iCnt);
384  WM.PutColIndex(9+iCnt, iNode2FirstVelIndex+iCnt);
385  }
386 
387  /* Setta gli indici delle reazioni */
388  for (int iCnt = 1; iCnt <= 6; iCnt++) {
389  WM.PutRowIndex(12+iCnt, iFirstReactionIndex+iCnt);
390  WM.PutColIndex(12+iCnt, iFirstReactionIndex+iCnt);
391  }
392 
393  /* Recupera i dati */
394  Mat3x3 R1(pNode1->GetRRef());
395  Mat3x3 R2(pNode2->GetRRef());
396  Vec3 Omega1(pNode1->GetWRef());
397  Vec3 Omega2(pNode2->GetWRef());
398  /* M e' gia' stato aggiornato da InitialAssRes */
399  Vec3 MPrime(XCurr, iReactionPrimeIndex+1);
400 
401  /* Distanze e matrici di rotazione dai nodi alla cerniera
402  * nel sistema globale */
403  Mat3x3 R1hTmp(R1*R1h);
404  Mat3x3 R2hTmp(R2*R2h);
405 
406  Vec3 e1a(R1hTmp.GetVec(1));
407  Vec3 e2a(R1hTmp.GetVec(2));
408  Vec3 e3a(R1hTmp.GetVec(3));
409  Vec3 e1b(R2hTmp.GetVec(1));
410  Vec3 e2b(R2hTmp.GetVec(2));
411  Vec3 e3b(R2hTmp.GetVec(3));
412 
413  /* */
414  Mat3x3 MWedge(Mat3x3(MatCrossCross, e3b, e2a*M(1))
415  + Mat3x3(MatCrossCross, e1b, e3a*M(2))
416  + Mat3x3(MatCrossCross, e2b, e1a*M(3)));
417  Mat3x3 MWedgeT(MWedge.Transpose());
418 
419  /* Equilibrio */
420  WM.Add(1, 1, MWedge);
421  WM.Sub(1, 7, MWedgeT);
422 
423  WM.Add(7, 1, MWedgeT);
424  WM.Sub(7, 7, MWedge);
425 
426  /* Derivate dell'equilibrio */
427  WM.Add(4, 4, MWedge);
428  WM.Sub(4, 10, MWedgeT);
429 
430  WM.Add(10, 4, MWedgeT);
431  WM.Sub(10, 10, MWedge);
432 
433 
434  MWedge =
435  ( (Mat3x3(MatCrossCross, e3b, Omega1) + Mat3x3(MatCross, Omega2.Cross(e3b*M(1))))
436  + Mat3x3(MatCross, e3b*MPrime(1)) )*Mat3x3(MatCross, e2a)
437  +( (Mat3x3(MatCrossCross, e1b, Omega1) + Mat3x3(MatCross, Omega2.Cross(e1b*M(2))))
438  +Mat3x3(MatCross, e1b*MPrime(2)) )*Mat3x3(MatCross, e3a)
439  +( (Mat3x3(MatCrossCross, e2b, Omega1) + Mat3x3(MatCross, Omega2.Cross(e2b*M(3))))
440  +Mat3x3(MatCross, e2b*MPrime(3)) )*Mat3x3(MatCross, e1a);
441 
442  WM.Add(4, 1, MWedge);
443  WM.Sub(10, 1, MWedge);
444 
445  MWedge =
446  ( (Mat3x3(MatCrossCross, e2a, Omega2) + Mat3x3(MatCross, Omega1.Cross(e2a*M(1))))
447  + Mat3x3(MatCross, e2a*MPrime(1)) )*Mat3x3(MatCross, e3b)
448  +( (Mat3x3(MatCrossCross, e3a, Omega2) + Mat3x3(MatCross, Omega1.Cross(e3a*M(2))))
449  + Mat3x3(MatCross, e3a*MPrime(2)) )*Mat3x3(MatCross, e1b)
450  +( (Mat3x3(MatCrossCross, e1a, Omega2) + Mat3x3(MatCross, Omega1.Cross(e1a*M(3))))
451  + Mat3x3(MatCross, e1a*MPrime(3)) )*Mat3x3(MatCross, e2b);
452 
453  WM.Sub(4, 7, MWedge);
454  WM.Add(10, 7, MWedge);
455 
456  Vec3 v1(e2a.Cross(e3b));
457  Vec3 v2(e3a.Cross(e1b));
458  Vec3 v3(e1a.Cross(e2b));
459 
460  /* Error handling: il programma si ferma, pero' segnala dov'e' l'errore */
461  if (v1.Dot() < std::numeric_limits<doublereal>::epsilon()
462  || v2.Dot() < std::numeric_limits<doublereal>::epsilon()
463  || v3.Dot() < std::numeric_limits<doublereal>::epsilon())
464  {
465  silent_cerr("PrismaticJoint(" << GetLabel() << "): "
466  "first and second node hinge axes are (nearly) orthogonal"
467  << std::endl);
469  }
470 
471  MWedge = Mat3x3(v1, v2, v3);
472 
473  /* Equilibrio */
474  WM.Add(1, 13, MWedge);
475  WM.Sub(7, 13, MWedge);
476 
477  /* Derivate dell'equilibrio */
478  WM.Add(4, 16, MWedge);
479  WM.Sub(10, 16, MWedge);
480 
481 
482  MWedge = MWedge.Transpose();
483 
484  /* Equaz. di vincolo */
485  WM.Add(13, 1, MWedge);
486  WM.Sub(13, 7, MWedge);
487 
488  /* Devivate delle equaz. di vincolo */
489  WM.Add(16, 4, MWedge);
490  WM.Sub(16, 10, MWedge);
491 
492  v1 = e3b.Cross(e2a.Cross(Omega1)) + e2a.Cross(Omega2.Cross(e3b));
493  v2 = e1b.Cross(e3a.Cross(Omega1)) + e3a.Cross(Omega2.Cross(e1b));
494  v3 = e2b.Cross(e1a.Cross(Omega1)) + e1a.Cross(Omega2.Cross(e2b));
495 
496  MWedge = Mat3x3(v1, v2, v3);
497 
498  /* Derivate dell'equilibrio */
499  WM.Add(4, 13, MWedge);
500  WM.Sub(10, 13, MWedge);
501 
502  /* Devivate delle equaz. di vincolo */
503  Omega1 = Omega1 - Omega2;
504 
505  v1 = e2a.Cross(e3b.Cross(Omega1));
506  Vec3 v1p(e3b.Cross(Omega1.Cross(e2a)));
507  v2 = e3a.Cross(e1b.Cross(Omega1));
508  Vec3 v2p(e1b.Cross(Omega1.Cross(e3a)));
509  v3 = e1a.Cross(e2b.Cross(Omega1));
510  Vec3 v3p(e2b.Cross(Omega1.Cross(e1a)));
511 
512  for (int iCnt = 1; iCnt <= 3; iCnt++) {
513  doublereal d = v1(iCnt);
514  WM.PutCoef(16, 0+iCnt, d);
515  d = v1p(iCnt);
516  WM.PutCoef(16, 6+iCnt, d);
517 
518  d = v2(iCnt);
519  WM.PutCoef(17, 0+iCnt, d);
520  d = v2p(iCnt);
521  WM.PutCoef(17, 6+iCnt, d);
522 
523  d = v3(iCnt);
524  WM.PutCoef(18, 0+iCnt, d);
525  d = v3p(iCnt);
526  WM.PutCoef(18, 6+iCnt, d);
527  }
528 
529  return WorkMat;
530 }
531 
532 
533 /* Contributo al residuo durante l'assemblaggio iniziale */
536  const VectorHandler& XCurr)
537 {
538  DEBUGCOUT("Entering PrismaticJoint::InitialAssRes()" << std::endl);
539 
540  /* Dimensiona e resetta la matrice di lavoro */
541  integer iNumRows = 0;
542  integer iNumCols = 0;
543  this->InitialWorkSpaceDim(&iNumRows, &iNumCols);
544  WorkVec.ResizeReset(iNumRows);
545 
546  /* Indici */
547  integer iNode1FirstPosIndex = pNode1->iGetFirstPositionIndex()+3;
548  integer iNode1FirstVelIndex = iNode1FirstPosIndex+6;
549  integer iNode2FirstPosIndex = pNode2->iGetFirstPositionIndex()+3;
550  integer iNode2FirstVelIndex = iNode2FirstPosIndex+6;
551  integer iFirstReactionIndex = iGetFirstIndex();
552  integer iReactionPrimeIndex = iFirstReactionIndex+3;
553 
554  /* Setta gli indici */
555  for (int iCnt = 1; iCnt <= 3; iCnt++) {
556  WorkVec.PutRowIndex(0+iCnt, iNode1FirstPosIndex+iCnt);
557  WorkVec.PutRowIndex(3+iCnt, iNode1FirstVelIndex+iCnt);
558  WorkVec.PutRowIndex(6+iCnt, iNode2FirstPosIndex+iCnt);
559  WorkVec.PutRowIndex(9+iCnt, iNode2FirstVelIndex+iCnt);
560  }
561 
562  for (int iCnt = 1; iCnt <= 6; iCnt++) {
563  WorkVec.PutRowIndex(12+iCnt, iFirstReactionIndex+iCnt);
564  }
565 
566  /* Recupera i dati */
567  Mat3x3 R1(pNode1->GetRCurr());
568  Mat3x3 R2(pNode2->GetRCurr());
569  Vec3 Omega1(pNode1->GetWCurr());
570  Vec3 Omega2(pNode2->GetWCurr());
571 
572  /* Aggiorna M, che resta anche per InitialAssJac */
573  M = Vec3(XCurr, iFirstReactionIndex+1);
574  Vec3 MPrime(XCurr, iReactionPrimeIndex+1);
575 
576  /* Vincolo nel sistema globale */
577  Mat3x3 R1hTmp(R1*R1h);
578  Mat3x3 R2hTmp(R2*R2h);
579 
580  Vec3 e1a(R1hTmp.GetVec(1));
581  Vec3 e2a(R1hTmp.GetVec(2));
582  Vec3 e3a(R1hTmp.GetVec(3));
583  Vec3 e1b(R2hTmp.GetVec(1));
584  Vec3 e2b(R2hTmp.GetVec(2));
585  Vec3 e3b(R2hTmp.GetVec(3));
586 
587  Vec3 MTmp(e2a.Cross(e3b*M(1))
588  +e3a.Cross(e1b*M(2))
589  +e1a.Cross(e2b*M(3)));
590 
591  /* Equazioni di equilibrio, nodo 1 */
592  WorkVec.Add(1, -MTmp);
593 
594  /* Equazioni di equilibrio, nodo 2 */
595  WorkVec.Add(7, MTmp);
596 
597  MTmp =
598  (e2a.Cross(Omega2.Cross(e3b)) - e3b.Cross(Omega1.Cross(e2a)))*M(1)
599  +(e3a.Cross(Omega2.Cross(e1b)) - e1b.Cross(Omega1.Cross(e3a)))*M(2)
600  +(e1a.Cross(Omega2.Cross(e2b)) - e2b.Cross(Omega1.Cross(e1a)))*M(3)
601  +e2a.Cross(e3b*MPrime(1))
602  +e3a.Cross(e1b*MPrime(2))
603  +e1a.Cross(e2b*MPrime(3));
604 
605  /* Derivate delle equazioni di equilibrio, nodo 1 */
606  WorkVec.Add(4, -MTmp);
607 
608  /* Derivate delle equazioni di equilibrio, nodo 2 */
609  WorkVec.Add(10, MTmp);
610 
611  /* Equazioni di vincolo di rotazione */
612  WorkVec.PutCoef(13, -(e3b.Dot(e2a)));
613  WorkVec.PutCoef(14, -(e1b.Dot(e3a)));
614  WorkVec.PutCoef(15, -(e2b.Dot(e1a)));
615 
616  /* Derivate delle equazioni di vincolo di rotazione */
617  Omega2 = Omega2-Omega1;
618  WorkVec.PutCoef(16, (e2a.Cross(e3b)).Dot(Omega2));
619  WorkVec.PutCoef(17, (e3a.Cross(e1b)).Dot(Omega2));
620  WorkVec.PutCoef(18, (e1a.Cross(e2b)).Dot(Omega2));
621 
622  return WorkVec;
623 }
624 
625 /* PrismaticJoint - end */
Definition: hint.h:38
VariableSubMatrixHandler & AssJac(VariableSubMatrixHandler &WorkMat, doublereal dCoef, const VectorHandler &XCurr, const VectorHandler &XPrimeCurr)
Definition: prismj.cc:92
void PutColIndex(integer iSubCol, integer iCol)
Definition: submat.h:325
const Vec3 Zero3(0., 0., 0.)
Vec3 Cross(const Vec3 &v) const
Definition: matvec3.h:218
#define ASSERTMSGBREAK(expr, msg)
Definition: myassert.h:222
long int flag
Definition: mbdyn.h:43
virtual const Mat3x3 & GetRRef(void) const
Definition: strnode.h:1006
void WorkSpaceDim(integer *piNumRows, integer *piNumCols) const
Definition: prismj.h:85
virtual bool bToBeOutput(void) const
Definition: output.cc:890
PrismaticJoint(unsigned int uL, const DofOwner *pDO, const StructNode *pN1, const StructNode *pN2, const Mat3x3 &R1hTmp, const Mat3x3 &R2hTmp, flag fOut)
Definition: prismj.cc:43
#define MBDYN_EXCEPT_ARGS
Definition: except.h:63
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
virtual Node::Type GetNodeType(void) const
Definition: strnode.cc:145
void Add(integer iRow, integer iCol, const Vec3 &v)
Definition: submat.cc:209
std::ostream & Write(std::ostream &out, const FullMatrixHandler &m, const char *s, const char *s2)
Definition: fullmh.cc:376
const StructNode * pNode1
Definition: prismj.h:51
virtual void Sub(integer iRow, const Vec3 &v)
Definition: vh.cc:78
SubVectorHandler & InitialAssRes(SubVectorHandler &WorkVec, const VectorHandler &XCurr)
Definition: prismj.cc:535
void PutCoef(integer iRow, integer iCol, const doublereal &dCoef)
Definition: submat.h:672
virtual void InitialWorkSpaceDim(integer *piNumRows, integer *piNumCols) const
Definition: prismj.h:115
Mat3x3 R2h
Definition: prismj.h:54
virtual const Vec3 & GetWRef(void) const
Definition: strnode.h:1024
#define NO_OP
Definition: myassert.h:74
std::vector< Hint * > Hints
Definition: simentity.h:89
void Output(OutputHandler &OH) const
Definition: prismj.cc:275
virtual std::ostream & Restart(std::ostream &out) const
Definition: prismj.cc:76
Vec3 GetVec(unsigned short int i) const
Definition: matvec3.h:893
virtual void PutRowIndex(integer iSubRow, integer iRow)=0
virtual unsigned int iGetNumDof(void) const
Definition: prismj.h:74
SubVectorHandler & AssRes(SubVectorHandler &WorkVec, doublereal dCoef, const VectorHandler &XCurr, const VectorHandler &XPrimeCurr)
Definition: prismj.cc:202
~PrismaticJoint(void)
Definition: prismj.cc:60
long GetCurrentStep(void) const
Definition: output.h:116
#define DEBUGCOUT(msg)
Definition: myassert.h:232
virtual integer iGetFirstMomentumIndex(void) const =0
virtual integer iGetFirstPositionIndex(void) const
Definition: strnode.h:452
virtual const Vec3 & GetWCurr(void) const
Definition: strnode.h:1030
virtual std::ostream & Restart(std::ostream &out) const
Definition: joint.h:195
std::ostream & Joints(void) const
Definition: output.h:443
DotTraits< VectorExprLhs, VectorExprRhs, N_rows, N_rows >::ExpressionType Dot(const VectorExpression< VectorExprLhs, N_rows > &u, const VectorExpression< VectorExprRhs, N_rows > &v)
Definition: matvec.h:3133
#define ASSERT(expression)
Definition: colamd.c:977
virtual void PutCoef(integer iRow, const doublereal &dCoef)=0
virtual Hint * ParseHint(DataManager *pDM, const char *s) const
Definition: prismj.cc:324
virtual void OutputPrepare_int(const std::string &type, OutputHandler &OH, std::string &name)
Definition: joint.cc:107
virtual void Add(integer iRow, const Vec3 &v)
Definition: vh.cc:63
VariableSubMatrixHandler & InitialAssJac(VariableSubMatrixHandler &WorkMat, const VectorHandler &XCurr)
Definition: prismj.cc:347
virtual void ResizeReset(integer, integer)
Definition: submat.cc:182
Mat3x3 Transpose(void) const
Definition: matvec3.h:816
const MatCrossCross_Manip MatCrossCross
Definition: matvec3.cc:640
void SetValue(DataManager *pDM, VectorHandler &X, VectorHandler &XP, SimulationEntity::Hints *ph=0)
Definition: prismj.cc:298
#define STRLENOF(s)
Definition: mbdyn.h:166
Definition: elem.h:75
Mat3x3 R1h
Definition: prismj.h:53
void PutRowIndex(integer iSubRow, integer iRow)
Definition: submat.h:311
const doublereal * pGetVec(void) const
Definition: matvec3.h:192
DofOrder::Order GetEqType(unsigned int i) const
Definition: prismj.cc:67
void Sub(integer iRow, integer iCol, const Vec3 &v)
Definition: submat.cc:215
Definition: joint.h:50
virtual integer iGetFirstIndex(void) const
Definition: dofown.h:127
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
unsigned int GetLabel(void) const
Definition: withlab.cc:62
const StructNode * pNode2
Definition: prismj.h:52
bool UseText(int out) const
Definition: output.cc:446
void OutputPrepare(OutputHandler &OH)
Definition: prismj.cc:262