MBDyn-1.7.3
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Groups
joint.cc
Go to the documentation of this file.
1 /* $Header: /var/cvs/mbdyn/mbdyn/mbdyn-1.0/mbdyn/struct/joint.cc,v 1.173 2017/07/23 18:36:36 zanoni Exp $ */
2 /*
3  * MBDyn (C) is a multibody analysis code.
4  * http://www.mbdyn.org
5  *
6  * Copyright (C) 1996-2014
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 /* joint */
33 
34 #include "mbconfig.h" /* This goes first in every *.c,*.cc file */
35 
36 #include "Rot.hh"
37 #include "strings.h"
38 
39 #include "joint.h"
40 #include "dataman.h"
41 
42 #include "tpldrive.h"
43 
44 #include "nestedelem.h"
45 
46 #include "accj.h" /* Vincoli di accelerazione imposta */
47 #include "beamslider.h"
48 #include "brake.h"
49 #include "distance.h"
50 #include "drvdisp.h"
51 #include "drvhinge.h"
52 #include "drvj.h" /* Vincoli di velocita' imposta */
53 #include "genj.h"
54 #include "gimbal.h"
55 #include "inplanej.h" /* Vincoli di giacitura nel piano */
56 #include "inline.h"
57 #include "modal.h"
58 #if 0 /* No longer supported */
59 #include "planedj.h"
60 #endif
61 #include "planej.h"
62 #include "point_contact.h"
63 #include "prismj.h" /* Vincolo prismatico */
64 #include "rodj.h" /* Aste elastiche */
65 #include "rodbezj.h"
66 #ifdef MBDYN_DEVEL
67 #include "screwjoint.h"
68 #endif // MBDYN_DEVEL
69 #include "spherj.h"
70 #include "totalequation.h"
71 #include "totalj.h"
72 #include "univj.h"
73 #include "vehj.h" /* Giunti deformabili */
74 #include "vehj2.h" /* "" */
75 #include "vehj3.h" /* "" */
76 #include "vehj4.h" /* "" */
77 #include "vb.h" // viscous body
78 
79 #define MBDYN_X_COMPATIBLE_INPUT
80 
81 /* Joint - begin */
82 
83 Joint::Joint(unsigned int uL, const DofOwner* pDO,
84  flag fOut)
85 : Elem(uL, fOut),
86 ElemGravityOwner(uL, fOut),
87 ElemWithDofs(uL, pDO, fOut),
88 InitialAssemblyElem(uL, fOut)
89 #ifdef USE_NETCDF
90 ,
91 Var_F_local(0),
92 Var_M_local(0),
93 Var_F_global(0),
94 Var_M_global(0)
95 #endif // USE_NETCDF
96 {
97  NO_OP;
98 }
99 
100 Joint::~Joint(void)
101 {
102  NO_OP;
103 }
104 
105 
106 void
107 Joint::OutputPrepare_int(const std::string& type, OutputHandler &OH, std::string& name)
108 {
109 #ifdef USE_NETCDF
110  ASSERT(OH.IsOpen(OutputHandler::NETCDF));
111 
112  std::ostringstream os;
113  os << "elem.joint." << GetLabel();
114  (void)OH.CreateVar(os.str(), type);
115 
116  // joint sub-data
117  os << '.';
118  name = os.str();
119 
120  Var_F_local = OH.CreateVar<Vec3>(name + "f", "N",
121  "local reaction force (Fx, Fy, Fz)");
122 
123  Var_M_local = OH.CreateVar<Vec3>(name + "m", "Nm",
124  "local reaction moment (Mx, My, Mz)");
125 
126  Var_F_global = OH.CreateVar<Vec3>(name + "F", "N",
127  "global reaction force (FX, FY, FZ)");
128 
129  Var_M_global = OH.CreateVar<Vec3>(name + "M", "Nm",
130  "global reaction moment (MX, MY, MZ)");
131 
132  // elements can add further data
133 #endif // USE_NETCDF
134 }
135 
136 /* Output specifico dei vincoli */
137 std::ostream&
138 Joint::Output(std::ostream& out, const char* /* sJointName */ ,
139  unsigned int uLabel,
140  const Vec3& FLocal, const Vec3& MLocal,
141  const Vec3& FGlobal, const Vec3& MGlobal) const
142 {
143 #if 0
144  /* Modificare le dimensioni del campo per il nome in base
145  * ai nomi dei vincoli futuri */
146  ASSERT(strlen(sJointName) <= 16);
147 
148  /* Nota: non c'e' *std::endl* perche' i vincoli possono aggiungere outut
149  * ulteriore a quello comune a tutti */
150  return out << sJointName << std::setw(16+8-strlen(sJointName)) << uLabel << " "
151  << FLocal << " " << MLocal << " " << FGlobal << " " << MGlobal;
152 #endif
153 
154  return out
155  << std::setw(8) << uLabel
156  << " " << FLocal << " " << MLocal
157  << " " << FGlobal << " " << MGlobal;
158 }
159 
160 /* Inverse Dynamics update */
161 
162 void
163 Joint::Update(const VectorHandler& XCurr, InverseDynamics::Order iOrder)
164 {
165  silent_cerr(psElemNames[GetElemType()] << "(" << GetLabel() << "): "
166  "Elem::Update(" << invdyn2str(iOrder) << ") for inverse dynamics not implemented yet" << std::endl);
167 }
168 
169 bool
170 Joint::bIsPrescribedMotion(void) const
171 {
172  return (GetInverseDynamicsFlags() & InverseDynamics::PRESCRIBED_MOTION);
173 }
174 
175 bool
176 Joint::bIsTorque(void) const
177 {
178  return (GetInverseDynamicsFlags() & InverseDynamics::TORQUE);
179 }
180 
181 /* Joint - end */
182 
183 
184 /* Legge un vincolo */
185 
186 Elem *
187 ReadJoint(DataManager* pDM,
188  MBDynParser& HP,
189  const DofOwner* pDO,
190  unsigned int uLabel)
191 {
192  DEBUGCOUTFNAME("ReadJoint");
193 
194  const char* sKeyWords[] = {
195  "distance",
196  "distance" "with" "offset",
197  "clamp",
198  "coincidence",
199  "spherical" "hinge",
200  "pin",
201  "spherical" "pin",
202  "universal" "hinge", // deprecated
203  "universal" "rotation", // deprecated
204  "universal" "pin", // deprecated
205  "cardano" "hinge",
206  "cardano" "rotation",
207  "cardano" "pin",
208  "plane" "hinge",
209  "revolute" "hinge",
210  "revolute" "rotation",
211  "plane" "pin",
212  "revolute" "pin",
213  "axial" "rotation",
214  "plane" "displacement",
215  "plane" "displacement" "pin",
216  "in" "plane",
217  "in" "line",
218  "rod",
219  "rod" "with" "offset",
220  "rod" "bezier",
221  "deformable" "hinge",
222  "deformable" "displacement" "hinge", // deprecated
223  "deformable" "displacement" "joint",
224  "deformable" "joint",
225  "deformable" "axial" "joint",
226  "viscous" "body",
227  "invariant" "deformable" "hinge",
228  "invariant" "deformable" "displacement" "joint",
229  "invariant" "deformable" "joint",
230  "linear" "velocity",
231  "angular" "velocity",
232  "linear" "acceleration",
233  "angular" "acceleration",
234  "prismatic",
235  "drive" "hinge",
236  "drive" "displacement",
237  "drive" "displacement" "pin",
238  "imposed" "displacement", // obsoleted
239  "imposed" "displacement" "pin", // obsoleted
240  "imposed" "orientation", // obsoleted
241  "total" "equation",
242  "total" "internal" "reaction",
243  "total" "joint",
244  "total" "pin" "joint",
245  "kinematic", // obsoleted
246  "beam" "slider",
247  "brake",
248  "gimbal" "rotation",
249  "modal",
250  "point" "contact",
251 #ifdef MBDYN_DEVEL
252  "screw",
253 #endif // MBDYN_DEVEL
254 
255  NULL
256  };
257 
258  /* enum delle parole chiave */
259  enum KeyWords {
260  UNKNOWN = -1,
261 
262  DISTANCE = 0,
263  DISTANCEWITHOFFSET,
264  CLAMP,
265  COINCIDENCE,
266  SPHERICALHINGE,
267  PIN,
268  SPHERICALPIN,
269  UNIVERSALHINGE, // deprecated
270  UNIVERSALROTATION, // deprecated
271  UNIVERSALPIN, // deprecated
272  CARDANOHINGE,
273  CARDANOROTATION,
274  CARDANOPIN,
275  PLANEHINGE,
276  REVOLUTEHINGE,
277  REVOLUTEROTATION,
278  PLANEPIN,
279  REVOLUTEPIN,
280  AXIALROTATION,
281  PLANEDISPLACEMENT,
282  PLANEDISPLACEMENTPIN,
283  INPLANE,
284  J_INLINE,
285  ROD,
286  RODWITHOFFSET,
287  RODBEZIER,
288  DEFORMABLEHINGE,
289  DEFORMABLEDISPHINGE, // deprecated
290  DEFORMABLEDISPJOINT,
291  DEFORMABLEJOINT,
292  DEFORMABLEAXIALJOINT,
293  VISCOUSBODY,
294  INVARIANTDEFORMABLEHINGE,
295  INVARIANTDEFORMABLEDISPJOINT,
296  INVARIANTDEFORMABLEJOINT,
297  LINEARVELOCITY,
298  ANGULARVELOCITY,
299  LINEARACCELERATION,
300  ANGULARACCELERATION,
301  PRISMATIC,
302  DRIVEHINGE,
303  DRIVEDISPLACEMENT,
304  DRIVEDISPLACEMENTPIN,
305  IMPOSEDDISPLACEMENT, // obsoleted
306  IMPOSEDDISPLACEMENTPIN, // obsoleted
307  IMPOSEDORIENTATION, // obsoleted
308  TOTALEQUATION,
309  TOTALINTERNALREACTION,
310  TOTALJOINT,
311  TOTALPINJOINT,
312  KINEMATIC, // obsoleted
313  BEAMSLIDER,
314  BRAKE,
315  GIMBALROTATION,
316  MODAL,
317  POINT_SURFACE_CONTACT,
318 #ifdef MBDYN_DEVEL
319  SCREWJOINT,
320 #endif // MBDYN_DEVEL
321 
322  LASTKEYWORD
323  };
324 
325  /* tabella delle parole chiave */
326  KeyTable K(HP, sKeyWords);
327 
328  /* lettura del tipo di vincolo */
329  KeyWords CurrKeyWord = KeyWords(HP.IsKeyWord());
330 
331 #ifdef DEBUG
332  if (CurrKeyWord >= 0) {
333  std::cout << "joint type: " << sKeyWords[CurrKeyWord] << std::endl;
334  }
335 #endif // DEBUG
336 
337  // Inverse dynamics
338  Joint* pEl = NULL;
339  bool bIsTorque(true);
340  bool bIsPrescribedMotion(true);
341  bool bIsErgonomy(false);
342  bool bIsRightHandSide(false);
343 
344  switch (CurrKeyWord) {
345 
346  /* vincolo di distanza */
347  case DISTANCE:
348  {
349  /* lettura dei dati specifici */
350  /* due nodi e tipo di drive, con dati specifici */
351 
352  bool bOffset(false);
353 
354  /* nodo collegato 1 */
355  const StructDispNode* pNode1 = pDM->ReadNode<const StructDispNode, Node::STRUCTURAL>(HP);
356  const StructNode *pN1 = dynamic_cast<const StructNode *>(pNode1);
357 
358  Vec3 f1(Zero3);
359  ReferenceFrame RF1(pNode1);
360  if (HP.IsKeyWord("position")) {
361  f1 = HP.GetPosRel(RF1);
362  bOffset = true;
363  }
364 
365  /* nodo collegato 2 */
366  const StructDispNode* pNode2 = pDM->ReadNode<const StructDispNode, Node::STRUCTURAL>(HP);
367  const StructNode *pN2 = dynamic_cast<const StructNode *>(pNode2);
368 
369  Vec3 f2(Zero3);
370  if (HP.IsKeyWord("position")) {
371  f2 = HP.GetPosRel(ReferenceFrame(pNode2), RF1, f1);
372  bOffset = true;
373  }
374 
375  if (bOffset) {
376  if (pN1 == 0) {
377  silent_cerr("Joint(" << uLabel << "): "
378  "invalid StructNode(" << pNode1->GetLabel() << ") for node #1" << std::endl);
379  throw ErrGeneric(MBDYN_EXCEPT_ARGS);
380  }
381 
382  if (pN2 == 0) {
383  silent_cerr("Joint(" << uLabel << "): "
384  "invalid StructNode(" << pNode2->GetLabel() << ") for node #2" << std::endl);
385  throw ErrGeneric(MBDYN_EXCEPT_ARGS);
386  }
387  }
388 
389  DriveCaller* pDC = NULL;
390  doublereal l;
391  if (bOffset) {
392  l = (pN2->GetXCurr()
393  + pN2->GetRCurr()*f2
394  - pN1->GetXCurr()
395  - pN1->GetRCurr()*f1).Norm();
396  } else {
397  l = (pNode2->GetXCurr() - pNode1->GetXCurr()).Norm();
398  }
399 
400  pedantic_cout("Distance(" << uLabel << "): "
401  "length from nodes = " << l << std::endl);
402 
403  if (HP.IsKeyWord("from" "nodes")) {
404  SAFENEWWITHCONSTRUCTOR(pDC, ConstDriveCaller, ConstDriveCaller(l));
405 
406  } else {
407  pDM->PushCurrData("L", l);
408  pDC = HP.GetDriveCaller();
409  pDM->PopCurrData("L");
410  }
411 
412  flag fOut = pDM->fReadOutput(HP, Elem::JOINT);
413 
414  /* allocazione e costruzione */
415  if (bOffset) {
416  SAFENEWWITHCONSTRUCTOR(pEl,
417  DistanceJointWithOffset,
418  DistanceJointWithOffset(uLabel, pDO,
419  pN1, pN2, f1, f2, pDC, fOut));
420 
421  } else {
422  SAFENEWWITHCONSTRUCTOR(pEl,
423  DistanceJoint,
424  DistanceJoint(uLabel, pDO,
425  pNode1, pNode2, pDC, fOut));
426  }
427 
428  std::ostream& out = pDM->GetLogFile();
429  out << "distance: " << uLabel
430  << " " << pNode1->GetLabel()
431  << " ", f1.Write(out, " ")
432  << " " << pNode2->GetLabel()
433  << " ", f2.Write(out, " ")
434  << std::endl;
435 
436  } break;
437 
438  /* vincolo di distanza con offset */
439  case DISTANCEWITHOFFSET:
440  {
441  /* lettura dei dati specifici */
442  /* due nodi e tipo di drive, con dati specifici */
443 
444 #ifdef MBDYN_X_COMPATIBLE_INPUT
445  pedantic_cerr("Joint(" << uLabel << "): \"distance with offset\" "
446  "is deprecated; use \"distance\" instead" << std::endl);
447 #endif /* MBDYN_X_COMPATIBLE_INPUT */
448 
449  /* nodo collegato 1 */
450  const StructNode* pNode1 = pDM->ReadNode<const StructNode, Node::STRUCTURAL>(HP);
451 
452  Vec3 f1(Zero3);
453  ReferenceFrame RF1(pNode1);
454  if (HP.IsKeyWord("position")) {
455 #ifdef MBDYN_X_COMPATIBLE_INPUT
456  NO_OP;
457  } else {
458  pedantic_cerr("Joint(" << uLabel << "): "
459  "missing keyword \"position\" at line "
460  << HP.GetLineData() << std::endl);
461  }
462 #endif /* MBDYN_X_COMPATIBLE_INPUT */
463  f1 = HP.GetPosRel(RF1);
464 #ifndef MBDYN_X_COMPATIBLE_INPUT
465  }
466 #endif /* !MBDYN_X_COMPATIBLE_INPUT */
467 
468  DEBUGCOUT("Offset 1: " << f1 << std::endl);
469 
470 
471  /* nodo collegato 2 */
472  const StructNode* pNode2 = pDM->ReadNode<const StructNode, Node::STRUCTURAL>(HP);
473 
474  Vec3 f2(Zero3);
475  if (HP.IsKeyWord("position")) {
476 #ifdef MBDYN_X_COMPATIBLE_INPUT
477  NO_OP;
478  } else {
479  pedantic_cerr("Joint(" << uLabel << "): "
480  "missing keyword \"position\" at line "
481  << HP.GetLineData() << std::endl);
482  }
483 #endif /* MBDYN_X_COMPATIBLE_INPUT */
484  f2 = HP.GetPosRel(ReferenceFrame(pNode2), RF1, f1);
485 #ifndef MBDYN_X_COMPATIBLE_INPUT
486  }
487 #endif /* !MBDYN_X_COMPATIBLE_INPUT */
488 
489  DEBUGCOUT("Offset 2: " << f2 << std::endl);
490 
491 
492  /* Legge e costruisce il drivecaller */
493  if (!HP.IsArg()) {
494  silent_cerr("line " << HP.GetLineData()
495  << ": driver data expected" << std::endl);
496  throw DataManager::ErrGeneric(MBDYN_EXCEPT_ARGS);
497  }
498 
499  DriveCaller* pDC = NULL;
500  doublereal l = (pNode2->GetXCurr()
501  - pNode1->GetXCurr()
502  + pNode2->GetRCurr()*f2
503  - pNode1->GetRCurr()*f1).Norm();
504 
505  pedantic_cout("DistanceWithOffset(" << uLabel << "): "
506  "length from nodes = " << l << std::endl);
507 
508  if (HP.IsKeyWord("from" "nodes")) {
509  SAFENEWWITHCONSTRUCTOR(pDC, ConstDriveCaller, ConstDriveCaller(l));
510  } else {
511  pDM->PushCurrData("L", l);
512  pDC = HP.GetDriveCaller();
513  pDM->PopCurrData("L");
514  }
515 
516  flag fOut = pDM->fReadOutput(HP, Elem::JOINT);
517 
518  /* allocazione e costruzione */
519  SAFENEWWITHCONSTRUCTOR(pEl,
520  DistanceJointWithOffset,
521  DistanceJointWithOffset(uLabel, pDO, pNode1, pNode2,
522  f1, f2, pDC, fOut));
523 
524  std::ostream& out = pDM->GetLogFile();
525  out << "distance: " << uLabel
526  << " " << pNode1->GetLabel()
527  << " ", f1.Write(out, " ")
528  << " " << pNode2->GetLabel()
529  << " ", f2.Write(out, " ")
530  << std::endl;
531 
532  /* scrittura dei dati specifici */
533  } break;
534 
535  /* vincolo di incastro */
536  case CLAMP:
537  {
538  /* lettura dei dati specifici */
539 
540  /* nodo collegato */
541  const StructNode* pNode = pDM->ReadNode<const StructNode, Node::STRUCTURAL>(HP);
542 
543  /* posizione (vettore di 3 elementi) */
544  ReferenceFrame RF(pNode);
545  /* stessa posizione del nodo */
546  Vec3 X0(pNode->GetXCurr());
547  if (HP.IsKeyWord("position")) {
548 #ifdef MBDYN_X_COMPATIBLE_INPUT
549  NO_OP;
550  } else {
551  pedantic_cerr("Joint(" << uLabel << "): "
552  "missing keyword \"position\" at line "
553  << HP.GetLineData() << std::endl);
554  }
555 #endif /* MBDYN_X_COMPATIBLE_INPUT */
556  if (!HP.IsKeyWord("node")) {
557  /* posizione arbitraria */
558  X0 = HP.GetPosAbs(RF);
559  }
560 #ifndef MBDYN_X_COMPATIBLE_INPUT
561  }
562 #endif /* !MBDYN_X_COMPATIBLE_INPUT */
563 
564  DEBUGCOUT("X0 =" << std::endl << X0 << std::endl);
565 
566  /* sistema di riferimento (trucco dei due vettori) */
567  /* stessa giacitura del nodo */
568  Mat3x3 R0(pNode->GetRCurr());
569  if (HP.IsKeyWord("orientation")) {
570 #ifdef MBDYN_X_COMPATIBLE_INPUT
571  NO_OP;
572  } else {
573  pedantic_cerr("Joint(" << uLabel << "): "
574  "missing keyword \"orientation\" at line "
575  << HP.GetLineData() << std::endl);
576  }
577 #endif /* MBDYN_X_COMPATIBLE_INPUT */
578  if (!HP.IsKeyWord("node")) {
579  /* giacitura arbitraria */
580  R0 = HP.GetRotAbs(RF);
581  }
582 #ifndef MBDYN_X_COMPATIBLE_INPUT
583  }
584 #endif /* !MBDYN_X_COMPATIBLE_INPUT */
585 
586  DEBUGCOUT("R0 =" << std::endl << R0 << std::endl);
587 
588  flag fOut = pDM->fReadOutput(HP, Elem::JOINT);
589 
590  /* allocazione e costruzione */
591  SAFENEWWITHCONSTRUCTOR(pEl,
592  ClampJoint,
593  ClampJoint(uLabel, pDO, pNode, X0, R0, fOut));
594  std::ostream& out = pDM->GetLogFile();
595  out << "clamp: " << uLabel
596  << " " << pNode->GetLabel()
597  << " " << Zero3
598  << " " << Eye3
599  << " " << pNode->GetLabel()
600  << " " << Zero3
601  << " " << Eye3
602  << std::endl;
603  } break;
604 
605  case PIN:
606  case SPHERICALPIN:
607  {
608  /* lettura dei dati specifici */
609 
610  /* nodo collegato */
611  const StructNode* pNode = pDM->ReadNode<const StructNode, Node::STRUCTURAL>(HP);
612 
613  ReferenceFrame RF(pNode);
614 
615  Vec3 d(Zero3);
616  if (HP.IsKeyWord("position")) {
617 #ifdef MBDYN_X_COMPATIBLE_INPUT
618  NO_OP;
619  } else {
620  pedantic_cerr("Joint(" << uLabel << "): "
621  "missing keyword \"position\" at line "
622  << HP.GetLineData() << std::endl);
623  }
624 #endif /* MBDYN_X_COMPATIBLE_INPUT */
625  d = HP.GetPosRel(RF);
626 #ifndef MBDYN_X_COMPATIBLE_INPUT
627  }
628 #endif /* MBDYN_X_COMPATIBLE_INPUT */
629 
630  /* currently unused */
631  if (HP.IsKeyWord("orientation")) {
632  (void)HP.GetRotRel(RF);
633  }
634 
635  DEBUGCOUT("Node reference frame d:" << std::endl << d << std::endl);
636 
637  /* posizione (vettore di 3 elementi) */
638  Vec3 X0(Zero3);
639  if (HP.IsKeyWord("position")) {
640 #ifdef MBDYN_X_COMPATIBLE_INPUT
641  NO_OP;
642  } else {
643  pedantic_cerr("Joint(" << uLabel << "): "
644  "missing keyword \"position\" at line "
645  << HP.GetLineData() << std::endl);
646  }
647 #endif /* MBDYN_X_COMPATIBLE_INPUT */
648  X0 = HP.GetPosAbs(::AbsRefFrame);
649 #ifndef MBDYN_X_COMPATIBLE_INPUT
650  }
651 #endif /* MBDYN_X_COMPATIBLE_INPUT */
652 
653  /* currently unused */
654  if (HP.IsKeyWord("orientation")) {
655  (void)HP.GetRotAbs(::AbsRefFrame);
656  }
657 
658 
659  flag fOut = pDM->fReadOutput(HP, Elem::JOINT);
660 
661  /* allocazione e creazione */
662  SAFENEWWITHCONSTRUCTOR(pEl,
663  PinJoint,
664  PinJoint(uLabel, pDO, pNode, X0, d, fOut));
665 
666  std::ostream& out = pDM->GetLogFile();
667  out << "sphericalpin: " << uLabel
668  << " " << pNode->GetLabel()
669  << " " << d
670  << " " << Eye3
671  << " " << pNode->GetLabel()
672  << " " << d
673  << " " << Eye3
674  << std::endl;
675  } break;
676 
677  /* vincolo di cerniera piana (PLANEHINGE)
678  * eventualmente con velocita' di rotazione imposta (AXIALROTATION) */
679  case SPHERICALHINGE:
680  case PLANEHINGE:
681  case REVOLUTEHINGE:
682  case BRAKE:
683  case REVOLUTEROTATION:
684  case UNIVERSALHINGE:
685  case UNIVERSALROTATION:
686  case CARDANOHINGE:
687  case CARDANOROTATION:
688  case AXIALROTATION:
689  case GIMBALROTATION:
690  case PLANEDISPLACEMENT:
691  {
692  /* nodo collegato 1 */
693  const StructNode* pNode1 = pDM->ReadNode<const StructNode, Node::STRUCTURAL>(HP);
694 
695  ReferenceFrame RF1(pNode1);
696  Vec3 d1(Zero3);
697 
698 #ifndef MBDYN_X_COMPATIBLE_INPUT
699  if (HP.IsKeyWord("position")) {
700  d1 = HP.GetPosRel(RF1);
701  }
702 #else /* MBDYN_X_COMPATIBLE_INPUT */
703  switch (CurrKeyWord) {
704  case REVOLUTEROTATION:
705  case UNIVERSALROTATION:
706  case CARDANOROTATION:
707  case GIMBALROTATION:
708  if (HP.IsKeyWord("position")) {
709  /* currently ignored */
710  (void)HP.GetPosRel(RF1);
711  } else {
712  pedantic_cerr("Joint(" << uLabel << "): "
713  "missing keyword \"position\" at line "
714  << HP.GetLineData() << std::endl);
715  }
716  break;
717 
718  default:
719  if (!HP.IsKeyWord("position")) {
720  pedantic_cerr("Joint(" << uLabel << "): "
721  "missing keyword \"position\" at line "
722  << HP.GetLineData() << std::endl);
723  }
724  d1 = HP.GetPosRel(RF1);
725  DEBUGCOUT("Node 1 reference frame d1:" << std::endl
726  << d1 << std::endl);
727  break;
728  }
729 #endif /* MBDYN_X_COMPATIBLE_INPUT */
730 
731  Mat3x3 R1h(Eye3);
732  if (HP.IsKeyWord("orientation")) {
733  DEBUGCOUT("Hinge orientation matrix is supplied" << std::endl);
734  R1h = HP.GetRotRel(RF1);
735 #ifdef MBDYN_X_COMPATIBLE_INPUT
736  } else if (HP.IsKeyWord("hinge")) {
737  pedantic_cerr("Joint(" << uLabel << "): "
738  "keyword \"hinge\" at line " << HP.GetLineData()
739  << " is deprecated; use \"orientation\" instead"
740  << std::endl);
741  DEBUGCOUT("Hinge orientation matrix is supplied" << std::endl);
742  R1h = HP.GetRotRel(RF1);
743 #endif /* MBDYN_X_COMPATIBLE_INPUT */
744  }
745 
746 
747  /* nodo collegato 2 */
748  const StructNode* pNode2 = pDM->ReadNode<const StructNode, Node::STRUCTURAL>(HP);
749 
750  /* Stessa cosa per il nodo 2 */
751 
752  ReferenceFrame RF2(pNode2);
753  Vec3 d2(Zero3);
754 #ifndef MBDYN_X_COMPATIBLE_INPUT
755  if (HP.IsKeyWord("position")) {
756  d2 = HP.GetPosRel(RF2, RF1, d1);
757  }
758 #else /* MBDYN_X_COMPATIBLE_INPUT */
759  switch (CurrKeyWord) {
760  case REVOLUTEROTATION:
761  case UNIVERSALROTATION:
762  case CARDANOROTATION:
763  case GIMBALROTATION:
764  if (HP.IsKeyWord("position")) {
765  /* currently ignored */
766  (void)HP.GetPosRel(RF2, RF1, d1);
767  } else {
768  pedantic_cerr("Joint(" << uLabel << "): "
769  "missing keyword \"position\" at line "
770  << HP.GetLineData() << std::endl);
771  }
772  break;
773 
774  default:
775  if (!HP.IsKeyWord("position")) {
776  pedantic_cerr("Joint(" << uLabel << "): "
777  "missing keyword \"position\" at line "
778  << HP.GetLineData() << std::endl);
779  }
780  d2 = HP.GetPosRel(RF2, RF1, d1);
781  DEBUGCOUT("Node 2 reference frame d2:" << std::endl
782  << d2 << std::endl);
783  break;
784  }
785 #endif /* MBDYN_X_COMPATIBLE_INPUT */
786 
787  Mat3x3 R2h(Eye3);
788  if (HP.IsKeyWord("orientation")) {
789  DEBUGCOUT("Hinge orientation matrix is supplied" << std::endl);
790  R2h = HP.GetRotRel(RF2, RF1, R1h);
791 #ifdef MBDYN_X_COMPATIBLE_INPUT
792  } else if (HP.IsKeyWord("hinge")) {
793  pedantic_cerr("Joint(" << uLabel << "): "
794  "keyword \"hinge\" at line " << HP.GetLineData()
795  << " is deprecated; use \"orientation\" instead"
796  << std::endl);
797  DEBUGCOUT("Hinge orientation matrix is supplied" << std::endl);
798  R2h = HP.GetRotRel(RF2, RF1, R1h);
799 #endif /* MBDYN_X_COMPATIBLE_INPUT */
800  }
801 
802 
803  DriveCaller* pDC = 0;
804  if (CurrKeyWord == AXIALROTATION) {
805  pDC = HP.GetDriveCaller();
806  }
807 
808  OrientationDescription od = UNKNOWN_ORIENTATION_DESCRIPTION;
809  switch (CurrKeyWord) {
810  case GIMBALROTATION:
811  case SPHERICALHINGE:
812  case UNIVERSALROTATION:
813  case CARDANOROTATION:
814  case REVOLUTEHINGE:
815  case AXIALROTATION:
816  case REVOLUTEROTATION:
817  od = ReadOptionalOrientationDescription(pDM, HP);
818  break;
819 
820  default:
821  break;
822  }
823 
824  flag fOut = pDM->fReadOutput(HP, Elem::JOINT);
825 
826  switch (CurrKeyWord) {
827 
828  /* allocazione e creazione cerniera sferica */
829  case SPHERICALHINGE:
830  {
831  SAFENEWWITHCONSTRUCTOR(pEl,
832  SphericalHingeJoint,
833  SphericalHingeJoint(uLabel, pDO,
834  pNode1, pNode2,
835  d1, R1h, d2, R2h, od, fOut));
836  std::ostream& out = pDM->GetLogFile();
837  out << "sphericalhinge: " << uLabel
838  << " " << pNode1->GetLabel()
839  << " " << d1
840  << " " << R1h
841  << " " << pNode2->GetLabel()
842  << " " << d2
843  << " " << R2h
844  << std::endl;
845  } break;
846 
847  /* allocazione e creazione cerniera piana */
848  case PLANEHINGE:
849  silent_cerr("line " << HP.GetLineData()
850  << ": deprecated \"plane hinge\" joint name;"
851  << " use \"revolute hinge\" instead" << std::endl);
852  case REVOLUTEHINGE:
853  {
854  bool calcInitdTheta = true;
855  doublereal initDTheta = 0.;
856  if (HP.IsKeyWord("initial" "theta")) {
857  initDTheta = HP.GetReal();
858  calcInitdTheta = false;
859  }
860  doublereal r = 0.;
861  doublereal preload = 0.;
862  BasicFriction *bf = 0;
863  BasicShapeCoefficient *bsh = 0;
864  if (HP.IsKeyWord("friction")) {
865  r = HP.GetReal();
866  if (HP.IsKeyWord("preload")) {
867  preload = HP.GetReal();
868  }
869  bf = ParseFriction(HP,pDM);
870  bsh = ParseShapeCoefficient(HP);
871  }
872  SAFENEWWITHCONSTRUCTOR(pEl,
873  PlaneHingeJoint,
874  PlaneHingeJoint(uLabel, pDO, pNode1, pNode2,
875  d1, d2, R1h, R2h, od, fOut,
876  calcInitdTheta, initDTheta,
877  r, preload, bsh, bf));
878  std::ostream& out = pDM->GetLogFile();
879  out << "revolutehinge: " << uLabel
880  << " " << pNode1->GetLabel()
881  << " " << d1
882  << " " << R1h
883  << " " << pNode2->GetLabel()
884  << " " << d2
885  << " " << R2h
886  << std::endl;
887  } break;
888 
889  case BRAKE:
890  {
891  if (!HP.IsKeyWord("friction")) {
892  silent_cerr("missing keyword \"friction\" at line "
893  << HP.GetLineData() << std::endl);
894  throw DataManager::ErrGeneric(MBDYN_EXCEPT_ARGS);
895  }
896  doublereal r = HP.GetReal();
897 
898  doublereal preload = 0.;
899  if (HP.IsKeyWord("preload")) {
900  preload = HP.GetReal();
901  }
902 
903  BasicFriction * bf = ParseFriction(HP,pDM);
904  BasicShapeCoefficient * bsh = ParseShapeCoefficient(HP);
905 
906 #if 0
907  // might be useful for a body sliding on a plane...
908  bool isForce(false);
909  Vec3 Dir(Zero3);
910 #endif
911 
912  DriveCaller *pDC = HP.GetDriveCaller();
913 
914  SAFENEWWITHCONSTRUCTOR(pEl,
915  Brake,
916  Brake(uLabel, pDO, pNode1, pNode2,
917  d1, d2, R1h, R2h, fOut,
918  r, preload, bsh, bf,
919  /* isForce, Dir, */ pDC));
920  std::ostream& out = pDM->GetLogFile();
921  out << "brake: " << uLabel
922  << " " << pNode1->GetLabel()
923  << " " << d1
924  << " " << R1h
925  << " " << pNode2->GetLabel()
926  << " " << d2
927  << " " << R2h
928  << std::endl;
929  } break;
930 
931  /* allocazione e creazione cerniera piana senza vincolo in pos. */
932  case REVOLUTEROTATION:
933  {
934  SAFENEWWITHCONSTRUCTOR(pEl,
935  PlaneRotationJoint,
936  PlaneRotationJoint(uLabel, pDO,
937  pNode1, pNode2, R1h, R2h, od, fOut));
938  std::ostream& out = pDM->GetLogFile();
939  out << "revoluterotation: " << uLabel
940  << " " << pNode1->GetLabel()
941  << " " << Zero3
942  << " " << R1h
943  << " " << pNode2->GetLabel()
944  << " " << Zero3
945  << " " << R2h
946  << std::endl;
947  } break;
948 
949  /* allocazione e creazione cerniera universale */
950  case UNIVERSALHINGE:
951  case CARDANOHINGE:
952  {
953  SAFENEWWITHCONSTRUCTOR(pEl,
954  UniversalHingeJoint,
955  UniversalHingeJoint(uLabel, pDO,
956  pNode1, pNode2,
957  d1, d2, R1h, R2h, fOut));
958  std::ostream& out = pDM->GetLogFile();
959  out << "cardanohinge: " << uLabel
960  << " " << pNode1->GetLabel()
961  << " " << d1
962  << " " << R1h
963  << " " << pNode2->GetLabel()
964  << " " << d2
965  << " " << R2h
966  << std::endl;
967  } break;
968 
969  /* allocazione e creazione cerniera universale senza vincolo pos. */
970  case UNIVERSALROTATION:
971  case CARDANOROTATION:
972  {
973  SAFENEWWITHCONSTRUCTOR(pEl,
974  UniversalRotationJoint,
975  UniversalRotationJoint(uLabel, pDO,
976  pNode1, pNode2, R1h, R2h, od, fOut));
977  std::ostream& out = pDM->GetLogFile();
978  out << "cardanorotation: " << uLabel
979  << " " << pNode1->GetLabel()
980  << " " << Zero3
981  << " " << R1h
982  << " " << pNode2->GetLabel()
983  << " " << Zero3
984  << " " << R2h
985  << std::endl;
986  } break;
987 
988  /* allocazione e creazione cerniera piana
989  * con velocita' di rotazione imposta */
990  case AXIALROTATION:
991  {
992  doublereal r = 0.;
993  doublereal preload = 0.;
994  BasicFriction *bf = 0;
995  BasicShapeCoefficient *bsh = 0;
996  if (HP.IsKeyWord("friction")) {
997  r = HP.GetReal();
998  if (HP.IsKeyWord("preload")) {
999  preload = HP.GetReal();
1000  }
1001  bf = ParseFriction(HP,pDM);
1002  bsh = ParseShapeCoefficient(HP);
1003  }
1004  SAFENEWWITHCONSTRUCTOR(pEl,
1005  AxialRotationJoint,
1006  AxialRotationJoint(uLabel, pDO,
1007  pNode1, pNode2,
1008  d1, d2, R1h, R2h, pDC, od,
1009  fOut,
1010  r, preload, bsh, bf));
1011  std::ostream& out = pDM->GetLogFile();
1012  out << "axialrotation: " << uLabel
1013  << " " << pNode1->GetLabel()
1014  << " " << d1
1015  << " " << R1h
1016  << " " << pNode2->GetLabel()
1017  << " " << d2
1018  << " " << R2h
1019  << std::endl;
1020  } break;
1021 
1022  /* allocazione e creazione vincolo gimbal */
1023  case GIMBALROTATION:
1024  {
1025  SAFENEWWITHCONSTRUCTOR(pEl,
1026  GimbalRotationJoint,
1027  GimbalRotationJoint(uLabel, pDO,
1028  pNode1, pNode2, R1h, R2h, od, fOut));
1029  std::ostream& out = pDM->GetLogFile();
1030  out << "gimbalrotation: " << uLabel
1031  << " " << pNode1->GetLabel()
1032  << " " << Zero3
1033  << " " << R1h
1034  << " " << pNode2->GetLabel()
1035  << " " << Zero3
1036  << " " << R2h
1037  << std::endl;
1038  } break;
1039 
1040  /* allocazione e creazione pattino */
1041  case PLANEDISPLACEMENT:
1042  silent_cerr("PlaneDispJoint(" << uLabel << "): "
1043  "unsupported; "
1044  "use an InPlane and a RevoluteRotation"
1045  << std::endl);
1046  throw DataManager::ErrGeneric(MBDYN_EXCEPT_ARGS);
1047 
1048  default:
1049  ASSERTMSG(0, "You shouldn't have reached this point");
1050  throw DataManager::ErrGeneric(MBDYN_EXCEPT_ARGS);
1051  }
1052 
1053  } break;
1054 
1055  case UNIVERSALPIN:
1056  case CARDANOPIN:
1057  case PLANEPIN:
1058  case REVOLUTEPIN:
1059  case PLANEDISPLACEMENTPIN:
1060  {
1061  /* nodo collegato */
1062  const StructNode* pNode = pDM->ReadNode<const StructNode, Node::STRUCTURAL>(HP);
1063 
1064  ReferenceFrame RF(pNode);
1065  Vec3 d(Zero3);
1066  if (HP.IsKeyWord("position")) {
1067 #ifdef MBDYN_X_COMPATIBLE_INPUT
1068  NO_OP;
1069  } else {
1070  pedantic_cerr("Joint(" << uLabel << "): "
1071  "missing keyword \"position\" at line "
1072  << HP.GetLineData() << std::endl);
1073  }
1074 #endif /* MBDYN_X_COMPATIBLE_INPUT */
1075  d = HP.GetPosRel(RF);
1076 #ifndef MBDYN_X_COMPATIBLE_INPUT
1077  }
1078 #endif /* MBDYN_X_COMPATIBLE_INPUT */
1079 
1080  DEBUGCOUT("Node reference frame d:" << std::endl << d << std::endl);
1081 
1082  Mat3x3 Rh(Eye3);
1083  if (HP.IsKeyWord("orientation")) {
1084  DEBUGCOUT("Hinge orientation matrix is supplied" << std::endl);
1085  Rh = HP.GetRotRel(RF);
1086 #ifdef MBDYN_X_COMPATIBLE_INPUT
1087  } else if (HP.IsKeyWord("hinge")) {
1088  pedantic_cerr("Joint(" << uLabel << "): "
1089  "keyword \"hinge\" at line " << HP.GetLineData()
1090  << " is deprecated; use \"orientation\" instead"
1091  << std::endl);
1092  DEBUGCOUT("Hinge orientation matrix is supplied" << std::endl);
1093  Rh = HP.GetRotRel(RF);
1094  DEBUGCOUT("Hinge Rotation matrix Rh:" << std::endl << Rh << std::endl);
1095 #endif /* MBDYN_X_COMPATIBLE_INPUT */
1096  }
1097 
1098 
1099  Vec3 X0(Zero3);
1100 #ifndef MBDYN_X_COMPATIBLE_INPUT
1101  if (HP.IsKeyWord("position"))
1102 #else /* MBDYN_X_COMPATIBLE_INPUT */
1103  if (!HP.IsKeyWord("position")) {
1104  pedantic_cerr("Joint(" << uLabel << "): "
1105  "keyword \"position\" expected at line " << HP.GetLineData()
1106  << std::endl);
1107  }
1108 #endif /* MBDYN_X_COMPATIBLE_INPUT */
1109  {
1110  X0 = HP.GetPosAbs(::AbsRefFrame);
1111  }
1112  DEBUGCOUT("Absolute X:" << std::endl << X0 << std::endl);
1113 
1114  Mat3x3 R0(Eye3);
1115  if (HP.IsKeyWord("orientation")) {
1116  DEBUGCOUT("Hinge orientation matrix is supplied" << std::endl);
1117  R0 = HP.GetRotAbs(::AbsRefFrame);
1118 #ifdef MBDYN_X_COMPATIBLE_INPUT
1119  } else if (HP.IsKeyWord("hinge")) {
1120  pedantic_cerr("Joint(" << uLabel << "): "
1121  "keyword \"hinge\" at line " << HP.GetLineData()
1122  << " is deprecated; use \"orientation\" instead"
1123  << std::endl);
1124  DEBUGCOUT("Hinge orientation matrix is supplied" << std::endl);
1125  R0 = HP.GetRotAbs(::AbsRefFrame);
1126 #endif /* MBDYN_X_COMPATIBLE_INPUT */
1127  }
1128 
1129  DEBUGCOUT("Absolute R:" << std::endl << R0 << std::endl);
1130 
1131 
1132  flag fOut = pDM->fReadOutput(HP, Elem::JOINT);
1133 
1134  switch (CurrKeyWord) {
1135 
1136  /* allocazione e creazione cerniera piana */
1137  case PLANEPIN:
1138  silent_cerr("deprecated \"plane pin\" joint name;"
1139  << " use \"revolute pin\" instead" << std::endl);
1140  case REVOLUTEPIN:
1141  {
1142  bool calcInitdTheta = true;
1143  doublereal initDTheta = 0.;
1144  if (HP.IsKeyWord("initial" "theta")) {
1145  initDTheta = HP.GetReal();
1146  calcInitdTheta = false;
1147  }
1148  SAFENEWWITHCONSTRUCTOR(pEl,
1149  PlanePinJoint,
1150  PlanePinJoint(uLabel, pDO, pNode,
1151  X0, R0, d, Rh, fOut,
1152  calcInitdTheta, initDTheta));
1153  std::ostream& out = pDM->GetLogFile();
1154  out << "revolutepin: " << uLabel
1155  << " " << pNode->GetLabel()
1156  << " " << d
1157  << " " << Rh
1158  << " " << pNode->GetLabel()
1159  << " " << pNode->GetRCurr().MulTV(X0 - pNode->GetXCurr())
1160  << " " << R0.MulMT(pNode->GetRCurr())
1161  << std::endl;
1162  } break;
1163 
1164  case UNIVERSALPIN:
1165  case CARDANOPIN:
1166  {
1167  SAFENEWWITHCONSTRUCTOR(pEl,
1168  UniversalPinJoint,
1169  UniversalPinJoint(uLabel, pDO, pNode,
1170  X0, R0, d, Rh, fOut));
1171  std::ostream& out = pDM->GetLogFile();
1172  out << "cardanopin: " << uLabel
1173  << " " << pNode->GetLabel()
1174  << " " << d
1175  << " " << Rh
1176  << " " << pNode->GetLabel()
1177  << " " << pNode->GetRCurr().MulTV(X0 - pNode->GetXCurr())
1178  << " " << R0.MulMT(pNode->GetRCurr())
1179  << std::endl;
1180  } break;
1181 
1182  /* allocazione e creazione cerniera piana */
1183  case PLANEDISPLACEMENTPIN:
1184  silent_cerr("PlaneDispJoint(" << uLabel << "): "
1185  "unsupported; "
1186  "use an \"inplane\" and a \"revolute rotation\" instead"
1187  << std::endl);
1188  throw DataManager::ErrGeneric(MBDYN_EXCEPT_ARGS);
1189 
1190  default:
1191  ASSERTMSG(0, "You shouldn't have reached this point");
1192  throw DataManager::ErrGeneric(MBDYN_EXCEPT_ARGS);
1193  }
1194 
1195  } break;
1196 
1197  case INPLANE:
1198  {
1199  /* nodo collegato 1 */
1200  const StructNode* pNode1 = pDM->ReadNode<const StructNode, Node::STRUCTURAL>(HP);
1201 
1202  ReferenceFrame RF(pNode1);
1203  Vec3 p(Zero3);
1204  if (HP.IsKeyWord("position")) {
1205 #ifdef MBDYN_X_COMPATIBLE_INPUT
1206  NO_OP;
1207  } else {
1208  pedantic_cerr("Joint(" << uLabel << "): "
1209  "missing keyword \"position\" at line "
1210  << HP.GetLineData() << std::endl);
1211  }
1212 #endif /* MBDYN_X_COMPATIBLE_INPUT */
1213  p = HP.GetPosRel(RF);
1214 #ifndef MBDYN_X_COMPATIBLE_INPUT
1215  }
1216 #endif /* MBDYN_X_COMPATIBLE_INPUT */
1217  DEBUGCOUT("Node 1 reference frame p:" << std::endl << p << std::endl);
1218 
1219  Vec3 v;
1220  try {
1221  v = HP.GetUnitVecRel(RF);
1222  } catch (ErrNullNorm) {
1223  silent_cerr("Joint(" << uLabel << "): "
1224  "null direction at line " << HP.GetLineData()
1225  << std::endl);
1226  throw ErrNullNorm(MBDYN_EXCEPT_ARGS);
1227  }
1228 
1229  /* nodo collegato 2 */
1230  const StructNode* pNode2 = pDM->ReadNode<const StructNode, Node::STRUCTURAL>(HP);
1231 
1232  Vec3 q(Zero3);
1233  bool bOffset(false);
1234 
1235  if (HP.IsArg()) {
1236  if (HP.IsKeyWord("offset")) {
1237  bOffset = true;
1238  q = HP.GetPosRel(ReferenceFrame(pNode2), RF, p);
1239  DEBUGCOUT("Node 2 reference frame q:" << std::endl
1240  << p << std::endl);
1241  }
1242  }
1243 
1244  flag fOut = pDM->fReadOutput(HP, Elem::JOINT);
1245 
1246  if (bOffset) {
1247  SAFENEWWITHCONSTRUCTOR(pEl,
1248  InPlaneWithOffsetJoint,
1249  InPlaneWithOffsetJoint(uLabel, pDO,
1250  pNode1, pNode2, v, p, q, fOut));
1251  } else {
1252  SAFENEWWITHCONSTRUCTOR(pEl,
1253  InPlaneJoint,
1254  InPlaneJoint(uLabel, pDO,
1255  pNode1, pNode2, v, p, fOut));
1256  }
1257  std::ostream& out = pDM->GetLogFile();
1258  Vec3 relrot(Vec3(0., 0., 1.).Cross(v));
1259  relrot *= std::asin(relrot.Norm());
1260  out << "inplane: " << uLabel
1261  << " " << pNode1->GetLabel()
1262  << " " << p
1263  << " " << pNode1->GetRCurr().MulTM(RotManip::Rot(relrot))
1264  << " " << pNode2->GetLabel()
1265  << " " << q
1266  << " " << Eye3
1267  << std::endl;
1268 
1269  } break;
1270 
1271  case J_INLINE:
1272  {
1273  /* nodo collegato 1 */
1274  const StructNode* pNode1 = pDM->ReadNode<const StructNode, Node::STRUCTURAL>(HP);
1275 
1276  ReferenceFrame RF(pNode1);
1277  Vec3 p(Zero3);
1278  if (HP.IsKeyWord("position")) {
1279 #ifdef MBDYN_X_COMPATIBLE_INPUT
1280  NO_OP;
1281  } else {
1282  pedantic_cerr("Joint(" << uLabel << "): "
1283  "missing keyword \"position\" at line "
1284  << HP.GetLineData() << std::endl);
1285  }
1286 #endif /* MBDYN_X_COMPATIBLE_INPUT */
1287  p = HP.GetPosRel(RF);
1288 #ifndef MBDYN_X_COMPATIBLE_INPUT
1289  }
1290 #endif /* MBDYN_X_COMPATIBLE_INPUT */
1291 
1292  DEBUGCOUT("Node 1 reference frame p:" << std::endl << p << std::endl);
1293 
1294  Mat3x3 R(Eye3);
1295  if (HP.IsKeyWord("orientation")) {
1296 #ifdef MBDYN_X_COMPATIBLE_INPUT
1297  NO_OP;
1298  } else {
1299  pedantic_cerr("Joint(" << uLabel << "): "
1300  "missing keyword \"orientation\" at line "
1301  << HP.GetLineData() << std::endl);
1302  }
1303 #endif /* MBDYN_X_COMPATIBLE_INPUT */
1304  R = HP.GetRotRel(RF);
1305 #ifndef MBDYN_X_COMPATIBLE_INPUT
1306  }
1307 #endif /* MBDYN_X_COMPATIBLE_INPUT */
1308 
1309 
1310  /* nodo collegato 2 */
1311  const StructNode* pNode2 = pDM->ReadNode<const StructNode, Node::STRUCTURAL>(HP);
1312 
1313  Vec3 q(Zero3);
1314  bool bOffset(false);
1315 
1316  if (HP.IsArg()) {
1317  if (HP.IsKeyWord("offset")) {
1318  bOffset = true;
1319  q = HP.GetPosRel(ReferenceFrame(pNode2), RF, p);
1320  DEBUGCOUT("Node 2 reference frame q:" << std::endl << p << std::endl);
1321  }
1322  }
1323 
1324  flag fOut = pDM->fReadOutput(HP, Elem::JOINT);
1325 
1326  if (bOffset) {
1327  SAFENEWWITHCONSTRUCTOR(pEl,
1328  InLineWithOffsetJoint,
1329  InLineWithOffsetJoint(uLabel, pDO,
1330  pNode1, pNode2, R, p, q, fOut));
1331  } else {
1332  SAFENEWWITHCONSTRUCTOR(pEl,
1333  InLineJoint,
1334  InLineJoint(uLabel, pDO,
1335  pNode1, pNode2, R, p, fOut));
1336  }
1337  std::ostream& out = pDM->GetLogFile();
1338  out << "inline: " << uLabel
1339  << " " << pNode1->GetLabel()
1340  << " " << p
1341  << " " << R
1342  << " " << pNode2->GetLabel()
1343  << " " << q
1344  << " " << Eye3
1345  << std::endl;
1346 
1347  } break;
1348 
1349  /* asta con pin alle estremita' */
1350  case ROD:
1351  {
1352  bIsTorque = false;
1353  bIsPrescribedMotion = false;
1354  bIsRightHandSide = true;
1355 
1356  /*
1357  * lettura dei dati specifici:
1358  * due nodi, lunghezza iniziale e tipo di legame elastico
1359  */
1360 
1361  bool bOffset(false);
1362 
1363  /* nodo collegato 1 */
1364  const StructDispNode* pNode1 = pDM->ReadNode<const StructDispNode, Node::STRUCTURAL>(HP);
1365  const StructNode *pN1 = dynamic_cast<const StructNode *>(pNode1);
1366  DEBUGCOUT("Linked to Node " << pNode1->GetLabel() << std::endl);
1367 
1368  Vec3 f1(Zero3);
1369  ReferenceFrame RF1(pNode1);
1370  if (HP.IsKeyWord("position")) {
1371  if (pN1 == 0) {
1372  silent_cerr("Joint(" << uLabel << "): "
1373  "\"position\" not allowed with rotationless node " << pNode1->GetLabel()
1374  << " at line " << HP.GetLineData() << std::endl);
1375  throw DataManager::ErrGeneric(MBDYN_EXCEPT_ARGS);
1376  }
1377 
1378  f1 = HP.GetPosRel(RF1);
1379  bOffset = true;
1380  }
1381 
1382  /* nodo collegato 2 */
1383  const StructDispNode* pNode2 = pDM->ReadNode<const StructDispNode, Node::STRUCTURAL>(HP);
1384  const StructNode *pN2 = dynamic_cast<const StructNode *>(pNode2);
1385  DEBUGCOUT("Linked to Node " << pNode2->GetLabel() << std::endl);
1386 
1387  Vec3 f2(Zero3);
1388  if (HP.IsKeyWord("position")) {
1389  if (pN1 == 0) {
1390  silent_cerr("Joint(" << uLabel << "): "
1391  "\"position\" not allowed on second node when first node "
1392  << pNode1->GetLabel() << " is rotationless"
1393  << " at line " << HP.GetLineData() << std::endl);
1394  throw DataManager::ErrGeneric(MBDYN_EXCEPT_ARGS);
1395  } else if (pN2 == 0) {
1396  silent_cerr("Joint(" << uLabel << "): "
1397  "\"position\" not allowed with rotationless node " << pNode2->GetLabel()
1398  << " at line " << HP.GetLineData() << std::endl);
1399  throw DataManager::ErrGeneric(MBDYN_EXCEPT_ARGS);
1400  }
1401 
1402  f2 = HP.GetPosRel(ReferenceFrame(pNode2), RF1, f1);
1403  bOffset = true;
1404  }
1405 
1406  /* Lunghezza iniziale */
1407  doublereal dL0 = 0.;
1408  bool bFromNodes(false);
1409  if (HP.IsKeyWord("from" "nodes")) {
1410  bFromNodes = true;
1411  DEBUGCOUT("Initial length will be computed from nodes position" << std::endl);
1412 
1413  } else {
1414  dL0 = HP.GetReal();
1415  DEBUGCOUT("Initial length = " << dL0 << std::endl);
1416  }
1417 
1418  /* Se si tratta di Rod con Offset, legge gli offset e poi passa
1419  * al tipo di legame costitutivo */
1420 #ifdef MBDYN_X_COMPATIBLE_INPUT
1421  if (HP.IsKeyWord("offset")) {
1422  pedantic_cerr("Joint(" << uLabel << "): "
1423  "keyword \"offset\" at line " << HP.GetLineData()
1424  << " is deprecated; use \"position\" instead"
1425  << std::endl);
1426  if (bOffset) {
1427  silent_cerr("Joint(" << uLabel << "): "
1428  "offsets already defined "
1429  "at line " << HP.GetLineData()
1430  << std::endl);
1431  throw DataManager::ErrGeneric(MBDYN_EXCEPT_ARGS);
1432  }
1433 
1434  if (pN1 == 0) {
1435  silent_cerr("Joint(" << uLabel << "): "
1436  "\"offset\" not allowed when first node "
1437  << pNode1->GetLabel() << " is rotationless"
1438  << " at line " << HP.GetLineData() << std::endl);
1439  throw DataManager::ErrGeneric(MBDYN_EXCEPT_ARGS);
1440  } else if (pN2 == 0) {
1441  silent_cerr("Joint(" << uLabel << "): "
1442  "\"offset\" not allowed when second node "
1443  << pNode2->GetLabel() << " is rotationless"
1444  << " at line " << HP.GetLineData() << std::endl);
1445  throw DataManager::ErrGeneric(MBDYN_EXCEPT_ARGS);
1446  }
1447 
1448  bOffset = true;
1449  ReferenceFrame RF1(pNode1);
1450  f1 = HP.GetPosRel(RF1);
1451  DEBUGCOUT("Offset 1: " << f1 << std::endl);
1452 
1453  f2 = HP.GetPosRel(ReferenceFrame(pNode2), RF1, f1);
1454  DEBUGCOUT("Offset 2: " << f2 << std::endl);
1455  }
1456 #endif /* MBDYN_X_COMPATIBLE_INPUT */
1457 
1458  Vec3 v = pNode2->GetXCurr() - pNode1->GetXCurr();
1459  if (bOffset) {
1460  v += pN2->GetRCurr()*f2 - pN1->GetRCurr()*f1;
1461  }
1462  doublereal dL1 = v.Norm();
1463 
1464  if (bFromNodes) {
1465  dL0 = dL1;
1466  pedantic_cout("Rod[WithOffset](" << uLabel << "): "
1467  "length from nodes = " << dL1 << std::endl);
1468  }
1469 
1470  DEBUGCOUT("Initial length = " << dL0 << std::endl);
1471 
1472  /* Legame costitutivo */
1473  ConstLawType::Type CLType = ConstLawType::UNKNOWN;
1474  pDM->PushCurrData("L0", dL0);
1475  pDM->PushCurrData("L", dL1);
1476  ConstitutiveLaw1D* pCL = HP.GetConstLaw1D(CLType);
1477  pDM->PopCurrData("L");
1478  pDM->PopCurrData("L0");
1479 
1480  if (pCL->iGetNumDof() != 0) {
1481  silent_cerr("line " << HP.GetLineData() << ": "
1482  "rod joint does not support "
1483  "dynamic constitutive laws yet"
1484  << std::endl);
1485  throw DataManager::ErrGeneric(MBDYN_EXCEPT_ARGS);
1486  }
1487 
1488  flag fOut = pDM->fReadOutput(HP, Elem::JOINT);
1489 
1490  std::ostream& out = pDM->GetLogFile();
1491  out << "rod: " << uLabel
1492  << " " << pNode1->GetLabel()
1493  << " ", f1.Write(out, " ")
1494  << " " << pNode2->GetLabel()
1495  << " ", f2.Write(out, " ")
1496  << std::endl;
1497 
1498  if (bOffset) {
1499  SAFENEWWITHCONSTRUCTOR(pEl,
1500  RodWithOffset,
1501  RodWithOffset(uLabel, pDO, pCL,
1502  pN1, pN2, f1, f2, dL0, fOut));
1503 
1504  } else {
1505  switch (CLType) {
1506  case ConstLawType::VISCOUS:
1507  case ConstLawType::VISCOELASTIC:
1508  SAFENEWWITHCONSTRUCTOR(pEl,
1509  ViscoElasticRod,
1510  ViscoElasticRod(uLabel, pDO, pCL,
1511  pNode1, pNode2, dL0, fOut));
1512  break;
1513 
1514  default:
1515  SAFENEWWITHCONSTRUCTOR(pEl,
1516  Rod,
1517  Rod(uLabel, pDO, pCL,
1518  pNode1, pNode2, dL0, fOut));
1519  break;
1520  }
1521  }
1522 
1523  } break;
1524 
1525  case RODWITHOFFSET:
1526  {
1527  bIsTorque = false;
1528  bIsPrescribedMotion = false;
1529  bIsRightHandSide = true;
1530 
1531  /*
1532  * lettura dei dati specifici:
1533  * due nodi, lunghezza iniziale e tipo di legame elastico.
1534  * Nota: prima gli offset erano un optional dei dati del rod normale;
1535  * questo e' lo stesso rod with offset, ma con un input piu' simile
1536  * a quello standard.
1537  */
1538 
1539 #ifdef MBDYN_X_COMPATIBLE_INPUT
1540  pedantic_cerr("Joint(" << uLabel << "): \"rod with offset\" "
1541  "is deprecated; use \"rod\" instead" << std::endl);
1542 #endif /* MBDYN_X_COMPATIBLE_INPUT */
1543 
1544  /* nodo collegato 1 */
1545  const StructNode* pNode1 = pDM->ReadNode<const StructNode, Node::STRUCTURAL>(HP);
1546 
1547  Vec3 f1(Zero3);
1548  ReferenceFrame RF1(pNode1);
1549  if (HP.IsKeyWord("position")) {
1550 #ifdef MBDYN_X_COMPATIBLE_INPUT
1551  NO_OP;
1552  } else {
1553  pedantic_cerr("Joint(" << uLabel << "): "
1554  "missing keyword \"position\" at line "
1555  << HP.GetLineData() << std::endl);
1556  }
1557 #endif /* MBDYN_X_COMPATIBLE_INPUT */
1558  f1 = HP.GetPosRel(RF1);
1559 #ifndef MBDYN_X_COMPATIBLE_INPUT
1560  }
1561 #endif /* MBDYN_X_COMPATIBLE_INPUT */
1562 
1563  DEBUGCOUT("Linked to Node " << pNode1->GetLabel()
1564  << "with offset " << f1 << std::endl);
1565 
1566 
1567  /* nodo collegato 2 */
1568  const StructNode* pNode2 = pDM->ReadNode<const StructNode, Node::STRUCTURAL>(HP);
1569 
1570  Vec3 f2(Zero3);
1571  if (HP.IsKeyWord("position")) {
1572 #ifdef MBDYN_X_COMPATIBLE_INPUT
1573  NO_OP;
1574  } else {
1575  pedantic_cerr("Joint(" << uLabel << "): "
1576  "missing keyword \"position\" at line "
1577  << HP.GetLineData() << std::endl);
1578  }
1579 #endif /* MBDYN_X_COMPATIBLE_INPUT */
1580  f2 = HP.GetPosRel(ReferenceFrame(pNode2), RF1, f1);
1581 #ifndef MBDYN_X_COMPATIBLE_INPUT
1582  }
1583 #endif /* MBDYN_X_COMPATIBLE_INPUT */
1584 
1585  DEBUGCOUT("Linked to Node " << pNode2->GetLabel()
1586  << "with offset " << f2 << std::endl);
1587 
1588  doublereal dL1 = (pNode2->GetXCurr()
1589  + pNode2->GetRCurr()*f2
1590  - pNode1->GetXCurr()
1591  - pNode1->GetRCurr()*f1).Norm();
1592 
1593  pedantic_cout("RodWithOffset(" << uLabel << "): "
1594  "length from nodes = " << dL1 << std::endl);
1595 
1596  /* Lunghezza iniziale */
1597  doublereal dL0 = 0.;
1598  if (HP.IsKeyWord("from" "nodes")) {
1599  dL0 = dL1;
1600 
1601  } else {
1602  dL0 = HP.GetReal();
1603  }
1604 
1605  DEBUGCOUT("Initial length = " << dL0 << std::endl);
1606 
1607  /* Legame costitutivo */
1608  ConstLawType::Type CLType = ConstLawType::UNKNOWN;
1609  pDM->PushCurrData("L0", dL0);
1610  pDM->PushCurrData("L", dL1);
1611  ConstitutiveLaw1D* pCL = HP.GetConstLaw1D(CLType);
1612  pDM->PopCurrData("L");
1613  pDM->PopCurrData("L0");
1614 
1615  if (pCL->iGetNumDof() != 0) {
1616  silent_cerr("line " << HP.GetLineData() << ": "
1617  "\"rod with offset\" joint does not support "
1618  "dynamic constitutive laws yet"
1619  << std::endl);
1620  throw DataManager::ErrGeneric(MBDYN_EXCEPT_ARGS);
1621  }
1622 
1623  flag fOut = pDM->fReadOutput(HP, Elem::JOINT);
1624 
1625  std::ostream& out = pDM->GetLogFile();
1626  out << "rod: " << uLabel
1627  << " " << pNode1->GetLabel()
1628  << " ", f1.Write(out, " ")
1629  << " " << pNode2->GetLabel()
1630  << " ", f2.Write(out, " ")
1631  << std::endl;
1632 
1633  SAFENEWWITHCONSTRUCTOR(pEl,
1634  RodWithOffset,
1635  RodWithOffset(uLabel, pDO, pCL,
1636  pNode1, pNode2, f1, f2, dL0, fOut));
1637 
1638  } break;
1639 
1640  case RODBEZIER:
1641  {
1642  bIsTorque = false;
1643  bIsPrescribedMotion = false;
1644  bIsRightHandSide = true;
1645 
1646  /* first node */
1647  const StructNode* pNode1 = pDM->ReadNode<const StructNode, Node::STRUCTURAL>(HP);
1648 
1649  Vec3 fO(Zero3);
1650  Vec3 fA(Zero3);
1651  ReferenceFrame RF1(pNode1);
1652 
1653  /* fist offset fO (origin) */
1654  if (HP.IsKeyWord("position")) {
1655 #ifdef MBDYN_X_COMPATIBLE_INPUT
1656  NO_OP;
1657  } else {
1658  pedantic_cerr("Joint(" << uLabel << "): "
1659  "missing keyword \"position\" at line "
1660  << HP.GetLineData() << std::endl);
1661  }
1662 #endif /* MBDYN_X_COMPATIBLE_INPUT */
1663  fO = HP.GetPosRel(RF1);
1664 #ifndef MBDYN_X_COMPATIBLE_INPUT
1665  }
1666 #endif /* MBDYN_X_COMPATIBLE_INPUT */
1667 
1668  /* second offset fA (second control point) */
1669  if (HP.IsKeyWord("position")) {
1670 #ifdef MBDYN_X_COMPATIBLE_INPUT
1671  NO_OP;
1672  } else {
1673  pedantic_cerr("Joint(" << uLabel << "): "
1674  "missing keyword \"position\" at line "
1675  << HP.GetLineData() << std::endl);
1676  }
1677 #endif /* MBDYN_X_COMPATIBLE_INPUT */
1678  fA = HP.GetPosRel(RF1);
1679 #ifndef MBDYN_X_COMPATIBLE_INPUT
1680  }
1681 #endif /* MBDYN_X_COMPATIBLE_INPUT */
1682 
1683  DEBUGCOUT("Linked to Node " << pNode1->GetLabel()
1684  << "with offset O " << fO
1685  << " and offset A " << fA << std::endl);
1686 
1687  /* second node */
1688  const StructNode* pNode2 = pDM->ReadNode<const StructNode, Node::STRUCTURAL>(HP);
1689  Vec3 fB(Zero3);
1690  Vec3 fI(Zero3);
1691 
1692  /* first of second node offset fB (third control point) */
1693  if (HP.IsKeyWord("position")) {
1694 #ifdef MBDYN_X_COMPATIBLE_INPUT
1695  NO_OP;
1696  } else {
1697  pedantic_cerr("Joint(" << uLabel << "): "
1698  "missing keyword \"position\" at line "
1699  << HP.GetLineData() << std::endl);
1700  }
1701 #endif /* MBDYN_X_COMPATIBLE_INPUT */
1702  // perchè non fB = HP.GetPosRel(RF2)?
1703  fB = HP.GetPosRel(ReferenceFrame(pNode2), RF1, fO);
1704 #ifndef MBDYN_X_COMPATIBLE_INPUT
1705  }
1706 #endif /* MBDYN_X_COMPATIBLE_INPUT */
1707 
1708  /* second offset of second node fI (insertion) */
1709  if (HP.IsKeyWord("position")) {
1710 #ifdef MBDYN_X_COMPATIBLE_INPUT
1711  NO_OP;
1712  } else {
1713  pedantic_cerr("Joint(" << uLabel << "): "
1714  "missing keyword \"position\" at line "
1715  << HP.GetLineData() << std::endl);
1716  }
1717 #endif /* MBDYN_X_COMPATIBLE_INPUT */
1718  // idem come sopra: perchè non fI = HP.GetPosRel(RF2)?
1719  fI = HP.GetPosRel(ReferenceFrame(pNode2), RF1, fO);
1720 #ifndef MBDYN_X_COMPATIBLE_INPUT
1721  }
1722 #endif /* MBDYN_X_COMPATIBLE_INPUT */
1723 
1724  bool bFromNodes = false;
1725  doublereal dL0 = 0.;
1726  if (HP.IsKeyWord("from" "nodes")) {
1727  bFromNodes = true;
1728  DEBUGCOUT("Initial length computed from nodes " << std::endl);
1729  } else {
1730  dL0 = HP.GetReal();
1731  DEBUGCOUT("Initial length = " << dL0 << std::endl);
1732  }
1733 
1734  int iIntOrder = 2;
1735  if (HP.IsKeyWord("integration" "order")) {
1736  iIntOrder = HP.GetInt();
1737  if (iIntOrder < 1) {
1738  pedantic_cerr("Joint(" << uLabel << "): "
1739  "integration order cannot be less than 1. "
1740  << std::endl);
1741  iIntOrder = 1;
1742  } else if (iIntOrder > 10) {
1743  pedantic_cerr("Joint(" << uLabel << "): "
1744  "maximum supported integration order is 10."
1745  << std::endl);
1746  }
1747  }
1748 
1749  int iIntSegments = 3;
1750  if (HP.IsKeyWord("integration" "segments")) {
1751  iIntSegments = HP.GetInt();
1752  if (iIntSegments < 1) {
1753  pedantic_cerr("Joint (" << uLabel << "): "
1754  "integration segments cannot be less than 1."
1755  << std::endl);
1756  }
1757  }
1758 
1759  DEBUGCOUT("Joint(" << uLabel << "): Linked to Node "
1760  << pNode2->GetLabel()
1761  << " with offset B " << fB
1762  << " and offset I " << fI
1763  << ". Integration order " << iIntOrder
1764  << " on " << iIntSegments << " segments"
1765  << std::endl);
1766 
1767  /* constitutive law */
1768  ConstLawType::Type CLType = ConstLawType::UNKNOWN;
1769  ConstitutiveLaw1D* pCL = HP.GetConstLaw1D(CLType);
1770 
1771  if (pCL->iGetNumDof() != 0) {
1772  silent_cerr("line " << HP.GetLineData() << ": "
1773  "rod bezier joint does not support "
1774  "dynamic constitutive laws yet"
1775  << std::endl);
1776  throw DataManager:: ErrGeneric(MBDYN_EXCEPT_ARGS);
1777  }
1778 
1779  flag fOut = pDM->fReadOutput(HP, Elem::JOINT);
1780 
1781  std::ostream& out = pDM->GetLogFile();
1782  out << "rod bezier: " << uLabel
1783  << " " << pNode1->GetLabel()
1784  << " ", fO.Write(out, " ")
1785  << " ", fA.Write(out, " ")
1786  << " " << pNode2->GetLabel()
1787  << " ", fB.Write(out, " ")
1788  << " ", fI.Write(out, " ")
1789  << std::endl;
1790 
1791  SAFENEWWITHCONSTRUCTOR(pEl,
1792  RodBezier,
1793  RodBezier(uLabel, pDO, pCL,
1794  pNode1, pNode2, fO, fA, fB, fI,
1795  dL0, bFromNodes,
1796  iIntOrder, iIntSegments, fOut));
1797  } break;
1798  case DEFORMABLEHINGE:
1799  case DEFORMABLEDISPHINGE: // deprecated
1800  case DEFORMABLEDISPJOINT:
1801  case DEFORMABLEAXIALJOINT:
1802  case INVARIANTDEFORMABLEHINGE:
1803  case INVARIANTDEFORMABLEDISPJOINT:
1804  {
1805  bIsTorque = false;
1806  bIsPrescribedMotion = false;
1807  bIsRightHandSide = true;
1808 
1809  /* lettura dei dati specifici */
1810 
1811  /* nodo collegato 1 */
1812  const StructNode* pNode1 = pDM->ReadNode<const StructNode, Node::STRUCTURAL>(HP);
1813 
1814  ReferenceFrame RF1(pNode1);
1815 
1816  /* Offset if displacement hinge */
1817  Vec3 f1(Zero3);
1818  switch (CurrKeyWord) {
1819  case DEFORMABLEDISPHINGE: // deprecated
1820  case DEFORMABLEDISPJOINT:
1821  case INVARIANTDEFORMABLEDISPJOINT:
1822  if (HP.IsKeyWord("position")) {
1823 #ifdef MBDYN_X_COMPATIBLE_INPUT
1824  NO_OP;
1825  } else {
1826  pedantic_cerr("Joint(" << uLabel << "): "
1827  "missing keyword \"position\" at line "
1828  << HP.GetLineData() << std::endl);
1829  }
1830 #endif /* MBDYN_X_COMPATIBLE_INPUT */
1831  f1 = HP.GetPosRel(RF1);
1832 #ifndef MBDYN_X_COMPATIBLE_INPUT
1833  }
1834 #endif /* MBDYN_X_COMPATIBLE_INPUT */
1835  break;
1836 
1837  case DEFORMABLEHINGE:
1838  case DEFORMABLEAXIALJOINT:
1839  case INVARIANTDEFORMABLEHINGE:
1840  if (HP.IsKeyWord("position")) {
1841  // ignored
1842  f1 = HP.GetPosRel(RF1);
1843  }
1844  break;
1845  default:
1846  break;
1847  }
1848 
1849  Mat3x3 R1(Eye3);
1850  if (HP.IsKeyWord("orientation")) {
1851  DEBUGCOUT("Hinge orientation matrix is supplied" << std::endl);
1852  R1 = HP.GetRotRel(RF1);
1853 #ifdef MBDYN_X_COMPATIBLE_INPUT
1854  } else if (HP.IsKeyWord("hinge")) {
1855  pedantic_cerr("Joint(" << uLabel << "): "
1856  "keyword \"hinge\" at line " << HP.GetLineData()
1857  << " is deprecated; use \"orientation\" instead"
1858  << std::endl);
1859  DEBUGCOUT("Hinge orientation matrix is supplied" << std::endl);
1860  R1 = HP.GetRotRel(RF1);
1861 #endif /* MBDYN_X_COMPATIBLE_INPUT */
1862  }
1863 
1864 
1865  /* nodo collegato 2 */
1866  const StructNode* pNode2 = pDM->ReadNode<const StructNode, Node::STRUCTURAL>(HP);
1867 
1868  ReferenceFrame RF2(pNode2);
1869 
1870  /* Offset if displacement hinge */
1871  Vec3 f2(Zero3);
1872  switch (CurrKeyWord) {
1873  case DEFORMABLEDISPHINGE: // deprecated
1874  case DEFORMABLEDISPJOINT:
1875  case INVARIANTDEFORMABLEDISPJOINT:
1876  if (HP.IsKeyWord("position")) {
1877 #ifdef MBDYN_X_COMPATIBLE_INPUT
1878  NO_OP;
1879  } else {
1880  pedantic_cerr("Joint(" << uLabel << "): "
1881  "missing keyword \"position\" at line "
1882  << HP.GetLineData() << std::endl);
1883  }
1884 #endif /* MBDYN_X_COMPATIBLE_INPUT */
1885  f2 = HP.GetPosRel(RF2, RF1, f1);
1886 #ifndef MBDYN_X_COMPATIBLE_INPUT
1887  }
1888 #endif /* MBDYN_X_COMPATIBLE_INPUT */
1889  break;
1890 
1891  case DEFORMABLEHINGE:
1892  case DEFORMABLEAXIALJOINT:
1893  case INVARIANTDEFORMABLEHINGE:
1894  if (HP.IsKeyWord("position")) {
1895  // ignored
1896  f2 = HP.GetPosRel(RF2, RF1, f1);
1897  }
1898  break;
1899  default:
1900  break;
1901  }
1902 
1903  Mat3x3 R2(Eye3);
1904  if (HP.IsKeyWord("orientation")) {
1905  DEBUGCOUT("Hinge orientation matrix is supplied" << std::endl);
1906  R2 = HP.GetRotRel(RF2, RF1, R1);
1907 #ifdef MBDYN_X_COMPATIBLE_INPUT
1908  } else if (HP.IsKeyWord("hinge")) {
1909  pedantic_cerr("Joint(" << uLabel << "): "
1910  "keyword \"hinge\" at line " << HP.GetLineData()
1911  << " is deprecated; use \"orientation\" instead"
1912  << std::endl);
1913  DEBUGCOUT("Hinge orientation matrix is supplied" << std::endl);
1914  R2 = HP.GetRotRel(RF2, RF1, R1);
1915 #endif /* MBDYN_X_COMPATIBLE_INPUT */
1916  }
1917 
1918 
1919  /* Legame costitutivo */
1920  ConstLawType::Type CLType;
1921  ConstitutiveLaw1D* pCL1 = 0;
1922  ConstitutiveLaw3D* pCL3 = 0;
1923  unsigned iCLNumDof = 0;
1924  switch (CurrKeyWord) {
1925  case DEFORMABLEAXIALJOINT:
1926  pCL1 = HP.GetConstLaw1D(CLType);
1927  iCLNumDof = pCL1->iGetNumDof();
1928  break;
1929 
1930  default:
1931  pCL3 = HP.GetConstLaw3D(CLType);
1932  iCLNumDof = pCL3->iGetNumDof();
1933  break;
1934  }
1935 
1936  if (iCLNumDof != 0) {
1937  silent_cerr("line " << HP.GetLineData() << ": "
1938  "generic deformable joint does not support "
1939  "constitutive laws with internal degrees of freedom yet"
1940  << std::endl);
1941  throw DataManager::ErrGeneric(MBDYN_EXCEPT_ARGS);
1942  }
1943 
1944  OrientationDescription od = UNKNOWN_ORIENTATION_DESCRIPTION;
1945  switch (CurrKeyWord) {
1946  case DEFORMABLEHINGE:
1947  case INVARIANTDEFORMABLEHINGE:
1948  od = ReadOptionalOrientationDescription(pDM, HP);
1949  break;
1950 
1951  default:
1952  break;
1953  }
1954 
1955  flag fOut = pDM->fReadOutput(HP, Elem::JOINT);
1956 
1957  const char *sJointLogName = 0;
1958 
1959  switch (CLType) {
1960  case ConstLawType::ELASTIC:
1961  switch (CurrKeyWord) {
1962  case DEFORMABLEHINGE:
1963  SAFENEWWITHCONSTRUCTOR(pEl,
1964  ElasticHingeJoint,
1965  ElasticHingeJoint(uLabel, pDO, pCL3,
1966  pNode1, pNode2,
1967  R1, R2, od, fOut));
1968  sJointLogName = "deformablehinge";
1969  break;
1970 
1971  case INVARIANTDEFORMABLEHINGE:
1972  SAFENEWWITHCONSTRUCTOR(pEl,
1973  ElasticHingeJointInv,
1974  ElasticHingeJointInv(uLabel, pDO, pCL3,
1975  pNode1, pNode2,
1976  R1, R2, od, fOut));
1977  sJointLogName = "deformablehinge";
1978  break;
1979 
1980  case DEFORMABLEDISPHINGE: // deprecated
1981  case DEFORMABLEDISPJOINT:
1982  SAFENEWWITHCONSTRUCTOR(pEl,
1983  ElasticDispJoint,
1984  ElasticDispJoint(uLabel, pDO, pCL3,
1985  pNode1, pNode2,
1986  f1, f2, R1, R2, fOut));
1987 
1988  sJointLogName = "deformabledisplacementjoint";
1989  break;
1990 
1991  case INVARIANTDEFORMABLEDISPJOINT:
1992 #if 0
1993  silent_cerr("\"invariant deformable displacement joint\" "
1994  "at line " << HP.GetLineData()
1995  << " not implemented yet" << std::endl);
1996  throw DataManager::ErrGeneric(MBDYN_EXCEPT_ARGS);
1997 #endif
1998 #if 1
1999  SAFENEWWITHCONSTRUCTOR(pEl,
2000  ElasticDispJointInv,
2001  ElasticDispJointInv(uLabel, pDO, pCL3,
2002  pNode1, pNode2,
2003  f1, f2, R1, R2, fOut));
2004 
2005  sJointLogName = "deformabledisplacementjoint";
2006 #endif
2007  break;
2008 
2009  case DEFORMABLEAXIALJOINT:
2010  SAFENEWWITHCONSTRUCTOR(pEl,
2011  ElasticAxialJoint,
2012  ElasticAxialJoint(uLabel, pDO, pCL1,
2013  pNode1, pNode2,
2014  R1, R2, fOut));
2015  sJointLogName = "deformableaxialjoint";
2016  break;
2017 
2018  default:
2019  ASSERTMSG(0, "You shouldn't have reached this point");
2020  throw DataManager::ErrGeneric(MBDYN_EXCEPT_ARGS);
2021  break;
2022  }
2023  break;
2024 
2025  case ConstLawType::VISCOUS:
2026  switch (CurrKeyWord) {
2027  case DEFORMABLEHINGE:
2028  SAFENEWWITHCONSTRUCTOR(pEl,
2029  ViscousHingeJoint,
2030  ViscousHingeJoint(uLabel, pDO, pCL3,
2031  pNode1, pNode2,
2032  R1, R2, od, fOut));
2033  sJointLogName = "deformablehinge";
2034  break;
2035 
2036  case INVARIANTDEFORMABLEHINGE:
2037  SAFENEWWITHCONSTRUCTOR(pEl,
2038  ViscousHingeJointInv,
2039  ViscousHingeJointInv(uLabel, pDO, pCL3,
2040  pNode1, pNode2,
2041  R1, R2, od, fOut));
2042  sJointLogName = "deformablehinge";
2043  break;
2044 
2045  case DEFORMABLEDISPHINGE: // deprecated
2046  case DEFORMABLEDISPJOINT:
2047  SAFENEWWITHCONSTRUCTOR(pEl,
2048  ViscousDispJoint,
2049  ViscousDispJoint(uLabel, pDO, pCL3,
2050  pNode1, pNode2,
2051  f1, f2, R1, R2, fOut));
2052  sJointLogName = "deformabledisplacementjoint";
2053  break;
2054 
2055  case INVARIANTDEFORMABLEDISPJOINT:
2056  silent_cerr("\"invariant deformable displacement joint\" "
2057  "at line " << HP.GetLineData()
2058  << " not implemented yet" << std::endl);
2059  throw DataManager::ErrGeneric(MBDYN_EXCEPT_ARGS);
2060 #if 0
2061  SAFENEWWITHCONSTRUCTOR(pEl,
2062  ViscousDispJoint,
2063  ViscousDispJoint(uLabel, pDO, pCL3,
2064  pNode1, pNode2,
2065  f1, f2, R1, R2, fOut));
2066  sJointLogName = "deformabledisplacementjoint";
2067 #endif
2068  break;
2069 
2070  case DEFORMABLEAXIALJOINT:
2071  SAFENEWWITHCONSTRUCTOR(pEl,
2072  ViscousAxialJoint,
2073  ViscousAxialJoint(uLabel, pDO, pCL1,
2074  pNode1, pNode2,
2075  R1, R2, fOut));
2076  sJointLogName = "deformableaxialjoint";
2077  break;
2078 
2079  default:
2080  ASSERTMSG(0, "You shouldn't have reached this point");
2081  throw DataManager::ErrGeneric(MBDYN_EXCEPT_ARGS);
2082  break;
2083  }
2084  break;
2085 
2086  case ConstLawType::VISCOELASTIC:
2087  switch (CurrKeyWord) {
2088  case DEFORMABLEHINGE:
2089  SAFENEWWITHCONSTRUCTOR(pEl,
2090  ViscoElasticHingeJoint,
2091  ViscoElasticHingeJoint(uLabel, pDO, pCL3,
2092  pNode1, pNode2,
2093  R1, R2, od, fOut));
2094  sJointLogName = "deformablehinge";
2095  break;
2096 
2097  case INVARIANTDEFORMABLEHINGE:
2098  SAFENEWWITHCONSTRUCTOR(pEl,
2099  ViscoElasticHingeJointInv,
2100  ViscoElasticHingeJointInv(uLabel, pDO, pCL3,
2101  pNode1, pNode2,
2102  R1, R2, od, fOut));
2103  sJointLogName = "deformablehinge";
2104  break;
2105 
2106  case DEFORMABLEDISPHINGE: // deprecated
2107  case DEFORMABLEDISPJOINT:
2108  SAFENEWWITHCONSTRUCTOR(pEl,
2109  ViscoElasticDispJoint,
2110  ViscoElasticDispJoint(uLabel,
2111  pDO, pCL3,
2112  pNode1, pNode2,
2113  f1, f2, R1, R2, fOut));
2114  sJointLogName = "deformabledisplacementjoint";
2115  break;
2116 
2117  case INVARIANTDEFORMABLEDISPJOINT:
2118  silent_cerr("\"invariant deformable displacement joint\" "
2119  "at line " << HP.GetLineData()
2120  << " not implemented yet" << std::endl);
2121  throw DataManager::ErrGeneric(MBDYN_EXCEPT_ARGS);
2122 #if 0
2123  SAFENEWWITHCONSTRUCTOR(pEl,
2124  ViscoElasticDispJointInv,
2125  ViscoElasticDispJointInv(uLabel,
2126  pDO, pCL3,
2127  pNode1, pNode2,
2128  f1, f2, R1, R2, fOut));
2129  sJointLogName = "deformabledisplacementjoint";
2130 #endif
2131  break;
2132 
2133  case DEFORMABLEAXIALJOINT:
2134  SAFENEWWITHCONSTRUCTOR(pEl,
2135  ViscoElasticAxialJoint,
2136  ViscoElasticAxialJoint(uLabel, pDO, pCL1,
2137  pNode1, pNode2,
2138  R1, R2, fOut));
2139  sJointLogName = "deformableaxialjoint";
2140  break;
2141 
2142  default:
2143  ASSERTMSG(0, "You shouldn't have reached this point");
2144  throw DataManager::ErrGeneric(MBDYN_EXCEPT_ARGS);
2145  break;
2146  }
2147  break;
2148 
2149  default:
2150  ASSERTMSG(0, "You shouldn't have reached this point");
2151  throw DataManager::ErrGeneric(MBDYN_EXCEPT_ARGS);
2152  }
2153 
2154  std::ostream& out = pDM->GetLogFile();
2155  out << sJointLogName << ": " << uLabel
2156  << " " << pNode1->GetLabel()
2157  << " " << f1
2158  << " " << R1
2159  << " " << pNode2->GetLabel()
2160  << " " << f2
2161  << " " << R2
2162  << std::endl;
2163  } break;
2164 
2165  case DEFORMABLEJOINT:
2166  case INVARIANTDEFORMABLEJOINT:
2167  {
2168  bIsTorque = false;
2169  bIsPrescribedMotion = false;
2170  bIsRightHandSide = true;
2171 
2172  /* lettura dei dati specifici */
2173 
2174  /* nodo collegato 1 */
2175  const StructNode* pNode1 = pDM->ReadNode<const StructNode, Node::STRUCTURAL>(HP);
2176 
2177  /* Offset if displacement hinge */
2178  ReferenceFrame RF1(pNode1);
2179  if (HP.IsKeyWord("position")) {
2180  NO_OP; // ignore it by now
2181  }
2182  Vec3 f1(HP.GetPosRel(RF1));
2183 
2184  Mat3x3 R1(Eye3);
2185  if (HP.IsKeyWord("hinge") || HP.IsKeyWord("orientation")) {
2186  R1 = HP.GetRotRel(RF1);
2187  }
2188 
2189  /* nodo collegato 2 */
2190  const StructNode* pNode2 = pDM->ReadNode<const StructNode, Node::STRUCTURAL>(HP);
2191 
2192  /* Offset */
2193  ReferenceFrame RF2(pNode2);
2194  if (HP.IsKeyWord("position")) {
2195  NO_OP; // ignore it by now
2196  }
2197  Vec3 f2(HP.GetPosRel(RF2, RF1, f1));
2198 
2199  Mat3x3 R2(Eye3);
2200  if (HP.IsKeyWord("hinge") || HP.IsKeyWord("orientation")) {
2201  R2 = HP.GetRotRel(RF2, RF1, R1);
2202  }
2203 
2204  /* Legame costitutivo */
2205  ConstLawType::Type CLType;
2206  ConstitutiveLaw6D* pCL = HP.GetConstLaw6D(CLType);
2207 
2208  if (pCL->iGetNumDof() != 0) {
2209  silent_cerr("line " << HP.GetLineData() << ": "
2210  "\"deformable joint\" does not support "
2211  "dynamic constitutive laws yet"
2212  << std::endl);
2213  throw DataManager::ErrGeneric(MBDYN_EXCEPT_ARGS);
2214  }
2215 
2216  OrientationDescription od = ReadOptionalOrientationDescription(pDM, HP);
2217  flag fOut = pDM->fReadOutput(HP, Elem::JOINT);
2218 
2219  switch (CLType) {
2220  case ConstLawType::ELASTIC:
2221  switch (CurrKeyWord) {
2222  case DEFORMABLEJOINT:
2223  SAFENEWWITHCONSTRUCTOR(pEl,
2224  ElasticJoint,
2225  ElasticJoint(uLabel, pDO, pCL,
2226  pNode1, pNode2,
2227  f1, f2, R1, R2, od, fOut));
2228  break;
2229 
2230  case INVARIANTDEFORMABLEJOINT:
2231  SAFENEWWITHCONSTRUCTOR(pEl,
2232  ElasticJointInv,
2233  ElasticJointInv(uLabel, pDO, pCL,
2234  pNode1, pNode2,
2235  f1, f2, R1, R2, od, fOut));
2236  break;
2237 
2238  default:
2239  ASSERTMSG(0, "You shouldn't have reached this point");
2240  throw DataManager::ErrGeneric(MBDYN_EXCEPT_ARGS);
2241  break;
2242  }
2243 
2244  break;
2245 
2246  case ConstLawType::VISCOUS:
2247  switch (CurrKeyWord) {
2248  case DEFORMABLEJOINT:
2249  SAFENEWWITHCONSTRUCTOR(pEl,
2250  ViscousJoint,
2251  ViscousJoint(uLabel, pDO, pCL,
2252  pNode1, pNode2,
2253  f1, f2, R1, R2, od, fOut));
2254  break;
2255 
2256  case INVARIANTDEFORMABLEJOINT:
2257  silent_cerr("\"invariant deformable joint\" "
2258  "with support for viscous constitutive law "
2259  "at line " << HP.GetLineData()
2260  << " not implemented yet" << std::endl);
2261  throw DataManager::ErrGeneric(MBDYN_EXCEPT_ARGS);
2262 #if 0
2263  SAFENEWWITHCONSTRUCTOR(pEl,
2264  ViscousJointInv,
2265  ViscousJointInv(uLabel, pDO, pCL,
2266  pNode1, pNode2,
2267  f1, f2, R1, R2, od, fOut));
2268 #endif
2269  break;
2270 
2271  default:
2272  ASSERTMSG(0, "You shouldn't have reached this point");
2273  throw DataManager::ErrGeneric(MBDYN_EXCEPT_ARGS);
2274  break;
2275  }
2276 
2277  break;
2278 
2279  case ConstLawType::VISCOELASTIC:
2280  switch (CurrKeyWord) {
2281  case DEFORMABLEJOINT:
2282  SAFENEWWITHCONSTRUCTOR(pEl,
2283  ViscoElasticJoint,
2284  ViscoElasticJoint(uLabel, pDO, pCL,
2285  pNode1, pNode2,
2286  f1, f2, R1, R2, od, fOut));
2287  break;
2288 
2289  case INVARIANTDEFORMABLEJOINT:
2290  silent_cerr("\"invariant deformable joint\" "
2291  "with support for viscoelastic constitutive law "
2292  "at line " << HP.GetLineData()
2293  << " not implemented yet" << std::endl);
2294  throw DataManager::ErrGeneric(MBDYN_EXCEPT_ARGS);
2295 #if 0
2296  SAFENEWWITHCONSTRUCTOR(pEl,
2297  ViscoElasticJointInv,
2298  ViscoElasticJointInv(uLabel, pDO, pCL,
2299  pNode1, pNode2,
2300  f1, f2, R1, R2, od, fOut));
2301 #endif
2302  break;
2303 
2304  default:
2305  ASSERTMSG(0, "You shouldn't have reached this point");
2306  throw DataManager::ErrGeneric(MBDYN_EXCEPT_ARGS);
2307  break;
2308  }
2309 
2310  break;
2311 
2312  default:
2313  ASSERTMSG(0, "You shouldn't have reached this point");
2314  silent_cerr("\"deformable joint\" "
2315  "at line " << HP.GetLineData()
2316  << " not implemented with the requested "
2317  "constitutive law type" << std::endl);
2318  throw DataManager::ErrGeneric(MBDYN_EXCEPT_ARGS);
2319  }
2320 
2321  std::ostream& out = pDM->GetLogFile();
2322  out << "deformablejoint: " << uLabel
2323  << " " << pNode1->GetLabel()
2324  << " " << f1
2325  << " " << R1
2326  << " " << pNode2->GetLabel()
2327  << " " << f2
2328  << " " << R2
2329  << std::endl;
2330 
2331  } break;
2332 
2333  case VISCOUSBODY:
2334  {
2335  bIsTorque = false;
2336  bIsPrescribedMotion = false;
2337  bIsRightHandSide = true;
2338 
2339  /* lettura dei dati specifici */
2340 
2341  /* nodo collegato 1 */
2342  const StructNode* pNode = pDM->ReadNode<const StructNode, Node::STRUCTURAL>(HP);
2343 
2344  /* Offset if displacement hinge */
2345  ReferenceFrame RF(pNode);
2346  if (HP.IsKeyWord("position")) {
2347  NO_OP; // ignore it by now
2348  }
2349  Vec3 f(HP.GetPosRel(RF));
2350 
2351  Mat3x3 R(Eye3);
2352  if (HP.IsKeyWord("hinge") || HP.IsKeyWord("orientation")) {
2353  R = HP.GetRotRel(RF);
2354  }
2355 
2356  /* Legame costitutivo */
2357  ConstLawType::Type CLType = ConstLawType::VISCOUS;
2358  ConstitutiveLaw6D* pCL = HP.GetConstLaw6D(CLType);
2359 
2360  if (pCL->iGetNumDof() != 0) {
2361  silent_cerr("line " << HP.GetLineData() << ": "
2362  "\"viscous body\" does not support "
2363  "dynamic constitutive laws yet"
2364  << std::endl);
2365  throw DataManager::ErrGeneric(MBDYN_EXCEPT_ARGS);
2366  }
2367 
2368  OrientationDescription od = ReadOptionalOrientationDescription(pDM, HP);
2369  flag fOut = pDM->fReadOutput(HP, Elem::JOINT);
2370 
2371  switch (CLType) {
2372  case ConstLawType::ELASTIC:
2373  case ConstLawType::VISCOELASTIC:
2374  silent_cerr("\"viscous body\" "
2375  "needs viscous constitutive law "
2376  "at line " << HP.GetLineData() << std::endl);
2377  throw DataManager::ErrGeneric(MBDYN_EXCEPT_ARGS);
2378 
2379  case ConstLawType::VISCOUS:
2380  SAFENEWWITHCONSTRUCTOR(pEl,
2381  ViscousBody,
2382  ViscousBody(uLabel, pDO, pCL,
2383  pNode, f, R, od, fOut));
2384  break;
2385 
2386  default:
2387  ASSERTMSG(0, "You shouldn't have reached this point");
2388  silent_cerr("\"viscous body\" "
2389  "at line " << HP.GetLineData()
2390  << " not implemented with the requested "
2391  "constitutive law type" << std::endl);
2392  throw DataManager::ErrGeneric(MBDYN_EXCEPT_ARGS);
2393  }
2394 
2395  std::ostream& out = pDM->GetLogFile();
2396  out << "viscousbody: " << uLabel
2397  << " " << pNode->GetLabel()
2398  << " " << f
2399  << " " << R
2400  << std::endl;
2401 
2402  } break;
2403 
2404  case LINEARVELOCITY:
2405  case ANGULARVELOCITY:
2406  {
2407  /* nodo collegato */
2408  const StructNode* pNode = pDM->ReadNode<const StructNode, Node::STRUCTURAL>(HP);
2409 
2410  Vec3 Dir;
2411  try {
2412  Dir = HP.GetUnitVecRel(ReferenceFrame(pNode));
2413  } catch (ErrNullNorm) {
2414  silent_cerr("linear/angular velocity direction is null" << std::endl);
2415  throw ErrNullNorm(MBDYN_EXCEPT_ARGS);
2416  }
2417 
2418  DriveCaller* pDC = HP.GetDriveCaller();
2419 
2420  flag fOut = pDM->fReadOutput(HP, Elem::JOINT);
2421 
2422  std::ostream& out = pDM->GetLogFile();
2423 
2424  switch (CurrKeyWord) {
2425  case LINEARVELOCITY:
2426  SAFENEWWITHCONSTRUCTOR(pEl,
2427  LinearVelocityJoint,
2428  LinearVelocityJoint(uLabel, pDO,
2429  pNode, Dir, pDC, fOut));
2430  out << "linearvelocity: ";
2431  break;
2432 
2433  case ANGULARVELOCITY:
2434  SAFENEWWITHCONSTRUCTOR(pEl,
2435  AngularVelocityJoint,
2436  AngularVelocityJoint(uLabel, pDO,
2437  pNode, Dir, pDC, fOut));
2438  out << "angularvelocity: ";
2439  break;
2440 
2441  default:
2442  ASSERTMSG(0, "You shouldn't have reached this point");
2443  throw DataManager::ErrGeneric(MBDYN_EXCEPT_ARGS);
2444  }
2445 
2446  out << uLabel
2447  << " " << pNode->GetLabel()
2448  << " " << Dir
2449  << std::endl;
2450 
2451  } break;
2452 
2453  case LINEARACCELERATION:
2454  case ANGULARACCELERATION:
2455  {
2456  /* nodo collegato */
2457  const StructNode* pNode = pDM->ReadNode<const StructNode, Node::STRUCTURAL>(HP);
2458 
2459  Vec3 Dir;
2460  try {
2461  Dir = HP.GetUnitVecRel(ReferenceFrame(pNode));
2462  } catch (ErrNullNorm) {
2463  silent_cerr("Joint(" << uLabel << "): "
2464  "direction is null" << std::endl);
2465  throw ErrNullNorm(MBDYN_EXCEPT_ARGS);
2466  }
2467 
2468  DriveCaller* pDC = HP.GetDriveCaller();
2469 
2470  flag fOut = pDM->fReadOutput(HP, Elem::JOINT);
2471 
2472  std::ostream& out = pDM->GetLogFile();
2473 
2474  switch (CurrKeyWord) {
2475  case LINEARACCELERATION:
2476  SAFENEWWITHCONSTRUCTOR(pEl,
2477  LinearAccelerationJoint,
2478  LinearAccelerationJoint(uLabel, pDO,
2479  pNode, Dir, pDC, fOut));
2480  out << "linearacceleration: ";
2481  break;
2482 
2483  case ANGULARACCELERATION:
2484  SAFENEWWITHCONSTRUCTOR(pEl,
2485  AngularAccelerationJoint,
2486  AngularAccelerationJoint(uLabel, pDO,
2487  pNode, Dir, pDC, fOut));
2488  out << "angularacceleration: ";
2489  break;
2490 
2491  default:
2492  ASSERTMSG(0, "You shouldn't have reached this point");
2493  throw DataManager::ErrGeneric(MBDYN_EXCEPT_ARGS);
2494  }
2495 
2496  out << uLabel
2497  << " " << pNode->GetLabel()
2498  << " " << Dir
2499  << std::endl;
2500 
2501  } break;
2502 
2503  case PRISMATIC:
2504  {
2505  /* nodo collegato 1 */
2506  const StructNode* pNode1 = pDM->ReadNode<const StructNode, Node::STRUCTURAL>(HP);
2507  ReferenceFrame RF1(pNode1);
2508 
2509  Mat3x3 R1h(Eye3);
2510  if (HP.IsKeyWord("orientation")) {
2511  DEBUGCOUT("Hinge orientation matrix is supplied" << std::endl);
2512  R1h = HP.GetRotRel(RF1);
2513 #ifdef MBDYN_X_COMPATIBLE_INPUT
2514  } else if (HP.IsKeyWord("hinge")) {
2515  pedantic_cerr("Joint(" << uLabel << "): "
2516  "keyword \"hinge\" at line " << HP.GetLineData()
2517  << " is deprecated; use \"orientation\" instead"
2518  << std::endl);
2519  DEBUGCOUT("Hinge orientation matrix is supplied" << std::endl);
2520  R1h = HP.GetRotRel(RF1);
2521 #endif /* MBDYN_X_COMPATIBLE_INPUT */
2522  }
2523 
2524  /* nodo collegato 2 */
2525  const StructNode* pNode2 = pDM->ReadNode<const StructNode, Node::STRUCTURAL>(HP);
2526  ReferenceFrame RF2(pNode2);
2527 
2528  /* Stessa cosa per il nodo 2 */
2529 
2530  Mat3x3 R2h(Eye3);
2531  if (HP.IsKeyWord("orientation")) {
2532  DEBUGCOUT("Hinge orientation matrix is supplied" << std::endl);
2533  R2h = HP.GetRotRel(RF2, RF1, R1h);
2534 #ifdef MBDYN_X_COMPATIBLE_INPUT
2535  } else if (HP.IsKeyWord("hinge")) {
2536  pedantic_cerr("Joint(" << uLabel << "): "
2537  "keyword \"hinge\" at line " << HP.GetLineData()
2538  << " is deprecated; use \"orientation\" instead"
2539  << std::endl);
2540  DEBUGCOUT("Hinge orientation matrix is supplied" << std::endl);
2541  R2h = HP.GetRotRel(RF2, RF1, R1h);
2542 #endif /* MBDYN_X_COMPATIBLE_INPUT */
2543  }
2544 
2545  flag fOut = pDM->fReadOutput(HP, Elem::JOINT);
2546 
2547  SAFENEWWITHCONSTRUCTOR(pEl,
2548  PrismaticJoint,
2549  PrismaticJoint(uLabel, pDO, pNode1, pNode2,
2550  R1h, R2h, fOut));
2551 
2552  std::ostream& out = pDM->GetLogFile();
2553  out << "prismatic: " << uLabel
2554  << " " << pNode1->GetLabel()
2555  << " " << Zero3
2556  << " " << R1h
2557  << " " << pNode2->GetLabel()
2558  << " " << Zero3
2559  << " " << R2h
2560  << std::endl;
2561  } break;
2562 
2563  case DRIVEHINGE:
2564  {
2565  /* nodo collegato 1 */
2566  const StructNode* pNode1 = pDM->ReadNode<const StructNode, Node::STRUCTURAL>(HP);
2567  ReferenceFrame RF1(pNode1);
2568 
2569  Mat3x3 R1h(Eye3);
2570  if (HP.IsKeyWord("orientation")) {
2571  DEBUGCOUT("Hinge orientation matrix is supplied" << std::endl);
2572  R1h = HP.GetRotRel(RF1);
2573 #ifdef MBDYN_X_COMPATIBLE_INPUT
2574  } else if (HP.IsKeyWord("hinge")) {
2575  pedantic_cerr("Joint(" << uLabel << "): "
2576  "keyword \"hinge\" at line " << HP.GetLineData()
2577  << " is deprecated; use \"orientation\" instead"
2578  << std::endl);
2579  DEBUGCOUT("Hinge orientation matrix is supplied" << std::endl);
2580  R1h = HP.GetRotRel(RF1);
2581 #endif /* MBDYN_X_COMPATIBLE_INPUT */
2582  }
2583 
2584 
2585  /* nodo collegato 2 */
2586  const StructNode* pNode2 = pDM->ReadNode<const StructNode, Node::STRUCTURAL>(HP);
2587  ReferenceFrame RF2(pNode2);
2588 
2589  /* Stessa cosa per il nodo 2 */
2590 
2591  Mat3x3 R2h(Eye3);
2592  if (HP.IsKeyWord("orientation")) {
2593  DEBUGCOUT("Hinge orientation matrix is supplied" << std::endl);
2594  R2h = HP.GetRotRel(RF2, RF1, R1h);
2595 #ifdef MBDYN_X_COMPATIBLE_INPUT
2596  } else if (HP.IsKeyWord("hinge")) {
2597  pedantic_cerr("Joint(" << uLabel << "): "
2598  "keyword \"hinge\" at line " << HP.GetLineData()
2599  << " is deprecated; use \"orientation\" instead"
2600  << std::endl);
2601  DEBUGCOUT("Hinge orientation matrix is supplied" << std::endl);
2602  R2h = HP.GetRotRel(RF2, RF1, R1h);
2603 #endif /* MBDYN_X_COMPATIBLE_INPUT */
2604  }
2605 
2606 
2607  TplDriveCaller<Vec3>* pDC = ReadDC3D(pDM, HP);
2608 
2609  flag fOut = pDM->fReadOutput(HP, Elem::JOINT);
2610 
2611  SAFENEWWITHCONSTRUCTOR(pEl,
2612  DriveHingeJoint,
2613  DriveHingeJoint(uLabel, pDO, pDC,
2614  pNode1, pNode2, R1h, R2h, fOut));
2615 
2616  } break;
2617 
2618  case DRIVEDISPLACEMENT:
2619  {
2620  /* nodo collegato 1 */
2621  const StructNode* pNode1 = pDM->ReadNode<const StructNode, Node::STRUCTURAL>(HP);
2622  ReferenceFrame RF(pNode1);
2623 
2624  Vec3 f1(Zero3);
2625  ReferenceFrame RF1(pNode1);
2626  if (HP.IsKeyWord("position")) {
2627 #ifdef MBDYN_X_COMPATIBLE_INPUT
2628  NO_OP;
2629  } else {
2630  pedantic_cerr("Joint(" << uLabel << "): "
2631  "missing keyword \"position\" at line "
2632  << HP.GetLineData() << std::endl);
2633  }
2634 #endif /* MBDYN_X_COMPATIBLE_INPUT */
2635  f1 = HP.GetPosRel(RF1);
2636 #ifndef MBDYN_X_COMPATIBLE_INPUT
2637  }
2638 #endif /* MBDYN_X_COMPATIBLE_INPUT */
2639 
2640  /* nodo collegato 2 */
2641  const StructNode* pNode2 = pDM->ReadNode<const StructNode, Node::STRUCTURAL>(HP);
2642  RF = ReferenceFrame(pNode2);
2643 
2644  /* Stessa cosa per il nodo 2 */
2645  Vec3 f2(Zero3);
2646  if (HP.IsKeyWord("position")) {
2647 #ifdef MBDYN_X_COMPATIBLE_INPUT
2648  NO_OP;
2649  } else {
2650  pedantic_cerr("Joint(" << uLabel << "): "
2651  "missing keyword \"position\" at line "
2652  << HP.GetLineData() << std::endl);
2653  }
2654 #endif /* MBDYN_X_COMPATIBLE_INPUT */
2655  f2 = HP.GetPosRel(ReferenceFrame(pNode2), RF1, f1);
2656 #ifndef MBDYN_X_COMPATIBLE_INPUT
2657  }
2658 #endif /* MBDYN_X_COMPATIBLE_INPUT */
2659 
2660  TplDriveCaller<Vec3>* pDC = ReadDC3D(pDM, HP);
2661 
2662  flag fOut = pDM->fReadOutput(HP, Elem::JOINT);
2663 
2664  SAFENEWWITHCONSTRUCTOR(pEl,
2665  DriveDisplacementJoint,
2666  DriveDisplacementJoint(uLabel, pDO, pDC,
2667  pNode1, pNode2, f1, f2, fOut));
2668 
2669  std::ostream& out = pDM->GetLogFile();
2670  out << "drivedisplacement: " << uLabel
2671  << " " << pNode1->GetLabel()
2672  << " " << f1
2673  << " " << pNode2->GetLabel()
2674  << " " << f2
2675  << std::endl;
2676 
2677  } break;
2678 
2679  case DRIVEDISPLACEMENTPIN:
2680  {
2681  /* nodo collegato */
2682  const StructNode* pNode = pDM->ReadNode<const StructNode, Node::STRUCTURAL>(HP);
2683  ReferenceFrame RF(pNode);
2684 
2685  Vec3 f(Zero3);
2686  if (HP.IsKeyWord("position")) {
2687 #ifdef MBDYN_X_COMPATIBLE_INPUT
2688  NO_OP;
2689  } else {
2690  pedantic_cerr("Joint(" << uLabel << "): "
2691  "missing keyword \"position\" at line "
2692  << HP.GetLineData() << std::endl);
2693  }
2694 #endif /* MBDYN_X_COMPATIBLE_INPUT */
2695  f = HP.GetPosRel(ReferenceFrame(pNode));
2696 #ifndef MBDYN_X_COMPATIBLE_INPUT
2697  }
2698 #endif /* MBDYN_X_COMPATIBLE_INPUT */
2699 
2700  /* Stessa cosa per il terreno */
2701  Vec3 x(Zero3);
2702  if (HP.IsKeyWord("position")) {
2703 #ifdef MBDYN_X_COMPATIBLE_INPUT
2704  NO_OP;
2705  } else {
2706  pedantic_cerr("Joint(" << uLabel << "): "
2707  "missing keyword \"position\" at line "
2708  << HP.GetLineData() << std::endl);
2709  }
2710 #endif /* MBDYN_X_COMPATIBLE_INPUT */
2711  x = HP.GetPosAbs(::AbsRefFrame);
2712 #ifndef MBDYN_X_COMPATIBLE_INPUT
2713  }
2714 #endif /* MBDYN_X_COMPATIBLE_INPUT */
2715 
2716  TplDriveCaller<Vec3>* pDC = ReadDC3D(pDM, HP);
2717 
2718  flag fOut = pDM->fReadOutput(HP, Elem::JOINT);
2719 
2720  SAFENEWWITHCONSTRUCTOR(pEl,
2721  DriveDisplacementPinJoint,
2722  DriveDisplacementPinJoint(uLabel, pDO, pDC,
2723  pNode, f, x, fOut));
2724 
2725  std::ostream& out = pDM->GetLogFile();
2726  out << "drivedisplacementpin: " << uLabel
2727  << " " << pNode->GetLabel()
2728  << " " << f
2729  << " " << x
2730  << std::endl;
2731 
2732  } break;
2733 
2734  case IMPOSEDDISPLACEMENT:
2735  {
2736  silent_cerr("ImposedDisplacement(" << uLabel << "): "
2737  "deprecated; using \"total joint\" instead" << std::endl);
2738 
2739  /* nodo collegato 1 */
2740  const StructNode* pNode1 = pDM->ReadNode<const StructNode, Node::STRUCTURAL>(HP);
2741  ReferenceFrame RF1(pNode1);
2742 
2743  Vec3 f1(HP.GetPosRel(RF1));
2744  Mat3x3 R1h(Eye3);
2745  Mat3x3 R1hr(Eye3);
2746 
2747  /* nodo collegato 2 */
2748  const StructNode* pNode2 = pDM->ReadNode<const StructNode, Node::STRUCTURAL>(HP);
2749  ReferenceFrame RF2(pNode2);
2750 
2751  Vec3 f2(HP.GetPosRel(RF2, RF1, f1));
2752  Mat3x3 R2h(Eye3);
2753  Mat3x3 R2hr(Eye3);
2754 
2755  bool bXActive[3] = { true, true, true };
2756  bool bVActive[3] = { false, false, false };
2757  TplDriveCaller<Vec3>* pXDC[3] = {0, 0, 0};
2758  pXDC[0] = ReadDCVecRel(pDM, HP, RF1);
2759 
2760  bool bRActive[3] = { false, false, false };
2761  bool bWActive[3] = { false, false, false };
2762  TplDriveCaller<Vec3>* pTDC[3] = {0, 0, 0};
2763  SAFENEW(pTDC[0], ZeroTplDriveCaller<Vec3>);
2764 
2765  flag fOut = pDM->fReadOutput(HP, Elem::JOINT);
2766 
2767  SAFENEWWITHCONSTRUCTOR(pEl,
2768  TotalJoint,
2769  TotalJoint(uLabel, pDO,
2770  bXActive, bVActive, pXDC,
2771  bRActive, bWActive, pTDC,
2772  pNode1, f1, R1h, R1hr,
2773  pNode2, f2, R2h, R2hr,
2774  fOut));
2775 
2776  std::ostream& out = pDM->GetLogFile();
2777  out << "totaljoint: " << uLabel
2778  << " " << pNode1->GetLabel()
2779  << " " << f1
2780  << " " << R1h
2781  << " " << R1hr
2782  << " " << pNode2->GetLabel()
2783  << " " << f2
2784  << " " << R2h
2785  << " " << R2hr
2786  << " " << bXActive[0]
2787  << " " << bXActive[1]
2788  << " " << bXActive[2]
2789  << " " << bVActive[0]
2790  << " " << bVActive[1]
2791  << " " << bVActive[2]
2792  << " " << bRActive[0]
2793  << " " << bRActive[1]
2794  << " " << bRActive[2]
2795  << " " << bWActive[0]
2796  << " " << bWActive[1]
2797  << " " << bWActive[2]
2798  << std::endl;
2799  } break;
2800 
2801  case IMPOSEDDISPLACEMENTPIN:
2802  {
2803  silent_cerr("ImposedDisplacementPin(" << uLabel << "): "
2804  "deprecated; using \"total pin joint\" instead" << std::endl);
2805 
2806  /* nodo collegato */
2807  const StructNode* pNode = pDM->ReadNode<const StructNode, Node::STRUCTURAL>(HP);
2808  ReferenceFrame RF(pNode);
2809 
2810  Vec3 fn(HP.GetPosRel(RF));
2811  Mat3x3 Rnh(Eye3);
2812  Mat3x3 Rnhr(Eye3);
2813 
2814  Vec3 Xc(HP.GetPosAbs(::AbsRefFrame));
2815  Mat3x3 Rch(Eye3);
2816  Mat3x3 Rchr(Eye3);
2817 
2818  bool bXActive[3] = { true, true, true };
2819  bool bVActive[3] = { false, false, false };
2820  TplDriveCaller<Vec3>* pXDC[3] = { 0, 0, 0 };
2821  pXDC[0] = ReadDCVecAbs(pDM, HP, ::AbsRefFrame);
2822 
2823  bool bRActive[3] = { false, false, false };
2824  bool bWActive[3] = { false, false, false };
2825  TplDriveCaller<Vec3>* pTDC[3] = { 0, 0, 0 };
2826  SAFENEW(pTDC[0], ZeroTplDriveCaller<Vec3>);
2827 
2828  flag fOut = pDM->fReadOutput(HP, Elem::JOINT);
2829 
2830  SAFENEWWITHCONSTRUCTOR(pEl,
2831  TotalPinJoint,
2832  TotalPinJoint(uLabel, pDO,
2833  bXActive, bVActive, pXDC,
2834  bRActive, bWActive, pTDC,
2835  Xc, Rch, Rchr,
2836  pNode, fn, Rnh, Rnhr,
2837  fOut));
2838 
2839  std::ostream& out = pDM->GetLogFile();
2840  out << "totalpinjoint: " << uLabel
2841  << " " << pNode->GetLabel()
2842  << " " << fn
2843  << " " << Rnh
2844  << " " << Rnhr
2845  << " " << Xc
2846  << " " << Rch
2847  << " " << Rchr
2848  << std::endl;
2849 
2850  } break;
2851 
2852  case IMPOSEDORIENTATION:
2853  {
2854  silent_cerr("ImposedOrientation(" << uLabel << "): "
2855  "using \"total joint\" instead" << std::endl);
2856 
2857  /* nodo collegato 1 */
2858  const StructNode* pNode1 = pDM->ReadNode<const StructNode, Node::STRUCTURAL>(HP);
2859  ReferenceFrame RF1(pNode1);
2860 
2861  Vec3 f1(Zero3);
2862  Mat3x3 R1h(Eye3);
2863  Mat3x3 R1hr(HP.GetRotRel(RF1));
2864 
2865  /* nodo collegato 2 */
2866  const StructNode* pNode2 = pDM->ReadNode<const StructNode, Node::STRUCTURAL>(HP);
2867  ReferenceFrame RF2(pNode2);
2868 
2869  Vec3 f2(Zero3);
2870  Mat3x3 R2h(Eye3);
2871  Mat3x3 R2hr(HP.GetRotRel(RF2, RF1, R1hr));
2872 
2873  bool bXActive[3] = { false, false, false };
2874  bool bVActive[3] = { false, false, false };
2875  TplDriveCaller<Vec3>* pXDC[3] = { 0, 0, 0 };
2876  SAFENEW(pXDC[0], ZeroTplDriveCaller<Vec3>);
2877 
2878  bool bRActive[3] = { true, true, true };
2879  bool bWActive[3] = { false, false, false };
2880  TplDriveCaller<Vec3>* pTDC[3] = { 0, 0, 0 };
2881 
2882  ReferenceFrame RF1hr(0, Zero3, pNode1->GetRCurr()*R1hr, Zero3, Zero3,
2883  pDM->GetOrientationDescription());
2884  pTDC[0] = ReadDCVecRel(pDM, HP, RF1hr);
2885 
2886  flag fOut = pDM->fReadOutput(HP, Elem::JOINT);
2887 
2888  SAFENEWWITHCONSTRUCTOR(pEl,
2889  TotalJoint,
2890  TotalJoint(uLabel, pDO,
2891  bXActive, bVActive, pXDC,
2892  bRActive, bWActive, pTDC,
2893  pNode1, f1, R1h, R1hr,
2894  pNode2, f2, R2h, R2hr,
2895  fOut));
2896 
2897  std::ostream& out = pDM->GetLogFile();
2898  out << "totaljoint: " << uLabel
2899  << " " << pNode1->GetLabel()
2900  << " " << f1
2901  << " " << R1h
2902  << " " << R1hr
2903  << " " << pNode2->GetLabel()
2904  << " " << f2
2905  << " " << R2h
2906  << " " << R2hr
2907  << " " << bXActive[0]
2908  << " " << bXActive[1]
2909  << " " << bXActive[2]
2910  << " " << bVActive[0]
2911  << " " << bVActive[1]
2912  << " " << bVActive[2]
2913  << " " << bRActive[0]
2914  << " " << bRActive[1]
2915  << " " << bRActive[2]
2916  << " " << bWActive[0]
2917  << " " << bWActive[1]
2918  << " " << bWActive[2]
2919  << std::endl;
2920  } break;
2921 
2922  case TOTALEQUATION:
2923  {
2924  /* nodo collegato 1 */
2925  const StructNode* pNode1 = pDM->ReadNode<const StructNode, Node::STRUCTURAL>(HP);
2926  ReferenceFrame RF1(pNode1);
2927 
2928  Vec3 f1(Zero3);
2929  if (HP.IsKeyWord("position")) {
2930  f1 = HP.GetPosRel(RF1);
2931  }
2932 
2933  Mat3x3 R1h(Eye3);
2934  if (HP.IsKeyWord("position" "orientation")) {
2935  DEBUGCOUT("Position orientation matrix is supplied" << std::endl);
2936  R1h = HP.GetRotRel(RF1);
2937  }
2938 
2939  Mat3x3 R1hr(Eye3);
2940  if (HP.IsKeyWord("rotation" "orientation")) {
2941  DEBUGCOUT("Rotation orientation matrix is supplied" << std::endl);
2942  R1hr = HP.GetRotRel(RF1);
2943  }
2944 
2945  /* nodo collegato 2 */
2946  const StructNode* pNode2 = pDM->ReadNode<const StructNode, Node::STRUCTURAL>(HP);
2947  ReferenceFrame RF2(pNode2);
2948 
2949  Vec3 f2(Zero3);
2950  if (HP.IsKeyWord("position")) {
2951  f2 = HP.GetPosRel(RF2, RF1, f1);
2952  }
2953 
2954  Mat3x3 R2h(Eye3);
2955  if (HP.IsKeyWord("position" "orientation")) {
2956  DEBUGCOUT("Position orientation matrix is supplied" << std::endl);
2957  R2h = HP.GetRotRel(RF2, RF1, R1h);
2958  }
2959 
2960  Mat3x3 R2hr(Eye3);
2961  if (HP.IsKeyWord("rotation" "orientation")) {
2962  DEBUGCOUT("Rotation orientation matrix is supplied" << std::endl);
2963  R2hr = HP.GetRotRel(RF2, RF1, R1hr);
2964  }
2965 
2966  bool bXActive[3] = { false, false, false };
2967  bool bVActive[3] = { false, false, false };
2968  TplDriveCaller<Vec3>* pXDC[3] = {0, 0, 0};
2969  if (HP.IsKeyWord("position" "constraint")) {
2970  for (unsigned i = 0; i < 3; i++) {
2971  if (HP.IsKeyWord("inactive")) {
2972  bXActive[i] = false;
2973  bVActive[i] = false;
2974 
2975  } else if (HP.IsKeyWord("position") || HP.IsKeyWord("active")) {
2976  bXActive[i] = true;
2977  bVActive[i] = false;
2978 
2979  } else if (HP.IsKeyWord("velocity")) {
2980  bVActive[i] = true;
2981  bXActive[i] = false;
2982 
2983  } else {
2984  if (HP.IsArg()) {
2985  bool bActive = HP.GetBool();
2986  bXActive[i] = bActive;
2987  bVActive[i] = bActive;
2988  continue;
2989  }
2990 
2991  silent_cerr("TotalEquation(" << uLabel << "): "
2992  "invalid status for position component #" << i + 1
2993  << " at line " << HP.GetLineData() << std::endl);
2994  throw DataManager::ErrGeneric(MBDYN_EXCEPT_ARGS);
2995  }
2996  }
2997 
2998  pXDC[0] = ReadDC3D(pDM, HP);
2999 
3000  if (pDM->bIsInverseDynamics()) {
3001  pXDC[1] = ReadDC3D(pDM, HP);
3002  pXDC[2] = ReadDC3D(pDM, HP);
3003  }
3004 
3005  } else {
3006  SAFENEW(pXDC[0], ZeroTplDriveCaller<Vec3>);
3007  }
3008 
3009  bool bRActive[3] = { false, false, false };
3010  bool bWActive[3] = { false, false, false };
3011  TplDriveCaller<Vec3>* pTDC[3] = {0, 0, 0};
3012  if (HP.IsKeyWord("orientation" "constraint")) {
3013  for (unsigned i = 0; i < 3; i++) {
3014  if (HP.IsKeyWord("inactive")) {
3015  bRActive[i] = false;
3016  bWActive[i] = false;
3017 
3018  } else if (HP.IsKeyWord("rotation") || HP.IsKeyWord("active")) {
3019  bRActive[i] = true;
3020  bWActive[i] = false;
3021 
3022  } else if (HP.IsKeyWord("angular" "velocity")) {
3023  bRActive[i] = false;
3024  bWActive[i] = true;
3025 
3026  } else {
3027  if (HP.IsArg()) {
3028  bool bActive = HP.GetInt();
3029  bRActive[i] = bActive;
3030  bWActive[i] = bActive;
3031  continue;
3032  }
3033 
3034  silent_cerr("TotalEquation(" << uLabel << "): "
3035  "invalid status for position component #" << i + 1
3036  << " at line " << HP.GetLineData() << std::endl);
3037  throw DataManager::ErrGeneric(MBDYN_EXCEPT_ARGS);
3038  }
3039  }
3040 
3041  pTDC[0] = ReadDC3D(pDM, HP);
3042 
3043  if (pDM->bIsInverseDynamics()) {
3044  pTDC[1] = ReadDC3D(pDM, HP);
3045  pTDC[2] = ReadDC3D(pDM, HP);
3046  }
3047 
3048  } else {
3049  SAFENEW(pTDC[0], ZeroTplDriveCaller<Vec3>);
3050  }
3051 
3052  flag fOut = pDM->fReadOutput(HP, Elem::JOINT);
3053 
3054  SAFENEWWITHCONSTRUCTOR(pEl,
3055  TotalEquation,
3056  TotalEquation(uLabel, pDO,
3057  bXActive, bVActive, pXDC,
3058  bRActive, bWActive, pTDC,
3059  pNode1, f1, R1h, R1hr,
3060  pNode2, f2, R2h, R2hr,
3061  fOut));
3062 
3063  std::ostream& out = pDM->GetLogFile();
3064  out << "totalequation: " << uLabel
3065  << " " << pNode1->GetLabel()
3066  << " " << f1
3067  << " " << R1h
3068  << " " << R1hr
3069  << " " << pNode2->GetLabel()
3070  << " " << f2
3071  << " " << R2h
3072  << " " << R2hr
3073  << std::endl;
3074  } break;
3075 
3076  case TOTALINTERNALREACTION:
3077  {
3078  /* nodo collegato 1 */
3079  const StructNode* pNode1 = pDM->ReadNode<const StructNode, Node::STRUCTURAL>(HP);
3080  ReferenceFrame RF1(pNode1);
3081 
3082  Vec3 f1(Zero3);
3083  if (HP.IsKeyWord("position")) {
3084  f1 = HP.GetPosRel(RF1);
3085  }
3086 
3087  Mat3x3 R1h(Eye3);
3088  if (HP.IsKeyWord("position" "orientation")) {
3089  DEBUGCOUT("Position orientation matrix is supplied" << std::endl);
3090  R1h = HP.GetRotRel(RF1);
3091  }
3092 
3093  Mat3x3 R1hr(Eye3);
3094  if (HP.IsKeyWord("rotation" "orientation")) {
3095  DEBUGCOUT("Rotation orientation matrix is supplied" << std::endl);
3096  R1hr = HP.GetRotRel(RF1);
3097  }
3098 
3099  /* nodo collegato 2 */
3100  const StructNode* pNode2 = pDM->ReadNode<const StructNode, Node::STRUCTURAL>(HP);
3101  ReferenceFrame RF2(pNode2);
3102 
3103  Vec3 f2(Zero3);
3104  if (HP.IsKeyWord("position")) {
3105  f2 = HP.GetPosRel(RF2, RF1, f1);
3106  }
3107 
3108  Mat3x3 R2h(Eye3);
3109  if (HP.IsKeyWord("position" "orientation")) {
3110  DEBUGCOUT("Position orientation matrix is supplied" << std::endl);
3111  R2h = HP.GetRotRel(RF2, RF1, R1h);
3112  }
3113 
3114  Mat3x3 R2hr(Eye3);
3115  if (HP.IsKeyWord("rotation" "orientation")) {
3116  DEBUGCOUT("Rotation orientation matrix is supplied" << std::endl);
3117  R2hr = HP.GetRotRel(RF2, RF1, R1hr);
3118  }
3119 
3120  bool bXActive[3] = { false, false, false };
3121  if (HP.IsKeyWord("position" "constraint")) {
3122  for (unsigned i = 0; i < 3; i++) {
3123  if (HP.IsKeyWord("inactive")) {
3124  bXActive[i] = false;
3125 
3126  } else if (HP.IsKeyWord("active")) {
3127  bXActive[i] = true;
3128 
3129  } else {
3130  if (HP.IsArg()) {
3131  bXActive[i] = HP.GetInt();
3132  continue;
3133  }
3134 
3135  silent_cerr("TotalReaction(" << uLabel << "): "
3136  "invalid status for position component #" << i + 1
3137  << " at line " << HP.GetLineData() << std::endl);
3138  throw DataManager::ErrGeneric(MBDYN_EXCEPT_ARGS);
3139  }
3140  }
3141  }
3142 
3143  bool bRActive[3] = { false, false, false };
3144  if (HP.IsKeyWord("orientation" "constraint")) {
3145  for (unsigned i = 0; i < 3; i++) {
3146  if (HP.IsKeyWord("inactive")) {
3147  bRActive[i] = false;
3148 
3149  } else if (HP.IsKeyWord("active")) {
3150  bRActive[i] = true;
3151 
3152  } else {
3153  if (HP.IsArg()) {
3154  bRActive[i] = HP.GetBool();
3155  continue;
3156  }
3157 
3158  silent_cerr("TotalReaction(" << uLabel << "): "
3159  "invalid status for position component #" << i + 1
3160  << " at line " << HP.GetLineData() << std::endl);
3161  throw DataManager::ErrGeneric(MBDYN_EXCEPT_ARGS);
3162  }
3163  }
3164 
3165  }
3166 
3167  TotalEquation *tot_eq_pt(0);
3168  if (HP.IsKeyWord("total" "equation")) {
3169  unsigned int tot_eq_j_label = HP.GetInt();
3170  Elem* el_pt = pDM->pFindElem(Elem::JOINT, tot_eq_j_label);
3171  NestedElem* nested_pt = dynamic_cast<NestedElem*>(el_pt);
3172  if (nested_pt != 0) {
3173  tot_eq_pt = dynamic_cast<TotalEquation*>(nested_pt->pGetElem());
3174  } else {
3175  tot_eq_pt = dynamic_cast<TotalEquation*>(el_pt);
3176  }
3177  if (tot_eq_pt == 0) {
3178  silent_cerr("TotalEquation" "(" << tot_eq_j_label << ") "
3179  "needed by TotalReaction(" << uLabel << ") "
3180  "at line " << HP.GetLineData()
3181  << " is not defined"
3182  << std::endl);
3183  throw DataManager::ErrGeneric(MBDYN_EXCEPT_ARGS);
3184  }
3185  } else {
3186  silent_cerr("Total Reaction(" << uLabel << ") "
3187  "needs a reference to a corresponding "
3188  "TotalEquation at line " << HP.GetLineData() <<
3189  std::endl);
3190  }
3191 
3192 
3193  flag fOut = pDM->fReadOutput(HP, Elem::JOINT);
3194 
3195  SAFENEWWITHCONSTRUCTOR(pEl,
3196  TotalReaction,
3197  TotalReaction(uLabel, pDO,
3198  bXActive,
3199  bRActive,
3200  pNode1, f1, R1h, R1hr,
3201  pNode2, f2, R2h, R2hr,
3202  tot_eq_pt,
3203  fOut));
3204 
3205  std::ostream& out = pDM->GetLogFile();
3206  out << "totalreaction: " << uLabel
3207  << " " << pNode1->GetLabel()
3208  << " " << f1
3209  << " " << R1h
3210  << " " << R1hr
3211  << " " << pNode2->GetLabel()
3212  << " " << f2
3213  << " " << R2h
3214  << " " << R2hr
3215  << " " << tot_eq_pt->GetLabel()
3216  << std::endl;
3217  } break;
3218 
3219  case TOTALJOINT:
3220  {
3221  /* nodo collegato 1 */
3222  const StructNode* pNode1 = pDM->ReadNode<const StructNode, Node::STRUCTURAL>(HP);
3223  ReferenceFrame RF1(pNode1);
3224 
3225  Vec3 f1(Zero3);
3226  if (HP.IsKeyWord("position")) {
3227  f1 = HP.GetPosRel(RF1);
3228  }
3229 
3230  Mat3x3 R1h(Eye3);
3231  if (HP.IsKeyWord("position" "orientation")) {
3232  DEBUGCOUT("Position orientation matrix is supplied" << std::endl);
3233  R1h = HP.GetRotRel(RF1);
3234  }
3235 
3236  Mat3x3 R1hr(Eye3);
3237  if (HP.IsKeyWord("rotation" "orientation")) {
3238  DEBUGCOUT("Rotation orientation matrix is supplied" << std::endl);
3239  R1hr = HP.GetRotRel(RF1);
3240  }
3241 
3242  /* nodo collegato 2 */
3243  const StructNode* pNode2 = pDM->ReadNode<const StructNode, Node::STRUCTURAL>(HP);
3244  ReferenceFrame RF2(pNode2);
3245 
3246  Vec3 f2(Zero3);
3247  if (HP.IsKeyWord("position")) {
3248  f2 = HP.GetPosRel(RF2, RF1, f1);
3249  }
3250 
3251  Mat3x3 R2h(Eye3);
3252  if (HP.IsKeyWord("position" "orientation")) {
3253  DEBUGCOUT("Position orientation matrix is supplied" << std::endl);
3254  R2h = HP.GetRotRel(RF2, RF1, R1h);
3255  }
3256 
3257  Mat3x3 R2hr(Eye3);
3258  if (HP.IsKeyWord("rotation" "orientation")) {
3259  DEBUGCOUT("Rotation orientation matrix is supplied" << std::endl);
3260  R2hr = HP.GetRotRel(RF2, RF1, R1hr);
3261  }
3262 
3263  bool bXActive[3] = { false, false, false };
3264  bool bVActive[3] = { false, false, false };
3265  TplDriveCaller<Vec3>* pXDC[3] = {0, 0, 0};
3266  if (HP.IsKeyWord("position" "constraint")) {
3267  for (unsigned i = 0; i < 3; i++) {
3268  if (HP.IsKeyWord("inactive")) {
3269  bXActive[i] = false;
3270  bVActive[i] = false;
3271 
3272  } else if (HP.IsKeyWord("position") || HP.IsKeyWord("active")) {
3273  bXActive[i] = true;
3274  bVActive[i] = false;
3275 
3276  } else if (HP.IsKeyWord("velocity")) {
3277  bXActive[i] = false;
3278  bVActive[i] = true;
3279 
3280  } else {
3281  if (HP.IsArg()) {
3282  bool bActive = HP.GetBool();
3283  bXActive[i] = bActive;
3284  bVActive[i] = false;
3285  continue;
3286  }
3287 
3288  silent_cerr("TotalJoint(" << uLabel << "): "
3289  "invalid status for position component #" << i + 1
3290  << " at line " << HP.GetLineData() << std::endl);
3291  throw DataManager::ErrGeneric(MBDYN_EXCEPT_ARGS);
3292  }
3293  }
3294 
3295  ReferenceFrame RF1h(0, Zero3, pNode1->GetRCurr()*R1h, Zero3, Zero3,
3296  pDM->GetOrientationDescription());
3297  pXDC[0] = ReadDCVecRel(pDM, HP, RF1h);
3298 
3299  if (pDM->bIsInverseDynamics()) {
3300  pXDC[1] = ReadDCVecRel(pDM, HP, RF1h);
3301  pXDC[2] = ReadDCVecRel(pDM, HP, RF1h);
3302  }
3303 
3304  } else {
3305  SAFENEW(pXDC[0], ZeroTplDriveCaller<Vec3>);
3306  }
3307 
3308  bool bRActive[3] = { false, false, false };
3309  bool bWActive[3] = { false, false, false };
3310  TplDriveCaller<Vec3>* pTDC[3] = {0, 0, 0};
3311  if (HP.IsKeyWord("orientation" "constraint")) {
3312  for (unsigned i = 0; i < 3; i++) {
3313  if (HP.IsKeyWord("inactive")) {
3314  bRActive[i] = false;
3315  bWActive[i] = false;
3316 
3317  } else if (HP.IsKeyWord("rotation") || HP.IsKeyWord("active")) {
3318  bRActive[i] = true;
3319  bWActive[i] = false;
3320 
3321  } else if (HP.IsKeyWord("angular" "velocity")) {
3322  bRActive[i] = false;
3323  bWActive[i] = true;
3324 
3325  } else {
3326  if (HP.IsArg()) {
3327  bool bActive = HP.GetInt();
3328  bRActive[i] = bActive;
3329  bWActive[i] = false;
3330  continue;
3331  }
3332 
3333  silent_cerr("TotalJoint(" << uLabel << "): "
3334  "invalid status for orientation component #" << i + 1
3335  << " at line " << HP.GetLineData() << std::endl);
3336  throw DataManager::ErrGeneric(MBDYN_EXCEPT_ARGS);
3337  }
3338  }
3339 
3340  ReferenceFrame RF1hr(0, Zero3, pNode1->GetRCurr()*R1hr, Zero3, Zero3,
3341  pDM->GetOrientationDescription());
3342  pTDC[0] = ReadDCVecRel(pDM, HP, RF1hr);
3343 
3344  if (pDM->bIsInverseDynamics()) {
3345  pTDC[1] = ReadDCVecRel(pDM, HP, RF1hr);
3346  pTDC[2] = ReadDCVecRel(pDM, HP, RF1hr);
3347  }
3348 
3349  } else {
3350  SAFENEW(pTDC[0], ZeroTplDriveCaller<Vec3>);
3351  }
3352 
3353  flag fOut = pDM->fReadOutput(HP, Elem::JOINT);
3354 
3355  SAFENEWWITHCONSTRUCTOR(pEl,
3356  TotalJoint,
3357  TotalJoint(uLabel, pDO,
3358  bXActive, bVActive, pXDC,
3359  bRActive, bWActive, pTDC,
3360  pNode1, f1, R1h, R1hr,
3361  pNode2, f2, R2h, R2hr,
3362  fOut));
3363 
3364  std::ostream& out = pDM->GetLogFile();
3365  out << "totaljoint: " << uLabel
3366  << " " << pNode1->GetLabel()
3367  << " " << f1
3368  << " " << R1h
3369  << " " << R1hr
3370  << " " << pNode2->GetLabel()
3371  << " " << f2
3372  << " " << R2h
3373  << " " << R2hr
3374  << " " << bXActive[0]
3375  << " " << bXActive[1]
3376  << " " << bXActive[2]
3377  << " " << bVActive[0]
3378  << " " << bVActive[1]
3379  << " " << bVActive[2]
3380  << " " << bRActive[0]
3381  << " " << bRActive[1]
3382  << " " << bRActive[2]
3383  << " " << bWActive[0]
3384  << " " << bWActive[1]
3385  << " " << bWActive[2]
3386  << std::endl;
3387  } break;
3388 
3389  case TOTALPINJOINT:
3390  {
3391  /* nodo collegato */
3392  const StructNode* pNode = pDM->ReadNode<const StructNode, Node::STRUCTURAL>(HP);
3393  ReferenceFrame RF(pNode);
3394 
3395  Vec3 fn(Zero3);
3396  if (HP.IsKeyWord("position")) {
3397  fn = HP.GetPosRel(RF);
3398  }
3399 
3400  Mat3x3 Rnh(Eye3);
3401  if (HP.IsKeyWord("position" "orientation")) {
3402  DEBUGCOUT("Position orientation matrix is supplied" << std::endl);
3403  Rnh = HP.GetRotRel(RF);
3404  }
3405 
3406  Mat3x3 Rnhr(Eye3);
3407  if (HP.IsKeyWord("rotation" "orientation")) {
3408  DEBUGCOUT("Rotation orientation matrix is supplied" << std::endl);
3409  Rnhr = HP.GetRotRel(RF);
3410  }
3411 
3412  // absolute
3413  Vec3 Xc(Zero3);
3414  if (HP.IsKeyWord("position")) {
3415  DEBUGCOUT("Position vector is supplied" << std::endl);
3416  Xc = HP.GetPosRel(::AbsRefFrame, RF, fn);
3417  }
3418 
3419  Mat3x3 Rch(Eye3);
3420  if (HP.IsKeyWord("position" "orientation")) {
3421  DEBUGCOUT("Position orientation matrix is supplied" << std::endl);
3422  Rch = HP.GetRotRel(::AbsRefFrame, RF, Rnh);
3423  }
3424 
3425  Mat3x3 Rchr(Eye3);
3426  if (HP.IsKeyWord("rotation" "orientation")) {
3427  DEBUGCOUT("Rotation orientation matrix is supplied" << std::endl);
3428  Rchr = HP.GetRotRel(::AbsRefFrame, RF, Rnhr);
3429  }
3430 
3431  bool bXActive[3] = { false, false, false };
3432  bool bVActive[3] = { false, false, false };
3433  TplDriveCaller<Vec3>* pXDC[3] = {0, 0, 0};
3434  if (HP.IsKeyWord("position" "constraint")) {
3435  for (unsigned i = 0; i < 3; i++) {
3436  if (HP.IsKeyWord("inactive")) {
3437  bXActive[i] = false;
3438  bVActive[i] = false;
3439 
3440  } else if (HP.IsKeyWord("position") || HP.IsKeyWord("active")) {
3441  bXActive[i] = true;
3442  bVActive[i] = false;
3443 
3444  } else if (HP.IsKeyWord("velocity")) {
3445  bXActive[i] = false;
3446  bVActive[i] = true;
3447 
3448  } else {
3449  if (HP.IsArg()) {
3450  bXActive[i] = HP.GetBool();
3451  bVActive[i] = false;
3452  continue;
3453  }
3454 
3455  silent_cerr("TotalPinJoint(" << uLabel << "): "
3456  "invalid status for position component #" << i + 1
3457  << " at line " << HP.GetLineData() << std::endl);
3458  throw DataManager::ErrGeneric(MBDYN_EXCEPT_ARGS);
3459  }
3460  }
3461 
3462  ReferenceFrame RFch(0, Zero3, Rch, Zero3, Zero3,
3463  pDM->GetOrientationDescription());
3464  pXDC[0] = ReadDCVecRel(pDM, HP, RFch);
3465 
3466  if (pDM->bIsInverseDynamics()) {
3467  pXDC[1] = ReadDCVecRel(pDM, HP, RFch);
3468  pXDC[2] = ReadDCVecRel(pDM, HP, RFch);
3469  }
3470 
3471  } else {
3472  SAFENEW(pXDC[0], ZeroTplDriveCaller<Vec3>);
3473  }
3474 
3475  bool bRActive[3] = { false, false, false };
3476  bool bWActive[3] = { false, false, false };
3477  TplDriveCaller<Vec3>* pTDC[3] = {0, 0, 0};
3478  if (HP.IsKeyWord("orientation" "constraint")) {
3479  for (unsigned i = 0; i < 3; i++) {
3480  if (HP.IsKeyWord("inactive")) {
3481  bRActive[i] = false;
3482  bWActive[i] = false;
3483 
3484  } else if (HP.IsKeyWord("rotation") || HP.IsKeyWord("active")) {
3485  bRActive[i] = true;
3486  bWActive[i] = false;
3487 
3488  } else if (HP.IsKeyWord("angular" "velocity")) {
3489  bRActive[i] = false;
3490  bWActive[i] = true;
3491 
3492  } else {
3493  if (HP.IsArg()) {
3494  bRActive[i] = HP.GetBool();
3495  bWActive[i] = false;
3496  continue;
3497  }
3498 
3499  silent_cerr("TotalPinJoint(" << uLabel << "): "
3500  "invalid status for position component #" << i + 1
3501  << " at line " << HP.GetLineData() << std::endl);
3502  throw DataManager::ErrGeneric(MBDYN_EXCEPT_ARGS);
3503  }
3504  }
3505 
3506  ReferenceFrame RFchr(0, Zero3, Rchr, Zero3, Zero3,
3507  pDM->GetOrientationDescription());
3508  pTDC[0] = ReadDCVecRel(pDM, HP, RFchr);
3509 
3510  if (pDM->bIsInverseDynamics()) {
3511  pTDC[1] = ReadDCVecRel(pDM, HP, RFchr);
3512  pTDC[2] = ReadDCVecRel(pDM, HP, RFchr);
3513  }
3514 
3515  } else {
3516  SAFENEW(pTDC[0], ZeroTplDriveCaller<Vec3>);
3517  }
3518 
3519  flag fOut = pDM->fReadOutput(HP, Elem::JOINT);
3520 
3521  SAFENEWWITHCONSTRUCTOR(pEl,
3522  TotalPinJoint,
3523  TotalPinJoint(uLabel, pDO,
3524  bXActive, bVActive, pXDC,
3525  bRActive, bWActive, pTDC,
3526  Xc, Rch, Rchr,
3527  pNode, fn, Rnh, Rnhr,
3528  fOut));
3529 
3530  std::ostream& out = pDM->GetLogFile();
3531  out << "totalpinjoint: " << uLabel
3532  << " " << pNode->GetLabel()
3533  << " " << fn
3534  << " " << Rnh
3535  << " " << Rnhr
3536  << " " << Xc
3537  << " " << Rch
3538  << " " << Rchr
3539  << " " << bXActive[0]
3540  << " " << bXActive[1]
3541  << " " << bXActive[2]
3542  << " " << bVActive[0]
3543  << " " << bVActive[1]
3544  << " " << bVActive[2]
3545  << " " << bRActive[0]
3546  << " " << bRActive[1]
3547  << " " << bRActive[2]
3548  << " " << bWActive[0]
3549  << " " << bWActive[1]
3550  << " " << bWActive[2]
3551  << std::endl;
3552 
3553  } break;
3554 
3555  case KINEMATIC:
3556  silent_cerr("Joint(" << uLabel << "): "
3557  "\"kinematic\" obolete; replace with a \"total [pin] joint\" "
3558  << "at line " << HP.GetLineData() << std::endl);
3559  throw ErrGeneric(MBDYN_EXCEPT_ARGS);
3560 
3561  case BEAMSLIDER:
3562  {
3563  /* Corpo slittante */
3564  const StructNode* pNode = pDM->ReadNode<const StructNode, Node::STRUCTURAL>(HP);
3565  std::ostream& out = pDM->GetLogFile();
3566  out << "beamslider: " << uLabel;
3567 
3568  ReferenceFrame RF(pNode);
3569  Vec3 f(HP.GetPosRel(RF));
3570  DEBUGCOUT("Linked to Node " << pNode->GetLabel()
3571  << "with offset " << f << std::endl);
3572 
3573  out
3574  << " " << pNode->GetLabel()
3575  << " " << f;
3576 
3577  Mat3x3 R = Eye3;
3578  if (HP.IsKeyWord("hinge")) {
3579  R = HP.GetRotRel(RF);
3580  }
3581  DEBUGLCOUT(MYDEBUG_INPUT,
3582  "Slider rotation matrix: " << std::endl << R << std::endl);
3583 
3584  out << " " << R;
3585 
3586  /* Slider type */
3587  BeamSliderJoint::Type sliderType = BeamSliderJoint::SPHERICAL;
3588  if (HP.IsKeyWord("type")) {
3589  if (HP.IsKeyWord("spherical")) {
3590  sliderType = BeamSliderJoint::SPHERICAL;
3591  } else if (HP.IsKeyWord("classic")) {
3592  sliderType = BeamSliderJoint::CLASSIC;
3593  } else if (HP.IsKeyWord("spline")) {
3594  sliderType = BeamSliderJoint::SPLINE;
3595  } else {
3596  silent_cerr("unknown slider type at line "
3597  << HP.GetLineData() << std::endl);
3598  throw DataManager::ErrGeneric(MBDYN_EXCEPT_ARGS);
3599  }
3600  }
3601 
3602  unsigned int nB = HP.GetInt();
3603  if (nB < 1) {
3604  silent_cerr("At least one beam is required at line "
3605  << HP.GetLineData() << std::endl);
3606  throw DataManager::ErrGeneric(MBDYN_EXCEPT_ARGS);
3607  }
3608 
3609  out << " " << nB;
3610 
3611  BeamConn **bc = NULL;
3612  SAFENEWARR(bc, BeamConn *, nB);
3613 
3614  const StructNode* pLastNode = NULL;
3615  for (unsigned int i = 0; i < nB; i++) {
3616  /* trave di appoggio */
3617  unsigned int uBeam = HP.GetInt();
3618 
3619  const Elem* p = dynamic_cast<const Elem *>(pDM->pFindElem(Elem::BEAM, uBeam));
3620  if (p == NULL) {
3621  silent_cerr(" at line " << HP.GetLineData()
3622  << ": Beam3(" << uBeam << ") "
3623  "not defined" << std::endl);
3624  throw DataManager::ErrGeneric(MBDYN_EXCEPT_ARGS);
3625  }
3626 
3627  const Beam *pBeam = dynamic_cast<const Beam *>(p);
3628  if (pBeam == 0) {
3629  silent_cerr(" at line " << HP.GetLineData()
3630  << ": Beam(" << uBeam << ") "
3631  "is not a Beam3" << std::endl);
3632  throw DataManager::ErrGeneric(MBDYN_EXCEPT_ARGS);
3633  }
3634 
3635  out << " " << pBeam->GetLabel();
3636 
3637  /* Nodo 1: */
3638 
3639  /* Offset del punto rispetto al nodo */
3640  const StructNode* pNode1 = pBeam->pGetNode(1);
3641 
3642  out << " " << pNode1->GetLabel();
3643 
3644  RF = ReferenceFrame(pNode1);
3645  Vec3 f1;
3646  Mat3x3 R1 = Eye3;
3647  if (i != 0) {
3648  if (pNode1 != pLastNode) {
3649  silent_cerr("line " << HP.GetLineData() << ": "
3650  "Beam(" << pBeam->GetLabel() << ")"
3651  ".Node1(" << pNode1->GetLabel() << ") "
3652  "and Beam(" << bc[i-1]->pGetBeam()->GetLabel() << ")"
3653  ".Node3(" << pLastNode->GetLabel() << ") "
3654  "do not match" << std::endl);
3655  throw DataManager::ErrGeneric(MBDYN_EXCEPT_ARGS);
3656  }
3657 
3658  if (HP.IsKeyWord("same")) {
3659  f1 = bc[i-1]->Getf(3);
3660  } else {
3661  f1 = HP.GetPosRel(RF);
3662  /* FIXME: allow tolerance? */
3663  if (!f1.IsExactlySame(bc[i-1]->Getf(3))) {
3664  silent_cerr("line " << HP.GetLineData() << ": "
3665  "Beam(" << pBeam->GetLabel() << ").f1 "
3666  "and Beam(" << bc[i-1]->pGetBeam()->GetLabel() << ").f3 "
3667  "do not match" << std::endl);
3668  throw DataManager::ErrGeneric(MBDYN_EXCEPT_ARGS);
3669  }
3670  }
3671  DEBUGLCOUT(MYDEBUG_INPUT, "Node 1 offset: " << f1 << std::endl);
3672 
3673  out << " " << f1;
3674 
3675  if (HP.IsKeyWord("hinge")) {
3676  if (HP.IsKeyWord("same")) {
3677  R1 = bc[i-1]->GetR(3);
3678  } else {
3679  R1 = HP.GetRotRel(RF);
3680  /* FIXME: allow tolerance? */
3681  if (!R1.IsExactlySame(bc[i-1]->GetR(3))) {
3682  silent_cerr("line " << HP.GetLineData() << ": "
3683  "Beam(" << pBeam->GetLabel() << ").R1 "
3684  "and Beam(" << bc[i-1]->pGetBeam()->GetLabel() << ").R3 "
3685  "do not match" << std::endl);
3686  throw DataManager::ErrGeneric(MBDYN_EXCEPT_ARGS);
3687  }
3688  }
3689  DEBUGLCOUT(MYDEBUG_INPUT, "Node 1 rotation matrix: "
3690  << std::endl << R1 << std::endl);
3691  }
3692 
3693  } else {
3694  f1 = HP.GetPosRel(RF);
3695  if (HP.IsKeyWord("hinge")) {
3696  R1 = HP.GetRotRel(RF);
3697  DEBUGLCOUT(MYDEBUG_INPUT, "Node 1 rotation matrix: "
3698  << std::endl << R1 << std::endl);
3699  }
3700  }
3701 
3702  out << " " << R1;
3703 
3704  /* Nodo 2: */
3705 
3706  /* Offset del punto rispetto al nodo */
3707  const StructNode* pNode2 = pBeam->pGetNode(2);
3708 
3709  out << " " << pNode2->GetLabel();
3710 
3711  RF = ReferenceFrame(pNode2);
3712  Vec3 f2(HP.GetPosRel(RF));
3713  DEBUGLCOUT(MYDEBUG_INPUT, "Node 2 offset: " << f2 << std::endl);
3714 
3715  out << " " << f2;
3716 
3717  Mat3x3 R2(Eye3);
3718  if (HP.IsKeyWord("hinge")) {
3719  R2 = HP.GetRotRel(RF);
3720  DEBUGLCOUT(MYDEBUG_INPUT,
3721  "Node 2 rotation matrix: " << std::endl
3722  << R2 << std::endl);
3723  }
3724 
3725  out << " " << R2;
3726 
3727  /* Nodo 3: */
3728 
3729  /* Offset del punto rispetto al nodo */
3730  const StructNode* pNode3 = pBeam->pGetNode(3);
3731 
3732  out << " " << pNode3->GetLabel();
3733 
3734  RF = ReferenceFrame(pNode3);
3735  Vec3 f3(HP.GetPosRel(RF));
3736  DEBUGLCOUT(MYDEBUG_INPUT, "Node 3 offset: " << f3 << std::endl);
3737 
3738  out << " " << f3;
3739 
3740  Mat3x3 R3(Eye3);
3741  if (HP.IsKeyWord("hinge")) {
3742  R3 = HP.GetRotRel(RF);
3743  DEBUGLCOUT(MYDEBUG_INPUT,
3744  "Node 3 rotation matrix: " << std::endl
3745  << R3 << std::endl);
3746  }
3747 
3748  out << " " << R3;
3749 
3750  pLastNode = pNode3;
3751 
3752  bc[i] = NULL;
3753  SAFENEWWITHCONSTRUCTOR(bc[i], BeamConn,
3754  BeamConn(pBeam, f1, f2, f3, R1, R2, R3));
3755  }
3756 
3757  unsigned uIB = 1;
3758  if (HP.IsKeyWord("initial" "beam")) {
3759  uIB = HP.GetInt();
3760 
3761  if (uIB < 1 || uIB > nB) {
3762  silent_cerr("illegal initial beam " << uIB
3763  << " at line " << HP.GetLineData()
3764  << std::endl);
3765  throw DataManager::ErrGeneric(MBDYN_EXCEPT_ARGS);
3766  }
3767  }
3768 
3769  unsigned uIN = 2;
3770  if (HP.IsKeyWord("initial" "node")) {
3771  uIN = HP.GetInt();
3772 
3773  if (uIN < 1 || uIN > 3) {
3774  silent_cerr("illegal initial node " << uIN
3775  << " at line " << HP.GetLineData() << std::endl);
3776  throw DataManager::ErrGeneric(MBDYN_EXCEPT_ARGS);
3777  }
3778  }
3779 
3780  doublereal dL = 0.;
3781  if (HP.IsKeyWord("smearing")) {
3782  dL = HP.GetReal();
3783  if (dL < 0. || dL > .4) {
3784  silent_cerr("illegal smearing factor " << dL << "; "
3785  "using default" << std::endl);
3786  dL = 0.;
3787  }
3788  }
3789 
3790  out << std::endl;
3791 
3792  flag fOut = pDM->fReadOutput(HP, Elem::JOINT);
3793  SAFENEWWITHCONSTRUCTOR(pEl, BeamSliderJoint,
3794  BeamSliderJoint(uLabel, pDO,
3795  pNode,
3796  sliderType,
3797  nB, bc,
3798  uIB, uIN, dL,
3799  f, R, fOut));
3800  } break;
3801 
3802  case MODAL:
3803  // FIXME: check whether it can be used in inverse dynamics
3804  pEl = ReadModal(pDM, HP, pDO, uLabel);
3805  break;
3806 
3807  case POINT_SURFACE_CONTACT: {
3808  /* leggo i due nodi */
3809  /* nodo collegato */
3810  StructNode* pNode1 = dynamic_cast<StructNode *>(pDM->ReadNode(HP, Node::STRUCTURAL));
3811 
3812 
3813  /* nodo superficie*/
3814  StructNode* pSup = dynamic_cast<StructNode *>(pDM->ReadNode(HP, Node::STRUCTURAL));
3815 
3816  /* leggo posizione e direzione della superficie nel sistema del nodo*/
3817  /* Normalizzo l'orientazione del terreno */
3818  Vec3 SupDirection;
3819  try {
3820  SupDirection = HP.GetUnitVecRel(ReferenceFrame(pSup));
3821  } catch (ErrNullNorm) {
3822  silent_cerr("PointSurfaceContact(" << uLabel << "): "
3823  "invalid direction at line " << HP.GetLineData()
3824  << std::endl);
3825  throw ErrNullNorm(MBDYN_EXCEPT_ARGS);
3826  }
3827 
3828  double ElasticStiffness = HP.GetReal();
3829 
3830  flag fOut = pDM->fReadOutput(HP, Elem::JOINT);
3831 
3832  SAFENEWWITHCONSTRUCTOR(pEl,
3833  PointSurfaceContact,
3834  PointSurfaceContact(uLabel, pDO,
3835  pNode1, pSup, SupDirection, ElasticStiffness, fOut));
3836 
3837  } break;
3838 
3839 #ifdef MBDYN_DEVEL
3840  case SCREWJOINT: {
3841  /* nodo collegato 1 */
3842  StructNode* pNode1 = dynamic_cast<StructNode *>(pDM->ReadNode(HP, Node::STRUCTURAL));
3843 
3844  ReferenceFrame RF(pNode1);
3845  Vec3 p(Zero3);
3846  if (HP.IsKeyWord("position")) {
3847 #ifdef MBDYN_X_COMPATIBLE_INPUT
3848  NO_OP;
3849  } else {
3850  pedantic_cerr("Joint(" << uLabel << "): "
3851  "missing keyword \"position\" at line "
3852  << HP.GetLineData() << std::endl);
3853  }
3854 #endif /* MBDYN_X_COMPATIBLE_INPUT */
3855  p = HP.GetPosRel(RF);
3856 #ifndef MBDYN_X_COMPATIBLE_INPUT
3857  }
3858 #endif /* MBDYN_X_COMPATIBLE_INPUT */
3859 
3860  DEBUGCOUT("Node 1 reference frame p:" << std::endl << p << std::endl);
3861 
3862  Mat3x3 R(Eye3);
3863  if (HP.IsKeyWord("orientation")) {
3864 #ifdef MBDYN_X_COMPATIBLE_INPUT
3865  NO_OP;
3866  } else {
3867  pedantic_cerr("Joint(" << uLabel << "): "
3868  "missing keyword \"orientation\" at line "
3869  << HP.GetLineData());
3870  }
3871 #endif /* MBDYN_X_COMPATIBLE_INPUT */
3872  R = HP.GetRotRel(RF);
3873 #ifndef MBDYN_X_COMPATIBLE_INPUT
3874  }
3875 #endif /* MBDYN_X_COMPATIBLE_INPUT */
3876 
3877  /* nodo collegato 2 */
3878  StructNode* pNode2 = dynamic_cast<StructNode *>(pDM->ReadNode(HP, Node::STRUCTURAL));
3879 
3880  Vec3 q(Zero3);
3881  // bool bOffset(false);
3882 
3883  if (HP.IsArg()) {
3884  if (HP.IsKeyWord("offset")) {
3885  // bOffset = true;
3886  q = HP.GetPosRel(ReferenceFrame(pNode2), RF, p);
3887  DEBUGCOUT("Node 2 reference frame q:" << std::endl << p << std::endl);
3888  }
3889  }
3890 
3891  if (!HP.IsKeyWord("pitch")) {
3892  silent_cerr("Joint(" << uLabel << "): "
3893  "missing keyword \"pitch\" at line "
3894  << HP.GetLineData());
3895  throw ErrGeneric(MBDYN_EXCEPT_ARGS);
3896  }
3897  doublereal pitch = HP.GetReal();
3898 
3899  flag fOut = pDM->fReadOutput(HP, Elem::JOINT);
3900 
3901  BasicFriction * bf = 0;
3902  BasicShapeCoefficient * bsh = 0;
3903  if (HP.IsKeyWord("friction")) {
3904  bf = ParseFriction(HP,pDM);
3905  bsh = ParseShapeCoefficient(HP);
3906  }
3907 
3908  SAFENEWWITHCONSTRUCTOR(pEl,
3909  ScrewJoint,
3910  ScrewJoint(uLabel, pDO, pNode1, pNode2,
3911  p, q, R, pitch, fOut, bsh, bf));
3912 
3913  std::ostream& out = pDM->GetLogFile();
3914  out << "screw: " << uLabel
3915  << " " << pNode1->GetLabel()
3916  << " " << p
3917  << " " << R
3918  << " " << pNode2->GetLabel()
3919  << " " << q
3920  << " " << Eye3
3921  << std::endl;
3922  } break;
3923 #endif // MBDYN_DEVEL
3924 
3925  /* Aggiungere qui altri vincoli */
3926 
3927  default:
3928  silent_cerr("Joint(" << uLabel << "): unknown joint type "
3929  "at line " << HP.GetLineData() << std::endl);
3930  throw DataManager::ErrGeneric(MBDYN_EXCEPT_ARGS);
3931  }
3932 
3933  /* Se non c'e' il punto e virgola finale */
3934  if (pEl == NULL) {
3935  DEBUGCERR("");
3936  silent_cerr("error in allocation of element "
3937  << uLabel << std::endl);
3938 
3939  throw ErrMemory(MBDYN_EXCEPT_ARGS);
3940  }
3941 
3942  if (HP.IsKeyWord("inverse" "dynamics")) {
3943  bIsTorque = false;
3944  if (HP.IsKeyWord("torque")) {
3945  bIsTorque = HP.GetYesNoOrBool(bIsTorque);
3946  }
3947 
3948  bIsPrescribedMotion = false;
3949  if (HP.IsKeyWord("prescribed" "motion")) {
3950  bIsPrescribedMotion = HP.GetYesNoOrBool(bIsPrescribedMotion);
3951  }
3952 
3953  if (HP.IsKeyWord("right" "hand" "side")) {
3954  bIsRightHandSide = HP.GetYesNoOrBool(bIsRightHandSide);
3955  }
3956 
3957  if (HP.IsKeyWord("ergonomy")) {
3958  bIsErgonomy = HP.GetYesNoOrBool(bIsErgonomy);
3959  if (bIsErgonomy) {
3960  ConstLawType::Type type = ConstLawType::UNKNOWN;
3961 
3962  if (const ConstitutiveLaw1DOwner *pCLDO = dynamic_cast<const ConstitutiveLaw1DOwner *>(pEl)) {
3963  type = pCLDO->pGetConstLaw()->GetConstLawType();
3964  } else if (const ConstitutiveLaw3DOwner *pCLDO = dynamic_cast<const ConstitutiveLaw3DOwner *>(pEl)) {
3965  type = pCLDO->pGetConstLaw()->GetConstLawType();
3966  } else if (const ConstitutiveLaw6DOwner *pCLDO = dynamic_cast<const ConstitutiveLaw6DOwner *>(pEl)) {
3967  type = pCLDO->pGetConstLaw()->GetConstLawType();
3968  } else {
3969  silent_cerr("Joint(" << uLabel << "): is \"ergonomy\" but cannot infer constitutive law type" << std::endl);
3970  throw ErrGeneric(MBDYN_EXCEPT_ARGS);
3971  }
3972 
3973  if (type != ConstLawType::ELASTIC) {
3974  silent_cerr("Joint(" << uLabel << "): invalid constitutive law type (must be ELASTIC)" << std::endl);
3975  throw ErrGeneric(MBDYN_EXCEPT_ARGS);
3976  }
3977 
3978  if (bIsRightHandSide) {
3979  silent_cerr("warning, Joint(" << uLabel << ") is both \"ergonomy\" and \"right hand side\"" << std::endl);
3980  }
3981  }
3982  }
3983  }
3984 
3985  // set flags for inverse dynamics
3986  if (pDM->bIsInverseDynamics() && pEl->bInverseDynamics()) {
3987  unsigned flags = 0;
3988  if (bIsTorque) {
3989  flags |= InverseDynamics::TORQUE;
3990  }
3991  if (bIsPrescribedMotion) {
3992  flags |= InverseDynamics::PRESCRIBED_MOTION;
3993  }
3994  if (bIsRightHandSide) {
3995  flags |= InverseDynamics::RIGHT_HAND_SIDE;
3996  }
3997  if (bIsErgonomy) {
3998  flags |= InverseDynamics::ERGONOMY;
3999  }
4000 
4001  if (flags == 0) {
4002  silent_cerr("warning, Joint(" << uLabel << ") is used for inverse dynamics but no flag (torque, prescribed motion, right hand side, ergonomy) is active." << std::endl);
4003  }
4004 
4005  pEl->SetInverseDynamicsFlags(flags);
4006  }
4007 
4008  if (HP.IsKeyWord("initial" "state")) {
4009  pEl->ReadInitialState(HP);
4010  /* FIXME: what if fails? */
4011  }
4012 
4013  if (HP.IsArg()) {
4014  silent_cerr("semicolon expected at line " << HP.GetLineData()
4015  << std::endl);
4016  throw DataManager::ErrGeneric(MBDYN_EXCEPT_ARGS);
4017  }
4018 
4019  return pEl;
4020 } /* ReadJoint() */
4021 
4022 
DataManager::fReadOutput
flag fReadOutput(MBDynParser &HP, const T &t) const
Definition: dataman.h:1064
BeamConn::Getf
const Vec3 & Getf(unsigned int i) const
Definition: beamslider.h:78
Brake
Definition: brake.h:42
grad::asin
GradientExpression< UnaryExpr< FuncAsin, Expr > > asin(const GradientExpression< Expr > &u)
Definition: gradient.h:2983
Zero3
const Vec3 Zero3(0., 0., 0.)
Vec3::Write
std::ostream & Write(std::ostream &out, const char *sFill=" ") const
Definition: matvec3.cc:738
flag
long int flag
Definition: mbdyn.h:43
MBDYN_EXCEPT_ARGS
#define MBDYN_EXCEPT_ARGS
Definition: except.h:63
Vec3
Definition: matvec3.h:98
DEBUGCOUTFNAME
#define DEBUGCOUTFNAME(fname)
Definition: myassert.h:256
StructNode::GetRCurr
virtual const Mat3x3 & GetRCurr(void) const
Definition: strnode.h:1012
Vec3::Norm
doublereal Norm(void) const
Definition: matvec3.h:263
Joint::Joint
Joint(unsigned int uL, const DofOwner *pD, flag fOut)
Definition: joint.cc:83
ASSERTMSG
#define ASSERTMSG(expr, msg)
Definition: myassert.h:219
Eye3
const Mat3x3 Eye3(1., 0., 0., 0., 1., 0., 0., 0., 1.)
OrientationDescription
OrientationDescription
Definition: matvec3.h:1597
ParseFriction
BasicFriction *const ParseFriction(MBDynParser &HP, DataManager *pDM)
Definition: friction.cc:608
DEBUGCERR
#define DEBUGCERR(msg)
Definition: myassert.h:235
ParseShapeCoefficient
BasicShapeCoefficient *const ParseShapeCoefficient(MBDynParser &HP)
Definition: friction.cc:662
NO_OP
#define NO_OP
Definition: myassert.h:74
ReadOptionalOrientationDescription
OrientationDescription ReadOptionalOrientationDescription(DataManager *pDM, MBDynParser &HP)
Definition: dataman3.cc:2531
ReadJoint
Elem * ReadJoint(DataManager *pDM, MBDynParser &HP, const DofOwner *pDO, unsigned int uLabel)
Definition: joint.cc:187
ViscousBody
Definition: vb.h:43
Vec3::IsExactlySame
bool IsExactlySame(const Vec3 &v) const
Definition: matvec3.h:476
Beam::pGetNode
virtual const StructNode * pGetNode(unsigned int i) const
Definition: beam.cc:1290
Mat3x3::MulTV
Vec3 MulTV(const Vec3 &v) const
Definition: matvec3.cc:482
Joint::bIsPrescribedMotion
bool bIsPrescribedMotion(void) const
Definition: joint.cc:170
ReadDCVecAbs
TplDriveCaller< Vec3 > * ReadDCVecAbs(const DataManager *pDM, MBDynParser &HP, const ReferenceFrame &rf)
Definition: tpldrive_impl.cc:923
point_contact.h
AbsRefFrame
const ReferenceFrame AbsRefFrame(0, Vec3(0., 0., 0), Mat3x3(1., 0., 0., 0., 1., 0., 0., 0., 1.), Vec3(0., 0., 0), Vec3(0., 0., 0), EULER_123)
ReadDC3D
TplDriveCaller< Vec3 > * ReadDC3D(const DataManager *pDM, MBDynParser &HP)
Definition: tpldrive_impl.cc:830
SAFENEW
#define SAFENEW(pnt, item)
Definition: mynewmem.h:695
MBDynParser::GetPosRel
Vec3 GetPosRel(const ReferenceFrame &rf)
Definition: mbpar.cc:1331
Mat3x3::IsExactlySame
bool IsExactlySame(const Mat3x3 &m) const
Definition: matvec3.h:1237
HighParser::IsKeyWord
virtual bool IsKeyWord(const char *sKeyWord)
Definition: parser.cc:910
Beam
Definition: beam.h:56
DEBUGCOUT
#define DEBUGCOUT(msg)
Definition: myassert.h:232
RotManip::Rot
Mat3x3 Rot(const Vec3 &phi)
Definition: Rot.cc:62
Elem::GetInverseDynamicsFlags
unsigned GetInverseDynamicsFlags(void) const
Definition: elem.cc:77
DataManager::PushCurrData
bool PushCurrData(const std::string &name, const TypedValue &value)
Definition: dataman.cc:930
OutputHandler::IsOpen
bool IsOpen(int out) const
Definition: output.cc:395
BeamConn::pGetBeam
const Beam * pGetBeam(void) const
Definition: beamslider.h:68
ASSERT
#define ASSERT(expression)
Definition: colamd.c:977
DataManager::PopCurrData
bool PopCurrData(const std::string &name)
Definition: dataman.cc:941
KeyWords
KeyWords
Definition: dataman4.cc:94
Mat3x3::MulTM
Mat3x3 MulTM(const Mat3x3 &m) const
Definition: matvec3.cc:500
grad::Cross
VectorExpression< VectorCrossExpr< VectorLhsExpr, VectorRhsExpr >, 3 > Cross(const VectorExpression< VectorLhsExpr, 3 > &u, const VectorExpression< VectorRhsExpr, 3 > &v)
Definition: matvec.h:3248
Joint::OutputPrepare_int
virtual void OutputPrepare_int(const std::string &type, OutputHandler &OH, std::string &name)
Definition: joint.cc:107
SAFENEWWITHCONSTRUCTOR
#define SAFENEWWITHCONSTRUCTOR(pnt, item, constructor)
Definition: mynewmem.h:698
StructDispNode::GetXCurr
virtual const Vec3 & GetXCurr(void) const
Definition: strnode.h:310
grad::Norm
VectorExpression< VectorExpr, N_rows >::ScalarType Norm(const VectorExpression< VectorExpr, N_rows > &u)
Definition: matvec.h:3163
BeamConn::GetR
const Mat3x3 & GetR(unsigned int i) const
Definition: beamslider.h:84
ConstitutiveLaw::iGetNumDof
virtual unsigned int iGetNumDof(void) const
Definition: constltp.h:138
HighParser::IsArg
virtual bool IsArg(void)
Definition: parser.cc:807
Elem
Definition: elem.h:75
Joint::bIsTorque
bool bIsTorque(void) const
Definition: joint.cc:176
psElemNames
const char * psElemNames[]
Definition: enums.cc:39
DataManager::GetLogFile
std::ostream & GetLogFile(void) const
Definition: dataman.h:326
Joint::GetElemType
virtual Elem::Type GetElemType(void) const
Definition: joint.h:187
SAFENEWARR
#define SAFENEWARR(pnt, item, sz)
Definition: mynewmem.h:701
Joint::~Joint
virtual ~Joint(void)
Definition: joint.cc:100
invdyn2str
const char * invdyn2str(InverseDynamics::Order iOrder)
Definition: invdyn.cc:36
MBDynParser::GetDriveCaller
DriveCaller * GetDriveCaller(bool bDeferred=false)
Definition: mbpar.cc:2033
Joint
Definition: joint.h:50
Joint::Update
virtual void Update(const VectorHandler &XCurr, InverseDynamics::Order iOrder=InverseDynamics::INVERSE_DYNAMICS)
Definition: joint.cc:163
doublereal
double doublereal
Definition: colamd.c:52
NestedElem::pGetElem
virtual Elem * pGetElem(void) const
Definition: nestedelem.cc:60
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
IncludeParser::GetLineData
virtual HighParser::ErrOut GetLineData(void) const
Definition: parsinc.cc:697
Rod
Definition: rodj.h:45
WithLabel::GetLabel
unsigned int GetLabel(void) const
Definition: withlab.cc:62
DataManager::ReadNode
Node * ReadNode(MBDynParser &HP, Node::Type type) const
Definition: dataman3.cc:2309
DEBUGLCOUT
#define DEBUGLCOUT(level, msg)
Definition: myassert.h:244
R
Mat3x3 R
Definition: mbdyn_util_oct.cc:74
ReadDCVecRel
TplDriveCaller< Vec3 > * ReadDCVecRel(const DataManager *pDM, MBDynParser &HP, const ReferenceFrame &rf)
Definition: tpldrive_impl.cc:917