MBDyn-1.7.3
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Groups
totalj.cc
Go to the documentation of this file.
1 /* $Header: /var/cvs/mbdyn/mbdyn/mbdyn-1.0/mbdyn/struct/totalj.cc,v 1.89 2017/01/12 14:46:44 masarati Exp $ */
2 /*
3  * MBDyn (C) is a multibody analysis code.
4  * http://www.mbdyn.org
5  *
6  * Copyright (C) 1996-2017
7  *
8  * Pierangelo Masarati <masarati@aero.polimi.it>
9  * Paolo Mantegazza <mantegazza@aero.polimi.it>
10  *
11  * Dipartimento di Ingegneria Aerospaziale - Politecnico di Milano
12  * via La Masa, 34 - 20156 Milano, Italy
13  * http://www.aero.polimi.it
14  *
15  * Changing this copyright notice is forbidden.
16  *
17  * This program is free software; you can redistribute it and/or modify
18  * it under the terms of the GNU General Public License as published by
19  * the Free Software Foundation (version 2 of the License).
20  *
21  *
22  * This program is distributed in the hope that it will be useful,
23  * but WITHOUT ANY WARRANTY; without even the implied warranty of
24  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
25  * GNU General Public License for more details.
26  *
27  * You should have received a copy of the GNU General Public License
28  * along with this program; if not, write to the Free Software
29  * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
30  */
31 
32 /* TotalJoint
33  * Authors: Alessandro Fumagalli, Pierangelo Masarati
34  *
35  * */
36 
37 #include "mbconfig.h" /* This goes first in every *.c,*.cc file */
38 
39 #include <iostream>
40 #include <fstream>
41 
42 #include "totalj.h"
43 #include "Rot.hh"
44 #include "hint_impl.h"
45 #include "invdyn.h"
46 
47 static const char idx2xyz[] = { 'x', 'y', 'z' };
48 
49 /* TotalJoint - begin */
50 
51 TotalJoint::TotalJoint(unsigned int uL, const DofOwner *pDO,
52  bool bPos[3], bool bVel[3],
53  TplDriveCaller<Vec3> *const pDCPos[3],
54  bool bRot[3], bool bAgv[3], /* Agv stands for AnGular Velocity */
55  TplDriveCaller<Vec3> *const pDCRot[3],
56  const StructNode *pN1,
57  const Vec3& f1Tmp, const Mat3x3& R1hTmp, const Mat3x3& R1hrTmp,
58  const StructNode *pN2,
59  const Vec3& f2Tmp, const Mat3x3& R2hTmp, const Mat3x3& R2hrTmp,
60  flag fOut)
61 : Elem(uL, fOut),
62 Joint(uL, pDO, fOut),
63 pNode1(pN1), pNode2(pN2),
64 f1(f1Tmp), R1h(R1hTmp), R1hr(R1hrTmp),
65 f2(f2Tmp), R2h(R2hTmp), R2hr(R2hrTmp),
66 XDrv(pDCPos[0]), XPDrv(pDCPos[1]), XPPDrv(pDCPos[2]),
67 ThetaDrv(pDCRot[0]), OmegaDrv(pDCRot[1]), OmegaPDrv(pDCRot[2]),
68 nConstraints(0), nPosConstraints(0), nRotConstraints(0),
69 nVelConstraints(0), nAgvConstraints(0),
70 tilde_f1(R1h.MulTV(f1)),
71 #ifdef USE_NETCDF
72 Var_X(0),
73 Var_Phi(0),
74 Var_V(0),
75 Var_Omega(0),
76 #endif // USE_NETCDF
77 M(::Zero3), F(::Zero3),
78 ThetaDelta(::Zero3), ThetaDeltaPrev(::Zero3),
79 ThetaDeltaRemnant(::Zero3),
80 ThetaDeltaTrue(::Zero3)
81 {
82  /* Equations 1->3: Positions
83  * Equations 4->6: Rotations */
84 
85  unsigned int index = 0;
86 
87  for (unsigned int i = 0; i < 3; i++) {
88  ASSERT(bPos[i] == false || bVel[i] == false);
89 
90  bPosActive[i] = bPos[i];
91  bVelActive[i] = bVel[i];
92 
93  if (bPosActive[i]) {
94  iPosIncid[nPosConstraints] = i + 1;
95  iPosEqIndex[nPosConstraints] = index;
96  nPosConstraints++;
97  index++;
98  }
99 
100  if (bVelActive[i]) {
101  iVelIncid[nVelConstraints] = i + 1;
102  iVelEqIndex[nVelConstraints] = index;
103  nVelConstraints++;
104  index++;
105  }
106  }
107 
108  index = 0;
109  for (unsigned int i = 0; i < 3; i++) {
110  ASSERT(bRot[i] == false || bAgv[i] == false);
111 
112  bRotActive[i] = bRot[i];
113  bAgvActive[i] = bAgv[i];
114 
115  if (bRotActive[i]) {
116  iRotIncid[nRotConstraints] = i + 1;
117  iRotEqIndex[nRotConstraints] = nPosConstraints + nVelConstraints + index;
118  nRotConstraints++;
119  index++;
120  }
121 
122  if (bAgvActive[i]) {
123  iAgvIncid[nAgvConstraints] = i + 1;
124  iAgvEqIndex[nAgvConstraints] = nPosConstraints + nVelConstraints + index;
125  nAgvConstraints++;
126  index++;
127  }
128  }
129 
130  // if only a fraction of the rotation degrees of freedom
131  // are constrained, store the incidence of the unconstrained ones
132  // in the remainder of array iRotIncid[] for later use
133  if (nRotConstraints > 0 && nRotConstraints < 3) {
134  switch (nRotConstraints) {
135  case 1:
136  switch (iRotIncid[0]) {
137  case 1:
138  iRotIncid[1] = 2;
139  iRotIncid[2] = 3;
140  break;
141 
142  case 2:
143  iRotIncid[1] = 1;
144  iRotIncid[2] = 3;
145  break;
146 
147  case 3:
148  iRotIncid[1] = 1;
149  iRotIncid[2] = 2;
150  break;
151 
152  default:
153  ASSERT(0);
154  throw ErrGeneric(MBDYN_EXCEPT_ARGS);
155  }
156  break;
157 
158  case 2:
159  switch (iRotIncid[0]) {
160  case 1:
161  iRotIncid[2] = 5 - iRotIncid[1];
162  break;
163 
164  case 2:
165  iRotIncid[2] = 1;
166  break;
167  }
168  break;
169 
170  default:
171  ASSERT(0);
172  throw ErrGeneric(MBDYN_EXCEPT_ARGS);
173  }
174  }
175 
176  nConstraints = nPosConstraints + nVelConstraints
177  + nRotConstraints + nAgvConstraints;
178 
179  ThetaDeltaTrue = RotManip::VecRot((pNode1->GetRCurr()*R1hr).MulTM(pNode2->GetRCurr()*R2hr));
180 }
181 
182 TotalJoint::~TotalJoint(void)
183 {
184  NO_OP;
185 };
186 
187 std::ostream&
188 TotalJoint::DescribeDof(std::ostream& out,
189  const char *prefix, bool bInitial) const
190 {
191  integer iIndex = iGetFirstIndex();
192 
193  if (nPosConstraints > 0 || nVelConstraints > 0) {
194  out << prefix << iIndex + 1;
195  if (nPosConstraints + nVelConstraints > 1) {
196  out << "->" << iIndex + nPosConstraints + nVelConstraints;
197  }
198  out << ": "
199  "reaction force(s) [";
200 
201  for (unsigned int i = 0, cnt = 0; i < 3; i++) {
202  if (bPosActive[i] || bVelActive[i]) {
203  cnt++;
204  if (cnt > 1) {
205  out << ",";
206  }
207  out << "F" << idx2xyz[i];
208  }
209  }
210  out << "]" << std::endl;
211  }
212 
213  if (nRotConstraints > 0 || nAgvConstraints > 0) {
214  out << prefix << iIndex + nPosConstraints + nVelConstraints + 1;
215  if (nRotConstraints + nAgvConstraints > 1) {
216  out << "->" << iIndex + nConstraints;
217  }
218  out << ": "
219  "reaction couple(s) [";
220 
221  for (unsigned int i = 0, cnt = 0; i < 3; i++) {
222  if (bRotActive[i] || bAgvActive[i]) {
223  cnt++;
224  if (cnt > 1) {
225  out << ",";
226  }
227  out << "m" << idx2xyz[i];
228  }
229  }
230  out << "]" << std::endl;
231  }
232 
233  if (bInitial) {
234  iIndex += nConstraints;
235 
236  if (nPosConstraints > 0) {
237  out << prefix << iIndex + 1;
238  if (nPosConstraints > 1) {
239  out << "->" << iIndex + nPosConstraints;
240  }
241  out << ": "
242  "reaction force(s) derivative(s) [";
243 
244  for (unsigned int i = 0, cnt = 0; i < 3; i++) {
245  if (bPosActive[i]) {
246  cnt++;
247  if (cnt > 1) {
248  out << ",";
249  }
250  out << "FP" << idx2xyz[i];
251  }
252  }
253  out << "]" << std::endl;
254  }
255 
256 
257  if (nRotConstraints > 0) {
258  out << prefix << iIndex + nPosConstraints + 1;
259  if (nRotConstraints > 1) {
260  out << "->" << iIndex + nConstraints;
261  }
262  out << ": "
263  "reaction couple(s) derivative(s) [";
264 
265  for (unsigned int i = 0, cnt = 0; i < 3; i++) {
266  if (bRotActive[i]) {
267  cnt++;
268  if (cnt > 1) {
269  out << ",";
270  }
271  out << "mP" << idx2xyz[i];
272  }
273  }
274  out << "]" << std::endl;
275  }
276  }
277 
278  return out;
279 }
280 
281 void
282 TotalJoint::DescribeDof(std::vector<std::string>& desc,
283  bool bInitial, int i) const
284 {
285  int ndof = 1;
286  if (i == -1) {
287  if (bInitial) {
288  ndof = iGetInitialNumDof();
289 
290  } else {
291  ndof = iGetNumDof();
292  }
293  }
294  desc.resize(ndof);
295 
296  std::ostringstream os;
297  os << "TotalJoint(" << GetLabel() << ")";
298 
299  if (i == -1) {
300  std::string name(os.str());
301  unsigned int cnt = 0;
302 
303  if (nPosConstraints > 0 || nVelConstraints > 0) {
304  for (unsigned int i = 0; i < 3; i++) {
305  if (bPosActive[i] || bVelActive[i]) {
306  os.str(name);
307  os.seekp(0, std::ios_base::end);
308  os << ": dof(" << cnt + 1 << ") F" << idx2xyz[i];
309  desc[cnt] = os.str();
310  cnt++;
311  }
312  }
313  }
314 
315  if (nRotConstraints > 0 || nAgvConstraints > 0) {
316  for (unsigned int i = 0; i < 3; i++) {
317  if (bRotActive[i] || bAgvActive[i]) {
318  os.str(name);
319  os.seekp(0, std::ios_base::end);
320  os << ": dof(" << cnt + 1 << ") M" << idx2xyz[i];
321  desc[cnt] = os.str();
322  cnt++;
323  }
324  }
325  }
326 
327  if (bInitial) {
328  if (nPosConstraints > 0) {
329  for (unsigned int i = 0; i < 3; i++) {
330  if (bPosActive[i]) {
331  os.str(name);
332  os.seekp(0, std::ios_base::end);
333  os << ": dof(" << cnt + 1 << ") FP" << idx2xyz[i];
334  desc[cnt] = os.str();
335  cnt++;
336  }
337  }
338  }
339 
340  if (nRotConstraints > 0) {
341  for (unsigned int i = 0; i < 3; i++) {
342  if (bRotActive[i]) {
343  os.str(name);
344  os.seekp(0, std::ios_base::end);
345  os << ": dof(" << cnt + 1 << ") MP" << idx2xyz[i];
346  desc[cnt] = os.str();
347  cnt++;
348  }
349  }
350  }
351  }
352 
353  ASSERT(cnt == ndof);
354 
355  } else {
356  os << ": dof(" << i + 1 << ")";
357  desc[0] = os.str();
358  }
359 }
360 
361 std::ostream&
362 TotalJoint::DescribeEq(std::ostream& out,
363  const char *prefix, bool bInitial) const
364 {
365  integer iIndex = iGetFirstIndex();
366 
367  if (nPosConstraints > 0 || nVelConstraints > 0) {
368  out << prefix << iIndex + 1;
369  if (nPosConstraints + nVelConstraints > 1) {
370  out << "->" << iIndex + nPosConstraints + nVelConstraints;
371  }
372  out << ": "
373  "position/velocity constraint(s) [";
374 
375  for (unsigned int i = 0, cnt = 0; i < 3; i++) {
376  if (bPosActive[i] || bVelActive[i]) {
377  cnt++;
378  if (cnt > 1) {
379  out << ",";
380  }
381 
382  if (bPosActive[i]) {
383  out << "P" << idx2xyz[i] << "1=P" << idx2xyz[i] << "2";
384  } else {
385  out << "V" << idx2xyz[i] << "1=V" << idx2xyz[i] << "2";
386  }
387  }
388  }
389 
390  out << "]" << std::endl;
391  }
392 
393  if (nRotConstraints > 0 || nAgvConstraints > 0) {
394  out << prefix << iIndex + nPosConstraints + nVelConstraints + 1;
395  if (nRotConstraints + nAgvConstraints > 1) {
396  out << "->" << iIndex + nConstraints ;
397  }
398  out << ": "
399  "orientation/angular velocity constraint(s) [";
400 
401  for (unsigned int i = 0, cnt = 0; i < 3; i++) {
402  if (bRotActive[i] || bAgvActive[i]) {
403  cnt++;
404  if (cnt > 1) {
405  out << ",";
406  }
407 
408  if (bRotActive[i]) {
409  out << "theta" << idx2xyz[i] << "1=theta" << idx2xyz[i] << "2";
410  } else {
411  out << "W" << idx2xyz[i] << "1=W" << idx2xyz[i] << "2";
412  }
413  }
414  }
415 
416  out << "]" << std::endl;
417  }
418 
419  if (bInitial) {
420  iIndex += nConstraints;
421 
422  if (nPosConstraints > 0) {
423  out << prefix << iIndex + 1;
424  if (nPosConstraints > 1) {
425  out << "->" << iIndex + nPosConstraints;
426  }
427  out << ": "
428  "velocity constraint(s) [";
429 
430  for (unsigned int i = 0, cnt = 0; i < 3; i++) {
431  if (bPosActive[i]) {
432  cnt++;
433  if (cnt > 1) {
434  out << ",";
435  }
436  out << "v" << idx2xyz[i] << "1=v" << idx2xyz[i] << "2";
437  }
438  }
439 
440  out << "]" << std::endl;
441  }
442 
443  if (nRotConstraints > 0) {
444  out << prefix << iIndex + nPosConstraints + 1;
445  if (nRotConstraints > 1) {
446  out << "->" << iIndex + nConstraints ;
447  }
448  out << ": "
449  "angular velocity constraint(s) [";
450 
451  for (unsigned int i = 0, cnt = 0; i < 3; i++) {
452  if (bRotActive[i]) {
453  cnt++;
454  if (cnt > 1) {
455  out << ",";
456  }
457  out << "w" << idx2xyz[i] << "1=w" << idx2xyz[i] << "2";
458  }
459  }
460 
461  out << "]" << std::endl;
462  }
463  }
464 
465  return out;
466 }
467 
468 void
469 TotalJoint::DescribeEq(std::vector<std::string>& desc,
470  bool bInitial, int i) const
471 {
472  int ndof = 1;
473  if (i == -1) {
474  if (bInitial) {
475  ndof = iGetInitialNumDof();
476 
477  } else {
478  ndof = iGetNumDof();
479  }
480  }
481  desc.resize(ndof);
482 
483  std::ostringstream os;
484  os << "TotalJoint(" << GetLabel() << ")";
485 
486  if (i == -1) {
487  std::string name(os.str());
488  unsigned int cnt = 0;
489 
490  if (nPosConstraints > 0 || nVelConstraints > 0) {
491  for (unsigned int i = 0; i < 3; i++) {
492  if (bPosActive[i]) {
493  os.str(name);
494  os.seekp(0, std::ios_base::end);
495  os << ": equation(" << cnt + 1 << ") P" << idx2xyz[i] << "1=P" << idx2xyz[i] << "2";
496  desc[cnt] = os.str();
497  cnt++;
498 
499  } else if (bVelActive[i]) {
500  os.str(name);
501  os.seekp(0, std::ios_base::end);
502  os << ": equation(" << cnt + 1 << ") V" << idx2xyz[i] << "1=V" << idx2xyz[i] << "2";
503  desc[cnt] = os.str();
504  cnt++;
505  }
506  }
507  }
508 
509  if (nRotConstraints > 0 || nAgvConstraints > 0) {
510  for (unsigned int i = 0; i < 3; i++) {
511  if (bRotActive[i]) {
512  os.str(name);
513  os.seekp(0, std::ios_base::end);
514  os << ": equation(" << cnt + 1 << ") theta" << idx2xyz[i] << "1=theta" << idx2xyz[i] << "2";
515  desc[cnt] = os.str();
516  cnt++;
517 
518  } else if (bAgvActive[i]) {
519  os.str(name);
520  os.seekp(0, std::ios_base::end);
521  os << ": equation(" << cnt + 1 << ") W" << idx2xyz[i] << "1=W" << idx2xyz[i] << "2";
522  desc[cnt] = os.str();
523  cnt++;
524  }
525  }
526  }
527 
528  if (bInitial) {
529  if (nPosConstraints > 0) {
530  for (unsigned int i = 0; i < 3; i++) {
531  if (bPosActive[i]) {
532  os.str(name);
533  os.seekp(0, std::ios_base::end);
534  os << ": equation(" << cnt + 1 << ") v" << idx2xyz[i] << "1=v" << idx2xyz[i] << "2";
535  desc[cnt] = os.str();
536  cnt++;
537  }
538  }
539  }
540 
541  if (nRotConstraints > 0) {
542  for (unsigned int i = 0; i < 3; i++) {
543  if (bRotActive[i]) {
544  os.str(name);
545  os.seekp(0, std::ios_base::end);
546  os << ": equation(" << cnt + 1 << ") w" << idx2xyz[i] << "1=w" << idx2xyz[i] << "2";
547  desc[cnt] = os.str();
548  cnt++;
549  }
550  }
551  }
552  }
553 
554  ASSERT(cnt == ndof);
555 
556  } else {
557  os << ": equation(" << i + 1 << ")";
558  desc[0] = os.str();
559  }
560 }
561 
562 void
563 TotalJoint::SetValue(DataManager *pDM,
564  VectorHandler& X, VectorHandler& XP,
565  SimulationEntity::Hints *ph)
566 {
567  if (ph) {
568  for (unsigned int i = 0; i < ph->size(); i++) {
569  Joint::JointHint *pjh = dynamic_cast<Joint::JointHint *>((*ph)[i]);
570 
571  if (pjh) {
572 
573  if (dynamic_cast<Joint::OffsetHint<1> *>(pjh)) {
574  Mat3x3 R1T(pNode1->GetRCurr().Transpose());
575  Vec3 fTmp2(pNode2->GetRCurr()*f2);
576 
577  f1 = R1T*(pNode2->GetXCurr() + fTmp2 - pNode1->GetXCurr());
578  tilde_f1 = R1h.MulTV(f1) - XDrv.Get();
579 
580  } else if (dynamic_cast<Joint::OffsetHint<2> *>(pjh)) {
581  Mat3x3 R2T(pNode2->GetRCurr().Transpose());
582  Mat3x3 R1(pNode1->GetRCurr()*R1h);
583  Vec3 fTmp1(pNode1->GetRCurr()*f1);
584 
585  f2 = R2T*(pNode1->GetXCurr() + fTmp1 - pNode2->GetXCurr() + R1*XDrv.Get());
586 
587  } else if (dynamic_cast<Joint::HingeHint<1> *>(pjh)) {
588  if (dynamic_cast<Joint::PositionHingeHint<1> *>(pjh)) {
589  R1h = pNode1->GetRCurr().MulTM(pNode2->GetRCurr()*R2h);
590 
591  } else if (dynamic_cast<Joint::OrientationHingeHint<1> *>(pjh)) {
592  R1hr = pNode1->GetRCurr().MulTM(pNode2->GetRCurr()*R2hr);
593  }
594 
595  } else if (dynamic_cast<Joint::HingeHint<2> *>(pjh)) {
596  if (dynamic_cast<Joint::PositionHingeHint<2> *>(pjh)) {
597  R2h = pNode2->GetRCurr().MulTM(pNode1->GetRCurr()*R1h);
598 
599  } else if (dynamic_cast<Joint::OrientationHingeHint<2> *>(pjh)) {
600  R2hr = pNode2->GetRCurr().MulTM(pNode1->GetRCurr()*R1hr);
601  }
602 
603  } else if (dynamic_cast<Joint::JointDriveHint<Vec3> *>(pjh)) {
604  Joint::JointDriveHint<Vec3> *pjdh
605  = dynamic_cast<Joint::JointDriveHint<Vec3> *>(pjh);
606  pedantic_cout("TotalJoint(" << uLabel << "): "
607  "creating drive from hint[" << i << "]..." << std::endl);
608 
609  TplDriveCaller<Vec3> *pDC = pjdh->pTDH->pCreateDrive(pDM);
610  if (pDC == 0) {
611  silent_cerr("TotalJoint(" << uLabel << "): "
612  "unable to create drive "
613  "after hint #" << i << std::endl);
614  throw ErrGeneric(MBDYN_EXCEPT_ARGS);
615  }
616 
617  if (dynamic_cast<Joint::PositionDriveHint<Vec3> *>(pjdh)) {
618  XDrv.Set(pDC);
619 
620  } else if (dynamic_cast<Joint::VelocityDriveHint<Vec3> *>(pjdh)) {
621  XPDrv.Set(pDC);
622 
623  } else if (dynamic_cast<Joint::AccelerationDriveHint<Vec3> *>(pjdh)) {
624  XPPDrv.Set(pDC);
625 
626  } else if (dynamic_cast<Joint::OrientationDriveHint<Vec3> *>(pjdh)) {
627  ThetaDrv.Set(pDC);
628 
629  } else if (dynamic_cast<Joint::AngularVelocityDriveHint<Vec3> *>(pjdh)) {
630  OmegaDrv.Set(pDC);
631 
632  } else if (dynamic_cast<Joint::AngularAccelerationDriveHint<Vec3> *>(pjdh)) {
633  OmegaPDrv.Set(pDC);
634 
635  } else {
636  delete pDC;
637  }
638 
639  } else if (dynamic_cast<Joint::ReactionsHint *>(pjh)) {
640  /* TODO */
641  }
642  continue;
643  }
644  }
645  }
646 }
647 
648 Hint *
649 TotalJoint::ParseHint(DataManager *pDM, const char *s) const
650 {
651  if (strncasecmp(s, "offset{" /*}*/ , STRLENOF("offset{" /*}*/ )) == 0)
652  {
653  s += STRLENOF("offset{" /*}*/ );
654 
655  if (strcmp(&s[1], /*{*/ "}") != 0) {
656  return 0;
657  }
658 
659  switch (s[0]) {
660  case '1':
661  return new Joint::OffsetHint<1>;
662 
663  case '2':
664  return new Joint::OffsetHint<2>;
665  }
666 
667  } else if (strncasecmp(s, "position-hinge{" /*}*/, STRLENOF("position-hinge{" /*}*/)) == 0) {
668  s += STRLENOF("position-hinge{" /*}*/);
669 
670  if (strcmp(&s[1], /*{*/ "}") != 0) {
671  return 0;
672  }
673 
674  switch (s[0]) {
675  case '1':
676  return new Joint::HingeHint<1>;
677 
678  case '2':
679  return new Joint::HingeHint<2>;
680  }
681 
682  } else if (strncasecmp(s, "position-drive3{" /*}*/, STRLENOF("position-drive3{" /*}*/)) == 0) {
683  s += STRLENOF("position-");
684 
685  Hint *pH = ::ParseHint(pDM, s);
686  if (pH) {
687  TplDriveHint<Vec3> *pTDH = dynamic_cast<TplDriveHint<Vec3> *>(pH);
688  if (pTDH) {
689  return new PositionDriveHint<Vec3>(pTDH);
690  }
691  }
692  return 0;
693 
694  } else if (strncasecmp(s, "orientation-hinge{" /*}*/, STRLENOF("orientation-hinge{" /*}*/)) == 0) {
695  s += STRLENOF("orientation-hinge{" /*}*/);
696 
697  if (strcmp(&s[1], /*{*/ "}") != 0) {
698  return 0;
699  }
700 
701  switch (s[0]) {
702  case '1':
703  return new Joint::HingeHint<1>;
704 
705  case '2':
706  return new Joint::HingeHint<2>;
707  }
708 
709  } else if (strncasecmp(s, "orientation-drive3{" /*}*/, STRLENOF("orientation-drive3{" /*}*/)) == 0) {
710  s += STRLENOF("orientation-");
711 
712  Hint *pH = ::ParseHint(pDM, s);
713  if (pH) {
714  TplDriveHint<Vec3> *pTDH = dynamic_cast<TplDriveHint<Vec3> *>(pH);
715  if (pTDH) {
716  return new OrientationDriveHint<Vec3>(pTDH);
717  }
718  }
719  return 0;
720  }
721 
722  return 0;
723 }
724 
725 void
726 TotalJoint::AfterConvergence(const VectorHandler& /* X */ ,
727  const VectorHandler& /* XP */ )
728 {
729  // "trick": correct ThetaDrv to keep ThetaDelta small
730  // See "A Vectorial Formulation of Generic Pair Constraints"
731  if (nRotConstraints < 3) {
732  Vec3 ThetaDeltaTmp(ThetaDelta);
733  for (unsigned i = 0; i < nRotConstraints; i++) {
734  // zero out constrained components
735  // NOTE: should already be zero within tolerance
736  ThetaDeltaTmp(iRotIncid[i]) = 0.;
737  }
738  ThetaDeltaRemnant += ThetaDeltaTmp;
739  }
740 
741  ThetaDeltaPrev = Unwrap(ThetaDeltaPrev, ThetaDeltaRemnant);
742  ThetaDeltaTrue = Unwrap(ThetaDeltaTrue, RotManip::VecRot((pNode1->GetRCurr()*R1hr).MulTM(pNode2->GetRCurr()*R2hr)));
743 }
744 
745 void
746 TotalJoint::AfterConvergence(const VectorHandler& X,
747  const VectorHandler& XP, const VectorHandler& XPP)
748 {
749  AfterConvergence(X, XP);
750 }
751 
752 std::ostream&
753 TotalJoint::Restart(std::ostream& out) const
754 {
755  Joint::Restart(out) << ", total joint, "
756  << pNode1->GetLabel() << ", "
757  << "position, ";
758  f1.Write(out, ", ") << ", "
759  << "position orientation, "
760  "1 , ";
761  R1h.GetVec(1).Write(out, ", ") << ", "
762  "2 , ";
763  R1h.GetVec(2).Write(out, ", ") << ", "
764  << "rotation orientation, "
765  "1 , ";
766  R1hr.GetVec(1).Write(out, ", ") << ", "
767  "2 , ";
768  R1hr.GetVec(2).Write(out, ", ") << ", "
769  << pNode2->GetLabel() << ", "
770  << "position, ";
771  f2.Write(out, ", ") << ", "
772  << "position orientation, "
773  "1 , ";
774  R2h.GetVec(1).Write(out, ", ") << ", "
775  "2 , ";
776  R2h.GetVec(2).Write(out, ", ") << ", "
777  << "rotation orientation, "
778  "1 , ";
779  R2hr.GetVec(1).Write(out, ", ") << ", "
780  "2 , ";
781  R2hr.GetVec(2).Write(out, ", ");
782 
783  if (bPosActive[0] || bPosActive[1] || bPosActive[2]
784  || bVelActive[0] || bVelActive[1] || bVelActive[2])
785  {
786  out << ", position constraint";
787  for (unsigned i = 0; i < 3; i++) {
788  if (bPosActive[i]) {
789  out << ", active";
790 
791  } else if (bVelActive[i]) {
792  out << ", velocity";
793 
794  } else {
795  out << ", inactive";
796  }
797  }
798 
799  out << ", ", XDrv.pGetDriveCaller()->Restart(out);
800  if (XPDrv.pGetDriveCaller()) {
801  out << ", ", XPDrv.pGetDriveCaller()->Restart(out)
802  << ", ", XPPDrv.pGetDriveCaller()->Restart(out);
803  }
804  }
805 
806  if (bRotActive[0] || bRotActive[1] || bRotActive[2]
807  || bAgvActive[0] || bAgvActive[1] || bAgvActive[2])
808  {
809  out << ", orientation constraint";
810  for (unsigned i = 0; i < 3; i++) {
811  if (bRotActive[i]) {
812  out << ", active";
813 
814  } else if (bAgvActive[i]) {
815  out << ", angular velocity";
816 
817  } else {
818  out << ", inactive";
819  }
820  }
821 
822  out << ", ", ThetaDrv.pGetDriveCaller()->Restart(out);
823  if (OmegaDrv.pGetDriveCaller()) {
824  out << ", ", OmegaDrv.pGetDriveCaller()->Restart(out)
825  << ", ", OmegaPDrv.pGetDriveCaller()->Restart(out);
826  }
827  }
828 
829  return out << ";" << std::endl;
830 }
831 
832 /* Assemblaggio jacobiano */
833 VariableSubMatrixHandler&
834 TotalJoint::AssJac(VariableSubMatrixHandler& WorkMat,
835  doublereal dCoef,
836  const VectorHandler& /* XCurr */ ,
837  const VectorHandler& /* XPrimeCurr */ )
838 {
839  /*
840  See tecman.pdf for details
841  */
842 
843  DEBUGCOUT("Entering TotalJoint::AssJac()" << std::endl);
844 
845  if (iGetNumDof() == 0) {
846  WorkMat.SetNullMatrix();
847  return WorkMat;
848  }
849 
850  FullSubMatrixHandler& WM = WorkMat.SetFull();
851 
852  /* Ridimensiona la sottomatrice in base alle esigenze */
853  integer iNumRows = 0;
854  integer iNumCols = 0;
855  WorkSpaceDim(&iNumRows, &iNumCols);
856  WM.ResizeReset(iNumRows, iNumCols);
857 
858  /* Recupera gli indici delle varie incognite */
859  integer iNode1FirstPosIndex = pNode1->iGetFirstPositionIndex();
860  integer iNode1FirstMomIndex = pNode1->iGetFirstMomentumIndex();
861  integer iNode2FirstPosIndex = pNode2->iGetFirstPositionIndex();
862  integer iNode2FirstMomIndex = pNode2->iGetFirstMomentumIndex();
863  integer iFirstReactionIndex = iGetFirstIndex();
864 
865  /* Setta gli indici delle equazioni */
866  for (int iCnt = 1; iCnt <= 6; iCnt++) {
867  WM.PutRowIndex(iCnt, iNode1FirstMomIndex + iCnt);
868  WM.PutColIndex(iCnt, iNode1FirstPosIndex + iCnt);
869  WM.PutRowIndex(6 + iCnt, iNode2FirstMomIndex + iCnt);
870  WM.PutColIndex(6 + iCnt, iNode2FirstPosIndex + iCnt);
871  }
872 
873  for (unsigned int iCnt = 1; iCnt <= nConstraints; iCnt++) {
874  WM.PutRowIndex(12 + iCnt, iFirstReactionIndex + iCnt);
875  WM.PutColIndex(12 + iCnt, iFirstReactionIndex + iCnt);
876  }
877 
878  /* Recupera i dati che servono */
879  Mat3x3 R1(pNode1->GetRCurr()*R1h);
880  Mat3x3 R1r(pNode1->GetRCurr()*R1hr);
881  Vec3 b2(pNode2->GetRCurr()*f2);
882  Vec3 b1(pNode2->GetXCurr() + b2 - pNode1->GetXCurr());
883 
884  /* Moltiplica il momento e la forza per il coefficiente del metodo
885  * SOLO se il vincolo è in posizione */
886 
887  Vec3 FTmp(R1*(F*dCoef));
888  Vec3 MTmp(R1r*(M*dCoef));
889 
890  /* Equilibrium: ((Phi/q)^T*Lambda)/q */
891 
892  Mat3x3 Tmp;
893 
894  /* [ F x ] */
895  Tmp = Mat3x3(MatCross, FTmp);
896 
897  /* Lines 1->3: */
898  WM.Add(1, 3 + 1, Tmp);
899 
900  /* Lines 4->6: */
901  WM.Sub(3 + 1, 1, Tmp);
902 
903  WM.Add(3 + 1, 6 + 1, Tmp);
904 
905  /* Lines 7->9: */
906  WM.Sub(6 + 1, 3 + 1, Tmp);
907 
908  /* [ F x ] [ b2 x ] */
909  Tmp = Mat3x3(MatCrossCross, FTmp, b2);
910 
911  /* Lines 4->6: */
912  WM.Sub(3 + 1, 9 + 1, Tmp);
913 
914  /* Lines 10->12: */
915  WM.Add(9 + 1, 9 + 1, Tmp);
916 
917  /* [ b1 x ] [ F x ] + [ M x ] */
918 
919  /* Lines 4->6: */
920  WM.Add(3 + 1, 3 + 1, Mat3x3(MatCrossCross, b1, FTmp) + Mat3x3(MatCross, MTmp));
921 
922  /* [ b2 x ] [ F x ] + [ M x ] */
923 
924  /* Lines 10->12: */
925  WM.Sub(9 + 1, 3 + 1, Mat3x3(MatCrossCross, b2, FTmp) + Mat3x3(MatCross, MTmp));
926 
927  /* Phi/q and (Phi/q)^T */
928 
929  Mat3x3 b1Cross_R1;
930  Mat3x3 b2Cross_R1;
931  if (nPosConstraints > 0 || nVelConstraints > 0) {
932  b1Cross_R1 = b1.Cross(R1); // = [ b1 x ] * R1
933  b2Cross_R1 = b2.Cross(R1); // = [ b2 x ] * R1
934  }
935 
936  if (nPosConstraints > 0) {
937  for (unsigned iCnt = 0 ; iCnt < nPosConstraints; iCnt++) {
938  Vec3 vR1(R1.GetVec(iPosIncid[iCnt]));
939  Vec3 vb1Cross_R1(b1Cross_R1.GetVec(iPosIncid[iCnt]));
940  Vec3 vb2Cross_R1(b2Cross_R1.GetVec(iPosIncid[iCnt]));
941 
942  /* Equilibrium, node 1 */
943  WM.Sub(1, 12 + 1 + iPosEqIndex[iCnt], vR1);
944  WM.Sub(3 + 1, 12 + 1 + iPosEqIndex[iCnt], vb1Cross_R1);
945 
946  /* Constraint, node 1 */
947  WM.SubT(12 + 1 + iPosEqIndex[iCnt], 1, vR1);
948  WM.SubT(12 + 1 + iPosEqIndex[iCnt], 3 + 1, vb1Cross_R1);
949 
950  /* Equilibrium, node 2 */
951  WM.Add(6 + 1, 12 + 1 + iPosEqIndex[iCnt], vR1);
952  WM.Add(9 + 1, 12 + 1 + iPosEqIndex[iCnt], vb2Cross_R1);
953 
954  /* Constraint, node 2 */
955  WM.AddT(12 + 1 + iPosEqIndex[iCnt], 6 + 1, vR1);
956  WM.AddT(12 + 1 + iPosEqIndex[iCnt], 9 + 1, vb2Cross_R1);
957  }
958  }
959 
960  if (nVelConstraints > 0) {
961  Mat3x3 Omega1Cross_R1(pNode1->GetWCurr().Cross(R1));
962  Mat3x3 Tmp12 = (
963  - Mat3x3(MatCrossCross, pNode1->GetWCurr(), b1)
964  + Mat3x3(MatCrossCross, b2, pNode1->GetWCurr())
965  - Mat3x3(MatCross, pNode2->GetVCurr())
966  - Mat3x3(MatCrossCross, pNode2->GetWCurr(), b2)
967  + Mat3x3(MatCross, pNode1->GetVCurr())
968  ) * R1;
969  Mat3x3 Tmp22(MatCrossCross, pNode2->GetWCurr(), b2);
970 
971  for (unsigned iCnt = 0 ; iCnt < nVelConstraints; iCnt++) {
972  Vec3 vR1(R1.GetVec(iVelIncid[iCnt]));
973  Vec3 vb1Cross_R1(b1Cross_R1.GetVec(iVelIncid[iCnt]));
974  Vec3 vb2Cross_R1(b2Cross_R1.GetVec(iVelIncid[iCnt]));
975 
976  Vec3 vOmega1Cross_R1(Omega1Cross_R1.GetVec(iVelIncid[iCnt])*dCoef);
977  Vec3 vTmp12(Tmp12.GetVec(iVelIncid[iCnt]));
978  Vec3 vTmp22(Tmp22.GetVec(iVelIncid[iCnt]));
979 
980  /* Equilibrium, node 1 */
981  /* The same as in position constraint*/
982  WM.Sub(1, 12 + 1 + iVelEqIndex[iCnt], vR1); // delta_F
983  WM.Sub(3 + 1, 12 + 1 + iVelEqIndex[iCnt], vb1Cross_R1); // delta_M
984 
985  /* Constraint, node 1 */
986  /* The same as in position constraint*/
987  WM.SubT(12 + 1 + iVelEqIndex[iCnt], 1, vR1); // delta_v1
988  WM.SubT(12 + 1 + iVelEqIndex[iCnt], 3 + 1, vb1Cross_R1); // delta_W1
989 
990  /* New contributions, related to delta_x1 and delta_g1 */
991  WM.SubT(12 + 1 + iVelEqIndex[iCnt], 1, vOmega1Cross_R1); // delta_x1
992  WM.AddT(12 + 1 + iVelEqIndex[iCnt], 3 + 1, vTmp12 * dCoef); // delta_g1
993 
994  /* Equilibrium, node 2 */
995  /* The same as in position constraint*/
996  WM.Add(6 + 1, 12 + 1 + iVelEqIndex[iCnt], vR1); // delta_F
997  WM.Add(9 + 1, 12 + 1 + iVelEqIndex[iCnt], vb2Cross_R1); // delta_M
998 
999  /* Constraint, node 2 */
1000  /* The same as in position constraint*/
1001  WM.AddT(12 + 1 + iVelEqIndex[iCnt], 6 + 1, vR1); // delta_v2
1002  WM.AddT(12 + 1 + iVelEqIndex[iCnt], 9 + 1, vb2Cross_R1); // delta_W2
1003 
1004  /* New contributions, related to delta_x1 and delta_g1 */
1005  WM.AddT(12 + 1 + iVelEqIndex[iCnt], 6 + 1, vOmega1Cross_R1); // delta_x2
1006  WM.AddT(12 + 1 + iVelEqIndex[iCnt], 9 + 1, vTmp22 * dCoef); // delta_g2
1007  }
1008  }
1009 
1010  if (nRotConstraints > 0) {
1011  for (unsigned iCnt = 0 ; iCnt < nRotConstraints; iCnt++) {
1012  Vec3 vR1(R1r.GetVec(iRotIncid[iCnt]));
1013 
1014  /* Equilibrium, node 1 */
1015  WM.Sub(3 + 1, 12 + 1 + iRotEqIndex[iCnt], vR1);
1016 
1017  /* Constraint, node 1 */
1018  WM.SubT(12 + 1 + iRotEqIndex[iCnt], 3 + 1, vR1);
1019 
1020  /* Equilibrium, node 2 */
1021  WM.Add(9 + 1, 12 + 1 + iRotEqIndex[iCnt], vR1);
1022 
1023  /* Constraint, node 2 */
1024  WM.AddT(12 + 1 + iRotEqIndex[iCnt], 9 + 1, vR1);
1025  }
1026  }
1027 
1028  if (nAgvConstraints > 0) {
1029  Mat3x3 W2_Cross_R1(pNode2->GetWCurr().Cross(R1r));
1030 
1031  for (unsigned iCnt = 0 ; iCnt < nAgvConstraints; iCnt++) {
1032  Vec3 vR1(R1r.GetVec(iAgvIncid[iCnt]));
1033  Vec3 vW2_Cross_R1(W2_Cross_R1.GetVec(iAgvIncid[iCnt])*dCoef);
1034 
1035  /* Equilibrium, node 1 */
1036  WM.Sub(3 + 1, 12 + 1 + iAgvEqIndex[iCnt], vR1); // delta_M
1037 
1038  /* Constraint, node 1 */
1039  WM.SubT(12 + 1 + iAgvEqIndex[iCnt], 3 + 1, vR1); // delta_W1
1040 
1041  /* New contribution, related to delta_g1 */
1042  WM.SubT(12 + 1 + iAgvEqIndex[iCnt], 3 + 1, vW2_Cross_R1); // delta_g1
1043 
1044  /* Equilibrium, node 2 */
1045  WM.Add(9 + 1, 12 + 1 + iAgvEqIndex[iCnt], vR1); // delta_M
1046 
1047  /* Constraint, node 2 */
1048  WM.AddT(12 + 1 + iAgvEqIndex[iCnt], 9 + 1, vR1);// delta_W2
1049 
1050  /* New contribution, related to delta_g2 */
1051  WM.AddT(12 + 1 + iAgvEqIndex[iCnt], 9 + 1, vW2_Cross_R1); // delta_g2
1052  }
1053  }
1054 
1055  return WorkMat;
1056 }
1057 
1058 /* Assemblaggio residuo */
1059 SubVectorHandler&
1060 TotalJoint::AssRes(SubVectorHandler& WorkVec,
1061  doublereal dCoef,
1062  const VectorHandler& XCurr,
1063  const VectorHandler& /* XPrimeCurr */ )
1064 {
1065  DEBUGCOUT("Entering TotalJoint::AssRes()" << std::endl);
1066 
1067  if (iGetNumDof() == 0) {
1068  WorkVec.ResizeReset(0);
1069  return WorkVec;
1070  }
1071 
1072  /* Dimensiona e resetta la matrice di lavoro */
1073  integer iNumRows = 0;
1074  integer iNumCols = 0;
1075  WorkSpaceDim(&iNumRows, &iNumCols);
1076  WorkVec.ResizeReset(iNumRows);
1077 
1078  /* Indici */
1079  integer iNode1FirstMomIndex = pNode1->iGetFirstMomentumIndex();
1080  integer iNode2FirstMomIndex = pNode2->iGetFirstMomentumIndex();
1081  integer iFirstReactionIndex = iGetFirstIndex();
1082 
1083  /* Indici dei nodi */
1084  for (int iCnt = 1; iCnt <= 6; iCnt++) {
1085  WorkVec.PutRowIndex(iCnt, iNode1FirstMomIndex + iCnt);
1086  WorkVec.PutRowIndex(6 + iCnt, iNode2FirstMomIndex + iCnt);
1087  }
1088 
1089  /* Indici del vincolo */
1090  for (unsigned int iCnt = 1; iCnt <= nConstraints; iCnt++) {
1091  WorkVec.PutRowIndex(12 + iCnt, iFirstReactionIndex + iCnt);
1092  }
1093 
1094  /* Get constraint reactions */
1095 
1096  for (unsigned iCnt = 0; iCnt < nPosConstraints; iCnt++) {
1097  F(iPosIncid[iCnt]) = XCurr(iFirstReactionIndex + 1 + iPosEqIndex[iCnt]);
1098  }
1099 
1100  for (unsigned iCnt = 0; iCnt < nRotConstraints; iCnt++) {
1101  M(iRotIncid[iCnt]) = XCurr(iFirstReactionIndex + 1 + iRotEqIndex[iCnt]);
1102  }
1103 
1104  for (unsigned iCnt = 0; iCnt < nVelConstraints; iCnt++) {
1105  F(iVelIncid[iCnt]) = XCurr(iFirstReactionIndex + 1 + iVelEqIndex[iCnt]);
1106  }
1107 
1108  for (unsigned iCnt = 0; iCnt < nAgvConstraints; iCnt++) {
1109  M(iAgvIncid[iCnt]) = XCurr(iFirstReactionIndex + 1 + iAgvEqIndex[iCnt]);
1110  }
1111 
1112  Vec3 b2(pNode2->GetRCurr()*f2);
1113  Vec3 b1(pNode2->GetXCurr() + b2 - pNode1->GetXCurr());
1114 
1115  Mat3x3 R1 = pNode1->GetRCurr()*R1h;
1116  Mat3x3 R1r = pNode1->GetRCurr()*R1hr;
1117 
1118  Vec3 XDelta;
1119  if (nPosConstraints) {
1120  XDelta = R1.MulTV(b1) - tilde_f1 - XDrv.Get();
1121  }
1122 
1123  Vec3 VDelta;
1124  if (nVelConstraints) {
1125  VDelta = R1.MulTV(
1126  b1.Cross(pNode1->GetWCurr())
1127  + pNode2->GetVCurr()
1128  - b2.Cross(pNode2->GetWCurr())
1129  - pNode1->GetVCurr()
1130  )
1131  - XDrv.Get();
1132  }
1133 
1134  if (nRotConstraints) {
1135  Mat3x3 R2r(pNode2->GetRCurr()*R2hr);
1136 
1137  Vec3 ThetaDrvTmp(ThetaDrv.Get());
1138  if (nRotConstraints < 3) {
1139  for (int i = nRotConstraints; i < 3; i++) {
1140  // zero out unconstrained components of drive
1141  ThetaDrvTmp(iRotIncid[i]) = 0.;
1142  }
1143  // add remnant to make ThetaDelta as close to zero
1144  // as possible
1145  ThetaDrvTmp += ThetaDeltaRemnant;
1146  }
1147  Mat3x3 R0(RotManip::Rot(ThetaDrvTmp));
1148  Mat3x3 RDelta(R1r.MulTM(R2r.MulMT(R0)));
1149  ThetaDelta = RotManip::VecRot(RDelta);
1150  }
1151 
1152  Vec3 WDelta;
1153  if (nAgvConstraints) {
1154  WDelta = R1r.MulTV(pNode2->GetWCurr() - pNode1->GetWCurr()) - ThetaDrv.Get();
1155  }
1156 
1157  Vec3 FTmp(R1*F);
1158  Vec3 MTmp(R1r*M);
1159 
1160  /* Equilibrium, node 1 */
1161  WorkVec.Add(1, FTmp);
1162  WorkVec.Add(3 + 1, MTmp + b1.Cross(FTmp));
1163 
1164  /* Equilibrium, node 2 */
1165  WorkVec.Sub(6 + 1, FTmp);
1166  WorkVec.Sub(9 + 1, MTmp + b2.Cross(FTmp));
1167 
1168  /* Holonomic constraint equations are divided by dCoef */
1169  ASSERT(dCoef != 0.);
1170 
1171  /* Position constraint: */
1172  for (unsigned iCnt = 0; iCnt < nPosConstraints; iCnt++) {
1173  WorkVec.PutCoef(12 + 1 + iPosEqIndex[iCnt],
1174  -XDelta(iPosIncid[iCnt])/dCoef);
1175  }
1176 
1177  /* Rotation constraints: */
1178  for (unsigned iCnt = 0; iCnt < nRotConstraints; iCnt++) {
1179  WorkVec.PutCoef(12 + 1 + iRotEqIndex[iCnt],
1180  -ThetaDelta(iRotIncid[iCnt])/dCoef);
1181  }
1182 
1183  /* Linear Velocity Constraint */
1184  for (unsigned iCnt = 0; iCnt < nVelConstraints; iCnt++) {
1185  WorkVec.PutCoef(12 + 1 + iVelEqIndex[iCnt],
1186  -VDelta(iVelIncid[iCnt]));
1187  }
1188 
1189  /* Angular Velocity Constraint */
1190  for (unsigned iCnt = 0; iCnt < nAgvConstraints; iCnt++) {
1191  WorkVec.PutCoef(12 + 1 + iAgvEqIndex[iCnt],
1192  -WDelta(iAgvIncid[iCnt]));
1193  }
1194 
1195  return WorkVec;
1196 }
1197 
1198 /* inverse dynamics capable element */
1199 bool
1200 TotalJoint::bInverseDynamics(void) const
1201 {
1202  return true;
1203 }
1204 
1205 /* Inverse Dynamics Jacobian matrix assembly */
1206 VariableSubMatrixHandler&
1207 TotalJoint::AssJac(VariableSubMatrixHandler& WorkMat,
1208  const VectorHandler& /* XCurr */ )
1209 {
1210  /*
1211  * identical to regular AssJac's lower-left block
1212  */
1213  DEBUGCOUT("Entering TotalJoint::AssJac()" << std::endl);
1214 
1215  if (iGetNumDof() == 0) {
1216  WorkMat.SetNullMatrix();
1217  return WorkMat;
1218  }
1219 
1220  /* Ridimensiona la sottomatrice in base alle esigenze */
1221  integer iNumRows = 0;
1222  integer iNumCols = 0;
1223  WorkSpaceDim(&iNumRows, &iNumCols);
1224 
1225  FullSubMatrixHandler& WM = WorkMat.SetFull();
1226 
1227  /* original - nodes, nodes */
1228  WM.ResizeReset(iNumRows - 12, 12);
1229 
1230  /* Recupera gli indici delle varie incognite */
1231  integer iNode1FirstPosIndex = pNode1->iGetFirstPositionIndex();
1232  integer iNode2FirstPosIndex = pNode2->iGetFirstPositionIndex();
1233  integer iFirstReactionIndex = iGetFirstIndex();
1234 
1235  /* Setta gli indici delle equazioni */
1236  for (int iCnt = 1; iCnt <= 6; iCnt++) {
1237  WM.PutColIndex(iCnt, iNode1FirstPosIndex + iCnt);
1238  WM.PutColIndex(6 + iCnt, iNode2FirstPosIndex + iCnt);
1239  }
1240 
1241  for (unsigned int iCnt = 1; iCnt <= nConstraints; iCnt++) {
1242  WM.PutRowIndex(iCnt, iFirstReactionIndex + iCnt);
1243  }
1244 
1245  /* Recupera i dati che servono */
1246  Mat3x3 R1(pNode1->GetRRef()*R1h);
1247 
1248  if (nPosConstraints > 0) {
1249  Vec3 b2(pNode2->GetRCurr()*f2);
1250  Vec3 b1(pNode2->GetXCurr() + b2 - pNode1->GetXCurr());
1251 
1252  Mat3x3 b1Cross_R1(b1.Cross(R1)); // = [ b1 x ] * R1
1253  Mat3x3 b2Cross_R1(b2.Cross(R1)); // = [ b2 x ] * R1
1254 
1255  for (unsigned iCnt = 0 ; iCnt < nPosConstraints; iCnt++) {
1256  Vec3 vR1(R1.GetVec(iPosIncid[iCnt]));
1257  Vec3 vb1Cross_R1(b1Cross_R1.GetVec(iPosIncid[iCnt]));
1258  Vec3 vb2Cross_R1(b2Cross_R1.GetVec(iPosIncid[iCnt]));
1259 
1260  /* Constraint, node 1 */
1261  WM.SubT(1 + iCnt, 1, vR1);
1262  WM.SubT(1 + iCnt, 3 + 1, vb1Cross_R1);
1263 
1264  /* Constraint, node 2 */
1265  WM.AddT(1 + iCnt, 6 + 1, vR1);
1266  WM.AddT(1 + iCnt, 9 + 1, vb2Cross_R1);
1267  }
1268  }
1269 
1270  if (nRotConstraints > 0) {
1271  Mat3x3 R1r(pNode1->GetRRef()*R1hr);
1272  for (unsigned iCnt = 0 ; iCnt < nRotConstraints; iCnt++) {
1273  Vec3 vR1(R1r.GetVec(iRotIncid[iCnt]));
1274 
1275  /* Constraint, node 1 */
1276  WM.SubT(1 + nPosConstraints + iCnt, 3 + 1, vR1);
1277 
1278  /* Constraint, node 2 */
1279  WM.AddT(1 + nPosConstraints + iCnt, 9 + 1, vR1);
1280  }
1281  }
1282 
1283  return WorkMat;
1284 }
1285 
1286 /* Inverse Dynamics residual assembly */
1287 SubVectorHandler&
1288 TotalJoint::AssRes(SubVectorHandler& WorkVec,
1289  const VectorHandler& XCurr,
1290  const VectorHandler& XPrimeCurr,
1291  const VectorHandler& /* XPrimePrimeCurr */,
1292  InverseDynamics::Order iOrder)
1293 {
1294  DEBUGCOUT("Entering TotalJoint::AssRes(" << invdyn2str(iOrder) << ")" << std::endl);
1295 
1296  if (iGetNumDof() == 0 || iOrder == InverseDynamics::INVERSE_DYNAMICS) {
1297  WorkVec.ResizeReset(0);
1298  return WorkVec;
1299  }
1300 
1301  /* Dimensiona e resetta la matrice di lavoro */
1302  integer iNumRows = 0;
1303  integer iNumCols = 0;
1304  WorkSpaceDim(&iNumRows, &iNumCols);
1305 
1306  /* original - node equations (6 * 2) */
1307  WorkVec.ResizeReset(iNumRows - 12);
1308 
1309  /* Indici */
1310  integer iFirstReactionIndex = iGetFirstIndex();
1311 
1312  /* Indici del vincolo */
1313  for (unsigned int iCnt = 1; iCnt <= nConstraints; iCnt++) {
1314  WorkVec.PutRowIndex(iCnt, iFirstReactionIndex + iCnt);
1315  }
1316 
1317  switch (iOrder) {
1318  case InverseDynamics::POSITION: // Position - Orientation
1319  /*
1320  * identical to regular AssRes' lower block
1321  */
1322 
1323  if (nPosConstraints > 0) {
1324  Mat3x3 R1 = pNode1->GetRCurr()*R1h;
1325  Vec3 b2(pNode2->GetRCurr()*f2);
1326  Vec3 b1(pNode2->GetXCurr() + b2 - pNode1->GetXCurr());
1327 
1328  Vec3 XDelta = R1.MulTV(b1) - tilde_f1 - XDrv.Get();
1329 
1330  /* Position constraint */
1331  for (unsigned iCnt = 0; iCnt < nPosConstraints; iCnt++) {
1332  WorkVec.DecCoef(1 + iPosEqIndex[iCnt], XDelta(iPosIncid[iCnt]));
1333  }
1334  }
1335 
1336  if (nRotConstraints > 0) {
1337  Mat3x3 R1r = pNode1->GetRCurr()*R1hr;
1338  Mat3x3 R2r = pNode2->GetRCurr()*R2hr;
1339 
1340  Vec3 ThetaDrvTmp(ThetaDrv.Get());
1341  if (nRotConstraints < 3) {
1342  for (int i = nRotConstraints; i < 3; i++) {
1343  // zero out unconstrained components of drive
1344  ThetaDrvTmp(iRotIncid[i]) = 0.;
1345  }
1346  // add remnant to make ThetaDelta as close to zero
1347  // as possible
1348  ThetaDrvTmp += ThetaDeltaRemnant;
1349  }
1350  Mat3x3 R0(RotManip::Rot(ThetaDrvTmp));
1351  Mat3x3 RDelta = R1r.MulTM(R2r.MulMT(R0));
1352  ThetaDelta = RotManip::VecRot(RDelta);
1353 
1354  /* Rotation constraint */
1355  for (unsigned iCnt = 0; iCnt < nRotConstraints; iCnt++) {
1356  WorkVec.DecCoef(1 + iRotEqIndex[iCnt], ThetaDelta(iRotIncid[iCnt]));
1357  }
1358  }
1359 
1360  break;
1361 
1362  case InverseDynamics::VELOCITY: // Velocity
1363  /*
1364  * first derivative of regular AssRes' lower block
1365  */
1366  if (nPosConstraints > 0) {
1367  Vec3 Tmp = XPDrv.Get();
1368 
1369  /* Position constraint derivative */
1370  for (unsigned iCnt = 0; iCnt < nPosConstraints; iCnt++) {
1371  WorkVec.PutCoef(1 + iPosEqIndex[iCnt], Tmp(iPosIncid[iCnt]));
1372  }
1373  }
1374 
1375  if (nRotConstraints > 0) {
1376  Mat3x3 R1r = pNode1->GetRCurr()*R1hr;
1377  Mat3x3 R2r = pNode2->GetRCurr()*R2hr;
1378 
1379  Mat3x3 R0 = RotManip::Rot(ThetaDrv.Get());
1380  Mat3x3 RDelta = R1r.MulTM(R2r.MulMT(R0));
1381 
1382  /*This name is only for clarity...*/
1383  Vec3 WDelta = RDelta * OmegaDrv.Get();
1384 
1385  /* Rotation constraint derivative */
1386  for (unsigned iCnt = 0; iCnt < nRotConstraints; iCnt++) {
1387  WorkVec.PutCoef(1 + iRotEqIndex[iCnt], WDelta(iRotIncid[iCnt]));
1388  }
1389  }
1390 
1391  break;
1392 
1393  case InverseDynamics::ACCELERATION: // Acceleration
1394  /*
1395  * second derivative of regular AssRes' lower block
1396  */
1397  if (nPosConstraints > 0) {
1398  Vec3 b2(pNode2->GetRCurr()*f2);
1399  Vec3 b1(pNode2->GetXCurr() + b2 - pNode1->GetXCurr());
1400 
1401  Mat3x3 R1 = pNode1->GetRCurr()*R1h;
1402 
1403  Vec3 b1Prime(pNode2->GetVCurr() + pNode2->GetWCurr().Cross(b2) - pNode1->GetVCurr());
1404 
1405  Vec3 Tmp2 = (- pNode1->GetWCurr().Cross(b1) + b1Prime).Cross(pNode1->GetWCurr());
1406  Tmp2 += (
1407  pNode1->GetWCurr().Cross(- pNode2->GetVCurr()
1408  + b2.Cross(pNode2->GetWCurr())
1409  + pNode1->GetVCurr())
1410  -pNode2->GetWCurr().Cross(b2.Cross(pNode2->GetWCurr()))
1411  );
1412  Vec3 Tmp = R1.MulTV(Tmp2) - XPPDrv.Get();
1413 
1414  /* Position constraint second derivative */
1415  for (unsigned iCnt = 0; iCnt < nPosConstraints; iCnt++) {
1416  WorkVec.DecCoef(1 + iPosEqIndex[iCnt], Tmp(iPosIncid[iCnt]));
1417  }
1418  }
1419 
1420  if (nRotConstraints > 0) {
1421  Mat3x3 R1r = pNode1->GetRCurr()*R1hr;
1422  Mat3x3 R2r = pNode2->GetRCurr()*R2hr;
1423  Mat3x3 R0 = RotManip::Rot(ThetaDrv.Get());
1424  Mat3x3 RDelta = R1r.MulTM(R2r.MulMT(R0));
1425 
1426  Vec3 Tmp = R0.MulTV(OmegaDrv.Get());
1427  Vec3 Tmp2 = R2r * Tmp;
1428  Tmp = (pNode2->GetWCurr() - pNode1->GetWCurr()).Cross(Tmp2);
1429  Tmp += pNode1->GetWCurr().Cross(pNode2->GetWCurr());
1430  Tmp2 = R1r.MulTV(Tmp);
1431  Tmp2 += RDelta * OmegaPDrv.Get();
1432 
1433  /* Rotation constraint second derivative */
1434  for (unsigned iCnt = 0; iCnt < nRotConstraints; iCnt++) {
1435  WorkVec.PutCoef(1 + iRotEqIndex[iCnt], Tmp2(iRotIncid[iCnt]));
1436  }
1437  }
1438 
1439  break;
1440 
1441  default:
1442  /*
1443  * higher-order derivatives make no sense right now
1444  */
1445 
1446  ASSERT(0);
1447  throw ErrGeneric(MBDYN_EXCEPT_ARGS);
1448  }
1449 
1450  return WorkVec;
1451 }
1452 
1453 /* Inverse Dynamics update */
1454 void
1455 TotalJoint::Update(const VectorHandler& XCurr, InverseDynamics::Order /* iOrder */)
1456 {
1457  integer iFirstReactionIndex = iGetFirstIndex();
1458 
1459  /* Get constraint reactions */
1460  for (unsigned iCnt = 0; iCnt < nPosConstraints; iCnt++) {
1461  F(iPosIncid[iCnt]) = XCurr(iFirstReactionIndex + 1 + iPosEqIndex[iCnt]);
1462  }
1463  for (unsigned iCnt = 0; iCnt < nRotConstraints; iCnt++) {
1464  M(iRotIncid[iCnt]) = XCurr(iFirstReactionIndex + 1 + iRotEqIndex[iCnt]);
1465  }
1466 };
1467 
1468 DofOrder::Order
1469 TotalJoint::GetEqType(unsigned int i) const
1470 {
1471  ASSERTMSGBREAK(i < iGetNumDof(),
1472  "INDEX ERROR in TotalJoint::GetEqType");
1473 
1474  return DofOrder::ALGEBRAIC;
1475 }
1476 
1477 void
1478 TotalJoint::OutputPrepare(OutputHandler& OH)
1479 {
1480  if (bToBeOutput()) {
1481 #ifdef USE_NETCDF
1482  if (OH.UseNetCDF(OutputHandler::JOINTS)) {
1483  std::string name;
1484  OutputPrepare_int("total", OH, name);
1485 
1486  Var_X = OH.CreateVar<Vec3>(name + "X", "m",
1487  "local relative position (x, y, z)");
1488 
1489  // NOTE: by now, force ORIENTATION_VECTOR
1490  Var_Phi = OH.CreateRotationVar(name, "", ORIENTATION_VECTOR, "local relative");
1491 
1492  Var_V = OH.CreateVar<Vec3>(name + "V", "m/s",
1493  "local relative velocity (x, y, z)");
1494 
1495  Var_Omega = OH.CreateVar<Vec3>(name + "Omega", "radian/s",
1496  "local relative angular velocity (x, y, z)");
1497  }
1498 #endif // USE_NETCDF
1499  }
1500 }
1501 
1502 /* Output (da mettere a punto), per ora solo reazioni */
1503 void
1504 TotalJoint::Output(OutputHandler& OH) const
1505 {
1506  if (bToBeOutput()) {
1507  const Vec3& X1(pNode1->GetXCurr());
1508  const Vec3& X2(pNode2->GetXCurr());
1509  const Mat3x3& R1(pNode1->GetRCurr());
1510  const Mat3x3& R2(pNode2->GetRCurr());
1511  const Vec3& V1(pNode1->GetVCurr());
1512  const Vec3& V2(pNode2->GetVCurr());
1513  const Vec3& Omega1(pNode1->GetWCurr());
1514  const Vec3& Omega2(pNode2->GetWCurr());
1515 
1516  Mat3x3 R1Tmp(R1*R1h);
1517  Mat3x3 R1rTmp(R1*R1hr);
1518  Mat3x3 R2rTmp(R2*R2hr);
1519  Mat3x3 RTmp(R1rTmp.MulTM(R2rTmp));
1520  Vec3 ThetaTmp(RotManip::VecRot(RTmp));
1521  Vec3 b2(R2*f2);
1522  Vec3 b1(X2 + b2 - X1);
1523  Vec3 XTmp(R1Tmp.MulTV(b1) - tilde_f1);
1524  Vec3 VTmp(R1Tmp.MulTV(V2 + Omega2.Cross(b2) - V1 - Omega1.Cross(b1)));
1525  Vec3 OmegaTmp(R1rTmp.MulTV(Omega2 - Omega1));
1526 
1527  Vec3 FTmp(R1Tmp*F);
1528  Vec3 MTmp(R1rTmp*M);
1529 
1530 #ifdef USE_NETCDF
1531  if (OH.UseNetCDF(OutputHandler::JOINTS)) {
1532  Var_F_local->put_rec(F.pGetVec(), OH.GetCurrentStep());
1533  Var_M_local->put_rec(M.pGetVec(), OH.GetCurrentStep());
1534  Var_F_global->put_rec(FTmp.pGetVec(), OH.GetCurrentStep());
1535  Var_M_global->put_rec(MTmp.pGetVec(), OH.GetCurrentStep());
1536 
1537  Var_X->put_rec(XTmp.pGetVec(), OH.GetCurrentStep());
1538  Var_Phi->put_rec(ThetaTmp.pGetVec(), OH.GetCurrentStep());
1539  Var_V->put_rec(VTmp.pGetVec(), OH.GetCurrentStep());
1540  Var_Omega->put_rec(OmegaTmp.pGetVec(), OH.GetCurrentStep());
1541  }
1542 #endif // USE_NETCDF
1543 
1544  if (OH.UseText(OutputHandler::JOINTS)) {
1545  Joint::Output(OH.Joints(), "TotalJoint", GetLabel(),
1546  F, M, FTmp, MTmp)
1547  << " " << XTmp
1548  << " " << ThetaTmp
1549  << " " << VTmp
1550  << " " << OmegaTmp
1551  << std::endl;
1552  // accelerations?
1553  }
1554  }
1555 }
1556 
1557 /* Contributo allo jacobiano durante l'assemblaggio iniziale */
1558 VariableSubMatrixHandler&
1559 TotalJoint::InitialAssJac(VariableSubMatrixHandler& WorkMat,
1560  const VectorHandler& XCurr)
1561 {
1562  if (iGetInitialNumDof() == 0) {
1563  WorkMat.SetNullMatrix();
1564  return WorkMat;
1565  }
1566 
1567  /* Per ora usa la matrice piena; eventualmente si puo'
1568  * passare a quella sparsa quando si ottimizza */
1569  FullSubMatrixHandler& WM = WorkMat.SetFull();
1570 
1571  /* Dimensiona e resetta la matrice di lavoro */
1572  integer iNumRows = 0;
1573  integer iNumCols = 0;
1574  InitialWorkSpaceDim(&iNumRows, &iNumCols);
1575  WM.ResizeReset(iNumRows, iNumCols);
1576 
1577  /* Equazioni: vedi joints.dvi */
1578 
1579  /* equazioni ed incognite
1580  * F1 Delta_x1 1
1581  * M1 Delta_g1 3 + 1
1582  * FP1 Delta_xP1 6 + 1
1583  * MP1 Delta_w1 9 + 1
1584  * F2 Delta_x2 12 + 1
1585  * M2 Delta_g2 15 + 1
1586  * FP2 Delta_xP2 18 + 1
1587  * MP2 Delta_w2 21 + 1
1588  * vincolo spostamento Delta_F 24 + 1
1589  * vincolo rotazione Delta_M 24 + nPosConstraints
1590  * derivata vincolo spostamento Delta_FP 24 + nConstraints
1591  * derivata vincolo rotazione Delta_MP 24 + nConstraints + nPosConstraints
1592  */
1593 
1594 
1595  /* Indici */
1596  integer iNode1FirstPosIndex = pNode1->iGetFirstPositionIndex();
1597  integer iNode1FirstVelIndex = iNode1FirstPosIndex + 6;
1598  integer iNode2FirstPosIndex = pNode2->iGetFirstPositionIndex();
1599  integer iNode2FirstVelIndex = iNode2FirstPosIndex + 6;
1600  integer iFirstReactionIndex = iGetFirstIndex();
1601  integer iReactionPrimeIndex = iFirstReactionIndex + nConstraints;
1602 
1603 
1604  /* Setta gli indici dei nodi */
1605  for (int iCnt = 1; iCnt <= 6; iCnt++) {
1606  WM.PutRowIndex(iCnt, iNode1FirstPosIndex + iCnt);
1607  WM.PutColIndex(iCnt, iNode1FirstPosIndex + iCnt);
1608  WM.PutRowIndex(6 + iCnt, iNode1FirstVelIndex + iCnt);
1609  WM.PutColIndex(6 + iCnt, iNode1FirstVelIndex + iCnt);
1610  WM.PutRowIndex(12 + iCnt, iNode2FirstPosIndex + iCnt);
1611  WM.PutColIndex(12 + iCnt, iNode2FirstPosIndex + iCnt);
1612  WM.PutRowIndex(18 + iCnt, iNode2FirstVelIndex + iCnt);
1613  WM.PutColIndex(18 + iCnt, iNode2FirstVelIndex + iCnt);
1614  }
1615 
1616  /* Setta gli indici delle reazioni */
1617  for (unsigned int iCnt = 1; iCnt <= nConstraints; iCnt++) {
1618  WM.PutRowIndex(24 + iCnt, iFirstReactionIndex + iCnt);
1619  WM.PutColIndex(24 + iCnt, iFirstReactionIndex + iCnt);
1620  WM.PutRowIndex(24 + nConstraints + iCnt, iReactionPrimeIndex + iCnt);
1621  WM.PutColIndex(24 + nConstraints + iCnt, iReactionPrimeIndex + iCnt);
1622  }
1623 
1624  /* Recupera i dati che servono */
1625  Mat3x3 R1(pNode1->GetRRef()*R1h);
1626  Mat3x3 R1r(pNode1->GetRRef()*R1hr);
1627 
1628  Vec3 b2(pNode2->GetRCurr()*f2);
1629  Vec3 b1(pNode2->GetXCurr() + b2 - pNode1->GetXCurr());
1630 
1631  const Vec3& Omega2(pNode2->GetWCurr());
1632 
1633  Vec3 b1Prime(pNode2->GetVCurr() + Omega2.Cross(b2) - pNode1->GetVCurr());
1634 
1635  /* F ed M sono gia' state aggiornate da InitialAssRes;
1636  * Recupero FPrime e MPrime*/
1637  Vec3 MPrime(::Zero3);
1638  Vec3 FPrime(::Zero3);
1639 
1640  for (unsigned iCnt = 0; iCnt < nPosConstraints; iCnt++) {
1641  FPrime(iPosIncid[iCnt]) = XCurr(iReactionPrimeIndex + 1 + iCnt);
1642  }
1643 
1644  for (unsigned iCnt = 0; iCnt < nRotConstraints; iCnt++) {
1645  MPrime(iRotIncid[iCnt]) = XCurr(iReactionPrimeIndex + 1 + nPosConstraints + iCnt);
1646  }
1647 
1648  Vec3 FTmp(R1 * F);
1649  Vec3 MTmp(R1r * M);
1650  Vec3 FPrimeTmp(R1 * FPrime);
1651  Vec3 MPrimeTmp(R1r * MPrime);
1652 
1653  Mat3x3 Tmp;
1654 
1655  /* [ F x ] */
1656  Tmp = Mat3x3(MatCross, FTmp);
1657 
1658  /* Force, Node 1, Lines 1->3: */
1659  WM.Add(1, 3 + 1, Tmp); // * Delta_g1
1660 
1661  /* Moment, Node 1, Lines 4->6: */
1662  WM.Sub(3 + 1, 1, Tmp); // * Delta_x1
1663 
1664  WM.Add(3 + 1, 12 + 1, Tmp); // * Delta_x2
1665 
1666  /* Force, Node 2, Lines 13->15: */
1667  WM.Sub(12 + 1, 3 + 1, Tmp); // * Delta_g1
1668 
1669  /* [ FP x ] */
1670  Tmp = Mat3x3(MatCross, FPrimeTmp);
1671 
1672  /* d/dt(Force), Node 1, Lines 7->9: */
1673  WM.Add(6 + 1, 9 + 1, Tmp); // * Delta_W1
1674 
1675  /* d/dt(Moment), Node 1, Lines 10->12: */
1676  WM.Sub(9 + 1, 6 + 1, Tmp); // * Delta_x1P
1677 
1678  WM.Add(9 + 1, 18 + 1, Tmp); // * Delta_x2P
1679 
1680  /* d/dt(Force), Node 2, Lines 19->21: */
1681  WM.Sub(18 + 1 , 9 + 1, Tmp); // * Delta_W1
1682 
1683  /* [ F x ] [ b2 x ] */
1684  Tmp = Mat3x3(MatCrossCross, FTmp, b2);
1685 
1686  /* Moment, Node1, Lines 4->6: */
1687  WM.Sub(3 + 1, 15 + 1, Tmp); // * Delta_g2
1688 
1689  /* Moment, Node2, Lines 16->18: */
1690  WM.Add(15 + 1, 15 + 1, Tmp); // * Delta_g2
1691 
1692  /* [ FP x ] [ b2 x ] */
1693  Tmp = Mat3x3(MatCrossCross, FPrimeTmp, b2);
1694 
1695  /* d/dt(Moment), Node1, Lines 10->12: */
1696  WM.Sub(9 + 1, 21 + 1, Tmp); // * Delta_W2
1697 
1698  /* d/dt(Moment), Node2, Lines 22->24: */
1699  WM.Add(21 + 1, 21 + 1, Tmp); // * Delta_W2
1700 
1701  /* [ b1 x ] [ F x ] + [ M x ] */
1702 
1703  /* Moment, Node1, Lines 4->6: */
1704  WM.Add(3 + 1, 3 + 1, Mat3x3(MatCrossCross, b1, FTmp) + Mat3x3(MatCross, MTmp)); // *Delta_g1
1705 
1706  /* d/dt(Moment), Node1, Lines 10->12: */
1707  WM.Add(9 + 1, 9 + 1, Mat3x3(MatCrossCross, b1, FPrimeTmp) + Mat3x3(MatCross, MPrimeTmp));// *Delta_W1
1708 
1709  /* [ b2 x ] [ F x ] + [ M x ] */
1710 
1711  /* Moment, Node2, Lines 16->18: */
1712  WM.Sub(15 + 1, 3 + 1, Mat3x3(MatCrossCross, b2, FTmp) + Mat3x3(MatCross, MTmp)); // * Delta_g1
1713 
1714  /* d/dt(Moment), Node2, Lines 22->24: */
1715  WM.Sub(21 + 1, 9 + 1, Mat3x3(MatCrossCross, b2, FTmp) + Mat3x3(MatCross, MTmp)); // * Delta_W1
1716 
1717  /* Constraints: Add only active rows/columns*/
1718 
1719  /* Positions contribution:*/
1720 
1721  Mat3x3 b1Cross_R1(b1.Cross(R1)); // = [ b1 x ] * R1
1722  Mat3x3 b2Cross_R1(b2.Cross(R1)); // = [ b2 x ] * R1
1723 
1724  for (unsigned iCnt = 0 ; iCnt < nPosConstraints; iCnt++) {
1725  Vec3 vR1(R1.GetVec(iPosIncid[iCnt]));
1726  Vec3 vb1Cross_R1(b1Cross_R1.GetVec(iPosIncid[iCnt]));
1727  Vec3 vb2Cross_R1(b2Cross_R1.GetVec(iPosIncid[iCnt]));
1728 
1729  /* Equilibrium, node 1 */
1730  WM.Sub(1, 24 + 1 + iCnt, vR1); // * Delta_F
1731  WM.Sub(3 + 1, 24 + 1 + iCnt, vb1Cross_R1); // * Delta_F
1732 
1733  /* Constraint, node 1 */
1734  WM.SubT(24 + 1 + iCnt, 1, vR1); // * Delta_x1
1735  WM.SubT(24 + 1 + iCnt, 3 + 1, vb1Cross_R1); // * Delta_g1
1736 
1737  /* d/dt(Equilibrium), node 1 */
1738  WM.Sub(6 + 1, 24 + 1 + nConstraints + iCnt, vR1); // * Delta_FP
1739  WM.Sub(9 + 1, 24 + 1 + nConstraints + iCnt, vb1Cross_R1); // * Delta_FP
1740 
1741  /* d/dt(Constraint), node 1 */
1742  WM.SubT(24 + 1 + nConstraints + iCnt, 6 + 1, vR1); // * Delta_xP1
1743  WM.SubT(24 + 1 + nConstraints + iCnt, 9 + 1, vb1Cross_R1); // * Delta_W1
1744 
1745  /* Equilibrium, node 2 */
1746  WM.Add(12 + 1, 24 + 1 + iCnt, vR1); // * Delta_F
1747  WM.Add(15 + 1, 24 + 1 + iCnt, vb2Cross_R1); // * Delta_F
1748 
1749  /* Constraint, node 2 */
1750  WM.AddT(24 + 1 + iCnt, 12 + 1, vR1); // * Delta_x2
1751  WM.AddT(24 + 1 + iCnt, 15 + 1, vb2Cross_R1); // * Delta_g2
1752 
1753  /* d/dt(Equilibrium), node 2 */
1754  WM.Add(18 + 1, 24 + 1 + nConstraints + iCnt, vR1); // * Delta_FP
1755  WM.Add(21 + 1, 24 + 1 + nConstraints + iCnt, vb2Cross_R1); // * Delta_FP
1756 
1757  /* d/dt(Constraint), node 2 */
1758  WM.AddT(24 + 1 + nConstraints + iCnt, 18 + 1, vR1); // * Delta_xP2
1759  WM.AddT(24 + 1 + nConstraints + iCnt, 21 + 1, vb2Cross_R1); // * Delta_W2
1760  }
1761 
1762  for (unsigned iCnt = 0 ; iCnt < nRotConstraints; iCnt++) {
1763  Vec3 vR1(R1r.GetVec(iRotIncid[iCnt]));
1764 
1765  /* Equilibrium, node 1 */
1766  WM.Sub(3 + 1, 24 + 1 + nPosConstraints + iCnt, vR1); // * Delta_M
1767 
1768  /* Constraint, node 1 */
1769  WM.SubT(24 + 1 + nPosConstraints + iCnt, 3 + 1, vR1); // * Delta_g1
1770 
1771  /* d/dt(Equilibrium), node 1 */
1772  WM.Sub(9 + 1, 24 + 1 + nConstraints + nPosConstraints + iCnt, vR1); // * Delta_MP
1773 
1774  /* d/dt(Constraint), node 1 */
1775  WM.SubT(24 + 1 + nConstraints + nPosConstraints + iCnt, 9 + 1, vR1); // * Delta_W1
1776 
1777  /* Equilibrium, node 2 */
1778  WM.Add(15 + 1, 24 + 1 + nPosConstraints + iCnt, vR1); // * Delta_M
1779 
1780  /* Constraint, node 2 */
1781  WM.AddT(24 + 1 + nPosConstraints + iCnt, 15 + 1, vR1); // * Delta_g2
1782 
1783  /* d/dt(Equilibrium), node 2 */
1784  WM.Add(21 + 1, 24 + 1 + nConstraints + nPosConstraints + iCnt, vR1); // * Delta_MP
1785 
1786  /* d/dt(Constraint), node 2 */
1787  WM.AddT(24 + 1 + nConstraints + nPosConstraints + iCnt, 21 + 1, vR1); // * Delta_W2
1788  }
1789 
1790  return WorkMat;
1791 }
1792 
1793 
1794 /* Contributo al residuo durante l'assemblaggio iniziale */
1795 SubVectorHandler&
1796 TotalJoint::InitialAssRes(SubVectorHandler& WorkVec,
1797  const VectorHandler& XCurr)
1798 {
1799  if (iGetInitialNumDof() == 0) {
1800  WorkVec.ResizeReset(0);
1801  return WorkVec;
1802  }
1803 
1804  DEBUGCOUT("Entering TotalJoint::InitialAssRes()" << std::endl);
1805 
1806  /* Dimensiona e resetta la matrice di lavoro */
1807  integer iNumRows = 0;
1808  integer iNumCols = 0;
1809  InitialWorkSpaceDim(&iNumRows, &iNumCols);
1810  WorkVec.ResizeReset(iNumRows);
1811 
1812  /* Indici */
1813  integer iNode1FirstPosIndex = pNode1->iGetFirstPositionIndex();
1814  integer iNode1FirstVelIndex = iNode1FirstPosIndex + 6;
1815  integer iNode2FirstPosIndex = pNode2->iGetFirstPositionIndex();
1816  integer iNode2FirstVelIndex = iNode2FirstPosIndex + 6;
1817  integer iFirstReactionIndex = iGetFirstIndex();
1818  integer iReactionPrimeIndex = iFirstReactionIndex + nConstraints;
1819 
1820  /* Setta gli indici */
1821  for (int iCnt = 1; iCnt <= 6; iCnt++) {
1822  WorkVec.PutRowIndex(iCnt, iNode1FirstPosIndex+iCnt);
1823  WorkVec.PutRowIndex(6 + iCnt, iNode1FirstVelIndex + iCnt);
1824  WorkVec.PutRowIndex(12 + iCnt, iNode2FirstPosIndex + iCnt);
1825  WorkVec.PutRowIndex(18 + iCnt, iNode2FirstVelIndex + iCnt);
1826  }
1827 
1828  for (unsigned int iCnt = 1; iCnt <= nConstraints; iCnt++) {
1829  WorkVec.PutRowIndex(24 + iCnt, iFirstReactionIndex + iCnt);
1830  WorkVec.PutRowIndex(24 + nConstraints + iCnt, iReactionPrimeIndex + iCnt);
1831  }
1832 
1833  /* Recupera i dati */
1834  /* Recupera i dati che servono */
1835  Mat3x3 R1(pNode1->GetRRef()*R1h);
1836  Mat3x3 R1r(pNode1->GetRRef()*R1hr);
1837  Mat3x3 R2r(pNode2->GetRCurr()*R2hr);
1838 
1839  Vec3 b2(pNode2->GetRCurr()*f2);
1840  Vec3 b1(pNode2->GetXCurr() + b2 - pNode1->GetXCurr());
1841 
1842  const Vec3& Omega1(pNode1->GetWCurr());
1843  const Vec3& Omega2(pNode2->GetWCurr());
1844 
1845  Vec3 b1Prime(pNode2->GetVCurr() + Omega2.Cross(b2) - pNode1->GetVCurr());
1846 
1847  Vec3 FPrime(::Zero3);
1848  Vec3 MPrime(::Zero3);
1849 
1850  /* Aggiorna F ed M, che restano anche per InitialAssJac */
1851  for (unsigned iCnt = 0; iCnt < nPosConstraints; iCnt++) {
1852  F(iPosIncid[iCnt]) = XCurr(iFirstReactionIndex + 1 + iCnt);
1853  FPrime(iPosIncid[iCnt]) = XCurr(iReactionPrimeIndex + 1 + iCnt);
1854  }
1855 
1856  for (unsigned iCnt = 0; iCnt < nRotConstraints; iCnt++) {
1857  M(iRotIncid[iCnt]) = XCurr(iFirstReactionIndex + 1 + nPosConstraints + iCnt);
1858  MPrime(iRotIncid[iCnt]) = XCurr(iReactionPrimeIndex + 1 + nPosConstraints + iCnt);
1859  }
1860 
1861  Vec3 FTmp(R1 * F);
1862  Vec3 MTmp(R1r * M);
1863  Vec3 FPrimeTmp(R1 * FPrime);
1864  Vec3 MPrimeTmp(R1r * MPrime);
1865 
1866  /* Equilibrium, node 1 */
1867  WorkVec.Add(1, FTmp);
1868  WorkVec.Add(3 + 1, b1.Cross(FTmp) + MTmp);
1869 
1870  /* d/dt(Equilibrium), node 1 */
1871  WorkVec.Add(6 + 1, FPrimeTmp);
1872  WorkVec.Add(9 + 1, b1.Cross(FPrimeTmp) + MPrimeTmp);
1873 
1874  /* Equilibrium, node 2 */
1875  WorkVec.Sub(12 + 1, FTmp);
1876  WorkVec.Sub(15 + 1, b2.Cross(FTmp) + MTmp);
1877 
1878  /* d/dt( Equilibrium ) , node 2 */
1879  WorkVec.Sub(18 + 1, FPrimeTmp);
1880  WorkVec.Sub(21 + 1, b2.Cross(FPrimeTmp) + MPrimeTmp);
1881 
1882  /* Constraint Equations */
1883 
1884  Vec3 XDelta = R1.MulTV(b1) - tilde_f1 - XDrv.Get();
1885  Vec3 XDeltaPrime = R1.MulTV(b1Prime + b1.Cross(Omega1));
1886 
1887  if(XDrv.bIsDifferentiable()) {
1888  XDeltaPrime -= XDrv.GetP();
1889  }
1890 
1891  Mat3x3 R0 = RotManip::Rot(ThetaDrv.Get());
1892  Mat3x3 RDelta = R1r.MulTM(R2r.MulMT(R0));
1893  ThetaDelta = RotManip::VecRot(RDelta);
1894  Vec3 ThetaDeltaPrime = R1r.MulTV(Omega2 - Omega1);
1895 
1896  if(ThetaDrv.bIsDifferentiable()) {
1897  ThetaDeltaPrime -= RDelta*ThetaDrv.GetP();
1898  }
1899 
1900 
1901  /* Position constraint: */
1902  for (unsigned iCnt = 0; iCnt < nPosConstraints; iCnt++) {
1903  WorkVec.PutCoef(24 + 1 + iCnt, -XDelta(iPosIncid[iCnt]));
1904  WorkVec.PutCoef(24 + 1 + nConstraints + iCnt, -XDeltaPrime(iPosIncid[iCnt]));
1905  }
1906 
1907  /* Rotation constraints: */
1908  for (unsigned iCnt = 0; iCnt < nRotConstraints; iCnt++) {
1909  WorkVec.PutCoef(24 + 1 + nPosConstraints + iCnt, -ThetaDelta(iRotIncid[iCnt]));
1910  WorkVec.PutCoef(24 + 1 + nPosConstraints + nConstraints + iCnt, -ThetaDeltaPrime(iRotIncid[iCnt]));
1911  }
1912 
1913  return WorkVec;
1914 }
1915 
1916 
1917 unsigned int
1918 TotalJoint::iGetNumPrivData(void) const
1919 {
1920  return 24;
1921 }
1922 
1923 unsigned int
1924 TotalJoint::iGetPrivDataIdx(const char *s) const
1925 {
1926  ASSERT(s != NULL);
1927 
1928  if (strlen(s) != 2) {
1929  return 0;
1930  }
1931 
1932  unsigned int off = 0;
1933 
1934  switch (s[0]) {
1935  case 'p':
1936  /* relative position */
1937  break;
1938 
1939  case 'r':
1940  /* relative orientation */
1941  off += 3;
1942  break;
1943 
1944  case 'F':
1945  /* force */
1946  off += 6;
1947  break;
1948 
1949  case 'M':
1950  /* moment */
1951  off += 9;
1952  break;
1953 
1954  case 'd':
1955  /* imposed relative position */
1956  off += 12;
1957  break;
1958 
1959  case 't':
1960  /* imposed relative orientation */
1961  off += 15;
1962  break;
1963 
1964  case 'v':
1965  /* relative linear velocity */
1966  off += 18;
1967  break;
1968 
1969  case 'w':
1970  /* relative angular velocity */
1971  off += 21;
1972  break;
1973 
1974  default:
1975  return 0;
1976  }
1977 
1978  switch (s[1]) {
1979  case 'x':
1980  return off + 1;
1981 
1982  case 'y':
1983  return off + 2;
1984 
1985  case 'z':
1986  return off + 3;
1987  }
1988 
1989  return 0;
1990 }
1991 
1992 doublereal
1993 TotalJoint::dGetPrivData(unsigned int i) const
1994 {
1995  switch (i) {
1996  case 1:
1997  case 2:
1998  case 3: {
1999  Vec3 x(pNode1->GetRCurr().MulTV(
2000  pNode2->GetXCurr() + pNode2->GetRCurr()*f2
2001  - pNode1->GetXCurr()) - f1);
2002  return R1h.GetVec(i)*x;
2003  }
2004 
2005  case 4:
2006  case 5:
2007  case 6: {
2008  Vec3 Theta(Unwrap(ThetaDeltaTrue, RotManip::VecRot((pNode1->GetRCurr()*R1hr).MulTM(pNode2->GetRCurr()*R2hr))));
2009  return Theta(i - 3);
2010  }
2011 
2012  case 7:
2013  case 8:
2014  case 9:
2015  return F(i - 6);
2016 
2017  case 10:
2018  case 11:
2019  case 12:
2020  return M(i - 9);
2021 
2022  case 13:
2023  case 14:
2024  case 15:
2025  if (!bPosActive[i - 13]) {
2026  return 0.;
2027  }
2028  return XDrv.Get()(i - 12);
2029 
2030  case 16:
2031  case 17:
2032  case 18:
2033  if (!bRotActive[i - 16]) {
2034  return 0.;
2035  }
2036  return ThetaDrv.Get()(i - 15);
2037 
2038  case 19:
2039  case 20:
2040  case 21:
2041  {
2042  Vec3 v( pNode1->GetRCurr().MulTV(
2043  (pNode2->GetVCurr() + pNode2->GetWCurr().Cross(pNode2->GetRCurr()*f2)
2044  - pNode1->GetVCurr()) -
2045  pNode1->GetWCurr().Cross( pNode2->GetXCurr() +
2046  pNode2->GetRCurr()*f2
2047  - pNode1->GetXCurr() - f1)
2048  )
2049  );
2050 
2051  return R1h.GetVec(i-18)*v;
2052  }
2053 
2054  case 22:
2055  case 23:
2056  case 24:
2057  {
2058  Vec3 W(pNode1->GetRCurr().MulTV(pNode2->GetWCurr() - pNode1->GetWCurr()))
2059  ;
2060 
2061  return R1hr.GetVec(i-21)*W;
2062  }
2063 
2064  default:
2065  ASSERT(0);
2066  }
2067 
2068  return 0.;
2069 }
2070 
2071 /* TotalJoint - end */
2072 
2073 /* TotalPinJoint - begin */
2074 
2075 TotalPinJoint::TotalPinJoint(unsigned int uL, const DofOwner *pDO,
2076  bool bPos[3], bool bVel[3],
2077  TplDriveCaller<Vec3> *const pDCPos[3],
2078  bool bRot[3], bool bAgv[3],
2079  TplDriveCaller<Vec3> *const pDCRot[3],
2080  const Vec3& XcTmp, const Mat3x3& RchTmp, const Mat3x3& RchrTmp,
2081  const StructNode *pN,
2082  const Vec3& fnTmp, const Mat3x3& RnhTmp, const Mat3x3& RnhrTmp,
2083  flag fOut)
2084 : Elem(uL, fOut),
2085 Joint(uL, pDO, fOut),
2086 pNode(pN),
2087 Xc(XcTmp), Rch(RchTmp), Rchr(RchrTmp),
2088 tilde_fn(fnTmp), tilde_Rnh(RnhTmp), tilde_Rnhr(RnhrTmp),
2089 RchT(Rch.Transpose()), tilde_Xc(RchT*Xc), RchrT(Rchr.Transpose()),
2090 XDrv(pDCPos[0]), XPDrv(pDCPos[1]), XPPDrv(pDCPos[2]),
2091 ThetaDrv(pDCRot[0]), OmegaDrv(pDCRot[1]), OmegaPDrv(pDCRot[2]),
2092 nConstraints(0), nPosConstraints(0), nRotConstraints(0),
2093 nVelConstraints(0), nAgvConstraints(0),
2094 #ifdef USE_NETCDF
2095 Var_X(0),
2096 Var_Phi(0),
2097 Var_V(0),
2098 Var_Omega(0),
2099 #endif // USE_NETCDF
2100 M(::Zero3), F(::Zero3),
2101 ThetaDelta(::Zero3),
2102 ThetaDeltaPrev(::Zero3),
2103 ThetaDeltaRemnant(::Zero3),
2104 ThetaDeltaTrue(::Zero3)
2105 {
2106  /* Equations 1->3: Positions
2107  * Equations 4->6: Rotations */
2108 
2109  unsigned int index = 0;
2110 
2111  for (unsigned int i = 0; i < 3; i++) {
2112  bPosActive[i] = bPos[i];
2113  bVelActive[i] = bVel[i];
2114 
2115  if (bPosActive[i]) {
2116  iPosIncid[nPosConstraints] = i + 1;
2117  iPosEqIndex[nPosConstraints] = index;
2118  nPosConstraints++;
2119  index++;
2120  }
2121 
2122  if (bVelActive[i]) {
2123  iVelIncid[nVelConstraints] = i + 1;
2124  iVelEqIndex[nVelConstraints] = index;
2125  nVelConstraints++;
2126  index++;
2127  }
2128  }
2129 
2130  index = 0;
2131  for (unsigned int i = 0; i < 3; i++) {
2132  bRotActive[i] = bRot[i];
2133  bAgvActive[i] = bAgv[i];
2134 
2135  if (bRotActive[i]) {
2136  iRotIncid[nRotConstraints] = i + 1;
2137  iRotEqIndex[nRotConstraints] = nPosConstraints + nVelConstraints + index;
2138  nRotConstraints++;
2139  index++;
2140  }
2141 
2142  if (bAgvActive[i]) {
2143  iAgvIncid[nAgvConstraints] = i + 1;
2144  iAgvEqIndex[nAgvConstraints] = nPosConstraints + nVelConstraints + index;
2145  nAgvConstraints++;
2146  index++;
2147  }
2148  }
2149 
2150  // if only a fraction of the rotation degrees of freedom
2151  // are constrained, store the incidence of the unconstrained ones
2152  // in the remainder of array iRotIncid[] for later use
2153  if (nRotConstraints > 0 && nRotConstraints < 3) {
2154  switch (nRotConstraints) {
2155  case 1:
2156  switch (iRotIncid[0]) {
2157  case 1:
2158  iRotIncid[1] = 2;
2159  iRotIncid[2] = 3;
2160  break;
2161 
2162  case 2:
2163  iRotIncid[1] = 1;
2164  iRotIncid[2] = 3;
2165  break;
2166 
2167  case 3:
2168  iRotIncid[1] = 1;
2169  iRotIncid[2] = 2;
2170  break;
2171 
2172  default:
2173  ASSERT(0);
2174  throw ErrGeneric(MBDYN_EXCEPT_ARGS);
2175  }
2176  break;
2177 
2178  case 2:
2179  switch (iRotIncid[0]) {
2180  case 1:
2181  iRotIncid[2] = 5 - iRotIncid[1];
2182  break;
2183 
2184  case 2:
2185  iRotIncid[2] = 1;
2186  break;
2187  }
2188  break;
2189 
2190  default:
2191  ASSERT(0);
2192  throw ErrGeneric(MBDYN_EXCEPT_ARGS);
2193  }
2194  }
2195 
2196  nConstraints = nPosConstraints + nVelConstraints
2197  + nRotConstraints + nAgvConstraints;
2198 
2199  ThetaDeltaTrue = RotManip::VecRot(RchT*pNode->GetRCurr()*tilde_Rnhr);
2200 }
2201 
2202 TotalPinJoint::~TotalPinJoint(void)
2203 {
2204  NO_OP;
2205 }
2206 
2207 std::ostream&
2208 TotalPinJoint::DescribeDof(std::ostream& out,
2209  const char *prefix, bool bInitial) const
2210 {
2211  integer iIndex = iGetFirstIndex();
2212 
2213  if (nPosConstraints > 0 || nVelConstraints > 0) {
2214  out << prefix << iIndex + 1;
2215  if (nPosConstraints + nVelConstraints > 1) {
2216  out << "->" << iIndex + nPosConstraints + nVelConstraints;
2217  }
2218  out << ": ";
2219  }
2220  out << "reaction force(s) [";
2221 
2222  for (unsigned int i = 0, cnt = 0; i < 3; i++) {
2223  if (bPosActive[i] || bVelActive[i]) {
2224  cnt++;
2225  if (cnt > 1) {
2226  out << ",";
2227  }
2228  out << "F" << idx2xyz[i];
2229  }
2230  }
2231  out << "]" << std::endl;
2232 
2233  if (nRotConstraints > 0 || nAgvConstraints > 0) {
2234  out << prefix << iIndex + nPosConstraints + nVelConstraints + 1;
2235  if (nRotConstraints + nAgvConstraints > 1) {
2236  out << "->" << iIndex + nConstraints;
2237  }
2238  out << ": ";
2239  }
2240  out << "reaction couple(s) [";
2241 
2242  for (unsigned int i = 0, cnt = 0; i < 3; i++) {
2243  if (bRotActive[i] || bAgvActive[i]) {
2244  cnt++;
2245  if (cnt > 1) {
2246  out << ",";
2247  }
2248  out << "m" << idx2xyz[i];
2249  }
2250  }
2251  out << "]" << std::endl;
2252 
2253  if (bInitial) {
2254  iIndex += nConstraints;
2255 
2256  if (nPosConstraints > 0) {
2257  out << prefix << iIndex + 1;
2258  if (nPosConstraints > 1) {
2259  out << "->" << iIndex + nPosConstraints;
2260  }
2261  out << ": ";
2262  }
2263  out << "reaction force(s) derivative(s) [";
2264 
2265  for (unsigned int i = 0, cnt = 0; i < 3; i++) {
2266  if (bPosActive[i]) {
2267  cnt++;
2268  if (cnt > 1) {
2269  out << ",";
2270  }
2271  out << "FP" << idx2xyz[i];
2272  }
2273  }
2274  out << "]" << std::endl;
2275 
2276 
2277  if (nRotConstraints > 0) {
2278  out << prefix << iIndex + nPosConstraints + 1;
2279  if (nRotConstraints > 1) {
2280  out << "->" << iIndex + nConstraints;
2281  }
2282  out << ": ";
2283  }
2284  out << "reaction couple(s) derivative(s) [";
2285 
2286  for (unsigned int i = 0, cnt = 0; i < 3; i++) {
2287  if (bRotActive[i]) {
2288  cnt++;
2289  if (cnt > 1) {
2290  out << ",";
2291  }
2292  out << "mP" << idx2xyz[i];
2293  }
2294  }
2295  out << "]" << std::endl;
2296  }
2297 
2298  return out;
2299 }
2300 
2301 void
2302 TotalPinJoint::DescribeDof(std::vector<std::string>& desc,
2303  bool bInitial, int i) const
2304 {
2305  int ndof = 1;
2306  if (i == -1) {
2307  if (bInitial) {
2308  ndof = iGetInitialNumDof();
2309 
2310  } else {
2311  ndof = iGetNumDof();
2312  }
2313  }
2314  desc.resize(ndof);
2315 
2316  std::ostringstream os;
2317  os << "TotalPinJoint(" << GetLabel() << ")";
2318 
2319  if (i == -1) {
2320  std::string name(os.str());
2321  unsigned int cnt = 0;
2322 
2323  if (nPosConstraints > 0 || nVelConstraints > 0) {
2324  for (unsigned int i = 0; i < 3; i++) {
2325  if (bPosActive[i] || bVelActive[i]) {
2326  os.str(name);
2327  os.seekp(0, std::ios_base::end);
2328  os << ": dof(" << cnt + 1 << ") F" << idx2xyz[i];
2329  desc[cnt] = os.str();
2330  cnt++;
2331  }
2332  }
2333  }
2334 
2335  if (nRotConstraints > 0 || nAgvConstraints > 0) {
2336  for (unsigned int i = 0; i < 3; i++) {
2337  if (bRotActive[i] || bAgvActive[i]) {
2338  os.str(name);
2339  os.seekp(0, std::ios_base::end);
2340  os << ": dof(" << cnt + 1 << ") M" << idx2xyz[i];
2341  desc[cnt] = os.str();
2342  cnt++;
2343  }
2344  }
2345  }
2346 
2347  if (bInitial) {
2348  if (nPosConstraints > 0) {
2349  for (unsigned int i = 0; i < 3; i++) {
2350  if (bPosActive[i]) {
2351  os.str(name);
2352  os.seekp(0, std::ios_base::end);
2353  os << ": dof(" << cnt + 1 << ") FP" << idx2xyz[i];
2354  desc[cnt] = os.str();
2355  cnt++;
2356  }
2357  }
2358  }
2359 
2360  if (nRotConstraints > 0) {
2361  for (unsigned int i = 0; i < 3; i++) {
2362  if (bRotActive[i]) {
2363  os.str(name);
2364  os.seekp(0, std::ios_base::end);
2365  os << ": dof(" << cnt + 1 << ") MP" << idx2xyz[i];
2366  desc[cnt] = os.str();
2367  cnt++;
2368  }
2369  }
2370  }
2371  }
2372 
2373  ASSERT(cnt == ndof);
2374 
2375  } else {
2376  os << ": dof(" << i + 1 << ")";
2377  desc[0] = os.str();
2378  }
2379 }
2380 
2381 std::ostream&
2382 TotalPinJoint::DescribeEq(std::ostream& out,
2383  const char *prefix, bool bInitial) const
2384 {
2385  integer iIndex = iGetFirstIndex();
2386 
2387  if (nPosConstraints > 0 || nVelConstraints > 0) {
2388  out << prefix << iIndex + 1;
2389  if (nPosConstraints + nVelConstraints > 1) {
2390  out << "->" << iIndex + nPosConstraints + nVelConstraints;
2391  }
2392  out << ": "
2393  "position/velocity constraint(s) [";
2394 
2395  for (unsigned int i = 0, cnt = 0; i < 3; i++) {
2396  if (bPosActive[i] || bVelActive[i]) {
2397  cnt++;
2398  if (cnt > 1) {
2399  out << ",";
2400  }
2401 
2402  if (bPosActive[i]) {
2403  out << "P" << idx2xyz[i] << "=P" << idx2xyz[i] << "0";
2404  } else {
2405  out << "V" << idx2xyz[i] << "=V" << idx2xyz[i] << "0";
2406  }
2407  }
2408  }
2409 
2410  out << "]" << std::endl;
2411  }
2412 
2413  if (nRotConstraints > 0 || nAgvConstraints > 0) {
2414  out << prefix << iIndex + nPosConstraints + nVelConstraints + 1;
2415  if (nRotConstraints + nAgvConstraints > 1) {
2416  out << "->" << iIndex + nConstraints ;
2417  }
2418  out << ": "
2419  "orientation/angular velocity constraint(s) [";
2420 
2421  for (unsigned int i = 0, cnt = 0; i < 3; i++) {
2422  if (bRotActive[i] || bAgvActive[i]) {
2423  cnt++;
2424  if (cnt > 1) {
2425  out << ",";
2426  }
2427 
2428  if (bRotActive[i]) {
2429  out << "theta" << idx2xyz[i] << "=theta" << idx2xyz[i] << "0";
2430  } else {
2431  out << "W" << idx2xyz[i] << "=W" << idx2xyz[i] << "0";
2432  }
2433  }
2434  }
2435 
2436  out << "]" << std::endl;
2437  }
2438 
2439  if (bInitial) {
2440  iIndex += nConstraints;
2441 
2442  if (nPosConstraints > 0) {
2443  out << prefix << iIndex + 1;
2444  out << "->" << iIndex + nPosConstraints;
2445  out << ": "
2446  "velocity constraint(s) [";
2447 
2448  for (unsigned int i = 0, cnt = 0; i < 3; i++) {
2449  if (bPosActive[i]) {
2450  cnt++;
2451  if (cnt > 1) {
2452  out << ",";
2453  }
2454  out << "v" << idx2xyz[i] << "=v" << idx2xyz[i] << "0";
2455  }
2456  }
2457 
2458  out << "]" << std::endl;
2459  }
2460 
2461  if (nRotConstraints > 0) {
2462  out << prefix << iIndex + nPosConstraints + 1;
2463  if (nRotConstraints > 1) {
2464  out << "->" << iIndex + nConstraints ;
2465  }
2466  out << ": "
2467  "angular velocity constraint(s) [";
2468 
2469  for (unsigned int i = 0, cnt = 0; i < 3; i++) {
2470  if (bRotActive[i]) {
2471  cnt++;
2472  if (cnt > 1) {
2473  out << ",";
2474  }
2475  out << "w" << idx2xyz[i] << "=w" << idx2xyz[i] << "0";
2476  }
2477  }
2478 
2479  out << "]" << std::endl;
2480  }
2481  }
2482 
2483  return out;
2484 }
2485 
2486 void
2487 TotalPinJoint::DescribeEq(std::vector<std::string>& desc,
2488  bool bInitial, int i) const
2489 {
2490  int ndof = 1;
2491  if (i == -1) {
2492  if (bInitial) {
2493  ndof = iGetInitialNumDof();
2494 
2495  } else {
2496  ndof = iGetNumDof();
2497  }
2498  }
2499  desc.resize(ndof);
2500 
2501  std::ostringstream os;
2502  os << "TotalPinJoint(" << GetLabel() << ")";
2503 
2504  if (i == -1) {
2505  std::string name(os.str());
2506  unsigned int cnt = 0;
2507 
2508  if (nPosConstraints > 0 || nVelConstraints > 0) {
2509  for (unsigned int i = 0; i < 3; i++) {
2510  if (bPosActive[i]) {
2511  os.str(name);
2512  os.seekp(0, std::ios_base::end);
2513  os << ": equation(" << cnt + 1 << ") P" << idx2xyz[i] << "1=P" << idx2xyz[i] << "2";
2514  desc[cnt] = os.str();
2515  cnt++;
2516 
2517  } else if (bVelActive[i]) {
2518  os.str(name);
2519  os.seekp(0, std::ios_base::end);
2520  os << ": equation(" << cnt + 1 << ") V" << idx2xyz[i] << "1=V" << idx2xyz[i] << "2";
2521  desc[cnt] = os.str();
2522  cnt++;
2523  }
2524  }
2525  }
2526 
2527  if (nRotConstraints > 0 || nAgvConstraints > 0) {
2528  for (unsigned int i = 0; i < 3; i++) {
2529  if (bRotActive[i]) {
2530  os.str(name);
2531  os.seekp(0, std::ios_base::end);
2532  os << ": equation(" << cnt + 1 << ") theta" << idx2xyz[i] << "1=theta" << idx2xyz[i] << "2";
2533  desc[cnt] = os.str();
2534  cnt++;
2535 
2536  } else if (bAgvActive[i]) {
2537  os.str(name);
2538  os.seekp(0, std::ios_base::end);
2539  os << ": equation(" << cnt + 1 << ") W" << idx2xyz[i] << "1=W" << idx2xyz[i] << "2";
2540  desc[cnt] = os.str();
2541  cnt++;
2542  }
2543  }
2544  }
2545 
2546  if (bInitial) {
2547  if (nPosConstraints > 0) {
2548  for (unsigned int i = 0; i < 3; i++) {
2549  if (bPosActive[i]) {
2550  os.str(name);
2551  os.seekp(0, std::ios_base::end);
2552  os << ": equation(" << cnt + 1 << ") v" << idx2xyz[i] << "1=v" << idx2xyz[i] << "2";
2553  desc[cnt] = os.str();
2554  cnt++;
2555  }
2556  }
2557  }
2558 
2559  if (nRotConstraints > 0) {
2560  for (unsigned int i = 0; i < 3; i++) {
2561  if (bRotActive[i]) {
2562  os.str(name);
2563  os.seekp(0, std::ios_base::end);
2564  os << ": equation(" << cnt + 1 << ") w" << idx2xyz[i] << "1=w" << idx2xyz[i] << "2";
2565  desc[cnt] = os.str();
2566  cnt++;
2567  }
2568  }
2569  }
2570  }
2571 
2572  ASSERT(cnt == ndof);
2573 
2574  } else {
2575  os << ": equation(" << i + 1 << ")";
2576  desc[0] = os.str();
2577  }
2578 }
2579 
2580 void
2581 TotalPinJoint::SetValue(DataManager *pDM,
2582  VectorHandler& X, VectorHandler& XP,
2583  SimulationEntity::Hints *ph)
2584 {
2585  if (ph) {
2586  for (unsigned int i = 0; i < ph->size(); i++) {
2587  Joint::JointHint *pjh = dynamic_cast<Joint::JointHint *>((*ph)[i]);
2588 
2589  if (pjh) {
2590  if (dynamic_cast<Joint::OffsetHint<1> *>(pjh)) {
2591  Mat3x3 RnT(pNode->GetRCurr().Transpose());
2592 
2593  tilde_fn = RnT*(Xc + Rch*XDrv.Get() - pNode->GetXCurr());
2594 
2595  } else if (dynamic_cast<Joint::OffsetHint<0> *>(pjh)) {
2596  Xc = pNode->GetXCurr() + pNode->GetRCurr()*tilde_fn
2597  - Rch*XDrv.Get();
2598  tilde_Xc = RchT*Xc;
2599 
2600  } else if (dynamic_cast<Joint::HingeHint<1> *>(pjh)) {
2601  if (dynamic_cast<Joint::PositionHingeHint<1> *>(pjh)) {
2602  tilde_Rnh = pNode->GetRCurr().MulTM(Rch);
2603  /* NOTE: pointless */
2604 
2605  } else if (dynamic_cast<Joint::OrientationHingeHint<1> *>(pjh)) {
2606  tilde_Rnhr = pNode->GetRCurr().MulTM(Rchr*RotManip::Rot(ThetaDrv.Get()));
2607  }
2608 
2609  } else if (dynamic_cast<Joint::HingeHint<0> *>(pjh)) {
2610  if (dynamic_cast<Joint::PositionHingeHint<0> *>(pjh)) {
2611  Rch = pNode->GetRCurr()*tilde_Rnh;
2612  RchT = Rch.Transpose();
2613  tilde_Xc = RchT*Xc;
2614  /* NOTE: results in constraint violation if XDrv is not 0 */
2615 
2616  } else if (dynamic_cast<Joint::OrientationHingeHint<0> *>(pjh)) {
2617  Rchr = pNode->GetRCurr()*tilde_Rnhr*RotManip::Rot(-ThetaDrv.Get());
2618  RchrT = Rchr.Transpose();
2619  }
2620 
2621  } else if (dynamic_cast<Joint::JointDriveHint<Vec3> *>(pjh)) {
2622  Joint::JointDriveHint<Vec3> *pjdh
2623  = dynamic_cast<Joint::JointDriveHint<Vec3> *>(pjh);
2624  pedantic_cout("TotalPinJoint(" << uLabel << "): "
2625  "creating drive from hint[" << i << "]..." << std::endl);
2626 
2627  TplDriveCaller<Vec3> *pDC = pjdh->pTDH->pCreateDrive(pDM);
2628  if (pDC == 0) {
2629  silent_cerr("TotalPinJoint(" << uLabel << "): "
2630  "unable to create drive "
2631  "after hint #" << i << std::endl);
2632  throw ErrGeneric(MBDYN_EXCEPT_ARGS);
2633  }
2634 
2635  if (dynamic_cast<Joint::PositionDriveHint<Vec3> *>(pjdh)) {
2636  XDrv.Set(pDC);
2637 
2638  } else if (dynamic_cast<Joint::VelocityDriveHint<Vec3> *>(pjdh)) {
2639  XPDrv.Set(pDC);
2640 
2641  } else if (dynamic_cast<Joint::AccelerationDriveHint<Vec3> *>(pjdh)) {
2642  XPPDrv.Set(pDC);
2643 
2644  } else if (dynamic_cast<Joint::OrientationDriveHint<Vec3> *>(pjdh)) {
2645  ThetaDrv.Set(pDC);
2646 
2647  } else if (dynamic_cast<Joint::AngularVelocityDriveHint<Vec3> *>(pjdh)) {
2648  OmegaDrv.Set(pDC);
2649 
2650  } else if (dynamic_cast<Joint::AngularAccelerationDriveHint<Vec3> *>(pjdh)) {
2651  OmegaPDrv.Set(pDC);
2652 
2653  } else {
2654  delete pDC;
2655  }
2656 
2657  } else if (dynamic_cast<Joint::ReactionsHint *>(pjh)) {
2658  /* TODO */
2659  }
2660  continue;
2661  }
2662  }
2663  }
2664 }
2665 
2666 Hint *
2667 TotalPinJoint::ParseHint(DataManager *pDM, const char *s) const
2668 {
2669  if (strncasecmp(s, "offset{" /*}*/ , STRLENOF("offset{" /*}*/ )) == 0)
2670  {
2671  s += STRLENOF("offset{" /*}*/ );
2672 
2673  if (strcmp(&s[1], /*{*/ "}") != 0) {
2674  return 0;
2675  }
2676 
2677  switch (s[0]) {
2678  case '1':
2679  return new Joint::OffsetHint<1>;
2680 
2681  case '0':
2682  return new Joint::OffsetHint<0>;
2683  }
2684 
2685  } else if (strncasecmp(s, "position-hinge{" /*}*/, STRLENOF("position-hinge{" /*}*/)) == 0) {
2686  s += STRLENOF("position-hinge{" /*}*/);
2687 
2688  if (strcmp(&s[1], /*{*/ "}") != 0) {
2689  return 0;
2690  }
2691 
2692  switch (s[0]) {
2693  case '1':
2694  return new Joint::HingeHint<1>;
2695 
2696  case '0':
2697  return new Joint::HingeHint<0>;
2698  }
2699 
2700  } else if (strncasecmp(s, "position-drive3{" /*}*/, STRLENOF("position-drive3{" /*}*/)) == 0) {
2701  s += STRLENOF("position-");
2702 
2703  Hint *pH = ::ParseHint(pDM, s);
2704  if (pH) {
2705  TplDriveHint<Vec3> *pTDH = dynamic_cast<TplDriveHint<Vec3> *>(pH);
2706  if (pTDH) {
2707  return new PositionDriveHint<Vec3>(pTDH);
2708  }
2709  }
2710  return 0;
2711 
2712  } else if (strncasecmp(s, "orientation-hinge{" /*}*/, STRLENOF("orientation-hinge{" /*}*/)) == 0) {
2713  s += STRLENOF("orientation-hinge{" /*}*/);
2714 
2715  if (strcmp(&s[1], /*{*/ "}") != 0) {
2716  return 0;
2717  }
2718 
2719  switch (s[0]) {
2720  case '1':
2721  return new Joint::HingeHint<1>;
2722 
2723  case '0':
2724  return new Joint::HingeHint<0>;
2725  }
2726 
2727  } else if (strncasecmp(s, "orientation-drive3{" /*}*/, STRLENOF("orientation-drive3{" /*}*/)) == 0) {
2728  s += STRLENOF("orientation-");
2729 
2730  Hint *pH = ::ParseHint(pDM, s);
2731  if (pH) {
2732  TplDriveHint<Vec3> *pTDH = dynamic_cast<TplDriveHint<Vec3> *>(pH);
2733  if (pTDH) {
2734  return new OrientationDriveHint<Vec3>(pTDH);
2735  }
2736  }
2737  return 0;
2738  }
2739 
2740  return 0;
2741 }
2742 
2743 void
2744 TotalPinJoint::AfterConvergence(const VectorHandler& X,
2745  const VectorHandler& XP)
2746 {
2747  // "trick": correct ThetaDrv to keep ThetaDelta small
2748  // See "A Vectorial Formulation of Generic Pair Constraints"
2749  if (nRotConstraints < 3) {
2750  Vec3 ThetaDeltaTmp(ThetaDelta);
2751  for (unsigned i = 0; i < nRotConstraints; i++) {
2752  // zero out constrained components
2753  // NOTE: should already be zero within tolerance
2754  ThetaDeltaTmp(iRotIncid[i]) = 0.;
2755  }
2756  ThetaDeltaRemnant += ThetaDeltaTmp;
2757  }
2758 
2759  ThetaDeltaPrev = Unwrap(ThetaDeltaPrev, ThetaDeltaRemnant);
2760  ThetaDeltaTrue = Unwrap(ThetaDeltaTrue, RotManip::VecRot(RchT*pNode->GetRCurr()*tilde_Rnhr));
2761 }
2762 
2763 /* Inverse Dynamics: */
2764 void
2765 TotalPinJoint::AfterConvergence(const VectorHandler& X,
2766  const VectorHandler& XP, const VectorHandler& XPP)
2767 {
2768  AfterConvergence(X, XP);
2769 }
2770 
2771 
2772 /* Contributo al file di restart */
2773 std::ostream&
2774 TotalPinJoint::Restart(std::ostream& out) const
2775 {
2776  Joint::Restart(out) << ", total pin joint, "
2777  << pNode->GetLabel() << ", "
2778  << "position, ";
2779  tilde_fn.Write(out, ", ") << ", "
2780  << "position orientation, "
2781  "1 , ";
2782  tilde_Rnh.GetVec(1).Write(out, ", ") << ", "
2783  "2 , ";
2784  tilde_Rnh.GetVec(2).Write(out, ", ") << ", "
2785  << "rotation orientation, "
2786  "1 , ";
2787  tilde_Rnhr.GetVec(1).Write(out, ", ") << ", "
2788  "2 , ";
2789  tilde_Rnhr.GetVec(2).Write(out, ", ") << ", "
2790  << "position, ";
2791  Xc.Write(out, ", ") << ", "
2792  << "position orientation, "
2793  "1 , ";
2794  Rch.GetVec(1).Write(out, ", ") << ", "
2795  "2 , ";
2796  Rch.GetVec(2).Write(out, ", ") << ", "
2797  << "rotation orientation, "
2798  "1 , ";
2799  Rchr.GetVec(1).Write(out, ", ") << ", "
2800  "2 , ";
2801  Rchr.GetVec(2).Write(out, ", ");
2802 
2803  if (bPosActive[0] || bPosActive[1] || bPosActive[2]
2804  || bVelActive[0] || bVelActive[1] || bVelActive[2])
2805  {
2806  out << ", position constraint";
2807  for (unsigned i = 0; i < 3; i++) {
2808  if (bPosActive[i]) {
2809  out << ", active";
2810  } else if (bVelActive[i]) {
2811  out << ", velocity";
2812  } else {
2813  out << ", inactive";
2814  }
2815  }
2816 
2817  out << ", ", XDrv.pGetDriveCaller()->Restart(out);
2818  if (XPDrv.pGetDriveCaller()) {
2819  out << ", ", XPDrv.pGetDriveCaller()->Restart(out)
2820  << ", ", XPPDrv.pGetDriveCaller()->Restart(out);
2821  }
2822  }
2823 
2824  if (bRotActive[0] || bRotActive[1] || bRotActive[2]
2825  || bAgvActive[0] || bAgvActive[1] || bAgvActive[2])
2826  {
2827  out << ", orientation constraint";
2828  for (unsigned i = 0; i < 3; i++) {
2829  if (bRotActive[i]) {
2830  out << ", active";
2831  } else if (bAgvActive[i]) {
2832  out << ", angular velocity";
2833  } else {
2834  out << ", inactive";
2835  }
2836  }
2837 
2838  out << ", ", ThetaDrv.pGetDriveCaller()->Restart(out);
2839  if (OmegaDrv.pGetDriveCaller()) {
2840  out << ", ", OmegaDrv.pGetDriveCaller()->Restart(out)
2841  << ", ", OmegaPDrv.pGetDriveCaller()->Restart(out);
2842  }
2843  }
2844 
2845  return out << ";" << std::endl;
2846 }
2847 
2848 /* Assemblaggio jacobiano */
2849 VariableSubMatrixHandler&
2850 TotalPinJoint::AssJac(VariableSubMatrixHandler& WorkMat,
2851  doublereal dCoef,
2852  const VectorHandler& /* XCurr */ ,
2853  const VectorHandler& /* XPrimeCurr */ )
2854 {
2855  /*
2856  * See tecman.pdf
2857  */
2858 
2859  DEBUGCOUT("Entering TotalPinJoint::AssJac()" << std::endl);
2860 
2861  if (iGetNumDof() == 0) {
2862  WorkMat.SetNullMatrix();
2863  return WorkMat;
2864  }
2865 
2866  FullSubMatrixHandler& WM = WorkMat.SetFull();
2867 
2868  /* Ridimensiona la sottomatrice in base alle esigenze */
2869  integer iNumRows = 0;
2870  integer iNumCols = 0;
2871  WorkSpaceDim(&iNumRows, &iNumCols);
2872  WM.ResizeReset(iNumRows, iNumCols);
2873 
2874  /* Recupera gli indici delle varie incognite */
2875  integer iNodeFirstPosIndex = pNode->iGetFirstPositionIndex();
2876  integer iNodeFirstMomIndex = pNode->iGetFirstMomentumIndex();
2877  integer iFirstReactionIndex = iGetFirstIndex();
2878 
2879  /* Setta gli indici delle equazioni */
2880  for (int iCnt = 1; iCnt <= 6; iCnt++) {
2881  WM.PutRowIndex(iCnt, iNodeFirstMomIndex + iCnt);
2882  WM.PutColIndex(iCnt, iNodeFirstPosIndex + iCnt);
2883  }
2884 
2885  for (unsigned int iCnt = 1; iCnt <= nConstraints; iCnt++) {
2886  WM.PutRowIndex(6 + iCnt, iFirstReactionIndex + iCnt);
2887  WM.PutColIndex(6 + iCnt, iFirstReactionIndex + iCnt);
2888  }
2889 
2890  /* Recupera i dati che servono */
2891  Vec3 fn(pNode->GetRCurr()*tilde_fn);
2892 
2893  /* Moltiplica la forza per il coefficiente del metodo */
2894  Vec3 FTmp(Rch*(F*dCoef));
2895 
2896  /* Equilibrium: ((Phi/q)^T*Lambda)/q */
2897 
2898  /* Lines 4->6: */
2899  /* [ F x ] [ b2 x ] */
2900  WM.Add(3 + 1, 3 + 1, Mat3x3(MatCrossCross, FTmp, fn));
2901 
2902 /* Phi/q and (Phi/q)^T */
2903 
2904  Mat3x3 fnCross_Rch(fn.Cross(Rch)); // = [ b2 x ] * R1
2905 
2906  for (unsigned iCnt = 0 ; iCnt < nPosConstraints; iCnt++) {
2907  Vec3 vRch(Rch.GetVec(iPosIncid[iCnt]));
2908  Vec3 vfnCross_Rch(fnCross_Rch.GetVec(iPosIncid[iCnt]));
2909 
2910  /* Equilibrium, node */
2911  WM.Add(1, 6 + 1 + iCnt, vRch);
2912  WM.Add(3 + 1, 6 + 1 + iCnt, vfnCross_Rch);
2913 
2914  /* Constraint, node */
2915  WM.AddT(6 + 1 + iCnt, 1, vRch);
2916  WM.AddT(6 + 1 + iCnt, 3 + 1, vfnCross_Rch);
2917  }
2918 
2919  for (unsigned iCnt = 0 ; iCnt < nRotConstraints; iCnt++) {
2920  Vec3 vRchr(Rchr.GetVec(iRotIncid[iCnt]));
2921 
2922  /* Equilibrium, node */
2923  WM.Add(3 + 1, 6 + 1 + nPosConstraints + iCnt, vRchr);
2924 
2925  /* Constraint, node */
2926  WM.AddT(6 + 1 + nPosConstraints + iCnt, 3 + 1, vRchr);
2927  }
2928 
2929  // TODO: velocity & angular velocity
2930 
2931  return WorkMat;
2932 }
2933 
2934 /* Assemblaggio residuo */
2935 SubVectorHandler&
2936 TotalPinJoint::AssRes(SubVectorHandler& WorkVec,
2937  doublereal dCoef,
2938  const VectorHandler& XCurr,
2939  const VectorHandler& /* XPrimeCurr */ )
2940 {
2941  DEBUGCOUT("Entering TotalPinJoint::AssRes()" << std::endl);
2942 
2943  if (iGetNumDof() == 0) {
2944  WorkVec.Resize(0);
2945  return WorkVec;
2946  }
2947 
2948  /* Dimensiona e resetta la matrice di lavoro */
2949  integer iNumRows = 0;
2950  integer iNumCols = 0;
2951  WorkSpaceDim(&iNumRows, &iNumCols);
2952  WorkVec.ResizeReset(iNumRows);
2953 
2954  /* Indici */
2955  integer iNodeFirstMomIndex = pNode->iGetFirstMomentumIndex();
2956  integer iFirstReactionIndex = iGetFirstIndex();
2957 
2958  /* Indici dei nodi */
2959  for (int iCnt = 1; iCnt <= 6; iCnt++) {
2960  WorkVec.PutRowIndex(iCnt, iNodeFirstMomIndex + iCnt);
2961  }
2962 
2963  /* Indici del vincolo */
2964  for (unsigned int iCnt = 1; iCnt <= nConstraints; iCnt++) {
2965  WorkVec.PutRowIndex(6 + iCnt, iFirstReactionIndex + iCnt);
2966  }
2967 
2968  /* Get constraint reactions */
2969 
2970  for (unsigned iCnt = 0; iCnt < nPosConstraints; iCnt++) {
2971  F(iPosIncid[iCnt]) = XCurr(iFirstReactionIndex + 1 + iPosEqIndex[iCnt]);
2972  }
2973 
2974  for (unsigned iCnt = 0; iCnt < nRotConstraints; iCnt++) {
2975  M(iRotIncid[iCnt]) = XCurr(iFirstReactionIndex + 1 + iRotEqIndex[iCnt]);
2976  }
2977 
2978  for (unsigned iCnt = 0; iCnt < nVelConstraints; iCnt++) {
2979  F(iVelIncid[iCnt]) = XCurr(iFirstReactionIndex + 1 + iVelEqIndex[iCnt]);
2980  }
2981 
2982  for (unsigned iCnt = 0; iCnt < nAgvConstraints; iCnt++) {
2983  M(iAgvIncid[iCnt]) = XCurr(iFirstReactionIndex + 1 + iAgvEqIndex[iCnt]);
2984  }
2985 
2986  Vec3 fn(pNode->GetRCurr()*tilde_fn);
2987 
2988  Vec3 XDelta;
2989  if (nPosConstraints > 0) {
2990  XDelta = RchT*(pNode->GetXCurr() + fn) - tilde_Xc - XDrv.Get();
2991  }
2992 
2993  Vec3 VDelta;
2994  if (nVelConstraints > 0) {
2995  VDelta = RchT*(pNode->GetVCurr() - fn.Cross(pNode->GetWCurr())) - XDrv.Get();
2996  }
2997 
2998  if (nRotConstraints > 0) {
2999  Mat3x3 Rnhr = pNode->GetRCurr()*tilde_Rnhr;
3000 
3001  Vec3 ThetaDrvTmp(ThetaDrv.Get());
3002  if (nRotConstraints < 3) {
3003  for (int i = nRotConstraints; i < 3; i++) {
3004  // zero out unconstrained components of drive
3005  ThetaDrvTmp(iRotIncid[i]) = 0.;
3006  }
3007  // add remnant to make ThetaDelta as close to zero
3008  // as possible
3009  ThetaDrvTmp += ThetaDeltaRemnant;
3010  }
3011  Mat3x3 R0(RotManip::Rot(ThetaDrvTmp));
3012  Mat3x3 RDelta = RchrT*Rnhr.MulMT(R0);
3013  ThetaDelta = RotManip::VecRot(RDelta);
3014  }
3015 
3016  Vec3 WDelta;
3017  if (nAgvConstraints > 0) {
3018  WDelta = RchT*(pNode->GetWCurr()) - ThetaDrv.Get();
3019  }
3020 
3021  Vec3 FTmp(Rch*F);
3022  Vec3 MTmp(Rchr*M);
3023 
3024  /* Equilibrium, node 2 */
3025  WorkVec.Sub(1, FTmp);
3026  WorkVec.Sub(3 + 1, MTmp + fn.Cross(FTmp));
3027 
3028  /* Holonomic constraint equations are divided by dCoef */
3029  ASSERT(dCoef != 0.);
3030 
3031  /* Position constraint: */
3032  for (unsigned iCnt = 0; iCnt < nPosConstraints; iCnt++) {
3033  WorkVec.PutCoef(6 + 1 + iPosEqIndex[iCnt],
3034  -XDelta(iPosIncid[iCnt])/dCoef);
3035  }
3036 
3037  /* Rotation constraints: */
3038  for (unsigned iCnt = 0; iCnt < nRotConstraints; iCnt++) {
3039  WorkVec.PutCoef(6 + 1 + iRotEqIndex[iCnt],
3040  -ThetaDelta(iRotIncid[iCnt])/dCoef);
3041  }
3042 
3043  /* Linear Velocity Constraint */
3044  for (unsigned iCnt = 0; iCnt < nVelConstraints; iCnt++) {
3045  WorkVec.PutCoef(6 + 1 + iVelEqIndex[iCnt],
3046  -VDelta(iVelIncid[iCnt]));
3047  }
3048 
3049  /* Angular Velocity Constraint */
3050  for (unsigned iCnt = 0; iCnt < nAgvConstraints; iCnt++) {
3051  WorkVec.PutCoef(6 + 1 + iAgvEqIndex[iCnt],
3052  -WDelta(iAgvIncid[iCnt]));
3053  }
3054 
3055  return WorkVec;
3056 }
3057 
3058 /* inverse dynamics capable element */
3059 bool
3060 TotalPinJoint::bInverseDynamics(void) const
3061 {
3062  return true;
3063 }
3064 
3065 /* Inverse Dynamics: Jacobian Assembly */
3066 
3067 VariableSubMatrixHandler&
3068 TotalPinJoint::AssJac(VariableSubMatrixHandler& WorkMat,
3069  const VectorHandler& XCurr)
3070 
3071 {
3072  /*
3073  * See tecman.pdf
3074  */
3075 
3076  DEBUGCOUT("Entering TotalPinJoint::AssJac()" << std::endl);
3077 
3078  if (iGetNumDof() == 0) {
3079  WorkMat.SetNullMatrix();
3080  return WorkMat;
3081  }
3082 
3083  FullSubMatrixHandler& WM = WorkMat.SetFull();
3084 
3085  /* Ridimensiona la sottomatrice in base alle esigenze */
3086  integer iNumRows = 0;
3087  integer iNumCols = 0;
3088  WorkSpaceDim(&iNumRows, &iNumCols);
3089  WM.ResizeReset(iNumRows - 6, 6);
3090 
3091  /* Recupera gli indici delle varie incognite */
3092  integer iNodeFirstPosIndex = pNode->iGetFirstPositionIndex();
3093  integer iFirstReactionIndex = iGetFirstIndex();
3094 
3095  /* Setta gli indici delle equazioni */
3096  for (int iCnt = 1; iCnt <= 6; iCnt++) {
3097  WM.PutColIndex(iCnt, iNodeFirstPosIndex + iCnt);
3098  }
3099 
3100  for (unsigned int iCnt = 1; iCnt <= nConstraints; iCnt++) {
3101  WM.PutRowIndex(iCnt, iFirstReactionIndex + iCnt);
3102  }
3103 
3104  /* Recupera i dati che servono */
3105  Vec3 fn(pNode->GetRCurr()*tilde_fn);
3106 
3107  Mat3x3 fnCross_Rch(fn.Cross(Rch)); // = [ b2 x ] * R1
3108 
3109  for (unsigned iCnt = 0 ; iCnt < nPosConstraints; iCnt++) {
3110  Vec3 vRch(Rch.GetVec(iPosIncid[iCnt]));
3111  Vec3 vfnCross_Rch(fnCross_Rch.GetVec(iPosIncid[iCnt]));
3112 
3113  /* Constraint, node */
3114  WM.AddT(1 + iCnt, 1, vRch);
3115  WM.AddT(3 + 1 + iCnt, 3 + 1, vfnCross_Rch);
3116  }
3117 
3118  for (unsigned iCnt = 0 ; iCnt < nRotConstraints; iCnt++) {
3119  Vec3 vRchr(Rchr.GetVec(iRotIncid[iCnt]));
3120 
3121  /* Constraint, node */
3122  WM.AddT(1 + nPosConstraints + iCnt, 3 + 1, vRchr);
3123  }
3124 
3125  return WorkMat;
3126 }
3127 
3128 /* Inverse Dynamics: Assemblaggio residuo */
3129 SubVectorHandler&
3130 TotalPinJoint::AssRes(SubVectorHandler& WorkVec,
3131  const VectorHandler& XCurr,
3132  const VectorHandler& XPrimeCurr,
3133  const VectorHandler& /* XPrimePrimeCurr*/,
3134  InverseDynamics::Order iOrder)
3135 {
3136  DEBUGCOUT("Entering TotalPinJoint::AssRes()" << std::endl);
3137 
3138  if (iGetNumDof() == 0 || iOrder == InverseDynamics::INVERSE_DYNAMICS) {
3139  WorkVec.ResizeReset(0);
3140  return WorkVec;
3141  }
3142 
3143  /* Dimensiona e resetta la matrice di lavoro */
3144  integer iNumRows = 0;
3145  integer iNumCols = 0;
3146  WorkSpaceDim(&iNumRows, &iNumCols);
3147 
3148  /* original - node equations */
3149  WorkVec.ResizeReset(iNumRows - 6);
3150 
3151  /* Indici */
3152  integer iFirstReactionIndex = iGetFirstIndex();
3153 
3154  /* Indici del vincolo */
3155  for (unsigned int iCnt = 1; iCnt <= nConstraints; iCnt++) {
3156  WorkVec.PutRowIndex(iCnt, iFirstReactionIndex + iCnt);
3157  }
3158 
3159  switch (iOrder) {
3160  case InverseDynamics::POSITION: {
3161 
3162  /* Position constraint: */
3163  if (nPosConstraints > 0) {
3164  Vec3 fn(pNode->GetRCurr()*tilde_fn);
3165  Vec3 XDelta = RchT*(pNode->GetXCurr() + fn) - tilde_Xc - XDrv.Get();
3166 
3167  for (unsigned iCnt = 0; iCnt < nPosConstraints; iCnt++) {
3168  WorkVec.PutCoef(1 + iPosEqIndex[iCnt], -XDelta(iPosIncid[iCnt]));
3169  }
3170  }
3171 
3172  /* Rotation constraints: */
3173  if (nRotConstraints > 0) {
3174  Mat3x3 Rnhr = pNode->GetRCurr()*tilde_Rnhr;
3175  Vec3 ThetaDrvTmp(ThetaDrv.Get());
3176  if (nRotConstraints < 3) {
3177  for (int i = nRotConstraints; i < 3; i++) {
3178  // zero out unconstrained components of drive
3179  ThetaDrvTmp(iRotIncid[i]) = 0.;
3180  }
3181  // add remnant to make ThetaDelta as close to zero
3182  // as possible
3183  ThetaDrvTmp += ThetaDeltaRemnant;
3184  }
3185  Mat3x3 R0(RotManip::Rot(ThetaDrvTmp));
3186  Mat3x3 RDelta = RchrT*Rnhr.MulMT(R0);
3187  ThetaDelta = RotManip::VecRot(RDelta);
3188 
3189  for (unsigned iCnt = 0; iCnt < nRotConstraints; iCnt++) {
3190  WorkVec.PutCoef(1 + iRotEqIndex[iCnt], -(ThetaDelta(iRotIncid[iCnt])));
3191  }
3192  }
3193  } break;
3194 
3195  case InverseDynamics::VELOCITY: {
3196  /* Position constraint derivative */
3197  if (nPosConstraints > 0) {
3198  // First derivative of regular AssRes' lower block
3199  Vec3 Tmp = XPDrv.Get();
3200 
3201  for (unsigned iCnt = 0; iCnt < nPosConstraints; iCnt++) {
3202  WorkVec.PutCoef(1 + iPosEqIndex[iCnt], Tmp(iPosIncid[iCnt]));
3203  }
3204  }
3205 
3206  /* Rotation constraint derivative */
3207  if (nRotConstraints > 0) {
3208  Mat3x3 Rnhr = pNode->GetRCurr()*tilde_Rnhr;
3209  Mat3x3 R0 = RotManip::Rot(ThetaDrv.Get());
3210  Mat3x3 RDelta = RchrT*Rnhr.MulMT(R0);
3211 
3212  /* This name is only for clarity... */
3213  Vec3 Tmp = RDelta * OmegaDrv.Get();
3214 
3215  for (unsigned iCnt = 0; iCnt < nRotConstraints; iCnt++) {
3216  WorkVec.PutCoef(1 + iRotEqIndex[iCnt], Tmp(iRotIncid[iCnt]));
3217  }
3218  }
3219  } break;
3220 
3221  case InverseDynamics::ACCELERATION: {
3222  /* Position constraint second derivative */
3223  if (nPosConstraints > 0) {
3224  // Second derivative of regular AssRes' lower block
3225  Vec3 Tmp = XPPDrv.Get();
3226 
3227  Vec3 fn(pNode->GetRCurr()*tilde_fn);
3228 
3229  Tmp -= pNode->GetWCurr().Cross(pNode->GetWCurr().Cross(fn));
3230 
3231  for (unsigned iCnt = 0; iCnt < nPosConstraints; iCnt++) {
3232  WorkVec.PutCoef(1 + iPosEqIndex[iCnt], Tmp(iPosIncid[iCnt]));
3233  }
3234  }
3235 
3236  /* Rotation constraint second derivative */
3237  if (nRotConstraints > 0) {
3238  Mat3x3 Rnhr = pNode->GetRCurr()*tilde_Rnhr;
3239  Mat3x3 R0 = RotManip::Rot(ThetaDrv.Get());
3240  Mat3x3 RDelta = RchrT*Rnhr.MulMT(R0);
3241 
3242  Vec3 Tmp = R0.MulTV(OmegaDrv.Get());
3243  Vec3 Tmp2 = Rnhr * Tmp;
3244 
3245  Tmp = pNode->GetWCurr().Cross(Tmp2);
3246 
3247  Tmp2 = RchrT*Tmp;
3248  Tmp2 += RDelta * OmegaPDrv.Get();
3249 
3250  for (unsigned iCnt = 0; iCnt < nRotConstraints; iCnt++) {
3251  WorkVec.PutCoef(1 + iRotEqIndex[iCnt], Tmp2(iRotIncid[iCnt]));
3252  }
3253  }
3254  } break;
3255 
3256  default:
3257  ASSERT(0);
3258  throw ErrGeneric(MBDYN_EXCEPT_ARGS);
3259  }
3260 
3261  return WorkVec;
3262 }
3263 
3264 
3265 /* Inverse Dynamics update */
3266 void
3267 TotalPinJoint::Update(const VectorHandler& XCurr, InverseDynamics::Order iOrder)
3268 {
3269  ASSERT(iOrder == InverseDynamics::INVERSE_DYNAMICS);
3270 
3271  integer iFirstReactionIndex = iGetFirstIndex();
3272 
3273  /* Get constraint reactions */
3274  for (unsigned iCnt = 0; iCnt < nPosConstraints; iCnt++) {
3275  F(iPosIncid[iCnt]) = XCurr(iFirstReactionIndex + 1 + iPosEqIndex[iCnt]);
3276  }
3277 
3278  for (unsigned iCnt = 0; iCnt < nRotConstraints; iCnt++) {
3279  M(iRotIncid[iCnt]) = XCurr(iFirstReactionIndex + 1 + iRotEqIndex[iCnt]);
3280  }
3281 }
3282 
3283 DofOrder::Order
3284 TotalPinJoint::GetEqType(unsigned int i) const
3285 {
3286  ASSERTMSGBREAK(i < iGetNumDof(),
3287  "INDEX ERROR in TotalPinJoint::GetEqType");
3288 
3289  return DofOrder::ALGEBRAIC;
3290 }
3291 
3292 void
3293 TotalPinJoint::OutputPrepare(OutputHandler& OH)
3294 {
3295  if (bToBeOutput()) {
3296 #ifdef USE_NETCDF
3297  if (OH.UseNetCDF(OutputHandler::JOINTS)) {
3298  std::string name;
3299  OutputPrepare_int("totalpin", OH, name);
3300 
3301  Var_X = OH.CreateVar<Vec3>(name + "X", "m",
3302  "local relative position (x, y, z)");
3303 
3304  // NOTE: by now, force ORIENTATION_VECTOR
3305  Var_Phi = OH.CreateRotationVar(name, "", ORIENTATION_VECTOR, "local relative");
3306 
3307  Var_V = OH.CreateVar<Vec3>(name + "V", "m/s",
3308  "local relative velocity (x, y, z)");
3309 
3310  Var_Omega = OH.CreateVar<Vec3>(name + "Omega", "radian/s",
3311  "local relative angular velocity (x, y, z)");
3312  }
3313 #endif // USE_NETCDF
3314  }
3315 }
3316 
3317 /* Output (da mettere a punto) */
3318 void
3319 TotalPinJoint::Output(OutputHandler& OH) const
3320 {
3321  if (bToBeOutput()) {
3322  const Vec3& Xn(pNode->GetXCurr());
3323  const Mat3x3& Rn(pNode->GetRCurr());
3324  const Vec3& Vn(pNode->GetVCurr());
3325  const Vec3& Omegan(pNode->GetWCurr());
3326 
3327  Vec3 fn(Rn*tilde_fn);
3328  Mat3x3 RnhrTmp(Rn*tilde_Rnhr);
3329  Mat3x3 RTmp(RchrT*RnhrTmp);
3330  Vec3 ThetaTmp(RotManip::VecRot(RTmp));
3331  Vec3 XTmp(RchT*(Xn + fn) - tilde_Xc);
3332  Vec3 VTmp(RchT*(Vn + Omegan.Cross(fn)));
3333  Vec3 OmegaTmp(RchrT*Omegan);
3334 
3335  Vec3 FTmp(Rch*F);
3336  Vec3 MTmp(Rchr*M);
3337 
3338 #ifdef USE_NETCDF
3339  if (OH.UseNetCDF(OutputHandler::JOINTS)) {
3340  Var_F_local->put_rec(F.pGetVec(), OH.GetCurrentStep());
3341  Var_M_local->put_rec(M.pGetVec(), OH.GetCurrentStep());
3342  Var_F_global->put_rec(FTmp.pGetVec(), OH.GetCurrentStep());
3343  Var_M_global->put_rec(MTmp.pGetVec(), OH.GetCurrentStep());
3344 
3345  Var_X->put_rec(XTmp.pGetVec(), OH.GetCurrentStep());
3346  Var_Phi->put_rec(ThetaTmp.pGetVec(), OH.GetCurrentStep());
3347  Var_V->put_rec(VTmp.pGetVec(), OH.GetCurrentStep());
3348  Var_Omega->put_rec(OmegaTmp.pGetVec(), OH.GetCurrentStep());
3349  }
3350 #endif // USE_NETCDF
3351 
3352  if (OH.UseText(OutputHandler::JOINTS)) {
3353  Joint::Output(OH.Joints(), "TotalPinJoint", GetLabel(),
3354  F, M, FTmp, MTmp)
3355  << " " << XTmp
3356  << " " << ThetaTmp
3357  << " " << VTmp
3358  << " " << OmegaTmp
3359  << std::endl;
3360  // accelerations?
3361  }
3362  }
3363 }
3364 
3365 /* Contributo allo jacobiano durante l'assemblaggio iniziale */
3366 VariableSubMatrixHandler&
3367 TotalPinJoint::InitialAssJac(VariableSubMatrixHandler& WorkMat,
3368  const VectorHandler& XCurr)
3369 {
3370  if (iGetInitialNumDof() == 0) {
3371  WorkMat.SetNullMatrix();
3372  return WorkMat;
3373  }
3374 
3375  /* Per ora usa la matrice piena; eventualmente si puo'
3376  * passare a quella sparsa quando si ottimizza */
3377  FullSubMatrixHandler& WM = WorkMat.SetFull();
3378 
3379  /* Dimensiona e resetta la matrice di lavoro */
3380  integer iNumRows = 0;
3381  integer iNumCols = 0;
3382  InitialWorkSpaceDim(&iNumRows, &iNumCols);
3383  WM.ResizeReset(iNumRows, iNumCols);
3384 
3385  /* Equazioni: vedi joints.dvi */
3386 
3387  /* equazioni ed incognite
3388  * F1 Delta_x1 1
3389  * M1 Delta_g1 3 + 1
3390  * FP1 Delta_xP1 6 + 1
3391  * MP1 Delta_w1 9 + 1
3392  * F2 Delta_x2 12 + 1
3393  * M2 Delta_g2 15 + 1
3394  * FP2 Delta_xP2 18 + 1
3395  * MP2 Delta_w2 21 + 1
3396  * vincolo spostamento Delta_F 24 + 1
3397  * vincolo rotazione Delta_M 24 + nPosConstraints
3398  * derivata vincolo spostamento Delta_FP 24 + nConstraints
3399  * derivata vincolo rotazione Delta_MP 24 + nConstraints + nPosConstraints
3400  */
3401 
3402 
3403  /* Indici */
3404  integer iNodeFirstPosIndex = pNode->iGetFirstPositionIndex();
3405  integer iNodeFirstVelIndex = iNodeFirstPosIndex + 6;
3406  integer iFirstReactionIndex = iGetFirstIndex();
3407  integer iReactionPrimeIndex = iFirstReactionIndex + nConstraints;
3408 
3409 
3410  /* Setta gli indici dei nodi */
3411  for (int iCnt = 1; iCnt <= 6; iCnt++) {
3412  WM.PutRowIndex(iCnt, iNodeFirstPosIndex + iCnt);
3413  WM.PutColIndex(iCnt, iNodeFirstPosIndex + iCnt);
3414  WM.PutRowIndex(6 + iCnt, iNodeFirstVelIndex + iCnt);
3415  WM.PutColIndex(6 + iCnt, iNodeFirstVelIndex + iCnt);
3416  }
3417 
3418  /* Setta gli indici delle reazioni */
3419  for (unsigned int iCnt = 1; iCnt <= nConstraints; iCnt++) {
3420  WM.PutRowIndex(12 + iCnt, iFirstReactionIndex + iCnt);
3421  WM.PutColIndex(12 + iCnt, iFirstReactionIndex + iCnt);
3422  WM.PutRowIndex(12 + nConstraints + iCnt, iReactionPrimeIndex + iCnt);
3423  WM.PutColIndex(12 + nConstraints + iCnt, iReactionPrimeIndex + iCnt);
3424  }
3425 
3426  /* Recupera i dati che servono */
3427  Vec3 fn(pNode->GetRCurr()*tilde_fn);
3428 
3429  Vec3 Omega(pNode->GetWCurr());
3430 
3431  Vec3 bPrime(pNode->GetVCurr() + Omega.Cross(fn));
3432 
3433  /* F ed M sono gia' state aggiornate da InitialAssRes;
3434  * Recupero FPrime e MPrime*/
3435  Vec3 MPrime(::Zero3);
3436  Vec3 FPrime(::Zero3);
3437 
3438  for (unsigned iCnt = 0; iCnt < nPosConstraints; iCnt++) {
3439  FPrime(iPosIncid[iCnt]) = XCurr(iReactionPrimeIndex + 1 + iCnt);
3440  }
3441 
3442  for (unsigned iCnt = 0; iCnt < nRotConstraints; iCnt++) {
3443  MPrime(iRotIncid[iCnt]) = XCurr(iReactionPrimeIndex + 1 + nPosConstraints + iCnt);
3444  }
3445 
3446  Vec3 FTmp(Rch*F);
3447  Vec3 FPrimeTmp(Rch*FPrime);
3448 
3449  WM.Add(3 + 1, 3 + 1, Mat3x3(MatCrossCross, FTmp, fn));
3450  WM.Add(9 + 1, 3 + 1, Mat3x3(MatCrossCross, FPrimeTmp, fn));
3451 
3452  /* Constraints: Add only active rows/columns*/
3453 
3454  /* Positions contribution:*/
3455 
3456  Mat3x3 fnCross_Rch(fn.Cross(Rch)); // = [ fn x ] * Rc
3457 
3458  for (unsigned iCnt = 0 ; iCnt < nPosConstraints; iCnt++) {
3459  Vec3 vRch(Rch.GetVec(iPosIncid[iCnt]));
3460  Vec3 vfnCross_Rch(fnCross_Rch.GetVec(iPosIncid[iCnt]));
3461 
3462  /* Equilibrium, node */
3463  WM.Add(1, 12 + 1 + iCnt, vRch); // * Delta_F
3464  WM.Add(3 + 1, 12 + 1 + iCnt, vfnCross_Rch); // * Delta_F
3465 
3466  /* Constraint, node */
3467  WM.AddT(12 + 1 + iCnt, 1, vRch); // * Delta_x2
3468  WM.AddT(12 + 1 + iCnt, 3 + 1, vfnCross_Rch); // * Delta_g2
3469 
3470  /* d/dt(Equilibrium), node */
3471  WM.Add(6 + 1, 12 + 1 + nConstraints + iCnt, vRch); // * Delta_FP
3472  WM.Add(9 + 1, 12 + 1 + nConstraints + iCnt, vfnCross_Rch); // * Delta_FP
3473 
3474  /* d/dt(Constraint), node */
3475  WM.AddT(12 + 1 + nConstraints + iCnt, 6 + 1, vRch); // * Delta_xP2
3476  WM.AddT(12 + 1 + nConstraints + iCnt, 9 + 1, vfnCross_Rch); // * Delta_W2
3477  }
3478 
3479  for (unsigned iCnt = 0 ; iCnt < nRotConstraints; iCnt++) {
3480  Vec3 vRchr(Rchr.GetVec(iRotIncid[iCnt]));
3481 
3482  /* Equilibrium, node */
3483  WM.Add(3 + 1, 12 + 1 + nPosConstraints + iCnt, vRchr); // * Delta_M
3484 
3485  /* Constraint, node */
3486  WM.AddT(12 + 1 + nPosConstraints + iCnt, 3 + 1, vRchr); // * Delta_g2
3487 
3488  /* d/dt(Equilibrium), node */
3489  WM.Add(9 + 1, 12 + 1 + nConstraints + nPosConstraints + iCnt, vRchr); // * Delta_MP
3490 
3491  /* d/dt(Constraint), node */
3492  WM.AddT(12 + 1 + nConstraints + nPosConstraints + iCnt, 9 + 1, vRchr); // * Delta_W2
3493  }
3494 
3495  return WorkMat;
3496 }
3497 
3498 
3499 /* Contributo al residuo durante l'assemblaggio iniziale */
3500 SubVectorHandler&
3501 TotalPinJoint::InitialAssRes(SubVectorHandler& WorkVec,
3502  const VectorHandler& XCurr)
3503 {
3504  if (iGetInitialNumDof() == 0) {
3505  WorkVec.ResizeReset(0);
3506  return WorkVec;
3507  }
3508 
3509  DEBUGCOUT("Entering TotalPinJoint::InitialAssRes()" << std::endl);
3510 
3511  /* Dimensiona e resetta la matrice di lavoro */
3512  integer iNumRows = 0;
3513  integer iNumCols = 0;
3514  InitialWorkSpaceDim(&iNumRows, &iNumCols);
3515  WorkVec.ResizeReset(iNumRows);
3516 
3517  /* Indici */
3518  integer iNodeFirstPosIndex = pNode->iGetFirstPositionIndex();
3519  integer iNodeFirstVelIndex = iNodeFirstPosIndex + 6;
3520  integer iFirstReactionIndex = iGetFirstIndex();
3521  integer iReactionPrimeIndex = iFirstReactionIndex + nConstraints;
3522 
3523  /* Setta gli indici */
3524  for (int iCnt = 1; iCnt <= 6; iCnt++) {
3525  WorkVec.PutRowIndex(iCnt, iNodeFirstPosIndex + iCnt);
3526  WorkVec.PutRowIndex(6 + iCnt, iNodeFirstVelIndex + iCnt);
3527  }
3528 
3529  for (unsigned int iCnt = 1; iCnt <= nConstraints; iCnt++) {
3530  WorkVec.PutRowIndex(12 + iCnt, iFirstReactionIndex + iCnt);
3531  WorkVec.PutRowIndex(12 + nConstraints + iCnt, iReactionPrimeIndex + iCnt);
3532  }
3533 
3534  /* Recupera i dati */
3535  /* Recupera i dati che servono */
3536  Vec3 fn(pNode->GetRCurr()*tilde_fn);
3537  Mat3x3 Rnhr = pNode->GetRCurr()*tilde_Rnhr;
3538  Vec3 Omega(pNode->GetWCurr());
3539 
3540  Vec3 FPrime(::Zero3);
3541  Vec3 MPrime(::Zero3);
3542 
3543  /* Aggiorna F ed M, che restano anche per InitialAssJac */
3544  for (unsigned iCnt = 0; iCnt < nPosConstraints; iCnt++) {
3545  F(iPosIncid[iCnt]) = XCurr(iFirstReactionIndex + 1 + iCnt);
3546  FPrime(iPosIncid[iCnt]) = XCurr(iReactionPrimeIndex + 1 + iCnt);
3547  }
3548 
3549  for (unsigned iCnt = 0; iCnt < nRotConstraints; iCnt++) {
3550  M(iRotIncid[iCnt]) = XCurr(iFirstReactionIndex + 1 + nPosConstraints + iCnt);
3551  MPrime(iRotIncid[iCnt]) = XCurr(iReactionPrimeIndex + 1 + nPosConstraints + iCnt);
3552  }
3553 
3554  Vec3 FTmp(Rch*F);
3555  Vec3 MTmp(Rchr*M);
3556  Vec3 FPrimeTmp(Rch*FPrime);
3557  Vec3 MPrimeTmp(Rchr*MPrime);
3558 
3559  /* Equilibrium, node 2 */
3560  WorkVec.Sub(1, FTmp);
3561  WorkVec.Sub(3 + 1, fn.Cross(FTmp) + MTmp);
3562 
3563  /* d/dt( Equilibrium ) , node 2 */
3564  WorkVec.Sub(6 + 1, FPrimeTmp);
3565  WorkVec.Sub(9 + 1, fn.Cross(FPrimeTmp) + MPrimeTmp);
3566 
3567  /* Constraint Equations */
3568  Vec3 XDelta = RchT*(pNode->GetXCurr() + fn) - tilde_Xc - XDrv.Get();
3569  Vec3 XDeltaPrime = RchT*(pNode->GetVCurr() + Omega.Cross(fn));
3570 
3571  if (XDrv.bIsDifferentiable()) {
3572  XDeltaPrime -= XDrv.GetP();
3573  }
3574 
3575  Mat3x3 R0 = RotManip::Rot(ThetaDrv.Get());
3576  Mat3x3 RDelta = RchrT*Rnhr.MulMT(R0);
3577  ThetaDelta = RotManip::VecRot(RDelta);
3578  Vec3 ThetaDeltaPrime = RchrT*Omega;
3579 
3580  if (ThetaDrv.bIsDifferentiable()) {
3581  ThetaDeltaPrime -= RDelta*ThetaDrv.GetP();
3582  }
3583 
3584  /* Position constraint: */
3585  for (unsigned iCnt = 0; iCnt < nPosConstraints; iCnt++) {
3586  WorkVec.PutCoef(12 + 1 + iCnt, -(XDelta(iPosIncid[iCnt])));
3587  WorkVec.PutCoef(12 + 1 + nConstraints + iCnt, -(XDeltaPrime(iPosIncid[iCnt])));
3588  }
3589 
3590  /* Rotation constraints: */
3591  for (unsigned iCnt = 0; iCnt < nRotConstraints; iCnt++) {
3592  WorkVec.PutCoef(12 + 1 + nPosConstraints + iCnt, -(ThetaDelta(iRotIncid[iCnt])));
3593  WorkVec.PutCoef(12 + 1 + nPosConstraints + nConstraints + iCnt, -(ThetaDeltaPrime(iRotIncid[iCnt])));
3594  }
3595 
3596  return WorkVec;
3597 }
3598 
3599 unsigned int
3600 TotalPinJoint::iGetNumPrivData(void) const
3601 {
3602  return 24;
3603 }
3604 
3605 unsigned int
3606 TotalPinJoint::iGetPrivDataIdx(const char *s) const
3607 {
3608  ASSERT(s != NULL);
3609 
3610  if (strlen(s) != 2) {
3611  return 0;
3612  }
3613 
3614  unsigned int off = 0;
3615 
3616  switch (s[0]) {
3617  case 'p':
3618  /* relative position */
3619  break;
3620 
3621  case 'r':
3622  /* relative orientation */
3623  off += 3;
3624  break;
3625 
3626  case 'F':
3627  /* force */
3628  off += 6;
3629  break;
3630 
3631  case 'M':
3632  /* moment */
3633  off += 9;
3634  break;
3635 
3636  case 'd':
3637  /* imposed relative position */
3638  off += 12;
3639  break;
3640 
3641  case 't':
3642  /* imposed relative orientation */
3643  off += 15;
3644  break;
3645 
3646  case 'v':
3647  /* relative linear velocity */
3648  off += 18;
3649  break;
3650 
3651  case 'w':
3652  /* relative angular velocity */
3653  off += 21;
3654  break;
3655 
3656  default:
3657  return 0;
3658  }
3659 
3660  switch (s[1]) {
3661  case 'x':
3662  return off + 1;
3663 
3664  case 'y':
3665  return off + 2;
3666 
3667  case 'z':
3668  return off + 3;
3669  }
3670 
3671  return 0;
3672 }
3673 
3674 doublereal
3675 TotalPinJoint::dGetPrivData(unsigned int i) const
3676 {
3677  switch (i) {
3678  case 1:
3679  case 2:
3680  case 3: {
3681  Vec3 x(RchT*(pNode->GetXCurr() + pNode->GetRCurr()*tilde_fn) - tilde_Xc);
3682  return x(i);
3683  }
3684 
3685  case 4:
3686  case 5:
3687  case 6: {
3688  Vec3 Theta(Unwrap(ThetaDeltaTrue, RotManip::VecRot(RchT*pNode->GetRCurr()*tilde_Rnhr)));
3689  return Theta(i - 3);
3690  }
3691 
3692  case 7:
3693  case 8:
3694  case 9:
3695  return F(i - 6);
3696 
3697  case 10:
3698  case 11:
3699  case 12:
3700  return M(i - 9);
3701 
3702  case 13:
3703  case 14:
3704  case 15:
3705  if (!bPosActive[i - 13]) {
3706  return 0.;
3707  }
3708  return XDrv.Get()(i - 12);
3709 
3710  case 16:
3711  case 17:
3712  case 18:
3713  if (!bRotActive[i - 16]) {
3714  return 0.;
3715  }
3716  return ThetaDrv.Get()(i - 15);
3717 
3718  case 19:
3719  case 20:
3720  case 21:
3721  {
3722  Vec3 v(RchrT*(pNode->GetVCurr() + pNode->GetWCurr().Cross(pNode->GetRCurr()*tilde_fn)));
3723 
3724  return v(i-18);
3725  }
3726 
3727  case 22:
3728  case 23:
3729  case 24:
3730  {
3731  Vec3 W(RchrT*pNode->GetWCurr());
3732  return W(i-21);
3733  }
3734 
3735  default:
3736  ASSERT(0);
3737  }
3738 
3739  return 0.;
3740 }
3741 
3742 /* TotalPinJoint - end */
3743 
3744 /* TotalForce - begin */
3745 
3746 TotalForce::TotalForce(unsigned int uL,
3747  TplDriveCaller<Vec3> *const pDCForce,
3748  TplDriveCaller<Vec3> *const pDCCouple,
3749  const StructNode *pN1,
3750  const Vec3& f1Tmp, const Mat3x3& R1hTmp, const Mat3x3& R1hrTmp,
3751  const StructNode *pN2,
3752  const Vec3& f2Tmp, const Mat3x3& R2hTmp, const Mat3x3& R2hrTmp,
3753  flag fOut)
3754 : Elem(uL, fOut),
3755 Force(uL, fOut),
3756 pNode1(pN1), pNode2(pN2),
3757 f1(f1Tmp), R1h(R1hTmp), R1hr(R1hrTmp),
3758 f2(f2Tmp), R2h(R2hTmp), R2hr(R2hrTmp),
3759 FDrv(pDCForce), MDrv(pDCCouple),
3760 M(::Zero3), F(::Zero3)
3761 {
3762  NO_OP;
3763 }
3764 
3765 /* Output (da mettere a punto), per ora solo reazioni */
3766 void
3767 TotalForce::Output(OutputHandler& OH) const
3768 {
3769  if (bToBeOutput()) {
3770  OH.Forces() << std::setw(8) << GetLabel()
3771  << " " << F << " " << M << std::endl;
3772  }
3773 }
3774 
3775 std::ostream&
3776 TotalForce::Restart(std::ostream& out) const
3777 {
3778  Force::Restart(out) << ", total internal, "
3779  << pNode1->GetLabel() << ", "
3780  "position, ", f1.Write(out, ", ") << ", "
3781  "force orientation, ", R1h.Write(out, ", ") << ", "
3782  "moment orientation, ", R1hr.Write(out, ", ") << ", "
3783  << pNode2->GetLabel() << ", "
3784  "position, ", f2.Write(out, ", ") << ", "
3785  "force orientation, ", R2h.Write(out, ", ") << ", "
3786  "moment orientation, ", R2hr.Write(out, ", ") << ", "
3787  "force, ", FDrv.pGetDriveCaller()->Restart(out) << ", "
3788  "moment , ", MDrv.pGetDriveCaller()->Restart(out) << ";"
3789  << std::endl;
3790  return out;
3791 }
3792 
3793 /* Assemblaggio jacobiano */
3794 VariableSubMatrixHandler&
3795 TotalForce::AssJac(VariableSubMatrixHandler& WorkMat,
3796  doublereal dCoef,
3797  const VectorHandler& /* XCurr */ ,
3798  const VectorHandler& /* XPrimeCurr */ )
3799 {
3800  DEBUGCOUT("Entering TotalForce::AssJac()" << std::endl);
3801 
3802  FullSubMatrixHandler& WM = WorkMat.SetFull();
3803 
3804  /* Ridimensiona la sottomatrice in base alle esigenze */
3805  integer iNumRows = 0;
3806  integer iNumCols = 0;
3807  WorkSpaceDim(&iNumRows, &iNumCols);
3808  WM.ResizeReset(iNumRows, iNumCols);
3809 
3810  /* Recupera gli indici delle varie incognite */
3811  integer iNode1FirstPosIndex = pNode1->iGetFirstPositionIndex();
3812  integer iNode1FirstMomIndex = pNode1->iGetFirstMomentumIndex();
3813  integer iNode2FirstPosIndex = pNode2->iGetFirstPositionIndex();
3814  integer iNode2FirstMomIndex = pNode2->iGetFirstMomentumIndex();
3815 
3816  /* Setta gli indici delle equazioni */
3817  for (int iCnt = 1; iCnt <= 6; iCnt++) {
3818  WM.PutRowIndex(iCnt, iNode1FirstMomIndex + iCnt);
3819  WM.PutColIndex(iCnt, iNode1FirstPosIndex + iCnt);
3820  WM.PutRowIndex(6 + iCnt, iNode2FirstMomIndex + iCnt);
3821  WM.PutColIndex(6 + iCnt, iNode2FirstPosIndex + iCnt);
3822  }
3823 
3824  /* Recupera i dati che servono */
3825  Mat3x3 R1(pNode1->GetRRef()*R1h);
3826  Mat3x3 R1r(pNode1->GetRRef()*R1hr);
3827  Vec3 b2(pNode2->GetRCurr()*f2);
3828  Vec3 b1(pNode2->GetXCurr() + b2 - pNode1->GetXCurr());
3829 
3830  /* Moltiplica il momento e la forza per il coefficiente del metodo */
3831  Vec3 FTmp(R1*(F*dCoef));
3832  Vec3 MTmp(R1r*(M*dCoef));
3833 
3834  /* Equilibrium: ((Phi/q)^T*F)/q */
3835 
3836  Mat3x3 Tmp;
3837 
3838  /* [ F x ] */
3839  Tmp = Mat3x3(MatCross, FTmp);
3840 
3841  /* Lines 1->3: */
3842  WM.Add(1, 3 + 1, Tmp);
3843 
3844  /* Lines 4->6: */
3845  WM.Sub(3 + 1, 1, Tmp);
3846 
3847  WM.Add(3 + 1, 6 + 1, Tmp);
3848 
3849  /* Lines 7->9: */
3850  WM.Sub(6 + 1, 3 + 1, Tmp);
3851 
3852  /* [ F x ] [ b2 x ] */
3853  Tmp = Mat3x3(MatCrossCross, FTmp, b2);
3854 
3855  /* Lines 4->6: */
3856  WM.Sub(3 + 1, 9 + 1, Tmp);
3857 
3858  /* Lines 10->12: */
3859  WM.Add(9 + 1, 9 + 1, Tmp);
3860 
3861  /* [ b1 x ] [ F x ] + [ M x ] */
3862 
3863  /* Lines 4->6: */
3864  WM.Add(3 + 1, 3 + 1, Mat3x3(MatCrossCross, b1, FTmp) + Mat3x3(MatCross, MTmp));
3865 
3866  /* [ b2 x ] [ F x ] + [ M x ] */
3867 
3868  /* Lines 10->12: */
3869  WM.Sub(9 + 1, 3 + 1, Mat3x3(MatCrossCross, b2, FTmp) + Mat3x3(MatCross, MTmp));
3870 
3871  return WorkMat;
3872 }
3873 
3874 
3875 /* Assemblaggio residuo */
3876 SubVectorHandler&
3877 TotalForce::AssRes(SubVectorHandler& WorkVec,
3878  doublereal dCoef,
3879  const VectorHandler& XCurr,
3880  const VectorHandler& /* XPrimeCurr */ )
3881 {
3882  DEBUGCOUT("Entering TotalForce::AssRes()" << std::endl);
3883 
3884  /* Dimensiona e resetta la matrice di lavoro */
3885  integer iNumRows = 0;
3886  integer iNumCols = 0;
3887  WorkSpaceDim(&iNumRows, &iNumCols);
3888  WorkVec.ResizeReset(iNumRows);
3889 
3890  /* Indici */
3891  integer iNode1FirstMomIndex = pNode1->iGetFirstMomentumIndex();
3892  integer iNode2FirstMomIndex = pNode2->iGetFirstMomentumIndex();
3893 
3894  /* Indici dei nodi */
3895  for (int iCnt = 1; iCnt <= 6; iCnt++) {
3896  WorkVec.PutRowIndex(iCnt, iNode1FirstMomIndex + iCnt);
3897  WorkVec.PutRowIndex(6+iCnt, iNode2FirstMomIndex + iCnt);
3898  }
3899 
3900  /* Get Forces */
3901 
3902  F = FDrv.Get();
3903  M = MDrv.Get();
3904 
3905  Vec3 b2(pNode2->GetRCurr()*f2);
3906  Vec3 b1(pNode2->GetXCurr() + b2 - pNode1->GetXCurr());
3907 
3908  Mat3x3 R1 = pNode1->GetRCurr()*R1h;
3909  Mat3x3 R1r = pNode1->GetRCurr()*R1hr;
3910  Mat3x3 R2r = pNode2->GetRCurr()*R2hr;
3911 
3912  Vec3 FTmp(R1*F);
3913  Vec3 MTmp(R1r*M);
3914 
3915  /* Equilibrium, node 1 */
3916  WorkVec.Add(1, FTmp);
3917  WorkVec.Add(3 + 1, MTmp + b1.Cross(FTmp));
3918 
3919  /* Equilibrium, node 2 */
3920  WorkVec.Sub(6 + 1, FTmp);
3921  WorkVec.Sub(9 + 1, MTmp + b2.Cross(FTmp));
3922 
3923  return WorkVec;
3924 }
3925 
3926 /* inverse dynamics capable element */
3927 bool
3928 TotalForce::bInverseDynamics(void) const
3929 {
3930  return true;
3931 }
3932 
3933 /* Inverse Dynamics Residual Assembly */
3934 SubVectorHandler&
3935 TotalForce::AssRes(SubVectorHandler& WorkVec,
3936  const VectorHandler& /* XCurr */,
3937  const VectorHandler& /* XPrimeCurr */,
3938  const VectorHandler& /* XPrimePrimeCurr */,
3939  InverseDynamics::Order iOrder)
3940 {
3941  DEBUGCOUT("Entering TotalForce::AssRes(" << invdyn2str(iOrder) << ")" << std::endl);
3942 
3943  ASSERT(iOrder == InverseDynamics::INVERSE_DYNAMICS);
3944 
3945  /* Dimensiona e resetta la matrice di lavoro */
3946  integer iNumRows = 0;
3947  integer iNumCols = 0;
3948  WorkSpaceDim(&iNumRows, &iNumCols);
3949  WorkVec.ResizeReset(iNumRows);
3950 
3951  /* Indici */
3952  integer iNode1FirstMomIndex = pNode1->iGetFirstPositionIndex();
3953  integer iNode2FirstMomIndex = pNode2->iGetFirstPositionIndex();
3954 
3955  /* Indici dei nodi */
3956  for (int iCnt = 1; iCnt <= 6; iCnt++) {
3957  WorkVec.PutRowIndex(iCnt, iNode1FirstMomIndex + iCnt);
3958  WorkVec.PutRowIndex(6+iCnt, iNode2FirstMomIndex + iCnt);
3959  }
3960 
3961  /* Get Forces */
3962 
3963  F = FDrv.Get();
3964  M = MDrv.Get();
3965 
3966  Vec3 b2(pNode2->GetRCurr()*f2);
3967  Vec3 b1(pNode2->GetXCurr() + b2 - pNode1->GetXCurr());
3968 
3969  Mat3x3 R1 = pNode1->GetRCurr()*R1h;
3970  Mat3x3 R1r = pNode1->GetRCurr()*R1hr;
3971  Mat3x3 R2r = pNode2->GetRCurr()*R2hr;
3972 
3973  Vec3 FTmp(R1*F);
3974  Vec3 MTmp(R1r*M);
3975 
3976  /* Equilibrium, node 1 */
3977  WorkVec.Add(1, FTmp);
3978  WorkVec.Add(3 + 1, MTmp + b1.Cross(FTmp));
3979 
3980  /* Equilibrium, node 2 */
3981  WorkVec.Sub(6 + 1, FTmp);
3982  WorkVec.Sub(9 + 1, MTmp + b2.Cross(FTmp));
3983 
3984  return WorkVec;
3985 }
3986 
3987 
3988 VariableSubMatrixHandler&
3989 TotalForce::InitialAssJac(VariableSubMatrixHandler& WorkMat,
3990  const VectorHandler& XCurr)
3991 {
3992 
3993  /* Per ora usa la matrice piena; eventualmente si puo'
3994  * passare a quella sparsa quando si ottimizza */
3995  FullSubMatrixHandler& WM = WorkMat.SetFull();
3996 
3997  /* Dimensiona e resetta la matrice di lavoro */
3998  integer iNumRows = 0;
3999  integer iNumCols = 0;
4000  InitialWorkSpaceDim(&iNumRows, &iNumCols);
4001  WM.ResizeReset(iNumRows, iNumCols);
4002 
4003  /* Indici */
4004  integer iNode1FirstPosIndex = pNode1->iGetFirstPositionIndex();
4005  integer iNode1FirstVelIndex = iNode1FirstPosIndex + 6;
4006  integer iNode2FirstPosIndex = pNode2->iGetFirstPositionIndex();
4007  integer iNode2FirstVelIndex = iNode2FirstPosIndex + 6;
4008 
4009 
4010  /* Setta gli indici dei nodi */
4011  for (int iCnt = 1; iCnt <= 6; iCnt++) {
4012  WM.PutRowIndex(iCnt, iNode1FirstPosIndex + iCnt);
4013  WM.PutColIndex(iCnt, iNode1FirstPosIndex + iCnt);
4014  WM.PutRowIndex(6 + iCnt, iNode1FirstVelIndex + iCnt);
4015  WM.PutColIndex(6 + iCnt, iNode1FirstVelIndex + iCnt);
4016  WM.PutRowIndex(12 + iCnt, iNode2FirstPosIndex + iCnt);
4017  WM.PutColIndex(12 + iCnt, iNode2FirstPosIndex + iCnt);
4018  WM.PutRowIndex(18 + iCnt, iNode2FirstVelIndex + iCnt);
4019  WM.PutColIndex(18 + iCnt, iNode2FirstVelIndex + iCnt);
4020  }
4021 
4022  /* Recupera i dati che servono */
4023  Mat3x3 R1(pNode1->GetRRef()*R1h);
4024  Mat3x3 R1r(pNode1->GetRRef()*R1hr);
4025 
4026  Vec3 b2(pNode2->GetRCurr()*f2);
4027  Vec3 b1(pNode2->GetXCurr() + b2 - pNode1->GetXCurr());
4028 
4029  Vec3 Omega1(pNode1->GetWCurr());
4030  Vec3 Omega2(pNode2->GetWCurr());
4031 
4032  Vec3 b1Prime(pNode2->GetVCurr() + Omega2.Cross(b2) - pNode1->GetVCurr());
4033 
4034  Vec3 FTmp(R1*F);
4035  Vec3 MTmp(R1r*M);
4036 
4037  Mat3x3 Tmp;
4038 
4039  /* [ F x ] */
4040  Tmp = Mat3x3(MatCross, FTmp);
4041 
4042  /* Force, Node 1, Lines 1->3: */
4043  WM.Add(1, 3 + 1, Tmp); // * Delta_g1
4044 
4045  /* Moment, Node 1, Lines 4->6: */
4046  WM.Sub(3 + 1, 1, Tmp); // * Delta_x1
4047 
4048  WM.Add(3 + 1, 12 + 1, Tmp); // * Delta_x2
4049 
4050  /* Force, Node 2, Lines 13->15: */
4051  WM.Sub(12 + 1, 3 + 1, Tmp); // * Delta_g1
4052 
4053  /* [ F x ] [ b2 x ] */
4054  Tmp = Mat3x3(MatCrossCross, FTmp, b2);
4055 
4056  /* Moment, Node1, Lines 4->6: */
4057  WM.Sub(3 + 1, 15 + 1, Tmp); // * Delta_g2
4058 
4059  /* Moment, Node2, Lines 16->18: */
4060  WM.Add(15 + 1, 15 + 1, Tmp); // * Delta_g2
4061 
4062  /* [ b1 x ] [ F x ] + [ M x ] */
4063 
4064  /* Moment, Node1, Lines 4->6: */
4065  WM.Add(3 + 1, 3 + 1, Mat3x3(MatCrossCross, b1, FTmp) + Mat3x3(MatCross, MTmp)); // *Delta_g1
4066 
4067  /* [ b2 x ] [ F x ] + [ M x ] */
4068 
4069  /* Moment, Node2, Lines 16->18: */
4070  WM.Sub(15 + 1, 3 + 1, Mat3x3(MatCrossCross, b2, FTmp) + Mat3x3(MatCross, MTmp)); // * Delta_g1
4071 
4072  /* d/dt(Moment), Node2, Lines 22->24: */
4073  WM.Sub(21 + 1, 9 + 1, Mat3x3(MatCrossCross, b2, FTmp) + Mat3x3(MatCross, MTmp)); // * Delta_W1
4074 
4075  return WorkMat;
4076 }
4077 
4078 
4079 /* Contributo al residuo durante l'assemblaggio iniziale */
4080 SubVectorHandler&
4081 TotalForce::InitialAssRes(SubVectorHandler& WorkVec,
4082  const VectorHandler& XCurr)
4083 {
4084 
4085  DEBUGCOUT("Entering TotalForce::InitialAssRes()" << std::endl);
4086 
4087  /* Dimensiona e resetta la matrice di lavoro */
4088  integer iNumRows = 0;
4089  integer iNumCols = 0;
4090  InitialWorkSpaceDim(&iNumRows, &iNumCols);
4091  WorkVec.ResizeReset(iNumRows);
4092 
4093  /* Indici */
4094  integer iNode1FirstPosIndex = pNode1->iGetFirstPositionIndex();
4095  integer iNode1FirstVelIndex = iNode1FirstPosIndex + 6;
4096  integer iNode2FirstPosIndex = pNode2->iGetFirstPositionIndex();
4097  integer iNode2FirstVelIndex = iNode2FirstPosIndex + 6;
4098 
4099  /* Setta gli indici */
4100  for (int iCnt = 1; iCnt <= 6; iCnt++) {
4101  WorkVec.PutRowIndex(iCnt, iNode1FirstPosIndex+iCnt);
4102  WorkVec.PutRowIndex(6 + iCnt, iNode1FirstVelIndex + iCnt);
4103  WorkVec.PutRowIndex(12 + iCnt, iNode2FirstPosIndex + iCnt);
4104  WorkVec.PutRowIndex(18 + iCnt, iNode2FirstVelIndex + iCnt);
4105  }
4106 
4107  /* Recupera i dati */
4108  /* Recupera i dati che servono */
4109  Mat3x3 R1(pNode1->GetRRef()*R1h);
4110  Mat3x3 R1r(pNode1->GetRRef()*R1hr);
4111  Mat3x3 R2r(pNode2->GetRCurr()*R2hr);
4112 
4113  Vec3 b2(pNode2->GetRCurr()*f2);
4114  Vec3 b1(pNode2->GetXCurr() + b2 - pNode1->GetXCurr());
4115 
4116  Vec3 Omega1(pNode1->GetWCurr());
4117  Vec3 Omega2(pNode2->GetWCurr());
4118 
4119  Vec3 b1Prime(pNode2->GetVCurr() + Omega2.Cross(b2) - pNode1->GetVCurr());
4120 
4121  /* Aggiorna F ed M, che restano anche per InitialAssJac */
4122 
4123  F = FDrv.Get();
4124  M = MDrv.Get();
4125 
4126  Vec3 FTmp(R1*F);
4127  Vec3 MTmp(R1r*M);
4128 
4129  /* Equilibrium, node 1 */
4130  WorkVec.Add(1, FTmp);
4131  WorkVec.Add(3 + 1, b1.Cross(FTmp) + MTmp);
4132 
4133  /* Equilibrium, node 2 */
4134  WorkVec.Sub(12 + 1, FTmp);
4135  WorkVec.Sub(15 + 1, b2.Cross(FTmp) + MTmp);
4136 
4137  return WorkVec;
4138 }
4139 
4140 
TotalJoint::R2h
Mat3x3 R2h
Definition: totalj.h:54
Hint
Definition: hint.h:38
TotalJoint::R1hr
Mat3x3 R1hr
Definition: totalj.h:52
TotalPinJoint::iVelEqIndex
unsigned int iVelEqIndex[3]
Definition: totalj.h:299
TotalJoint::iVelEqIndex
unsigned int iVelEqIndex[3]
Definition: totalj.h:83
TotalJoint::GetEqType
DofOrder::Order GetEqType(unsigned int i) const
Definition: totalj.cc:1469
TotalPinJoint::Rchr
Mat3x3 Rchr
Definition: totalj.h:265
TotalForce::R2hr
Mat3x3 R2hr
Definition: totalj.h:491
TotalJoint::Restart
virtual std::ostream & Restart(std::ostream &out) const
Definition: totalj.cc:753
TotalJoint::iGetInitialNumDof
virtual unsigned int iGetInitialNumDof(void) const
Definition: totalj.h:220
TotalJoint::R1h
Mat3x3 R1h
Definition: totalj.h:51
TotalPinJoint::~TotalPinJoint
~TotalPinJoint(void)
Definition: totalj.cc:2202
TotalJoint::bVelActive
bool bVelActive[3]
Definition: totalj.h:59
TotalForce::R1hr
Mat3x3 R1hr
Definition: totalj.h:488
FullSubMatrixHandler::PutColIndex
void PutColIndex(integer iSubCol, integer iCol)
Definition: submat.h:325
Zero3
const Vec3 Zero3(0., 0., 0.)
Vec3::Cross
Vec3 Cross(const Vec3 &v) const
Definition: matvec3.h:218
ASSERTMSGBREAK
#define ASSERTMSGBREAK(expr, msg)
Definition: myassert.h:222
TotalPinJoint::Restart
virtual std::ostream & Restart(std::ostream &out) const
Definition: totalj.cc:2774
TotalPinJoint::iAgvIncid
unsigned int iAgvIncid[3]
Definition: totalj.h:295
TotalForce::Output
virtual void Output(OutputHandler &OH) const
Definition: totalj.cc:3767
Vec3::Write
std::ostream & Write(std::ostream &out, const char *sFill=" ") const
Definition: matvec3.cc:738
TotalJoint::iGetNumDof
virtual unsigned int iGetNumDof(void) const
Definition: totalj.h:129
flag
long int flag
Definition: mbdyn.h:43
StructNode::GetRRef
virtual const Mat3x3 & GetRRef(void) const
Definition: strnode.h:1006
ToBeOutput::bToBeOutput
virtual bool bToBeOutput(void) const
Definition: output.cc:890
grad::Transpose
MatrixExpression< TransposedMatrix< MatrixDirectExpr< Matrix< T, N_rows, N_cols > > >, N_cols, N_rows > Transpose(const Matrix< T, N_rows, N_cols > &A)
Definition: matvec.h:2206
TotalPinJoint::bPosActive
bool bPosActive[3]
Definition: totalj.h:269
TplDriveOwner::bIsDifferentiable
virtual bool bIsDifferentiable(void) const
Definition: tpldrive.h:118
TotalPinJoint::Rch
Mat3x3 Rch
Definition: totalj.h:264
MBDYN_EXCEPT_ARGS
#define MBDYN_EXCEPT_ARGS
Definition: except.h:63
TotalPinJoint::XPDrv
TplDriveOwner< Vec3 > XPDrv
Definition: totalj.h:279
Vec3
Definition: matvec3.h:98
TotalPinJoint::InitialWorkSpaceDim
virtual void InitialWorkSpaceDim(integer *piNumRows, integer *piNumCols) const
Definition: totalj.h:438
VectorHandler::ResizeReset
virtual void ResizeReset(integer)
Definition: vh.cc:55
TotalJoint::AssJac
VariableSubMatrixHandler & AssJac(VariableSubMatrixHandler &WorkMat, doublereal dCoef, const VectorHandler &XCurr, const VectorHandler &XPrimeCurr)
Definition: totalj.cc:834
TotalJoint::DescribeDof
virtual std::ostream & DescribeDof(std::ostream &out, const char *prefix="", bool bInitial=false) const
Definition: totalj.cc:188
MatCross
const MatCross_Manip MatCross
Definition: matvec3.cc:639
OutputHandler::UseNetCDF
bool UseNetCDF(int out) const
Definition: output.cc:491
VariableSubMatrixHandler::SetFull
FullSubMatrixHandler & SetFull(void)
Definition: submat.h:1168
StructNode::GetRCurr
virtual const Mat3x3 & GetRCurr(void) const
Definition: strnode.h:1012
TotalPinJoint::bRotActive
bool bRotActive[3]
Definition: totalj.h:270
Joint::JointDriveHint::pTDH
TplDriveHint< T > * pTDH
Definition: joint.h:137
TotalJoint::f1
Vec3 f1
Definition: totalj.h:50
FullSubMatrixHandler::Add
void Add(integer iRow, integer iCol, const Vec3 &v)
Definition: submat.cc:209
TotalJoint::AssRes
SubVectorHandler & AssRes(SubVectorHandler &WorkVec, doublereal dCoef, const VectorHandler &XCurr, const VectorHandler &XPrimeCurr)
Definition: totalj.cc:1060
TotalPinJoint::dGetPrivData
virtual doublereal dGetPrivData(unsigned int i) const
Definition: totalj.cc:3675
TotalPinJoint::nRotConstraints
unsigned int nRotConstraints
Definition: totalj.h:288
TotalPinJoint::nAgvConstraints
unsigned int nAgvConstraints
Definition: totalj.h:290
TotalJoint::ParseHint
virtual Hint * ParseHint(DataManager *pDM, const char *s) const
Definition: totalj.cc:649
TotalJoint::OmegaPDrv
TplDriveOwner< Vec3 > OmegaPDrv
Definition: totalj.h:68
TotalJoint::nPosConstraints
unsigned int nPosConstraints
Definition: totalj.h:71
TotalJoint::bInverseDynamics
virtual bool bInverseDynamics(void) const
Definition: totalj.cc:1200
TotalJoint::nVelConstraints
unsigned int nVelConstraints
Definition: totalj.h:73
VectorHandler::Sub
virtual void Sub(integer iRow, const Vec3 &v)
Definition: vh.cc:78
TotalJoint::ThetaDrv
TplDriveOwner< Vec3 > ThetaDrv
Definition: totalj.h:66
TotalPinJoint::tilde_Rnh
Mat3x3 tilde_Rnh
Definition: totalj.h:267
TotalForce::TotalForce
TotalForce(unsigned int uL, TplDriveCaller< Vec3 > *const pDCForce, TplDriveCaller< Vec3 > *const pDCCouple, const StructNode *pN1, const Vec3 &f1Tmp, const Mat3x3 &R1hTmp, const Mat3x3 &R1hrTmp, const StructNode *pN2, const Vec3 &f2Tmp, const Mat3x3 &R2hTmp, const Mat3x3 &R2hrTmp, flag fOut)
Definition: totalj.cc:3746
TotalJoint::iPosIncid
unsigned int iPosIncid[3]
Definition: totalj.h:76
TotalPinJoint::nVelConstraints
unsigned int nVelConstraints
Definition: totalj.h:289
TotalPinJoint::DescribeDof
virtual std::ostream & DescribeDof(std::ostream &out, const char *prefix="", bool bInitial=false) const
Definition: totalj.cc:2208
Force
Definition: force.h:46
Force::Restart
virtual std::ostream & Restart(std::ostream &out) const
Definition: force.cc:52
TotalJoint::pNode2
const StructNode * pNode2
Definition: totalj.h:49
TotalPinJoint::bVelActive
bool bVelActive[3]
Definition: totalj.h:271
TotalForce::InitialWorkSpaceDim
virtual void InitialWorkSpaceDim(integer *piNumRows, integer *piNumCols) const
Definition: totalj.h:548
NO_OP
#define NO_OP
Definition: myassert.h:74
TotalPinJoint::tilde_fn
Vec3 tilde_fn
Definition: totalj.h:266
V2
Definition: matvec3.h:50
SimulationEntity::Hints
std::vector< Hint * > Hints
Definition: simentity.h:89
TotalPinJoint::OutputPrepare
void OutputPrepare(OutputHandler &OH)
Definition: totalj.cc:3293
TotalJoint::tilde_f1
Vec3 tilde_f1
Definition: totalj.h:86
TotalForce::AssRes
SubVectorHandler & AssRes(SubVectorHandler &WorkVec, doublereal dCoef, const VectorHandler &XCurr, const VectorHandler &XPrimeCurr)
Definition: totalj.cc:3877
Mat3x3::GetVec
Vec3 GetVec(unsigned short int i) const
Definition: matvec3.h:893
TotalJoint::XPPDrv
TplDriveOwner< Vec3 > XPPDrv
Definition: totalj.h:64
TotalJoint::iRotEqIndex
unsigned int iRotEqIndex[3]
Definition: totalj.h:82
TotalForce::MDrv
TplDriveOwner< Vec3 > MDrv
Definition: totalj.h:495
TplDriveOwner::Set
void Set(const TplDriveCaller< T > *pDC)
Definition: tpldrive.h:97
FullSubMatrixHandler::AddT
void AddT(integer iRow, integer iCol, const Vec3 &v)
Definition: submat.cc:227
RotManip::VecRot
Vec3 VecRot(const Mat3x3 &Phi)
Definition: Rot.cc:136
TotalPinJoint::iPosIncid
unsigned int iPosIncid[3]
Definition: totalj.h:292
TotalPinJoint::ThetaDeltaRemnant
Vec3 ThetaDeltaRemnant
Definition: totalj.h:314
TplDriveOwner::GetP
virtual T GetP(void) const
Definition: tpldrive.h:121
TotalForce::Restart
virtual std::ostream & Restart(std::ostream &out) const
Definition: totalj.cc:3776
Mat3x3::MulTV
Vec3 MulTV(const Vec3 &v) const
Definition: matvec3.cc:482
TotalPinJoint::iVelIncid
unsigned int iVelIncid[3]
Definition: totalj.h:294
TotalJoint::ThetaDelta
Vec3 ThetaDelta
Definition: totalj.h:97
SubVectorHandler::PutRowIndex
virtual void PutRowIndex(integer iSubRow, integer iRow)=0
VectorHandler::DecCoef
virtual void DecCoef(integer iRow, const doublereal &dCoef)=0
TotalJoint::iAgvIncid
unsigned int iAgvIncid[3]
Definition: totalj.h:79
TotalJoint::OutputPrepare
void OutputPrepare(OutputHandler &OH)
Definition: totalj.cc:1478
TotalJoint::Update
void Update(const VectorHandler &XCurr, InverseDynamics::Order iOrder=InverseDynamics::INVERSE_DYNAMICS)
Definition: totalj.cc:1455
TotalPinJoint::iPosEqIndex
unsigned int iPosEqIndex[3]
Definition: totalj.h:297
TotalPinJoint::OmegaPDrv
TplDriveOwner< Vec3 > OmegaPDrv
Definition: totalj.h:284
TotalPinJoint::tilde_Rnhr
Mat3x3 tilde_Rnhr
Definition: totalj.h:268
TotalPinJoint::ParseHint
virtual Hint * ParseHint(DataManager *pDM, const char *s) const
Definition: totalj.cc:2667
TplDriveOwner::pGetDriveCaller
TplDriveCaller< T > * pGetDriveCaller(void) const
Definition: tpldrive.h:105
TotalPinJoint::OmegaDrv
TplDriveOwner< Vec3 > OmegaDrv
Definition: totalj.h:283
TotalPinJoint::ThetaDeltaPrev
Vec3 ThetaDeltaPrev
Definition: totalj.h:312
TotalPinJoint::AssJac
VariableSubMatrixHandler & AssJac(VariableSubMatrixHandler &WorkMat, doublereal dCoef, const VectorHandler &XCurr, const VectorHandler &XPrimeCurr)
Definition: totalj.cc:2850
TotalPinJoint::AssRes
SubVectorHandler & AssRes(SubVectorHandler &WorkVec, doublereal dCoef, const VectorHandler &XCurr, const VectorHandler &XPrimeCurr)
Definition: totalj.cc:2936
TotalPinJoint::pNode
const StructNode * pNode
Definition: totalj.h:262
VariableSubMatrixHandler::SetNullMatrix
void SetNullMatrix(void)
Definition: submat.h:1159
TotalJoint::bRotActive
bool bRotActive[3]
Definition: totalj.h:57
TotalPinJoint::TotalPinJoint
TotalPinJoint(unsigned int uL, const DofOwner *pDO, bool bPos[3], bool bVel[3], TplDriveCaller< Vec3 > *const pDCPos[3], bool bRot[3], bool bAgv[3], TplDriveCaller< Vec3 > *const pDCRot[3], const Vec3 &XcTmp, const Mat3x3 &RchTmp, const Mat3x3 &RchrTmp, const StructNode *pN, const Vec3 &fnTmp, const Mat3x3 &RnhTmp, const Mat3x3 &RnhrTmp, flag fOut)
Definition: totalj.cc:2075
TotalForce::WorkSpaceDim
void WorkSpaceDim(integer *piNumRows, integer *piNumCols) const
Definition: totalj.h:521
TotalPinJoint::iAgvEqIndex
unsigned int iAgvEqIndex[3]
Definition: totalj.h:300
TotalJoint::nAgvConstraints
unsigned int nAgvConstraints
Definition: totalj.h:74
TotalForce::pNode2
const StructNode * pNode2
Definition: totalj.h:485
OutputHandler::GetCurrentStep
long GetCurrentStep(void) const
Definition: output.h:116
hint_impl.h
TotalJoint::ThetaDeltaRemnant
Vec3 ThetaDeltaRemnant
Definition: totalj.h:100
TotalPinJoint::Update
virtual void Update(const VectorHandler &XCurr, InverseDynamics::Order iOrder=InverseDynamics::INVERSE_DYNAMICS)
Definition: totalj.cc:3267
DEBUGCOUT
#define DEBUGCOUT(msg)
Definition: myassert.h:232
TplDriveOwner::Get
T Get(const doublereal &dVar) const
Definition: tpldrive.h:109
TotalJoint::R2hr
Mat3x3 R2hr
Definition: totalj.h:55
TotalPinJoint::InitialAssJac
VariableSubMatrixHandler & InitialAssJac(VariableSubMatrixHandler &WorkMat, const VectorHandler &XCurr)
Definition: totalj.cc:3367
RotManip::Rot
Mat3x3 Rot(const Vec3 &phi)
Definition: Rot.cc:62
TotalJoint::XPDrv
TplDriveOwner< Vec3 > XPDrv
Definition: totalj.h:63
StructDispNode::iGetFirstMomentumIndex
virtual integer iGetFirstMomentumIndex(void) const =0
StructDispNode::iGetFirstPositionIndex
virtual integer iGetFirstPositionIndex(void) const
Definition: strnode.h:452
TotalPinJoint::XPPDrv
TplDriveOwner< Vec3 > XPPDrv
Definition: totalj.h:280
StructNode::GetWCurr
virtual const Vec3 & GetWCurr(void) const
Definition: strnode.h:1030
TotalJoint::iAgvEqIndex
unsigned int iAgvEqIndex[3]
Definition: totalj.h:84
WithLabel::uLabel
unsigned int uLabel
Definition: withlab.h:44
TotalPinJoint::GetEqType
DofOrder::Order GetEqType(unsigned int i) const
Definition: totalj.cc:3284
Joint::Restart
virtual std::ostream & Restart(std::ostream &out) const
Definition: joint.h:195
TotalJoint::bAgvActive
bool bAgvActive[3]
Definition: totalj.h:60
TotalJoint::pNode1
const StructNode * pNode1
Definition: totalj.h:48
TotalJoint::Output
void Output(OutputHandler &OH) const
Definition: totalj.cc:1504
TotalPinJoint::bInverseDynamics
virtual bool bInverseDynamics(void) const
Definition: totalj.cc:3060
OutputHandler::Joints
std::ostream & Joints(void) const
Definition: output.h:443
TotalPinJoint::iRotEqIndex
unsigned int iRotEqIndex[3]
Definition: totalj.h:298
TotalForce::f2
Vec3 f2
Definition: totalj.h:489
TplDriveCaller::Restart
virtual std::ostream & Restart(std::ostream &out) const =0
TotalPinJoint::AfterConvergence
virtual void AfterConvergence(const VectorHandler &X, const VectorHandler &XP)
Definition: totalj.cc:2744
TotalJoint::ThetaDeltaTrue
Vec3 ThetaDeltaTrue
Definition: totalj.h:101
ASSERT
#define ASSERT(expression)
Definition: colamd.c:977
TotalPinJoint::iGetInitialNumDof
virtual unsigned int iGetInitialNumDof(void) const
Definition: totalj.h:434
TotalForce::AssJac
VariableSubMatrixHandler & AssJac(VariableSubMatrixHandler &WorkMat, doublereal dCoef, const VectorHandler &XCurr, const VectorHandler &XPrimeCurr)
Definition: totalj.cc:3795
TotalJoint::OmegaDrv
TplDriveOwner< Vec3 > OmegaDrv
Definition: totalj.h:67
TotalForce::bInverseDynamics
virtual bool bInverseDynamics(void) const
Definition: totalj.cc:3928
Mat3x3::MulTM
Mat3x3 MulTM(const Mat3x3 &m) const
Definition: matvec3.cc:500
VectorHandler::PutCoef
virtual void PutCoef(integer iRow, const doublereal &dCoef)=0
grad::Cross
VectorExpression< VectorCrossExpr< VectorLhsExpr, VectorRhsExpr >, 3 > Cross(const VectorExpression< VectorLhsExpr, 3 > &u, const VectorExpression< VectorRhsExpr, 3 > &v)
Definition: matvec.h:3248
Joint::OutputPrepare_int
virtual void OutputPrepare_int(const std::string &type, OutputHandler &OH, std::string &name)
Definition: joint.cc:107
TotalForce::F
Vec3 F
Definition: totalj.h:498
StructDispNode::GetXCurr
virtual const Vec3 & GetXCurr(void) const
Definition: strnode.h:310
VectorHandler::Add
virtual void Add(integer iRow, const Vec3 &v)
Definition: vh.cc:63
TotalJoint::~TotalJoint
~TotalJoint(void)
Definition: totalj.cc:182
FullSubMatrixHandler::ResizeReset
virtual void ResizeReset(integer, integer)
Definition: submat.cc:182
TotalPinJoint::tilde_Xc
Vec3 tilde_Xc
Definition: totalj.h:275
TotalJoint::M
Vec3 M
Definition: totalj.h:95
TotalJoint::nRotConstraints
unsigned int nRotConstraints
Definition: totalj.h:72
TotalPinJoint::ThetaDelta
Vec3 ThetaDelta
Definition: totalj.h:311
TotalPinJoint::iGetNumDof
virtual unsigned int iGetNumDof(void) const
Definition: totalj.h:342
Unwrap
Vec3 Unwrap(const Vec3 &vPrev, const Vec3 &v)
Definition: matvec3.cc:1089
TotalJoint::WorkSpaceDim
void WorkSpaceDim(integer *piNumRows, integer *piNumCols) const
Definition: totalj.h:177
Mat3x3::Transpose
Mat3x3 Transpose(void) const
Definition: matvec3.h:816
TotalJoint::XDrv
TplDriveOwner< Vec3 > XDrv
Definition: totalj.h:62
TotalPinJoint::DescribeEq
virtual std::ostream & DescribeEq(std::ostream &out, const char *prefix="", bool bInitial=false) const
Definition: totalj.cc:2382
TotalJoint::iGetPrivDataIdx
virtual unsigned int iGetPrivDataIdx(const char *s) const
Definition: totalj.cc:1924
MatCrossCross
const MatCrossCross_Manip MatCrossCross
Definition: matvec3.cc:640
FullSubMatrixHandler::SubT
void SubT(integer iRow, integer iCol, const Vec3 &v)
Definition: submat.cc:233
STRLENOF
#define STRLENOF(s)
Definition: mbdyn.h:166
TotalPinJoint::nPosConstraints
unsigned int nPosConstraints
Definition: totalj.h:287
TotalJoint::SetValue
virtual void SetValue(DataManager *pDM, VectorHandler &X, VectorHandler &XP, SimulationEntity::Hints *ph=0)
Definition: totalj.cc:563
Elem
Definition: elem.h:75
TotalPinJoint::iGetNumPrivData
virtual unsigned int iGetNumPrivData(void) const
Definition: totalj.cc:3600
FullSubMatrixHandler::PutRowIndex
void PutRowIndex(integer iSubRow, integer iRow)
Definition: submat.h:311
V1
Definition: matvec3.h:49
Vec3::pGetVec
const doublereal * pGetVec(void) const
Definition: matvec3.h:192
TotalJoint::TotalJoint
TotalJoint(unsigned int uL, const DofOwner *pDO, bool bPos[3], bool bVel[3], TplDriveCaller< Vec3 > *const pDCPos[3], bool bRot[3], bool bAgv[3], TplDriveCaller< Vec3 > *const pDCRot[3], const StructNode *pN1, const Vec3 &f1Tmp, const Mat3x3 &R1hTmp, const Mat3x3 &R1hrTmp, const StructNode *pN2, const Vec3 &f2Tmp, const Mat3x3 &R2hTmp, const Mat3x3 &R2hrTmp, flag fOut)
Definition: totalj.cc:51
TotalJoint::InitialAssJac
VariableSubMatrixHandler & InitialAssJac(VariableSubMatrixHandler &WorkMat, const VectorHandler &XCurr)
Definition: totalj.cc:1559
TotalJoint::dGetPrivData
virtual doublereal dGetPrivData(unsigned int i) const
Definition: totalj.cc:1993
TotalForce::R2h
Mat3x3 R2h
Definition: totalj.h:490
TotalPinJoint::RchT
Mat3x3 RchT
Definition: totalj.h:274
StructDispNode::GetVCurr
virtual const Vec3 & GetVCurr(void) const
Definition: strnode.h:322
TotalJoint::ThetaDeltaPrev
Vec3 ThetaDeltaPrev
Definition: totalj.h:98
FullSubMatrixHandler::Sub
void Sub(integer iRow, integer iCol, const Vec3 &v)
Definition: submat.cc:215
TotalPinJoint::Output
void Output(OutputHandler &OH) const
Definition: totalj.cc:3319
TotalJoint::iVelIncid
unsigned int iVelIncid[3]
Definition: totalj.h:78
TotalForce::InitialAssRes
virtual SubVectorHandler & InitialAssRes(SubVectorHandler &WorkVec, const VectorHandler &XCurr)
Definition: totalj.cc:4081
invdyn2str
const char * invdyn2str(InverseDynamics::Order iOrder)
Definition: invdyn.cc:36
TotalPinJoint::bAgvActive
bool bAgvActive[3]
Definition: totalj.h:272
Joint
Definition: joint.h:50
TotalPinJoint::iRotIncid
unsigned int iRotIncid[3]
Definition: totalj.h:293
TotalPinJoint::WorkSpaceDim
void WorkSpaceDim(integer *piNumRows, integer *piNumCols) const
Definition: totalj.h:389
TotalJoint::nConstraints
unsigned int nConstraints
Definition: totalj.h:70
Mat3x3::MulMT
Mat3x3 MulMT(const Mat3x3 &m) const
Definition: matvec3.cc:444
TotalPinJoint::iGetPrivDataIdx
virtual unsigned int iGetPrivDataIdx(const char *s) const
Definition: totalj.cc:3606
TotalPinJoint::ThetaDeltaTrue
Vec3 ThetaDeltaTrue
Definition: totalj.h:315
TotalJoint::InitialWorkSpaceDim
virtual void InitialWorkSpaceDim(integer *piNumRows, integer *piNumCols) const
Definition: totalj.h:224
DofOwnerOwner::iGetFirstIndex
virtual integer iGetFirstIndex(void) const
Definition: dofown.h:127
TotalJoint::F
Vec3 F
Definition: totalj.h:96
TotalPinJoint::nConstraints
unsigned int nConstraints
Definition: totalj.h:286
TotalPinJoint::ThetaDrv
TplDriveOwner< Vec3 > ThetaDrv
Definition: totalj.h:282
doublereal
double doublereal
Definition: colamd.c:52
TotalJoint::iRotIncid
unsigned int iRotIncid[3]
Definition: totalj.h:77
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
integer
long int integer
Definition: colamd.c:51
TotalPinJoint::InitialAssRes
SubVectorHandler & InitialAssRes(SubVectorHandler &WorkVec, const VectorHandler &XCurr)
Definition: totalj.cc:3501
TotalPinJoint::XDrv
TplDriveOwner< Vec3 > XDrv
Definition: totalj.h:278
TotalForce::M
Vec3 M
Definition: totalj.h:497
Mat3x3::Write
std::ostream & Write(std::ostream &out, const char *sFill=" ", const char *sFill2=NULL) const
Definition: matvec3.cc:722
TotalPinJoint::SetValue
virtual void SetValue(DataManager *pDM, VectorHandler &X, VectorHandler &XP, SimulationEntity::Hints *ph=0)
Definition: totalj.cc:2581
WithLabel::GetLabel
unsigned int GetLabel(void) const
Definition: withlab.cc:62
TotalJoint::f2
Vec3 f2
Definition: totalj.h:53
TotalForce::pNode1
const StructNode * pNode1
Definition: totalj.h:484
TotalForce::FDrv
TplDriveOwner< Vec3 > FDrv
Definition: totalj.h:493
idx2xyz
static const char idx2xyz[]
Definition: totalj.cc:47
TotalForce::R1h
Mat3x3 R1h
Definition: totalj.h:487
TotalJoint::iPosEqIndex
unsigned int iPosEqIndex[3]
Definition: totalj.h:81
VectorHandler::Resize
virtual void Resize(integer iNewSize)=0
OutputHandler::Forces
std::ostream & Forces(void) const
Definition: output.h:450
OutputHandler::UseText
bool UseText(int out) const
Definition: output.cc:446
TotalJoint::InitialAssRes
SubVectorHandler & InitialAssRes(SubVectorHandler &WorkVec, const VectorHandler &XCurr)
Definition: totalj.cc:1796
TotalJoint::AfterConvergence
virtual void AfterConvergence(const VectorHandler &X, const VectorHandler &XP)
Definition: totalj.cc:726
TotalJoint::bPosActive
bool bPosActive[3]
Definition: totalj.h:56
TotalJoint::iGetNumPrivData
virtual unsigned int iGetNumPrivData(void) const
Definition: totalj.cc:1918
TotalForce::InitialAssJac
virtual VariableSubMatrixHandler & InitialAssJac(VariableSubMatrixHandler &WorkMat, const VectorHandler &XCurr)
Definition: totalj.cc:3989
TotalPinJoint::RchrT
Mat3x3 RchrT
Definition: totalj.h:276
TotalForce::f1
Vec3 f1
Definition: totalj.h:486
TotalJoint::DescribeEq
virtual std::ostream & DescribeEq(std::ostream &out, const char *prefix="", bool bInitial=false) const
Definition: totalj.cc:362