MBDyn-1.7.3
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Groups
drvdisp.cc
Go to the documentation of this file.
1 /* $Header: /var/cvs/mbdyn/mbdyn/mbdyn-1.0/mbdyn/struct/drvdisp.cc,v 1.32 2017/01/12 14:46:43 masarati Exp $ */
2 /*
3  * MBDyn (C) is a multibody analysis code.
4  * http://www.mbdyn.org
5  *
6  * Copyright (C) 1996-2017
7  *
8  * Pierangelo Masarati <masarati@aero.polimi.it>
9  * Paolo Mantegazza <mantegazza@aero.polimi.it>
10  *
11  * Dipartimento di Ingegneria Aerospaziale - Politecnico di Milano
12  * via La Masa, 34 - 20156 Milano, Italy
13  * http://www.aero.polimi.it
14  *
15  * Changing this copyright notice is forbidden.
16  *
17  * This program is free software; you can redistribute it and/or modify
18  * it under the terms of the GNU General Public License as published by
19  * the Free Software Foundation (version 2 of the License).
20  *
21  *
22  * This program is distributed in the hope that it will be useful,
23  * but WITHOUT ANY WARRANTY; without even the implied warranty of
24  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
25  * GNU General Public License for more details.
26  *
27  * You should have received a copy of the GNU General Public License
28  * along with this program; if not, write to the Free Software
29  * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
30  */
31 
32 /* Cerniera pilotata */
33 
34 #include "mbconfig.h" /* This goes first in every *.c,*.cc file */
35 
36 #include "dataman.h"
37 #include "drvdisp.h"
38 #include "hint_impl.h"
39 
40 /* DriveDisplacementJoint - begin */
41 
42 /* Costruttore non banale */
44  const DofOwner* pDO,
45  const TplDriveCaller<Vec3>* pDC,
46  const StructNode* pN1,
47  const StructNode* pN2,
48  const Vec3& f1,
49  const Vec3& f2,
50  flag fOut)
51 : Elem(uL, fOut),
52 Joint(uL, pDO, fOut),
53 TplDriveOwner<Vec3>(pDC),
54 pNode1(pN1), pNode2(pN2), f1(f1), f2(f2),
55 R1Ref(Eye3),
56 RRef(Eye3),
57 f2Ref(Zero3),
58 dRef(Zero3),
59 F(Zero3)
60 {
61  ASSERT(pNode1 != NULL);
62  ASSERT(pNode2 != NULL);
65 }
66 
67 
68 /* Distruttore */
70 {
71  NO_OP;
72 }
73 
74 
75 /* Contributo al file di restart */
76 std::ostream&
77 DriveDisplacementJoint::Restart(std::ostream& out) const
78 {
79  Joint::Restart(out) << ", drive displacement, "
80  << pNode1->GetLabel() << ", reference, node, ",
81  f1.Write(out, ", ") << ", "
82  << pNode2->GetLabel() << ", reference, node, ",
83  f2.Write(out, ", ") << ", ",
84  pGetDriveCaller()->Restart(out) << ';' << std::endl;
85  return out;
86 }
87 
88 
89 void
91 {
92  if (bToBeOutput()) {
94  - pNode1->GetXCurr() - pNode1->GetRCurr()*f1);
95  Joint::Output(OH.Joints(), "DriveDisplacementJoint", GetLabel(),
97  << " " << d << std::endl;
98  }
99 }
100 
101 void
105 {
106  if (ph) {
107  for (unsigned i = 0; i < ph->size(); i++) {
108  Joint::JointHint *pjh = dynamic_cast<Joint::JointHint *>((*ph)[i]);
109 
110  if (pjh) {
111  if (dynamic_cast<Joint::OffsetHint<1> *>(pjh)) {
112  Mat3x3 R1t(pNode1->GetRCurr().Transpose());
113  Vec3 fTmp2(pNode2->GetRCurr()*f2);
114 
115  f1 = R1t*(pNode2->GetXCurr() + fTmp2 - pNode1->GetXCurr() - Get());
116 
117  } else if (dynamic_cast<Joint::OffsetHint<2> *>(pjh)) {
118  Mat3x3 R2t(pNode2->GetRCurr().Transpose());
119  Vec3 fTmp1(pNode1->GetRCurr()*f1);
120 
121  f2 = R2t*(pNode1->GetXCurr() + fTmp1 - pNode2->GetXCurr() + Get());
122 
123  } else if (dynamic_cast<Joint::ReactionsHint *>(pjh)) {
124  /* TODO */
125  }
126  continue;
127  }
128 
129  TplDriveHint<Vec3> *pdh = dynamic_cast<TplDriveHint<Vec3> *>((*ph)[i]);
130 
131  if (pdh) {
132  pedantic_cout("DriveDisplacementJoint(" << uLabel << "): "
133  "creating drive from hint[" << i << "]..." << std::endl);
134 
135  TplDriveCaller<Vec3> *pDC = pdh->pCreateDrive(pDM);
136  if (pDC == 0) {
137  silent_cerr("DriveDisplacementJoint(" << uLabel << "): "
138  "unable to create drive "
139  "after hint #" << i << std::endl);
141  }
142 
144  continue;
145  }
146  }
147  }
148 }
149 
150 Hint *
152 {
153  if (strncasecmp(s, "offset{" /*}*/ , STRLENOF("offset{" /*}*/ )) == 0)
154  {
155  s += STRLENOF("offset{" /*}*/ );
156 
157  if (strcmp(&s[1], /*{*/ "}") != 0) {
158  return 0;
159  }
160 
161  switch (s[0]) {
162  case '1':
163  return new Joint::OffsetHint<1>;
164 
165  case '2':
166  return new Joint::OffsetHint<2>;
167  }
168  }
169 
170  /* take care of "drive" hint... */
171  return SimulationEntity::ParseHint(pDM, s);
172 }
173 
174 std::ostream&
176  const char *prefix, bool bInitial) const
177 {
178  integer iIndex = iGetFirstIndex();
179 
180  out
181  << prefix << iIndex + 1 << "->" << iIndex + 3 << ": "
182  "reaction forces [fx,fy,fz]" << std::endl;
183 
184  if (bInitial) {
185  iIndex += 3;
186  out
187  << prefix << iIndex + 1 << "->" << iIndex + 3 << ": "
188  "reaction force derivatives [fPx,fPy,fPz]" << std::endl;
189  }
190 
191  return out;
192 }
193 
194 static const char xyz[] = "xyz";
195 static const char *dof[] = { "reaction force f", "reaction force derivative fP" };
196 static const char *eq[] = { "displacement constraint P", "displacement constraint derivative v" };
197 
198 void
199 DriveDisplacementJoint::DescribeDof(std::vector<std::string>& desc,
200  bool bInitial, int i) const
201 {
202  int iend = 1;
203  if (i == -1) {
204  if (bInitial) {
205  iend = 6;
206 
207  } else {
208  iend = 3;
209  }
210  }
211  desc.resize(iend);
212 
213  std::ostringstream os;
214  os << "DriveDisplacementJoint(" << GetLabel() << ")";
215 
216  if (i == -1) {
217  std::string name = os.str();
218  for (i = 0; i < iend; i++) {
219  os.str(name);
220  os.seekp(0, std::ios_base::end);
221  os << ": " << dof[i/3] << xyz[i%3];
222 
223  desc[i] = os.str();
224  }
225 
226  } else {
227  os << ": " << dof[i/3] << xyz[i%3];
228  desc[0] = os.str();
229  }
230 }
231 
232 std::ostream&
234  const char *prefix, bool bInitial) const
235 {
236  integer iIndex = iGetFirstIndex();
237 
238  out
239  << prefix << iIndex + 1 << "->" << iIndex + 3 << ": "
240  "position constraints [px1=px2,py1=py2,pz1=pz2]" << std::endl;
241 
242  if (bInitial) {
243  iIndex += 3;
244  out
245  << prefix << iIndex + 1 << "->" << iIndex + 3 << ": "
246  "velocity constraints [vx1=vx2,vy1=vy2,vz1=vz2]" << std::endl;
247  }
248 
249  return out;
250 
251 }
252 
253 void
254 DriveDisplacementJoint::DescribeEq(std::vector<std::string>& desc,
255  bool bInitial, int i) const
256 {
257  int iend = 1;
258  if (i == -1) {
259  if (bInitial) {
260  iend = 6;
261 
262  } else {
263  iend = 3;
264  }
265  }
266  desc.resize(iend);
267 
268  std::ostringstream os;
269  os << "DriveDisplacementJoint(" << GetLabel() << ")";
270 
271  if (i == -1) {
272  std::string name = os.str();
273  for (i = 0; i < iend; i++) {
274  os.str(name);
275  os.seekp(0, std::ios_base::end);
276  os << ": " << eq[i/3] << xyz[i%3];
277 
278  desc[i] = os.str();
279  }
280 
281  } else {
282  os << ": " << eq[i/3] << xyz[i%3];
283  desc[0] = os.str();
284  }
285 }
286 
287 /* Dati privati (aggiungere magari le reazioni vincolari) */
288 unsigned int
290 {
291  return 6;
292 };
293 
294 unsigned int
296 {
297  ASSERT(s != NULL);
298  ASSERT(s[0] != '\0');
299 
300  if (s[2] != '\0') {
301  return 0;
302  }
303 
304  unsigned int idx = 0;
305 
306  switch (s[0]) {
307  case 'f':
308  idx += 3;
309  /* fallthru */
310  case 'd':
311  break;
312 
313  default:
314  return 0;
315  }
316 
317  switch (s[1]) {
318  case 'x':
319  return idx + 1;
320  case 'y':
321  return idx + 2;
322  case 'z':
323  return idx + 3;
324  }
325 
326  return 0;
327 }
328 
331 {
332  ASSERT(i >= 1 && i <= 6);
333 
334  switch (i) {
335  case 1:
336  case 2:
337  case 3:
338  return Get().dGet(i);
339 
340  case 4:
341  case 5:
342  case 6:
343  return F(i - 3);
344  }
345 
347 }
348 
349 /* assemblaggio jacobiano */
352  doublereal dCoef,
353  const VectorHandler& /* XCurr */ ,
354  const VectorHandler& /* XPrimeCurr */ )
355 {
356  DEBUGCOUT("Entering DriveDisplacementJoint::AssJac()" << std::endl);
357 
358  FullSubMatrixHandler& WM = WorkMat.SetFull();
359 
360  /* Dimensiona e resetta la matrice di lavoro */
361  integer iNumRows = 0;
362  integer iNumCols = 0;
363  WorkSpaceDim(&iNumRows, &iNumCols);
364  WM.ResizeReset(iNumRows, iNumCols);
365 
366  /* Recupera gli indici */
367  integer iNode1FirstPosIndex = pNode1->iGetFirstPositionIndex();
368  integer iNode1FirstMomIndex = pNode1->iGetFirstMomentumIndex();
369  integer iNode2FirstPosIndex = pNode2->iGetFirstPositionIndex();
370  integer iNode2FirstMomIndex = pNode2->iGetFirstMomentumIndex();
371  integer iFirstReactionIndex = iGetFirstIndex();
372 
373  /* Setta gli indici della matrice */
374  for (int iCnt = 1; iCnt <= 6; iCnt++) {
375  WM.PutRowIndex(iCnt, iNode1FirstMomIndex + iCnt);
376  WM.PutColIndex(iCnt, iNode1FirstPosIndex + iCnt);
377  WM.PutRowIndex(6 + iCnt, iNode2FirstMomIndex + iCnt);
378  WM.PutColIndex(6 + iCnt, iNode2FirstPosIndex + iCnt);
379  }
380  for (int iCnt = 1; iCnt <= 3; iCnt++) {
381  WM.PutRowIndex(12 + iCnt, iFirstReactionIndex + iCnt);
382  WM.PutColIndex(12 + iCnt, iFirstReactionIndex + iCnt);
383  }
384 
385  AssMat(WM, dCoef);
386 
387  return WorkMat;
388 }
389 
390 
391 void
393  VectorHandler& /* XP */ )
394 {
395  /* Recupera i dati */
396  f2Ref = pNode2->GetRRef()*f2;
397  dRef = pNode1->GetRRef()*(f1 + Get());
398 }
399 
400 
401 void
403 {
404  for (int iCnt = 1; iCnt <= 3; iCnt++) {
405  /* node 1 force */
406  WM.DecCoef(iCnt, 12 + iCnt, 1.);
407  /* node 2 force */
408  WM.IncCoef(6 + iCnt, 12 + iCnt, 1.);
409 
410  /* node 1 constraint */
411  WM.DecCoef(12 + iCnt, iCnt, 1.);
412  /* node 2 constraint */
413  WM.IncCoef(12 + iCnt, 6 + iCnt, 1.);
414  }
415 
416  Mat3x3 MTmp(MatCross, dRef);
417 
418  /* node 1 moment */
419  WM.Sub(3 + 1, 12 + 1, MTmp);
420  /* node 1 constraint */
421  WM.Add(12 + 1, 3 + 1, MTmp);
422 
423  MTmp = Mat3x3(MatCross, f2Ref);
424  /* node 2 moment */
425  WM.Add(9 + 1, 12 + 1, MTmp);
426  /* node 2 constraint */
427  WM.Sub(12 + 1, 9 + 1, MTmp);
428 
429  MTmp = Mat3x3(MatCrossCross, F, dRef*dCoef);
430  /* node 1 moment */
431  WM.Sub(3 + 1, 3 + 1, MTmp);
432 
433  MTmp = Mat3x3(MatCrossCross, F, f2Ref*dCoef);
434  /* node 2 moment */
435  WM.Add(9 + 1, 9 + 1, MTmp);
436 }
437 
438 
439 /* assemblaggio residuo */
442  doublereal dCoef,
443  const VectorHandler& XCurr,
444  const VectorHandler& /* XPrimeCurr */ )
445 {
446  DEBUGCOUT("Entering DriveDisplacementJoint::AssRes()" << std::endl);
447 
448  /* Dimensiona e resetta la matrice di lavoro */
449  integer iNumRows = 0;
450  integer iNumCols = 0;
451  WorkSpaceDim(&iNumRows, &iNumCols);
452  WorkVec.ResizeReset(iNumRows);
453 
454  /* Recupera gli indici */
455  integer iNode1FirstMomIndex = pNode1->iGetFirstMomentumIndex();
456  integer iNode2FirstMomIndex = pNode2->iGetFirstMomentumIndex();
457  integer iFirstReactionIndex = iGetFirstIndex();
458 
459  /* Setta gli indici della matrice */
460  for (int iCnt = 1; iCnt <= 6; iCnt++) {
461  WorkVec.PutRowIndex(iCnt, iNode1FirstMomIndex + iCnt);
462  WorkVec.PutRowIndex(6 + iCnt, iNode2FirstMomIndex + iCnt);
463  }
464 
465  for (int iCnt = 1; iCnt <= 3; iCnt++) {
466  WorkVec.PutRowIndex(12 + iCnt, iFirstReactionIndex + iCnt);
467  }
468 
469  F = Vec3(XCurr, iFirstReactionIndex + 1);
470 
471  AssVec(WorkVec, dCoef);
472 
473  return WorkVec;
474 }
475 
476 
477 void
479 {
480  Mat3x3 R1(pNode1->GetRCurr());
481 
482  Vec3 f2Tmp = pNode2->GetRCurr()*f2;
483  Vec3 d = pNode1->GetRCurr()*(f1 + Get());
484 
485  WorkVec.Add(1, F);
486  WorkVec.Add(3 + 1, d.Cross(F));
487  WorkVec.Sub(6 + 1, F);
488  WorkVec.Sub(9 + 1, f2Tmp.Cross(F));
489 
490  ASSERT(dCoef != 0.);
491  WorkVec.Sub(12 + 1, (pNode2->GetXCurr() + f2Tmp - pNode1->GetXCurr() - d)/dCoef);
492 }
493 
494 
495 /* Contributo allo jacobiano durante l'assemblaggio iniziale */
498  const VectorHandler& XCurr)
499 {
500  DEBUGCOUT("Entering DriveDisplacementJoint::InitialAssJac()" << std::endl);
501 
502  FullSubMatrixHandler& WM = WorkMat.SetFull();
503 
504  /* Dimensiona e resetta la matrice di lavoro */
505  integer iNumRows = 0;
506  integer iNumCols = 0;
507  InitialWorkSpaceDim(&iNumRows, &iNumCols);
508  WM.ResizeReset(iNumRows, iNumCols);
509 
510  /* Recupera gli indici */
511  integer iNode1FirstPosIndex = pNode1->iGetFirstPositionIndex();
512  integer iNode2FirstPosIndex = pNode2->iGetFirstPositionIndex();
513  integer iFirstReactionIndex = iGetFirstIndex();
514  integer iNode1FirstVelIndex = iNode1FirstPosIndex + 6;
515  integer iNode2FirstVelIndex = iNode2FirstPosIndex + 6;
516  integer iFirstReactionPrimeIndex = iFirstReactionIndex + 3;
517 
518  /* Setta gli indici della matrice */
519  for (int iCnt = 1; iCnt <= 6; iCnt++) {
520  WM.PutRowIndex(iCnt, iNode1FirstPosIndex + iCnt);
521  WM.PutColIndex(iCnt, iNode1FirstPosIndex + iCnt);
522  WM.PutRowIndex(6 + iCnt, iNode1FirstVelIndex + iCnt);
523  WM.PutColIndex(6 + iCnt, iNode1FirstVelIndex + iCnt);
524  WM.PutRowIndex(12 + iCnt, iNode2FirstPosIndex + iCnt);
525  WM.PutColIndex(12 + iCnt, iNode2FirstPosIndex + iCnt);
526  WM.PutRowIndex(18 + iCnt, iNode2FirstVelIndex + iCnt);
527  WM.PutColIndex(18 + iCnt, iNode2FirstVelIndex + iCnt);
528  WM.PutRowIndex(24 + iCnt, iFirstReactionIndex + iCnt);
529  WM.PutColIndex(24 + iCnt, iFirstReactionIndex + iCnt);
530  }
531 
532  Vec3 FPrime = Vec3(XCurr, iFirstReactionPrimeIndex + 1);
533 
534  for (int iCnt = 1; iCnt <= 3; iCnt++) {
535  /* node 1 force */
536  WM.DecCoef(iCnt, 24 + iCnt, 1.);
537  /* node 2 force */
538  WM.IncCoef(12 + iCnt, 24 + iCnt, 1.);
539 
540  /* node 1 force derivative */
541  WM.DecCoef(6 + iCnt, 27 + iCnt, 1.);
542  /* node 2 force derivative */
543  WM.IncCoef(18 + iCnt, 27 + iCnt, 1.);
544 
545  /* node 1 constraint */
546  WM.DecCoef(24 + iCnt, iCnt, 1.);
547  /* node 2 constraint */
548  WM.IncCoef(24 + iCnt, 12 + iCnt, 1.);
549 
550  /* node 1 constraint derivative */
551  WM.DecCoef(27 + iCnt, 6 + iCnt, 1.);
552  /* node 2 constraint derivative */
553  WM.IncCoef(27 + iCnt, 18 + iCnt, 1.);
554  }
555 
556  Mat3x3 MTmp(MatCross, dRef);
557 
558  /* node 1 moment */
559  WM.Sub(3 + 1, 24 + 1, MTmp);
560  /* node 1 moment derivative */
561  WM.Sub(9 + 1, 27 + 1, MTmp);
562  /* node 1 constraint */
563  WM.Add(24 + 1, 3 + 1, MTmp);
564  /* node 1 constraint derivative */
565  WM.Add(27 + 1, 9 + 1, MTmp);
566 
567  /* in case the drive is differentiable... */
568  if (bIsDifferentiable()) {
569  WM.Add(27 + 1, 3 + 1, Mat3x3(MatCross, pNode1->GetRCurr()*GetP()));
570  }
571 
572  MTmp = Mat3x3(MatCross, f2Ref);
573  /* node 2 moment */
574  WM.Add(15 + 1, 24 + 1, MTmp);
575  /* node 2 moment derivatives */
576  WM.Add(21 + 1, 27 + 1, MTmp);
577  /* node 2 constraint */
578  WM.Sub(24 + 1, 15 + 1, MTmp);
579  /* node 2 constraint derivative */
580  WM.Sub(27 + 1, 21 + 1, MTmp);
581 
582  MTmp = Mat3x3(MatCrossCross, F, dRef);
583  /* node 1 moment */
584  WM.Sub(3 + 1, 3 + 1, MTmp);
585 
586  MTmp = Mat3x3(MatCrossCross, FPrime, dRef);
587  /* node 1 moment derivative */
588  WM.Sub(9 + 1, 9 + 1, MTmp);
589 
590  MTmp = Mat3x3(MatCrossCross, F, f2Ref);
591  /* node 2 moment */
592  WM.Add(15 + 1, 15 + 1, MTmp);
593 
594  MTmp = Mat3x3(MatCrossCross, FPrime, f2Ref);
595  /* node 2 moment derivative */
596  WM.Add(21 + 1, 21 + 1, MTmp);
597 
598  MTmp = Mat3x3(MatCrossCross, pNode1->GetWRef(), dRef);
599  /* node 2 constraint derivative */
600  WM.Sub(27 + 1, 3 + 1, MTmp);
601 
602  MTmp = Mat3x3(MatCrossCross, pNode2->GetWRef(), f2Ref);
603  /* node 2 constraint derivative */
604  WM.Add(27 + 1, 15 + 1, MTmp);
605 
606  return WorkMat;
607 }
608 
609 
610 /* Contributo al residuo durante l'assemblaggio iniziale */
613  const VectorHandler& XCurr)
614 {
615  DEBUGCOUT("Entering DriveDisplacementJoint::InitialAssRes()" << std::endl);
616 
617  /* Dimensiona e resetta la matrice di lavoro */
618  integer iNumRows = 0;
619  integer iNumCols = 0;
620  InitialWorkSpaceDim(&iNumRows, &iNumCols);
621  WorkVec.ResizeReset(iNumRows);
622 
623  /* Recupera gli indici */
624  integer iNode1FirstPosIndex = pNode1->iGetFirstPositionIndex();
625  integer iNode2FirstPosIndex = pNode2->iGetFirstPositionIndex();
626  integer iFirstReactionIndex = iGetFirstIndex();
627  integer iNode1FirstVelIndex = iNode1FirstPosIndex + 6;
628  integer iNode2FirstVelIndex = iNode2FirstPosIndex + 6;
629  integer iReactionPrimeIndex = iFirstReactionIndex + 3;
630 
631  /* Setta gli indici del vettore */
632  for (int iCnt = 1; iCnt <= 6; iCnt++) {
633  WorkVec.PutRowIndex(iCnt, iNode1FirstPosIndex + iCnt);
634  WorkVec.PutRowIndex(6 + iCnt, iNode1FirstVelIndex + iCnt);
635  WorkVec.PutRowIndex(12 + iCnt, iNode2FirstPosIndex + iCnt);
636  WorkVec.PutRowIndex(18 + iCnt, iNode2FirstVelIndex + iCnt);
637  WorkVec.PutRowIndex(24 + iCnt, iFirstReactionIndex + iCnt);
638  }
639 
640  F = Vec3(XCurr, iFirstReactionIndex + 1);
641  Vec3 FPrime = Vec3(XCurr, iReactionPrimeIndex + 1);
642 
643  dRef = pNode1->GetRCurr()*(f1 + Get());
644  f2Ref = pNode2->GetRCurr()*f2;
645 
646  WorkVec.Add(1, F);
647  WorkVec.Add(3 + 1, dRef.Cross(F));
648  WorkVec.Add(6 + 1, FPrime);
649  WorkVec.Add(9 + 1, dRef.Cross(FPrime));
650 
651  WorkVec.Sub(12 + 1, F);
652  WorkVec.Sub(15 + 1, f2Ref.Cross(F));
653  WorkVec.Sub(18 + 1, FPrime);
654  WorkVec.Sub(21 + 1, f2Ref.Cross(FPrime));
655 
656  WorkVec.Add(24 + 1, pNode1->GetXCurr() + dRef - pNode2->GetXCurr() - f2Ref);
657 
658  /* in case the drive is differentiable... */
659  Vec3 PhiPrime = pNode1->GetVCurr() + pNode1->GetWCurr().Cross(dRef)
661  if (bIsDifferentiable()) {
662  PhiPrime += pNode1->GetRCurr()*GetP();
663  }
664  WorkVec.Add(27 + 1, PhiPrime);
665 
666  return WorkVec;
667 }
668 
669 /* DriveDisplacementJoint - end */
670 
671 /* DriveDisplacementPinJoint - begin */
672 
673 /* Costruttore non banale */
675  const DofOwner* pDO,
676  const TplDriveCaller<Vec3>* pDC,
677  const StructNode* pN,
678  const Vec3& f,
679  const Vec3& x,
680  flag fOut)
681 : Elem(uL, fOut),
682 Joint(uL, pDO, fOut),
683 TplDriveOwner<Vec3>(pDC),
684 pNode(pN), f(f), x(x),
685 fRef(Zero3),
686 dRef(Zero3),
687 F(Zero3)
688 {
689  ASSERT(pNode != NULL);
691 }
692 
693 
694 /* Distruttore */
696 {
697  NO_OP;
698 }
699 
700 
701 /* Contributo al file di restart */
702 std::ostream&
703 DriveDisplacementPinJoint::Restart(std::ostream& out) const
704 {
705  Joint::Restart(out) << ", drive displacement pin, "
706  << pNode->GetLabel() << ", reference, node, ",
707  f.Write(out, ", ") << ", ",
708  x.Write(out, ", ") << ", ",
709  pGetDriveCaller()->Restart(out) << ';' << std::endl;
710  return out;
711 }
712 
713 
714 void
716 {
717  if (bToBeOutput()) {
718  Vec3 d(pNode->GetXCurr() + pNode->GetRCurr()*f - x);
719  Joint::Output(OH.Joints(), "DriveDisplacementPinJoint", GetLabel(),
720  F, Zero3, F, Zero3)
721  << " " << d << std::endl;
722  }
723 }
724 
725 void
729 {
730  if (ph) {
731  for (unsigned i = 0; i < ph->size(); i++) {
732  Joint::JointHint *pjh = dynamic_cast<Joint::JointHint *>((*ph)[i]);
733 
734  if (pjh) {
735  if (dynamic_cast<Joint::OffsetHint<1> *>(pjh)) {
736  Mat3x3 Rt(pNode->GetRCurr().Transpose());
737 
738  f = Rt*(x + Get() - pNode->GetXCurr());
739 
740  } else if (dynamic_cast<Joint::OffsetHint<0> *>(pjh)) {
741  Vec3 fTmp(pNode->GetRCurr()*f);
742 
743  x = pNode->GetXCurr() + fTmp - Get();
744 
745  } else if (dynamic_cast<Joint::ReactionsHint *>(pjh)) {
746  /* TODO */
747  }
748  continue;
749  }
750 
751  TplDriveHint<Vec3> *pdh = dynamic_cast<TplDriveHint<Vec3> *>((*ph)[i]);
752 
753  if (pdh) {
754  pedantic_cout("DriveDisplacementPinJoint(" << uLabel << "): "
755  "creating drive from hint[" << i << "]..." << std::endl);
756 
757  TplDriveCaller<Vec3> *pDC = pdh->pCreateDrive(pDM);
758  if (pDC == 0) {
759  silent_cerr("DriveDisplacementPinJoint(" << uLabel << "): "
760  "unable to create drive "
761  "after hint #" << i << std::endl);
763  }
764 
766  continue;
767  }
768  }
769  }
770 }
771 
772 Hint *
774 {
775  if (strncasecmp(s, "offset{" /*}*/ , STRLENOF("offset{" /*}*/ )) == 0)
776  {
777  s += STRLENOF("offset{" /*}*/ );
778 
779  if (strcmp(&s[1], /*{*/ "}") != 0) {
780  return 0;
781  }
782 
783  switch (s[0]) {
784  case '1':
785  return new Joint::OffsetHint<1>;
786 
787  case '0':
788  return new Joint::OffsetHint<0>;
789  }
790  }
791 
792  /* take care of "drive" hint... */
793  return SimulationEntity::ParseHint(pDM, s);
794 }
795 
796 std::ostream&
798  const char *prefix, bool bInitial) const
799 {
800  integer iIndex = iGetFirstIndex();
801 
802  out
803  << prefix << iIndex + 1 << "->" << iIndex + 3 << ": "
804  "reaction forces [fx,fy,fz]" << std::endl;
805 
806  if (bInitial) {
807  iIndex += 3;
808  out
809  << prefix << iIndex + 1 << "->" << iIndex + 3 << ": "
810  "reaction force derivatives [fPx,fPy,fPz]" << std::endl;
811  }
812 
813  return out;
814 }
815 
816 void
817 DriveDisplacementPinJoint::DescribeDof(std::vector<std::string>& desc,
818  bool bInitial, int i) const
819 {
820  int iend = 1;
821  if (i == -1) {
822  if (bInitial) {
823  iend = 6;
824 
825  } else {
826  iend = 3;
827  }
828  }
829  desc.resize(iend);
830 
831  std::ostringstream os;
832  os << "DriveDisplacementPinJoint(" << GetLabel() << ")";
833 
834  if (i == -1) {
835  std::string name = os.str();
836  for (i = 0; i < iend; i++) {
837  os.str(name);
838  os.seekp(0, std::ios_base::end);
839  os << ": " << dof[i/3] << xyz[i%3];
840 
841  desc[i] = os.str();
842  }
843 
844  } else {
845  os << ": " << dof[i/3] << xyz[i%3];
846  desc[0] = os.str();
847  }
848 }
849 
850 std::ostream&
852  const char *prefix, bool bInitial) const
853 {
854  integer iIndex = iGetFirstIndex();
855 
856  out
857  << prefix << iIndex + 1 << "->" << iIndex + 3 << ": "
858  "position constraints [px1=px2,py1=py2,pz1=pz2]" << std::endl;
859 
860  if (bInitial) {
861  iIndex += 3;
862  out
863  << prefix << iIndex + 1 << "->" << iIndex + 3 << ": "
864  "velocity constraints [vx1=vx2,vy1=vy2,vz1=vz2]" << std::endl;
865  }
866 
867  return out;
868 
869 }
870 
871 void
872 DriveDisplacementPinJoint::DescribeEq(std::vector<std::string>& desc,
873  bool bInitial, int i) const
874 {
875  int iend = 1;
876  if (i == -1) {
877  if (bInitial) {
878  iend = 6;
879 
880  } else {
881  iend = 3;
882  }
883  }
884  desc.resize(iend);
885 
886  std::ostringstream os;
887  os << "DriveDisplacementPinJoint(" << GetLabel() << ")";
888 
889  if (i == -1) {
890  std::string name = os.str();
891  for (i = 0; i < iend; i++) {
892  os.str(name);
893  os.seekp(0, std::ios_base::end);
894  os << ": " << eq[i/3] << xyz[i%3];
895 
896  desc[i] = os.str();
897  }
898 
899  } else {
900  os << ": " << eq[i/3] << xyz[i%3];
901  desc[0] = os.str();
902  }
903 }
904 
905 /* Dati privati (aggiungere magari le reazioni vincolari) */
906 unsigned int
908 {
909  return 6;
910 };
911 
912 unsigned int
914 {
915  ASSERT(s != NULL);
916  ASSERT(s[0] != '\0');
917 
918  if (s[2] != '\0') {
919  return 0;
920  }
921 
922  unsigned int idx = 0;
923 
924  switch (s[0]) {
925  case 'f':
926  idx += 3;
927  /* fallthru */
928  case 'd':
929  break;
930 
931  default:
932  return 0;
933  }
934 
935  switch (s[1]) {
936  case 'x':
937  return idx + 1;
938  case 'y':
939  return idx + 2;
940  case 'z':
941  return idx + 3;
942  }
943 
944  return 0;
945 }
946 
949 {
950  ASSERT(i >= 1 && i <= 6);
951 
952  switch (i) {
953  case 1:
954  case 2:
955  case 3:
956  return Get().dGet(i);
957 
958  case 4:
959  case 5:
960  case 6:
961  return F(i - 3);
962  }
963 
965 }
966 
967 /* assemblaggio jacobiano */
970  doublereal dCoef,
971  const VectorHandler& /* XCurr */ ,
972  const VectorHandler& /* XPrimeCurr */ )
973 {
974  DEBUGCOUT("Entering DriveDisplacementPinJoint::AssJac()" << std::endl);
975 
976  FullSubMatrixHandler& WM = WorkMat.SetFull();
977 
978  /* Dimensiona e resetta la matrice di lavoro */
979  integer iNumRows = 0;
980  integer iNumCols = 0;
981  WorkSpaceDim(&iNumRows, &iNumCols);
982  WM.ResizeReset(iNumRows, iNumCols);
983 
984  /* Recupera gli indici */
985  integer iNodeFirstPosIndex = pNode->iGetFirstPositionIndex();
986  integer iNodeFirstMomIndex = pNode->iGetFirstMomentumIndex();
987  integer iFirstReactionIndex = iGetFirstIndex();
988 
989  /* Setta gli indici della matrice */
990  for (int iCnt = 1; iCnt <= 6; iCnt++) {
991  WM.PutRowIndex(iCnt, iNodeFirstMomIndex + iCnt);
992  WM.PutColIndex(iCnt, iNodeFirstPosIndex + iCnt);
993  }
994  for (int iCnt = 1; iCnt <= 3; iCnt++) {
995  WM.PutRowIndex(6 + iCnt, iFirstReactionIndex + iCnt);
996  WM.PutColIndex(6 + iCnt, iFirstReactionIndex + iCnt);
997  }
998 
999  AssMat(WM, dCoef);
1000 
1001  return WorkMat;
1002 }
1003 
1004 
1005 void
1007  VectorHandler& /* XP */ )
1008 {
1009  /* Recupera i dati */
1010  fRef = pNode->GetRRef()*f;
1011  dRef = Get();
1012 }
1013 
1014 
1015 void
1017 {
1018  for (int iCnt = 1; iCnt <= 3; iCnt++) {
1019  /* node force */
1020  WM.IncCoef(iCnt, 6 + iCnt, 1.);
1021 
1022  /* node constraint */
1023  WM.IncCoef(6 + iCnt, iCnt, 1.);
1024  }
1025 
1026  Mat3x3 MTmp(MatCross, fRef);
1027 
1028  /* node moment */
1029  WM.Add(3 + 1, 6 + 1, MTmp);
1030 
1031  /* node constraint */
1032  WM.Sub(6 + 1, 3 + 1, MTmp);
1033 
1034  MTmp = Mat3x3(MatCrossCross, F, fRef*dCoef);
1035 
1036  /* node moment */
1037  WM.Add(3 + 1, 3 + 1, MTmp);
1038 }
1039 
1040 
1041 /* assemblaggio residuo */
1044  doublereal dCoef,
1045  const VectorHandler& XCurr,
1046  const VectorHandler& /* XPrimeCurr */ )
1047 {
1048  DEBUGCOUT("Entering DriveDisplacementPinJoint::AssRes()" << std::endl);
1049 
1050  /* Dimensiona e resetta la matrice di lavoro */
1051  integer iNumRows = 0;
1052  integer iNumCols = 0;
1053  WorkSpaceDim(&iNumRows, &iNumCols);
1054  WorkVec.ResizeReset(iNumRows);
1055 
1056  /* Recupera gli indici */
1057  integer iNodeFirstMomIndex = pNode->iGetFirstMomentumIndex();
1058  integer iFirstReactionIndex = iGetFirstIndex();
1059 
1060  /* Setta gli indici della matrice */
1061  for (int iCnt = 1; iCnt <= 6; iCnt++) {
1062  WorkVec.PutRowIndex(iCnt, iNodeFirstMomIndex + iCnt);
1063  }
1064 
1065  for (int iCnt = 1; iCnt <= 3; iCnt++) {
1066  WorkVec.PutRowIndex(6 + iCnt, iFirstReactionIndex + iCnt);
1067  }
1068 
1069  F = Vec3(XCurr, iFirstReactionIndex + 1);
1070 
1071  AssVec(WorkVec, dCoef);
1072 
1073  return WorkVec;
1074 }
1075 
1076 
1077 void
1079 {
1080  Vec3 fTmp = pNode->GetRCurr()*f;
1081  Vec3 d = Get();
1082 
1083  WorkVec.Sub(1, F);
1084  WorkVec.Sub(3 + 1, fTmp.Cross(F));
1085 
1086  ASSERT(dCoef != 0.);
1087  WorkVec.Sub(6 + 1, (pNode->GetXCurr() + fTmp - x - d)/dCoef);
1088 }
1089 
1090 
1091 /* Contributo allo jacobiano durante l'assemblaggio iniziale */
1094  const VectorHandler& XCurr)
1095 {
1096  DEBUGCOUT("Entering DriveDisplacementPinJoint::InitialAssJac()" << std::endl);
1097 
1098  FullSubMatrixHandler& WM = WorkMat.SetFull();
1099 
1100  /* Dimensiona e resetta la matrice di lavoro */
1101  integer iNumRows = 0;
1102  integer iNumCols = 0;
1103  InitialWorkSpaceDim(&iNumRows, &iNumCols);
1104  WM.ResizeReset(iNumRows, iNumCols);
1105 
1106  /* Recupera gli indici */
1107  integer iNodeFirstPosIndex = pNode->iGetFirstPositionIndex();
1108  integer iFirstReactionIndex = iGetFirstIndex();
1109  integer iNodeFirstVelIndex = iNodeFirstPosIndex + 6;
1110  integer iReactionPrimeIndex = iFirstReactionIndex + 3;
1111 
1112  /* Setta gli indici della matrice */
1113  for (int iCnt = 1; iCnt <= 6; iCnt++) {
1114  WM.PutRowIndex(iCnt, iNodeFirstPosIndex + iCnt);
1115  WM.PutColIndex(iCnt, iNodeFirstPosIndex + iCnt);
1116  WM.PutRowIndex(6 + iCnt, iNodeFirstVelIndex + iCnt);
1117  WM.PutColIndex(6 + iCnt, iNodeFirstVelIndex + iCnt);
1118  WM.PutRowIndex(12 + iCnt, iFirstReactionIndex + iCnt);
1119  WM.PutColIndex(12 + iCnt, iFirstReactionIndex + iCnt);
1120  }
1121 
1122  Vec3 FPrime = Vec3(XCurr, iReactionPrimeIndex + 1);
1123 
1124  for (int iCnt = 1; iCnt <= 3; iCnt++) {
1125  /* node 2 force */
1126  WM.IncCoef(iCnt, 12 + iCnt, 1.);
1127 
1128  /* node 2 force derivative */
1129  WM.IncCoef(6 + iCnt, 15 + iCnt, 1.);
1130 
1131  /* node 2 constraint */
1132  WM.IncCoef(12 + iCnt, iCnt, 1.);
1133 
1134  /* node 2 constraint derivative */
1135  WM.IncCoef(15 + iCnt, 6 + iCnt, 1.);
1136  }
1137 
1138  Mat3x3 MTmp(MatCross, fRef);
1139  /* node 2 moment */
1140  WM.Add(3 + 1, 12 + 1, MTmp);
1141  /* node 2 moment derivatives */
1142  WM.Add(9 + 1, 15 + 1, MTmp);
1143  /* node 2 constraint */
1144  WM.Sub(12 + 1, 3 + 1, MTmp);
1145  /* node 2 constraint derivative */
1146  WM.Sub(15 + 1, 9 + 1, MTmp);
1147 
1148  MTmp = Mat3x3(MatCrossCross, F, fRef);
1149  /* node 2 moment */
1150  WM.Add(3 + 1, 3 + 1, MTmp);
1151 
1152  MTmp = Mat3x3(MatCrossCross, FPrime, fRef);
1153  /* node 2 moment derivative */
1154  WM.Add(9 + 1, 9 + 1, MTmp);
1155 
1156  MTmp = Mat3x3(MatCrossCross, pNode->GetWRef(), fRef);
1157  /* node 2 constraint derivative */
1158  WM.Add(15 + 1, 3 + 1, MTmp);
1159 
1160  return WorkMat;
1161 }
1162 
1163 
1164 /* Contributo al residuo durante l'assemblaggio iniziale */
1167  const VectorHandler& XCurr)
1168 {
1169  DEBUGCOUT("Entering DriveDisplacementPinJoint::InitialAssRes()" << std::endl);
1170 
1171  /* Dimensiona e resetta la matrice di lavoro */
1172  integer iNumRows = 0;
1173  integer iNumCols = 0;
1174  InitialWorkSpaceDim(&iNumRows, &iNumCols);
1175  WorkVec.ResizeReset(iNumRows);
1176 
1177  /* Recupera gli indici */
1178  integer iNodeFirstPosIndex = pNode->iGetFirstPositionIndex();
1179  integer iFirstReactionIndex = iGetFirstIndex();
1180  integer iNodeFirstVelIndex = iNodeFirstPosIndex + 6;
1181  integer iFirstReactionPrimeIndex = iFirstReactionIndex + 3;
1182 
1183  /* Setta gli indici del vettore */
1184  for (int iCnt = 1; iCnt <= 6; iCnt++) {
1185  WorkVec.PutRowIndex(iCnt, iNodeFirstPosIndex + iCnt);
1186  WorkVec.PutRowIndex(6 + iCnt, iNodeFirstVelIndex + iCnt);
1187  WorkVec.PutRowIndex(12 + iCnt, iFirstReactionIndex + iCnt);
1188  }
1189 
1190  F = Vec3(XCurr, iFirstReactionIndex + 1);
1191  Vec3 FPrime = Vec3(XCurr, iFirstReactionPrimeIndex + 1);
1192 
1193  dRef = Get();
1194  fRef = pNode->GetRCurr()*f;
1195 
1196  WorkVec.Sub(1, F);
1197  WorkVec.Sub(3 + 1, fRef.Cross(F));
1198  WorkVec.Sub(6 + 1, FPrime);
1199  WorkVec.Sub(9 + 1, fRef.Cross(FPrime));
1200 
1201  WorkVec.Add(12 + 1, dRef - pNode->GetXCurr() - fRef);
1202 
1203  /* in case the drive is differentiable... */
1204  Vec3 PhiPrime = pNode->GetVCurr() + pNode->GetWCurr().Cross(fRef);
1205  if (bIsDifferentiable()) {
1206  PhiPrime -= GetP();
1207  }
1208  WorkVec.Sub(15 + 1, PhiPrime);
1209 
1210  return WorkVec;
1211 }
1212 
1213 /* DriveDisplacementPinJoint - end */
Definition: hint.h:38
virtual VariableSubMatrixHandler & AssJac(VariableSubMatrixHandler &WorkMat, doublereal dCoef, const VectorHandler &XCurr, const VectorHandler &XPrimeCurr)
Definition: drvdisp.cc:351
void PutColIndex(integer iSubCol, integer iCol)
Definition: submat.h:325
virtual ~DriveDisplacementJoint(void)
Definition: drvdisp.cc:69
void AssVec(SubVectorHandler &WorkVec, doublereal dCoef)
Definition: drvdisp.cc:1078
const Vec3 Zero3(0., 0., 0.)
Vec3 Cross(const Vec3 &v) const
Definition: matvec3.h:218
std::ostream & Write(std::ostream &out, const char *sFill=" ") const
Definition: matvec3.cc:738
long int flag
Definition: mbdyn.h:43
virtual const Mat3x3 & GetRRef(void) const
Definition: strnode.h:1006
DriveDisplacementJoint(unsigned int uL, const DofOwner *pDO, const TplDriveCaller< Vec3 > *pDC, const StructNode *pN1, const StructNode *pN2, const Vec3 &f1, const Vec3 &f2, flag fOut)
Definition: drvdisp.cc:43
virtual bool bToBeOutput(void) const
Definition: output.cc:890
virtual bool bIsDifferentiable(void) const
Definition: tpldrive.h:118
#define MBDYN_EXCEPT_ARGS
Definition: except.h:63
Definition: matvec3.h:98
virtual void ResizeReset(integer)
Definition: vh.cc:55
const MatCross_Manip MatCross
Definition: matvec3.cc:639
FullSubMatrixHandler & SetFull(void)
Definition: submat.h:1168
virtual const Mat3x3 & GetRCurr(void) const
Definition: strnode.h:1012
virtual Node::Type GetNodeType(void) const
Definition: strnode.cc:145
virtual void WorkSpaceDim(integer *piNumRows, integer *piNumCols) const
Definition: drvdisp.h:265
unsigned int iGetPrivDataIdx(const char *s) const
Definition: drvdisp.cc:295
void Add(integer iRow, integer iCol, const Vec3 &v)
Definition: submat.cc:209
static const char xyz[]
Definition: drvdisp.cc:194
virtual void InitialWorkSpaceDim(integer *piNumRows, integer *piNumCols) const
Definition: drvdisp.h:292
void SetValue(DataManager *pDM, VectorHandler &X, VectorHandler &XP, SimulationEntity::Hints *ph=0)
Definition: drvdisp.cc:726
virtual Hint * ParseHint(DataManager *pDM, const char *s) const
Definition: simentity.cc:76
virtual SubVectorHandler & AssRes(SubVectorHandler &WorkVec, doublereal dCoef, const VectorHandler &XCurr, const VectorHandler &XPrimeCurr)
Definition: drvdisp.cc:441
virtual void Sub(integer iRow, const Vec3 &v)
Definition: vh.cc:78
const Mat3x3 Eye3(1., 0., 0., 0., 1., 0., 0., 0., 1.)
virtual void AfterPredict(VectorHandler &X, VectorHandler &XP)
Definition: drvdisp.cc:392
void AssMat(FullSubMatrixHandler &WM, doublereal dCoef)
Definition: drvdisp.cc:402
virtual const Vec3 & GetWRef(void) const
Definition: strnode.h:1024
#define NO_OP
Definition: myassert.h:74
const StructNode * pNode
Definition: drvdisp.h:194
virtual Hint * ParseHint(DataManager *pDM, const char *s) const
Definition: drvdisp.cc:151
std::vector< Hint * > Hints
Definition: simentity.h:89
virtual doublereal dGetPrivData(unsigned int i=0) const
Definition: drvdisp.cc:330
void IncCoef(integer iRow, integer iCol, const doublereal &dCoef)
Definition: submat.h:683
DriveDisplacementPinJoint(unsigned int uL, const DofOwner *pDO, const TplDriveCaller< Vec3 > *pDC, const StructNode *pN, const Vec3 &f, const Vec3 &x, flag fOut)
Definition: drvdisp.cc:674
void SetValue(DataManager *pDM, VectorHandler &X, VectorHandler &XP, SimulationEntity::Hints *ph=0)
Definition: drvdisp.cc:102
void Set(const TplDriveCaller< T > *pDC)
Definition: tpldrive.h:97
virtual std::ostream & Restart(std::ostream &out) const
Definition: drvdisp.cc:703
virtual unsigned int iGetNumPrivData(void) const
Definition: drvdisp.cc:907
virtual std::ostream & Restart(std::ostream &out) const
Definition: drvdisp.cc:77
virtual Hint * ParseHint(DataManager *pDM, const char *s) const
Definition: drvdisp.cc:773
void DecCoef(integer iRow, integer iCol, const doublereal &dCoef)
Definition: submat.h:694
virtual VariableSubMatrixHandler & InitialAssJac(VariableSubMatrixHandler &WorkMat, const VectorHandler &XCurr)
Definition: drvdisp.cc:497
virtual void WorkSpaceDim(integer *piNumRows, integer *piNumCols) const
Definition: drvdisp.h:123
virtual Vec3 GetP(void) const
Definition: tpldrive.h:121
virtual SubVectorHandler & InitialAssRes(SubVectorHandler &WorkVec, const VectorHandler &XCurr)
Definition: drvdisp.cc:612
virtual void Output(OutputHandler &OH) const
Definition: drvdisp.cc:715
virtual void PutRowIndex(integer iSubRow, integer iRow)=0
virtual unsigned int iGetNumPrivData(void) const
Definition: drvdisp.cc:289
TplDriveCaller< Vec3 > * pGetDriveCaller(void) const
Definition: tpldrive.h:105
virtual void InitialWorkSpaceDim(integer *piNumRows, integer *piNumCols) const
Definition: drvdisp.h:150
virtual SubVectorHandler & AssRes(SubVectorHandler &WorkVec, doublereal dCoef, const VectorHandler &XCurr, const VectorHandler &XPrimeCurr)
Definition: drvdisp.cc:1043
static const char * dof[]
Definition: drvdisp.cc:195
const doublereal & dGet(unsigned short int iRow) const
Definition: matvec3.h:285
virtual std::ostream & DescribeEq(std::ostream &out, const char *prefix="", bool bInitial=false) const
Definition: drvdisp.cc:233
virtual std::ostream & DescribeDof(std::ostream &out, const char *prefix="", bool bInitial=false) const
Definition: drvdisp.cc:175
#define DEBUGCOUT(msg)
Definition: myassert.h:232
virtual std::ostream & DescribeDof(std::ostream &out, const char *prefix="", bool bInitial=false) const
Definition: drvdisp.cc:797
const StructNode * pNode1
Definition: drvdisp.h:47
virtual std::ostream & DescribeEq(std::ostream &out, const char *prefix="", bool bInitial=false) const
Definition: drvdisp.cc:851
static const char * eq[]
Definition: drvdisp.cc:196
virtual integer iGetFirstMomentumIndex(void) const =0
virtual integer iGetFirstPositionIndex(void) const
Definition: strnode.h:452
virtual const Vec3 & GetWCurr(void) const
Definition: strnode.h:1030
unsigned int uLabel
Definition: withlab.h:44
virtual std::ostream & Restart(std::ostream &out) const
Definition: joint.h:195
std::ostream & Joints(void) const
Definition: output.h:443
virtual std::ostream & Restart(std::ostream &out) const =0
TplDriveCaller< T > * pCreateDrive(DataManager *pDM) const
Definition: hint_impl.h:124
#define ASSERT(expression)
Definition: colamd.c:977
virtual const Vec3 & GetXCurr(void) const
Definition: strnode.h:310
virtual void Add(integer iRow, const Vec3 &v)
Definition: vh.cc:63
virtual ~DriveDisplacementPinJoint(void)
Definition: drvdisp.cc:695
virtual void ResizeReset(integer, integer)
Definition: submat.cc:182
virtual void Output(OutputHandler &OH) const
Definition: drvdisp.cc:90
virtual void AfterPredict(VectorHandler &X, VectorHandler &XP)
Definition: drvdisp.cc:1006
const StructNode * pNode2
Definition: drvdisp.h:48
Mat3x3 Transpose(void) const
Definition: matvec3.h:816
const MatCrossCross_Manip MatCrossCross
Definition: matvec3.cc:640
#define STRLENOF(s)
Definition: mbdyn.h:166
Definition: elem.h:75
void PutRowIndex(integer iSubRow, integer iRow)
Definition: submat.h:311
virtual VariableSubMatrixHandler & InitialAssJac(VariableSubMatrixHandler &WorkMat, const VectorHandler &XCurr)
Definition: drvdisp.cc:1093
unsigned int iGetPrivDataIdx(const char *s) const
Definition: drvdisp.cc:913
virtual const Vec3 & GetVCurr(void) const
Definition: strnode.h:322
void Sub(integer iRow, integer iCol, const Vec3 &v)
Definition: submat.cc:215
virtual VariableSubMatrixHandler & AssJac(VariableSubMatrixHandler &WorkMat, doublereal dCoef, const VectorHandler &XCurr, const VectorHandler &XPrimeCurr)
Definition: drvdisp.cc:969
Definition: joint.h:50
virtual SubVectorHandler & InitialAssRes(SubVectorHandler &WorkVec, const VectorHandler &XCurr)
Definition: drvdisp.cc:1166
virtual integer iGetFirstIndex(void) const
Definition: dofown.h:127
void AssMat(FullSubMatrixHandler &WM, doublereal dCoef)
Definition: drvdisp.cc:1016
double doublereal
Definition: colamd.c:52
std::ostream & Output(std::ostream &out, const char *sJointName, unsigned int uLabel, const Vec3 &FLocal, const Vec3 &MLocal, const Vec3 &FGlobal, const Vec3 &MGlobal) const
Definition: joint.cc:138
long int integer
Definition: colamd.c:51
unsigned int GetLabel(void) const
Definition: withlab.cc:62
Vec3 Get(void) const
Definition: tpldrive.h:113
virtual doublereal dGetPrivData(unsigned int i=0) const
Definition: drvdisp.cc:948
void AssVec(SubVectorHandler &WorkVec, doublereal dCoef)
Definition: drvdisp.cc:478