MBDyn-1.7.3
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Groups
totalequation.cc
Go to the documentation of this file.
1 /*
2  * MBDyn (C) is a multibody analysis code.
3  * http://www.mbdyn.org
4  *
5  * Copyright (C) 1996-2017
6  *
7  * Pierangelo Masarati <masarati@aero.polimi.it>
8  * Paolo Mantegazza <mantegazza@aero.polimi.it>
9  *
10  * Dipartimento di Ingegneria Aerospaziale - Politecnico di Milano
11  * via La Masa, 34 - 20156 Milano, Italy
12  * http://www.aero.polimi.it
13  *
14  * Changing this copyright notice is forbidden.
15  *
16  * This program is free software; you can redistribute it and/or modify
17  * it under the terms of the GNU General Public License as published by
18  * the Free Software Foundation (version 2 of the License).
19  *
20  *
21  * This program is distributed in the hope that it will be useful,
22  * but WITHOUT ANY WARRANTY; without even the implied warranty of
23  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
24  * GNU General Public License for more details.
25  *
26  * You should have received a copy of the GNU General Public License
27  * along with this program; if not, write to the Free Software
28  * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
29  */
30 
31 #include "mbconfig.h" /* This goes first in every *.c,*.cc file */
32 
33 #include <iostream>
34 #include <fstream>
35 
36 #include "totalequation.h"
37 #include "Rot.hh"
38 #include "hint_impl.h"
39 
40 static const char idx2xyz[] = { 'x', 'y', 'z' };
41 
42 /* TotalEquation - begin */
43 
44 TotalEquation::TotalEquation(unsigned int uL, const DofOwner *pDO,
45  bool bPos[3], bool bVel[3],
46  TplDriveCaller<Vec3> *const pDCPos[3],
47  bool bRot[3], bool bAgv[3], /* Agv stands for AnGular Velocity */
48  TplDriveCaller<Vec3> *const pDCRot[3],
49  const StructNode *pN1,
50  const Vec3& f1Tmp, const Mat3x3& R1hTmp, const Mat3x3& R1hrTmp,
51  const StructNode *pN2,
52  const Vec3& f2Tmp, const Mat3x3& R2hTmp, const Mat3x3& R2hrTmp,
53  flag fOut)
54 : Elem(uL, fOut),
55 Joint(uL, pDO, fOut),
56 pNode1(pN1), pNode2(pN2),
57 f1(f1Tmp), R1h(R1hTmp), R1hr(R1hrTmp),
58 f2(f2Tmp), R2h(R2hTmp), R2hr(R2hrTmp),
59 XDrv(pDCPos[0]), XPDrv(pDCPos[1]), XPPDrv(pDCPos[2]),
60 ThetaDrv(pDCRot[0]), OmegaDrv(pDCRot[1]), OmegaPDrv(pDCRot[2]),
61 nConstraints(0), nPosConstraints(0), nRotConstraints(0),
62 nVelConstraints(0), nAgvConstraints(0),
63 tilde_f1(R1h.MulTV(f1)),
64 M(Zero3), F(Zero3), ThetaDelta(Zero3), ThetaDeltaPrev(Zero3)
65 {
66  /* Equations 1->3: Positions
67  * Equations 4->6: Rotations */
68 
69  unsigned int index = 0;
70 
71  for (unsigned int i = 0; i < 3; i++) {
72  ASSERT(bPos[i] == false || bVel[i] == false);
73 
74  bPosActive[i] = bPos[i];
75  bVelActive[i] = bVel[i];
76 
77  if (bPosActive[i]) {
78  iPosIncid[nPosConstraints] = i + 1;
79  iPosEqIndex[nPosConstraints] = index;
80  nPosConstraints++;
81  index++;
82  }
83 
84  if (bVelActive[i]) {
85  iVelIncid[nVelConstraints] = i + 1;
86  iVelEqIndex[nVelConstraints] = index;
87  nVelConstraints++;
88  index++;
89  }
90  }
91 
92  index = 0;
93  for (unsigned int i = 0; i < 3; i++) {
94  ASSERT(bRot[i] == false || bAgv[i] == false);
95 
96  bRotActive[i] = bRot[i];
97  bAgvActive[i] = bAgv[i];
98 
99  if (bRotActive[i]) {
100  iRotIncid[nRotConstraints] = i + 1;
101  iRotEqIndex[nRotConstraints] = nPosConstraints + nVelConstraints + index;
102  nRotConstraints++;
103  index++;
104  }
105 
106  if (bAgvActive[i]) {
107  iAgvIncid[nAgvConstraints] = i + 1;
108  iAgvEqIndex[nAgvConstraints] = nPosConstraints + nVelConstraints + index;
109  nAgvConstraints++;
110  index++;
111  }
112  }
113  nConstraints = nPosConstraints + nRotConstraints + nVelConstraints + nAgvConstraints;
114 
115 }
116 
117 TotalEquation::~TotalEquation(void)
118 {
119  NO_OP;
120 };
121 
122 /* FIXME: velocity stuff not implemented yet */
123 std::ostream&
124 TotalEquation::DescribeDof(std::ostream& out,
125  const char *prefix, bool bInitial) const
126 {
127  integer iIndex = iGetFirstIndex();
128 
129  if (nPosConstraints > 0 || nVelConstraints > 0) {
130  out << prefix << iIndex + 1;
131  if (nPosConstraints + nVelConstraints > 1) {
132  out << "->" << iIndex + nPosConstraints + nVelConstraints;
133  }
134  out << ": ";
135  out << "reaction force(s) [";
136 
137  for (unsigned int i = 0, cnt = 0; i < 3; i++) {
138  if (bPosActive[i] || bVelActive[i]) {
139  cnt++;
140  if (cnt > 1) {
141  out << ",";
142  }
143  out << "F" << idx2xyz[i];
144  }
145  }
146  out << "]" << std::endl;
147  }
148 
149  if (nRotConstraints > 0 || nAgvConstraints > 0) {
150  out << prefix << iIndex + nPosConstraints + nVelConstraints + 1;
151  if (nRotConstraints + nAgvConstraints > 1) {
152  out << "->" << iIndex + nConstraints ;
153  }
154  out << ": ";
155  out << "reaction couple(s) [";
156 
157  for (unsigned int i = 0, cnt = 0; i < 3; i++) {
158  if (bRotActive[i] || bAgvActive[i]) {
159  cnt++;
160  if (cnt > 1) {
161  out << ",";
162  }
163  out << "m" << idx2xyz[i];
164  }
165  }
166  out << "]" << std::endl;
167  }
168 
169  if (bInitial) {
170  iIndex += nConstraints;
171 
172  if (nPosConstraints > 0) {
173  out << prefix << iIndex + 1;
174  if (nPosConstraints > 1) {
175  out << "->" << iIndex + nPosConstraints;
176  }
177  out << ": ";
178  out << "reaction force(s) derivative(s) [";
179 
180  for (unsigned int i = 0, cnt = 0; i < 3; i++) {
181  if (bPosActive[i]) {
182  cnt++;
183  if (cnt > 1) {
184  out << ",";
185  }
186  out << "FP" << idx2xyz[i];
187  }
188  }
189  out << "]" << std::endl;
190  }
191 
192  if (nRotConstraints > 0) {
193  out << prefix << iIndex + nPosConstraints + 1;
194  if (nRotConstraints > 1) {
195  out << "->" << iIndex + nConstraints;
196  }
197  out << ": ";
198  out << "reaction couple(s) derivative(s) [";
199 
200  for (unsigned int i = 0, cnt = 0; i < 3; i++) {
201  if (bRotActive[i]) {
202  cnt++;
203  if (cnt > 1) {
204  out << ",";
205  }
206  out << "mP" << idx2xyz[i];
207  }
208  }
209  out << "]" << std::endl;
210  }
211  }
212 
213  return out;
214 }
215 
216 void
217 TotalEquation::DescribeDof(std::vector<std::string>& desc,
218  bool bInitial, int i) const
219 {
220  // FIXME: todo
221  int ndof = 1;
222  if (i == -1) {
223  if (bInitial) {
224  ndof = iGetNumDof();
225 
226  } else {
227  ndof = iGetInitialNumDof();
228  }
229  }
230  desc.resize(ndof);
231 
232  std::ostringstream os;
233  os << "TotalEquation(" << GetLabel() << ")";
234 
235  if (i == -1) {
236  std::string name(os.str());
237 
238  for (i = 0; i < ndof; i++) {
239  os.str(name);
240  os.seekp(0, std::ios_base::end);
241  os << ": dof(" << i + 1 << ")";
242  desc[i] = os.str();
243  }
244 
245  } else {
246  os << ": dof(" << i + 1 << ")";
247  desc[0] = os.str();
248  }
249 }
250 
251 /* FIXME: velocity stuff not implemented yet */
252 std::ostream&
253 TotalEquation::DescribeEq(std::ostream& out,
254  const char *prefix, bool bInitial) const
255 {
256  integer iIndex = iGetFirstIndex();
257 
258  if (nPosConstraints > 0 || nVelConstraints > 0) {
259  out << prefix << iIndex + 1;
260  if (nPosConstraints + nVelConstraints > 1) {
261  out << "->" << iIndex + nPosConstraints + nVelConstraints;
262  }
263  out << ": ";
264  out << "position/velocity constraint(s) [";
265 
266  for (unsigned int i = 0, cnt = 0; i < 3; i++) {
267  if (bPosActive[i]) {
268  cnt++;
269  if (cnt > 1) {
270  out << ",";
271  }
272  out << "P" << idx2xyz[i] << "1=P" << idx2xyz[i] << "2";
273  } else if (bVelActive[i]) {
274  cnt++;
275  if (cnt > 1) {
276  out << ",";
277  }
278  out << "V" << idx2xyz[i] << "1=V" << idx2xyz[i] << "2";
279  }
280  }
281 
282  out << "]" << std::endl;
283  }
284 
285  if (nRotConstraints > 0 || nAgvConstraints > 0) {
286  out << prefix << iIndex + nPosConstraints + nVelConstraints + 1;
287  if (nRotConstraints + nAgvConstraints > 1) {
288  out << "->" << iIndex + nConstraints ;
289  }
290  out << ": ";
291  out << "orientation/angular velocity constraint(s) [";
292 
293  for (unsigned int i = 0, cnt = 0; i < 3; i++) {
294  if (bRotActive[i]) {
295  cnt++;
296  if (cnt > 1) {
297  out << ",";
298  }
299  out << "g" << idx2xyz[i] << "1=g" << idx2xyz[i] << "2";
300  } else if (bAgvActive[i]) {
301  cnt++;
302  if (cnt > 1) {
303  out << ",";
304  }
305  out << "W" << idx2xyz[i] << "1=W" << idx2xyz[i] << "2";
306  }
307  }
308 
309  out << "]" << std::endl;
310  }
311 
312  if (bInitial) {
313  iIndex += nConstraints;
314 
315  if (nPosConstraints > 0) {
316  out << prefix << iIndex + 1;
317  if (nPosConstraints > 1) {
318  out << "->" << iIndex + nPosConstraints;
319  }
320  out << ": ";
321  out << "velocity constraint(s) [";
322 
323  for (unsigned int i = 0, cnt = 0; i < 3; i++) {
324  if (bPosActive[i]) {
325  cnt++;
326  if (cnt > 1) {
327  out << ",";
328  }
329  out << "v" << idx2xyz[i] << "1=v" << idx2xyz[i] << "2";
330  }
331  }
332 
333  out << "]" << std::endl;
334  }
335 
336  if (nRotConstraints > 0) {
337  out << prefix << iIndex + nPosConstraints + 1;
338  if (nRotConstraints > 1) {
339  out << "->" << iIndex + nConstraints ;
340  }
341  out << ": ";
342  out << "angular velocity constraint(s) [";
343 
344  for (unsigned int i = 0, cnt = 0; i < 3; i++) {
345  if (bRotActive[i]) {
346  cnt++;
347  if (cnt > 1) {
348  out << ",";
349  }
350  out << "w" << idx2xyz[i] << "1=w" << idx2xyz[i] << "2";
351  }
352  }
353 
354  out << "]" << std::endl;
355  }
356  }
357 
358  return out;
359 }
360 
361 void
362 TotalEquation::DescribeEq(std::vector<std::string>& desc,
363  bool bInitial, int i) const
364 {
365  // FIXME: todo
366  int ndof = 1;
367  if (i == -1) {
368  if (bInitial) {
369  ndof = iGetNumDof();
370 
371  } else {
372  ndof = iGetInitialNumDof();
373  }
374  }
375  desc.resize(ndof);
376 
377  std::ostringstream os;
378  os << "TotalEquation(" << GetLabel() << ")";
379 
380  if (i == -1) {
381  std::string name(os.str());
382 
383  for (i = 0; i < ndof; i++) {
384  os.str(name);
385  os.seekp(0, std::ios_base::end);
386  os << ": equation(" << i + 1 << ")";
387  desc[i] = os.str();
388  }
389 
390  } else {
391  os << ": equation(" << i + 1 << ")";
392  desc[0] = os.str();
393  }
394 }
395 
396 void
397 TotalEquation::SetValue(DataManager *pDM,
398  VectorHandler& X, VectorHandler& XP,
399  SimulationEntity::Hints *ph)
400 {
401  if (ph) {
402  for (unsigned int i = 0; i < ph->size(); i++) {
403  Joint::JointHint *pjh = dynamic_cast<Joint::JointHint *>((*ph)[i]);
404 
405  if (pjh) {
406 
407  if (dynamic_cast<Joint::OffsetHint<1> *>(pjh)) {
408  const Mat3x3& R1(pNode1->GetRCurr());
409  Vec3 fTmp2(pNode2->GetRCurr()*f2);
410 
411  f1 = R1.MulTV(pNode2->GetXCurr() + fTmp2 - pNode1->GetXCurr());
412  tilde_f1 = R1h.MulTV(f1) - XDrv.Get();
413 
414  } else if (dynamic_cast<Joint::OffsetHint<2> *>(pjh)) {
415  const Mat3x3& R2(pNode2->GetRCurr());
416  Mat3x3 R1(pNode1->GetRCurr()*R1h);
417  Vec3 fTmp1(pNode1->GetRCurr()*f1);
418 
419  f2 = R2.MulTV(pNode1->GetXCurr() + fTmp1 - pNode2->GetXCurr() + R1*XDrv.Get());
420 
421  } else if (dynamic_cast<Joint::HingeHint<1> *>(pjh)) {
422  if (dynamic_cast<Joint::PositionHingeHint<1> *>(pjh)) {
423  const Mat3x3& R1(pNode1->GetRCurr());
424  R1h = R1.MulTM(pNode2->GetRCurr()*R2h);
425 
426  } else if (dynamic_cast<Joint::OrientationHingeHint<1> *>(pjh)) {
427  const Mat3x3& R1(pNode1->GetRCurr());
428  R1hr = R1.MulTM(pNode2->GetRCurr()*R2hr);
429  }
430 
431  } else if (dynamic_cast<Joint::HingeHint<2> *>(pjh)) {
432  if (dynamic_cast<Joint::PositionHingeHint<2> *>(pjh)) {
433  const Mat3x3& R2(pNode2->GetRCurr());
434  R2h = R2.MulTM(pNode1->GetRCurr()*R1h);
435 
436  } else if (dynamic_cast<Joint::OrientationHingeHint<2> *>(pjh)) {
437  const Mat3x3& R2(pNode2->GetRCurr());
438  R2hr = R2.MulTM(pNode1->GetRCurr()*R1hr);
439  }
440 
441  } else if (dynamic_cast<Joint::JointDriveHint<Vec3> *>(pjh)) {
442  Joint::JointDriveHint<Vec3> *pjdh
443  = dynamic_cast<Joint::JointDriveHint<Vec3> *>(pjh);
444  pedantic_cout("TotalEquation(" << uLabel << "): "
445  "creating drive from hint[" << i << "]..." << std::endl);
446 
447  TplDriveCaller<Vec3> *pDC = pjdh->pTDH->pCreateDrive(pDM);
448  if (pDC == 0) {
449  silent_cerr("TotalEquation(" << uLabel << "): "
450  "unable to create drive "
451  "after hint #" << i << std::endl);
452  throw ErrGeneric(MBDYN_EXCEPT_ARGS);
453  }
454 
455  if (dynamic_cast<Joint::PositionDriveHint<Vec3> *>(pjdh)) {
456  XDrv.Set(pDC);
457 
458  } else if (dynamic_cast<Joint::VelocityDriveHint<Vec3> *>(pjdh)) {
459  XPDrv.Set(pDC);
460 
461  } else if (dynamic_cast<Joint::AccelerationDriveHint<Vec3> *>(pjdh)) {
462  XPPDrv.Set(pDC);
463 
464  } else if (dynamic_cast<Joint::OrientationDriveHint<Vec3> *>(pjdh)) {
465  ThetaDrv.Set(pDC);
466 
467  } else if (dynamic_cast<Joint::AngularVelocityDriveHint<Vec3> *>(pjdh)) {
468  OmegaDrv.Set(pDC);
469 
470  } else if (dynamic_cast<Joint::AngularAccelerationDriveHint<Vec3> *>(pjdh)) {
471  OmegaPDrv.Set(pDC);
472 
473  } else {
474  delete pDC;
475  }
476 
477  } else if (dynamic_cast<Joint::ReactionsHint *>(pjh)) {
478  /* TODO */
479  }
480  continue;
481  }
482  }
483  }
484 }
485 
486 Hint *
487 TotalEquation::ParseHint(DataManager *pDM, const char *s) const
488 {
489  if (strncasecmp(s, "offset{" /*}*/ , STRLENOF("offset{" /*}*/ )) == 0)
490  {
491  s += STRLENOF("offset{" /*}*/ );
492 
493  if (strcmp(&s[1], /*{*/ "}") != 0) {
494  return 0;
495  }
496 
497  switch (s[0]) {
498  case '1':
499  return new Joint::OffsetHint<1>;
500 
501  case '2':
502  return new Joint::OffsetHint<2>;
503  }
504 
505  } else if (strncasecmp(s, "position-hinge{" /*}*/, STRLENOF("position-hinge{" /*}*/)) == 0) {
506  s += STRLENOF("position-hinge{" /*}*/);
507 
508  if (strcmp(&s[1], /*{*/ "}") != 0) {
509  return 0;
510  }
511 
512  switch (s[0]) {
513  case '1':
514  return new Joint::HingeHint<1>;
515 
516  case '2':
517  return new Joint::HingeHint<2>;
518  }
519 
520  } else if (strncasecmp(s, "position-drive3{" /*}*/, STRLENOF("position-drive3{" /*}*/)) == 0) {
521  s += STRLENOF("position-");
522 
523  Hint *pH = ::ParseHint(pDM, s);
524  if (pH) {
525  TplDriveHint<Vec3> *pTDH = dynamic_cast<TplDriveHint<Vec3> *>(pH);
526  if (pTDH) {
527  return new PositionDriveHint<Vec3>(pTDH);
528  }
529  }
530  return 0;
531 
532  } else if (strncasecmp(s, "orientation-hinge{" /*}*/, STRLENOF("orientation-hinge{" /*}*/)) == 0) {
533  s += STRLENOF("orientation-hinge{" /*}*/);
534 
535  if (strcmp(&s[1], /*{*/ "}") != 0) {
536  return 0;
537  }
538 
539  switch (s[0]) {
540  case '1':
541  return new Joint::HingeHint<1>;
542 
543  case '2':
544  return new Joint::HingeHint<2>;
545  }
546 
547  } else if (strncasecmp(s, "orientation-drive3{" /*}*/, STRLENOF("orientation-drive3{" /*}*/)) == 0) {
548  s += STRLENOF("orientation-");
549 
550  Hint *pH = ::ParseHint(pDM, s);
551  if (pH) {
552  TplDriveHint<Vec3> *pTDH = dynamic_cast<TplDriveHint<Vec3> *>(pH);
553  if (pTDH) {
554  return new OrientationDriveHint<Vec3>(pTDH);
555  }
556  }
557  return 0;
558  }
559 
560  return 0;
561 }
562 
563 void
564 TotalEquation::AfterConvergence(const VectorHandler& /* X */ ,
565  const VectorHandler& /* XP */ )
566 {
567  ThetaDeltaPrev = Unwrap(ThetaDeltaPrev, ThetaDelta);
568 }
569 
570 void
571 TotalEquation::AfterConvergence(const VectorHandler& /* X */ ,
572  const VectorHandler& /* XP */, const VectorHandler& /* XP */)
573 {
574  ThetaDeltaPrev = Unwrap(ThetaDeltaPrev, ThetaDelta);
575 }
576 
577 /* Contributo al file di restart */
578 /* FIXME: velocity stuffs not implemented yet */
579 std::ostream&
580 TotalEquation::Restart(std::ostream& out) const
581 {
582  Joint::Restart(out) << ", total equation, "
583  << pNode1->GetLabel() << ", "
584  << "position, ";
585  f1.Write(out, ", ") << ", "
586  << "position orientation, "
587  "1 , ";
588  R1h.GetVec(1).Write(out, ", ") << ", "
589  "2 , ";
590  R1h.GetVec(2).Write(out, ", ") << ", "
591  << "rotation orientation, "
592  "1 , ";
593  R1hr.GetVec(1).Write(out, ", ") << ", "
594  "2 , ";
595  R1hr.GetVec(2).Write(out, ", ") << ", "
596  << pNode2->GetLabel() << ", "
597  << "position, ";
598  f2.Write(out, ", ") << ", "
599  << "position orientation, "
600  "1 , ";
601  R2h.GetVec(1).Write(out, ", ") << ", "
602  "2 , ";
603  R2h.GetVec(2).Write(out, ", ") << ", "
604  << "rotation orientation, "
605  "1 , ";
606  R2hr.GetVec(1).Write(out, ", ") << ", "
607  "2 , ";
608  R2hr.GetVec(2).Write(out, ", ");
609 
610  if (bPosActive[0] || bPosActive[1] || bPosActive[2]) {
611  out << ", position constraint";
612  for (unsigned i = 0; i < 3; i++) {
613  if (bPosActive[i]) {
614  out << ", active";
615  } else {
616  out << ", inactive";
617  }
618  }
619  }
620 
621  if (bRotActive[0] || bRotActive[1] || bRotActive[2]) {
622  out << ", orientation constraint";
623  for (unsigned i = 0; i < 3; i++) {
624  if (bRotActive[i]) {
625  out << ", active";
626  } else {
627  out << ", inactive";
628  }
629  }
630  }
631 
632  return out << std::endl;
633 }
634 
635 /* Assemblaggio jacobiano */
636 VariableSubMatrixHandler&
637 TotalEquation::AssJac(VariableSubMatrixHandler& WorkMat,
638  doublereal dCoef,
639  const VectorHandler& /* XCurr */ ,
640  const VectorHandler& /* XPrimeCurr */ )
641 {
642  /*
643  See tecman.pdf for details
644  */
645 
646  DEBUGCOUT("Entering TotalEquation::AssJac()" << std::endl);
647 
648  FullSubMatrixHandler& WM = WorkMat.SetFull();
649 
650  /* Ridimensiona la sottomatrice in base alle esigenze */
651  integer iNumRows = 0;
652  integer iNumCols = 0;
653  WorkSpaceDim(&iNumRows, &iNumCols);
654  WM.ResizeReset(iNumRows, iNumCols);
655 
656  /* Recupera gli indici delle varie incognite */
657  integer iNode1FirstPosIndex = pNode1->iGetFirstPositionIndex();
658  integer iNode2FirstPosIndex = pNode2->iGetFirstPositionIndex();
659  integer iFirstReactionIndex = iGetFirstIndex();
660 
661  /* Setta gli indici delle equazioni */
662  for (int iCnt = 1; iCnt <= 6; iCnt++) {
663  WM.PutColIndex(iCnt, iNode1FirstPosIndex + iCnt);
664  WM.PutColIndex(6 + iCnt, iNode2FirstPosIndex + iCnt);
665  }
666 
667  for (unsigned int iCnt = 1; iCnt <= nConstraints; iCnt++) {
668  WM.PutRowIndex(iCnt, iFirstReactionIndex + iCnt);
669  }
670 
671  /* Recupera i dati che servono */
672  Mat3x3 R1(pNode1->GetRRef()*R1h);
673  Mat3x3 R1r(pNode1->GetRRef()*R1hr);
674  Vec3 b2(pNode2->GetRCurr()*f2);
675  Vec3 b1(pNode2->GetXCurr() + b2 - pNode1->GetXCurr());
676 
677 /* Phi/q is the only contribution... */
678 
679  Mat3x3 b1Cross_R1(b1.Cross(R1)); // = [ b1 x ] * R1
680  Mat3x3 b2Cross_R1(b2.Cross(R1)); // = [ b2 x ] * R1
681 
682  for (unsigned iCnt = 0 ; iCnt < nPosConstraints; iCnt++) {
683  Vec3 vR1(R1.GetVec(iPosIncid[iCnt]));
684  Vec3 vb1Cross_R1(b1Cross_R1.GetVec(iPosIncid[iCnt]));
685  Vec3 vb2Cross_R1(b2Cross_R1.GetVec(iPosIncid[iCnt]));
686 
687  /* Constraint, node 1 */
688  WM.SubT(1 + iPosEqIndex[iCnt], 1, vR1);
689  WM.SubT(1 + iPosEqIndex[iCnt], 3 + 1, vb1Cross_R1);
690 
691  /* Constraint, node 2 */
692  WM.AddT(1 + iPosEqIndex[iCnt], 6 + 1, vR1);
693  WM.AddT(1 + iPosEqIndex[iCnt], 9 + 1, vb2Cross_R1);
694  }
695 
696  for (unsigned iCnt = 0 ; iCnt < nRotConstraints; iCnt++) {
697  Vec3 vR1(R1r.GetVec(iRotIncid[iCnt]));
698 
699  /* Constraint, node 1 */
700  WM.SubT(1 + iRotEqIndex[iCnt], 3 + 1, vR1);
701 
702  /* Constraint, node 2 */
703  WM.AddT(1 + iRotEqIndex[iCnt], 9 + 1, vR1);
704  }
705 
706  if (nVelConstraints > 0) {
707  Mat3x3 Omega1Cross_R1(pNode1->GetWCurr().Cross(R1));
708  Mat3x3 Tmp12 = (
709  Mat3x3(MatCrossCross, pNode1->GetWCurr(), -b1)
710  + Mat3x3(MatCrossCross, b2, pNode1->GetWCurr())
711  - Mat3x3(MatCross, pNode2->GetVCurr())
712  - Mat3x3(MatCrossCross, pNode2->GetWCurr(), b2)
713  + Mat3x3(MatCross, pNode1->GetVCurr())
714  ) * R1;
715  Mat3x3 Tmp22(MatCrossCross, pNode2->GetWCurr(), b2);
716 
717  for (unsigned iCnt = 0 ; iCnt < nVelConstraints; iCnt++) {
718  Vec3 vR1(R1.GetVec(iVelIncid[iCnt]));
719  Vec3 vb1Cross_R1(b1Cross_R1.GetVec(iVelIncid[iCnt]));
720  Vec3 vb2Cross_R1(b2Cross_R1.GetVec(iVelIncid[iCnt]));
721 
722  Vec3 vOmega1Cross_R1(Omega1Cross_R1.GetVec(iVelIncid[iCnt]));
723  Vec3 vTmp12(Tmp12.GetVec(iVelIncid[iCnt]));
724  Vec3 vTmp22(Tmp22.GetVec(iVelIncid[iCnt]));
725 
726  /* Constraint, node 1 */
727  /* The same as in position constraint*/
728  WM.SubT(1 + iVelEqIndex[iCnt], 1, vR1); // delta_v1
729  WM.SubT(1 + iVelEqIndex[iCnt], 3 + 1, vb1Cross_R1); // delta_W1
730  /* New contributions, related to delta_x1 and delta_g1 */
731  WM.SubT(1 + iVelEqIndex[iCnt], 1, vOmega1Cross_R1 * dCoef); // delta_x1
732  WM.AddT(1 + iVelEqIndex[iCnt], 3 + 1, vTmp12 * dCoef); // delta_g1
733 
734  /* Constraint, node 2 */
735  /* The same as in position constraint*/
736  WM.AddT(1 + iVelEqIndex[iCnt], 6 + 1, vR1); // delta_v2
737  WM.AddT(1 + iVelEqIndex[iCnt], 9 + 1, vb2Cross_R1); // delta_W2
738  /* New contributions, related to delta_x1 and delta_g1 */
739  WM.AddT(1 + iVelEqIndex[iCnt], 6 + 1, vOmega1Cross_R1 * dCoef); // delta_x2
740  WM.AddT(1 + iVelEqIndex[iCnt], 9 + 1, vTmp22 * dCoef); // delta_g2
741  }
742  }
743 
744  if(nAgvConstraints > 0) {
745  Mat3x3 DeltaWCross_R1((pNode1->GetWCurr() - pNode2->GetWCurr()).Cross(R1r));
746  for (unsigned iCnt = 0 ; iCnt < nAgvConstraints; iCnt++) {
747  Vec3 vR1(R1r.GetVec(iAgvIncid[iCnt]));
748  Vec3 vDeltaWCross_R1(DeltaWCross_R1.GetVec(iAgvIncid[iCnt]));
749 
750  /* Constraint, node 1 */
751  WM.SubT(1 + iAgvEqIndex[iCnt], 3 + 1, vR1); // delta_W1
752  /* New contribution, related to delta_g1 */
753  WM.SubT(1 + iAgvEqIndex[iCnt], 3 + 1, vDeltaWCross_R1 * dCoef); // delta_g1
754 
755  /* Constraint, node 2 */
756  WM.AddT(1 + iAgvEqIndex[iCnt], 9 + 1, vR1);// delta_W2
757  }
758  }
759 
760  return WorkMat;
761 }
762 
763 /* Assemblaggio residuo */
764 SubVectorHandler&
765 TotalEquation::AssRes(SubVectorHandler& WorkVec,
766  doublereal dCoef,
767  const VectorHandler& XCurr,
768  const VectorHandler& /* XPrimeCurr */ )
769 {
770  DEBUGCOUT("Entering TotalEquation::AssRes()" << std::endl);
771 
772  /* Dimensiona e resetta la matrice di lavoro */
773  integer iNumRows = 0;
774  integer iNumCols = 0;
775  WorkSpaceDim(&iNumRows, &iNumCols);
776  WorkVec.ResizeReset(iNumRows);
777 
778  /* Indici */
779  integer iFirstReactionIndex = iGetFirstIndex();
780 
781  /* Indici del vincolo */
782  for (unsigned int iCnt = 1; iCnt <= nConstraints; iCnt++) {
783  WorkVec.PutRowIndex(iCnt, iFirstReactionIndex + iCnt);
784  }
785 
786  Vec3 b2(pNode2->GetRCurr()*f2);
787  Vec3 b1(pNode2->GetXCurr() + b2 - pNode1->GetXCurr());
788 
789  Mat3x3 R1 = pNode1->GetRCurr()*R1h;
790  Mat3x3 R1r = pNode1->GetRCurr()*R1hr;
791  Mat3x3 R2r = pNode2->GetRCurr()*R2hr;
792 
793  Vec3 XDelta = R1.MulTV(b1) - tilde_f1 - XDrv.Get();
794  Vec3 VDelta = R1.MulTV(
795  pNode2->GetVCurr()
796  - b2.Cross(pNode2->GetWCurr())
797  - pNode1->GetVCurr()
798  + b1.Cross(pNode1->GetWCurr())
799  )
800  - XDrv.Get();
801 
802  Mat3x3 R0T = RotManip::Rot(-ThetaDrv.Get()); // -Theta0 to get R0 transposed
803  Mat3x3 RDelta = R1r.MulTM(R2r*R0T);
804  ThetaDelta = RotManip::VecRot(RDelta);
805 
806  Vec3 WDelta = R1r.MulTV(pNode2->GetWCurr() - pNode1->GetWCurr()) - ThetaDrv.Get();
807 
808  /* Constraint equations are divided by dCoef
809  * ONLY if the constraint is on position */
810  ASSERT(dCoef != 0.);
811 
812  /* Position constraint: */
813  for (unsigned iCnt = 0; iCnt < nPosConstraints; iCnt++) {
814  WorkVec.PutCoef(1 + iPosEqIndex[iCnt],
815  -(XDelta(iPosIncid[iCnt])/dCoef));
816  }
817 
818  /* Rotation constraints: */
819  for (unsigned iCnt = 0; iCnt < nRotConstraints; iCnt++) {
820  WorkVec.PutCoef(1 + iRotEqIndex[iCnt],
821  -(ThetaDelta(iRotIncid[iCnt])/dCoef));
822  }
823 
824  /* Linear Velocity Constraint */
825  for (unsigned iCnt = 0; iCnt < nVelConstraints; iCnt++) {
826  WorkVec.PutCoef(1 + iVelEqIndex[iCnt],
827  -(VDelta(iVelIncid[iCnt])));
828  }
829 
830  /* Angular Velocity Constraint */
831  for (unsigned iCnt = 0; iCnt < nAgvConstraints; iCnt++) {
832  WorkVec.PutCoef(1 + iAgvEqIndex[iCnt],
833  -(WDelta(iAgvIncid[iCnt])));
834  }
835 
836  return WorkVec;
837 }
838 
839 void
840 TotalEquation::Output(OutputHandler& OH) const
841 {
842  if (bToBeOutput()) {
843  const Vec3& X1(pNode1->GetXCurr());
844  const Vec3& X2(pNode2->GetXCurr());
845  const Mat3x3& R1(pNode1->GetRCurr());
846  const Mat3x3& R2(pNode2->GetRCurr());
847  const Vec3& V1(pNode1->GetVCurr());
848  const Vec3& V2(pNode2->GetVCurr());
849  const Vec3& Omega1(pNode1->GetWCurr());
850  const Vec3& Omega2(pNode2->GetWCurr());
851 
852  Mat3x3 R1Tmp(R1*R1h);
853  Mat3x3 R1rTmp(R1*R1hr);
854  Mat3x3 R2rTmp(R2*R2hr);
855  Mat3x3 RTmp(R1rTmp.MulTM(R2rTmp));
856  Vec3 b2(R2*f2);
857  Vec3 b1(X2 + b2 - X1);
858  Vec3 X(R1Tmp.MulTV(b1) - tilde_f1);
859 
860  Joint::Output(OH.Joints(), "TotalEquation", GetLabel(),
861  Zero3, Zero3, Zero3, Zero3)
862  << " " << X
863  << " " << RotManip::VecRot(RTmp)
864  << " " << R1Tmp.MulTV(V2 + Omega2.Cross(b2) - V1 - Omega1.Cross(b1))
865  << " " << R1rTmp.MulTV(Omega2 - Omega1)
866  << std::endl;
867  // accelerations?
868  }
869 }
870 
871 /* Contributo allo jacobiano durante l'assemblaggio iniziale */
872 VariableSubMatrixHandler&
873 TotalEquation::InitialAssJac(VariableSubMatrixHandler& WorkMat,
874  const VectorHandler& XCurr)
875 {
876 
877  /* Per ora usa la matrice piena; eventualmente si puo'
878  * passare a quella sparsa quando si ottimizza */
879  FullSubMatrixHandler& WM = WorkMat.SetFull();
880 
881  /* Dimensiona e resetta la matrice di lavoro */
882  integer iNumRows = 0;
883  integer iNumCols = 0;
884  InitialWorkSpaceDim(&iNumRows, &iNumCols);
885  WM.ResizeReset(iNumRows, iNumCols);
886 
887  /* Equazioni: vedi joints.dvi */
888 
889  /* equazioni ed incognite
890  * F1 Delta_x1 1
891  * M1 Delta_g1 3 + 1
892  * FP1 Delta_xP1 6 + 1
893  * MP1 Delta_w1 9 + 1
894  * F2 Delta_x2 12 + 1
895  * M2 Delta_g2 15 + 1
896  * FP2 Delta_xP2 18 + 1
897  * MP2 Delta_w2 21 + 1
898  * vincolo spostamento Delta_F 24 + 1
899  * vincolo rotazione Delta_M 24 + nPosConstraints
900  * derivata vincolo spostamento Delta_FP 24 + nConstraints
901  * derivata vincolo rotazione Delta_MP 24 + nConstraints + nPosConstraints
902  */
903 
904 
905  /* Indici */
906  integer iNode1FirstPosIndex = pNode1->iGetFirstPositionIndex();
907  integer iNode1FirstVelIndex = iNode1FirstPosIndex + 6;
908  integer iNode2FirstPosIndex = pNode2->iGetFirstPositionIndex();
909  integer iNode2FirstVelIndex = iNode2FirstPosIndex + 6;
910  integer iFirstReactionIndex = iGetFirstIndex();
911  integer iReactionPrimeIndex = iFirstReactionIndex + nConstraints;
912 
913 
914  /* Setta gli indici dei nodi */
915  for (int iCnt = 1; iCnt <= 6; iCnt++) {
916  WM.PutColIndex(iCnt, iNode1FirstPosIndex + iCnt);
917  WM.PutColIndex(6 + iCnt, iNode1FirstVelIndex + iCnt);
918  WM.PutColIndex(12 + iCnt, iNode2FirstPosIndex + iCnt);
919  WM.PutColIndex(18 + iCnt, iNode2FirstVelIndex + iCnt);
920  }
921 
922  /* Setta gli indici delle reazioni */
923  for (unsigned int iCnt = 1; iCnt <= nConstraints; iCnt++) {
924  WM.PutRowIndex(24 + iCnt, iFirstReactionIndex + iCnt);
925  WM.PutRowIndex(24 + nConstraints + iCnt, iReactionPrimeIndex + iCnt);
926  }
927 
928  /* Recupera i dati che servono */
929  Mat3x3 R1(pNode1->GetRRef()*R1h);
930  Mat3x3 R1r(pNode1->GetRRef()*R1hr);
931 
932  Vec3 b2(pNode2->GetRCurr()*f2);
933  Vec3 b1(pNode2->GetXCurr() + b2 - pNode1->GetXCurr());
934 
935  Vec3 Omega1(pNode1->GetWCurr());
936  Vec3 Omega2(pNode2->GetWCurr());
937 
938  Vec3 b1Prime(pNode2->GetVCurr() + Omega2.Cross(b2) - pNode1->GetVCurr());
939 
940  /* Constraints: Add only active rows/columns*/
941 
942  /* Positions contribution:*/
943 
944  Mat3x3 b1Cross_R1(b1.Cross(R1)); // = [ b1 x ] * R1
945  Mat3x3 b2Cross_R1(b2.Cross(R1)); // = [ b2 x ] * R1
946 
947  for (unsigned iCnt = 0 ; iCnt < nPosConstraints; iCnt++) {
948  Vec3 vR1(R1.GetVec(iPosIncid[iCnt]));
949  Vec3 vb1Cross_R1(b1Cross_R1.GetVec(iPosIncid[iCnt]));
950  Vec3 vb2Cross_R1(b2Cross_R1.GetVec(iPosIncid[iCnt]));
951 
952  /* Constraint, node 1 */
953  WM.SubT(24 + 1 + iCnt, 1, vR1); // * Delta_x1
954  WM.SubT(24 + 1 + iCnt, 3 + 1, vb1Cross_R1); // * Delta_g1
955 
956  /* d/dt(Constraint), node 1 */
957  WM.SubT(24 + 1 + nConstraints + iCnt, 6 + 1, vR1); // * Delta_xP1
958  WM.SubT(24 + 1 + nConstraints + iCnt, 9 + 1, vb1Cross_R1); // * Delta_W1
959 
960  /* Constraint, node 2 */
961  WM.AddT(24 + 1 + iCnt, 12 + 1, vR1); // * Delta_x2
962  WM.AddT(24 + 1 + iCnt, 15 + 1, vb2Cross_R1); // * Delta_g2
963 
964  /* d/dt(Constraint), node 2 */
965  WM.AddT(24 + 1 + nConstraints + iCnt, 18 + 1, vR1); // * Delta_xP2
966  WM.AddT(24 + 1 + nConstraints + iCnt, 21 + 1, vb2Cross_R1); // * Delta_W2
967  }
968 
969  for (unsigned iCnt = 0 ; iCnt < nRotConstraints; iCnt++) {
970  Vec3 vR1(R1r.GetVec(iRotIncid[iCnt]));
971 
972  /* Constraint, node 1 */
973  WM.SubT(24 + 1 + nPosConstraints + iCnt, 3 + 1, vR1); // * Delta_g1
974 
975  /* d/dt(Constraint), node 1 */
976  WM.SubT(24 + 1 + nConstraints + nPosConstraints + iCnt, 9 + 1, vR1); // * Delta_W1
977 
978  /* Constraint, node 2 */
979  WM.AddT(24 + 1 + nPosConstraints + iCnt, 15 + 1, vR1); // * Delta_g2
980 
981  /* d/dt(Constraint), node 2 */
982  WM.AddT(24 + 1 + nConstraints + nPosConstraints + iCnt, 21 + 1, vR1); // * Delta_W2
983  }
984 
985  return WorkMat;
986 }
987 
988 
989 /* Contributo al residuo durante l'assemblaggio iniziale */
990 SubVectorHandler&
991 TotalEquation::InitialAssRes(SubVectorHandler& WorkVec,
992  const VectorHandler& XCurr)
993 {
994 
995  DEBUGCOUT("Entering TotalEquation::InitialAssRes()" << std::endl);
996 
997  /* Dimensiona e resetta la matrice di lavoro */
998  integer iNumRows = 0;
999  integer iNumCols = 0;
1000  InitialWorkSpaceDim(&iNumRows, &iNumCols);
1001  WorkVec.ResizeReset(iNumRows);
1002 
1003  /* Indici */
1004  integer iNode1FirstPosIndex = pNode1->iGetFirstPositionIndex();
1005  integer iNode1FirstVelIndex = iNode1FirstPosIndex + 6;
1006  integer iNode2FirstPosIndex = pNode2->iGetFirstPositionIndex();
1007  integer iNode2FirstVelIndex = iNode2FirstPosIndex + 6;
1008  integer iFirstReactionIndex = iGetFirstIndex();
1009  integer iReactionPrimeIndex = iFirstReactionIndex + nConstraints;
1010 
1011  /* Setta gli indici */
1012  for (int iCnt = 1; iCnt <= 6; iCnt++) {
1013  WorkVec.PutRowIndex(iCnt, iNode1FirstPosIndex+iCnt);
1014  WorkVec.PutRowIndex(6 + iCnt, iNode1FirstVelIndex + iCnt);
1015  WorkVec.PutRowIndex(12 + iCnt, iNode2FirstPosIndex + iCnt);
1016  WorkVec.PutRowIndex(18 + iCnt, iNode2FirstVelIndex + iCnt);
1017  }
1018 
1019  for (unsigned int iCnt = 1; iCnt <= nConstraints; iCnt++) {
1020  WorkVec.PutRowIndex(24 + iCnt, iFirstReactionIndex + iCnt);
1021  WorkVec.PutRowIndex(24 + nConstraints + iCnt, iReactionPrimeIndex + iCnt);
1022  }
1023 
1024  /* Recupera i dati */
1025  /* Recupera i dati che servono */
1026  Mat3x3 R1(pNode1->GetRRef()*R1h);
1027  Mat3x3 R1r(pNode1->GetRRef()*R1hr);
1028  Mat3x3 R2r(pNode2->GetRCurr()*R2hr);
1029 
1030  Vec3 b2(pNode2->GetRCurr()*f2);
1031  Vec3 b1(pNode2->GetXCurr() + b2 - pNode1->GetXCurr());
1032 
1033  Vec3 Omega1(pNode1->GetWCurr());
1034  Vec3 Omega2(pNode2->GetWCurr());
1035 
1036  Vec3 b1Prime(pNode2->GetVCurr() + Omega2.Cross(b2) - pNode1->GetVCurr());
1037 
1038 
1039  /* Constraint Equations */
1040 
1041  Vec3 XDelta = R1.MulTV(b1) - tilde_f1 - XDrv.Get();
1042  Vec3 XDeltaPrime = R1.MulTV(b1Prime + b1.Cross(Omega1));
1043 
1044  if(XDrv.bIsDifferentiable()) {
1045  XDeltaPrime -= XDrv.GetP();
1046  }
1047 
1048  Mat3x3 R0T = RotManip::Rot(-ThetaDrv.Get()); // -Theta0 to get R0 transposed
1049  Mat3x3 RDelta = R1r.MulTM(R2r*R0T);
1050  ThetaDelta = RotManip::VecRot(RDelta);
1051  Vec3 ThetaDeltaPrime = R1r.MulTV(Omega2 - Omega1);
1052 
1053  if(ThetaDrv.bIsDifferentiable()) {
1054  ThetaDeltaPrime -= RDelta*ThetaDrv.GetP();
1055  }
1056 
1057 
1058  /* Position constraint: */
1059  for (unsigned iCnt = 0; iCnt < nPosConstraints; iCnt++) {
1060  WorkVec.PutCoef(24 + 1 + iCnt, -XDelta(iPosIncid[iCnt]));
1061  WorkVec.PutCoef(24 + 1 + nConstraints + iCnt, -XDeltaPrime(iPosIncid[iCnt]));
1062  }
1063 
1064  /* Rotation constraints: */
1065  for (unsigned iCnt = 0; iCnt < nRotConstraints; iCnt++) {
1066  WorkVec.PutCoef(24 + 1 + nPosConstraints + iCnt, -ThetaDelta(iRotIncid[iCnt]));
1067  WorkVec.PutCoef(24 + 1 + nPosConstraints + nConstraints + iCnt, -ThetaDeltaPrime(iRotIncid[iCnt]));
1068  }
1069 
1070 return WorkVec;
1071 }
1072 
1073 
1074 unsigned int
1075 TotalEquation::iGetNumPrivData(void) const
1076 {
1077  return 24;
1078 }
1079 
1080 unsigned int
1081 TotalEquation::iGetPrivDataIdx(const char *s) const
1082 {
1083  ASSERT(s != NULL);
1084 
1085  unsigned int off = 0;
1086 
1087  switch (s[0]) {
1088  case 'p':
1089  /* relative position */
1090  break;
1091 
1092  case 'r':
1093  /* relative orientation */
1094  off += 3;
1095  break;
1096 
1097  case 'F':
1098  /* force */
1099  off += 6;
1100  break;
1101 
1102  case 'M':
1103  /* moment */
1104  off += 9;
1105  break;
1106 
1107  case 'd':
1108  /* imposed relative position */
1109  off += 12;
1110  break;
1111 
1112  case 't':
1113  /* imposed relative orientation */
1114  off += 15;
1115  break;
1116 
1117  case 'v':
1118  /* relative linear velocity */
1119  off += 18;
1120  break;
1121 
1122  case 'w':
1123  /* relative angular velocity */
1124  off += 21;
1125  break;
1126 
1127 
1128  default:
1129  return 0;
1130  }
1131 
1132  switch (s[1]) {
1133  case 'x':
1134  return off + 1;
1135 
1136  case 'y':
1137  return off + 2;
1138 
1139  case 'z':
1140  return off + 3;
1141  }
1142 
1143  return 0;
1144 }
1145 
1146 doublereal
1147 TotalEquation::dGetPrivData(unsigned int i) const
1148 {
1149  switch (i) {
1150  case 1:
1151  case 2:
1152  case 3: {
1153  Vec3 x(pNode1->GetRCurr().MulTV(
1154  pNode2->GetXCurr() + pNode2->GetRCurr()*f2
1155  - pNode1->GetXCurr()) - f1);
1156  return R1h.GetVec(i)*x;
1157  }
1158 
1159  case 4:
1160  case 5:
1161  case 6: {
1162  Vec3 Theta(Unwrap(ThetaDeltaPrev, ThetaDelta) + ThetaDrv.Get());
1163  return Theta(i - 3);
1164  }
1165 
1166  case 7:
1167  case 8:
1168  case 9:
1169  return 0.;
1170 
1171  case 10:
1172  case 11:
1173  case 12:
1174  return 0.;
1175 
1176  case 13:
1177  case 14:
1178  case 15:
1179  if (!bPosActive[i - 13]) {
1180  return 0.;
1181  }
1182  return XDrv.Get()(i - 12);
1183 
1184  case 16:
1185  case 17:
1186  case 18:
1187  if (!bRotActive[i - 16]) {
1188  return 0.;
1189  }
1190  return ThetaDrv.Get()(i - 15);
1191  case 19:
1192  case 20:
1193  case 21:
1194  // FIXME: blind fix
1195  {
1196  Vec3 v(pNode1->GetRCurr().MulTV(
1197  (pNode2->GetVCurr() + pNode2->GetWCurr().Cross(pNode2->GetRCurr()*f2)
1198  - pNode1->GetVCurr()) -
1199  pNode1->GetWCurr().Cross(pNode2->GetXCurr() +
1200  pNode2->GetRCurr()*f2
1201  - pNode1->GetXCurr() - f1)
1202  )
1203  );
1204 
1205  return R1h.GetVec(i-18)*v;
1206  }
1207  case 22:
1208  case 23:
1209  case 24:
1210  {
1211  Vec3 W(pNode1->GetRCurr().MulTV(pNode2->GetWCurr() - pNode1->GetWCurr()))
1212  ;
1213 
1214  return R1hr.GetVec(i-21)*W;
1215  }
1216 
1217 
1218 
1219  default:
1220  ASSERT(0);
1221  }
1222 
1223  return 0.;
1224 }
1225 
1226 
1227 /* TotalEquation - end */
1228 
1229 
1230 /* TotalReaction - begin */
1231 
1232 TotalReaction::TotalReaction(unsigned int uL, const DofOwner *pDO,
1233  bool bPos[3],
1234  bool bRot[3],
1235  const StructNode *pN1,
1236  const Vec3& f1Tmp, const Mat3x3& R1hTmp, const Mat3x3& R1hrTmp,
1237  const StructNode *pN2,
1238  const Vec3& f2Tmp, const Mat3x3& R2hTmp, const Mat3x3& R2hrTmp,
1239  TotalEquation* t_elm,
1240  flag fOut)
1241 : Elem(uL, fOut),
1242 Joint(uL, pDO, fOut),
1243 pNode1(pN1), pNode2(pN2),
1244 f1(f1Tmp), R1h(R1hTmp), R1hr(R1hrTmp),
1245 f2(f2Tmp), R2h(R2hTmp), R2hr(R2hrTmp),
1246 nConstraints(0), nPosConstraints(0), nRotConstraints(0),
1247 tilde_f1(R1h.MulTV(f1)),
1248 M(Zero3), F(Zero3), ThetaDelta(Zero3), ThetaDeltaPrev(Zero3),
1249 total_equation_element(t_elm)
1250 {
1251  /* Equations 1->3: Positions
1252  * Equations 4->6: Rotations */
1253 
1254  for (unsigned int i = 0; i < 3; i++) {
1255  bPosActive[i] = bPos[i];
1256  bRotActive[i] = bRot[i];
1257  if (bPosActive[i]) {
1258  iPosIncid[nPosConstraints] = i + 1;
1259  nPosConstraints++;
1260  }
1261  if (bRotActive[i]) {
1262  iRotIncid[nRotConstraints] = i + 1;
1263  nRotConstraints++;
1264  }
1265  }
1266  nConstraints = nPosConstraints + nRotConstraints;
1267  if (nConstraints != total_equation_element->nConstraints) {
1268  silent_cerr("Error: TotalReaction element " <<
1269  GetLabel() << " has " << nConstraints <<
1270  " reactions, while the corresponding TotalEquation joint "
1271  << total_equation_element->GetLabel() <<
1272  " defines only " << total_equation_element->nConstraints <<
1273  " contraints" << std::endl);
1274  throw ErrGeneric(MBDYN_EXCEPT_ARGS);
1275  }
1276 }
1277 
1278 TotalReaction::~TotalReaction(void)
1279 {
1280  NO_OP;
1281 };
1282 
1283 std::ostream&
1284 TotalReaction::DescribeDof(std::ostream& out,
1285  const char *prefix, bool bInitial) const
1286 {
1287  integer iIndex = total_equation_element->iGetFirstIndex();
1288 
1289  if (nPosConstraints > 0) {
1290  out << prefix << iIndex + 1;
1291  if (nPosConstraints > 1) {
1292  out << "->" << iIndex + nPosConstraints;
1293  }
1294  out << ": ";
1295  out << "reaction force(s) [";
1296 
1297  for (unsigned int i = 0, cnt = 0; i < 3; i++) {
1298  if (bPosActive[i]) {
1299  cnt++;
1300  if (cnt > 1) {
1301  out << ",";
1302  }
1303  out << "F" << idx2xyz[i];
1304  }
1305  }
1306  out << "]" << std::endl;
1307  }
1308 
1309  if (nRotConstraints > 0) {
1310  out << prefix << iIndex + nPosConstraints + 1;
1311  if (nRotConstraints > 1) {
1312  out << "->" << iIndex + nConstraints ;
1313  }
1314  out << ": ";
1315  out << "reaction couple(s) [";
1316 
1317  for (unsigned int i = 0, cnt = 0; i < 3; i++) {
1318  if (bRotActive[i]) {
1319  cnt++;
1320  if (cnt > 1) {
1321  out << ",";
1322  }
1323  out << "m" << idx2xyz[i];
1324  }
1325  }
1326  out << "]" << std::endl;
1327  }
1328 
1329  if (bInitial) {
1330  iIndex += nConstraints;
1331 
1332  if (nPosConstraints > 0) {
1333  out << prefix << iIndex + 1;
1334  if (nPosConstraints > 1) {
1335  out << "->" << iIndex + nPosConstraints;
1336  }
1337  out << ": ";
1338  out << "reaction force(s) derivative(s) [";
1339 
1340  for (unsigned int i = 0, cnt = 0; i < 3; i++) {
1341  if (bPosActive[i]) {
1342  cnt++;
1343  if (cnt > 1) {
1344  out << ",";
1345  }
1346  out << "FP" << idx2xyz[i];
1347  }
1348  }
1349  out << "]" << std::endl;
1350  }
1351 
1352  if (nRotConstraints > 0) {
1353  out << prefix << iIndex + nPosConstraints + 1;
1354  if (nRotConstraints > 1) {
1355  out << "->" << iIndex + nConstraints;
1356  }
1357  out << ": ";
1358  out << "reaction couple(s) derivative(s) [";
1359 
1360  for (unsigned int i = 0, cnt = 0; i < 3; i++) {
1361  if (bRotActive[i]) {
1362  cnt++;
1363  if (cnt > 1) {
1364  out << ",";
1365  }
1366  out << "mP" << idx2xyz[i];
1367  }
1368  }
1369  out << "]" << std::endl;
1370  }
1371  }
1372  return out;
1373 }
1374 
1375 void
1376 TotalReaction::DescribeDof(std::vector<std::string>& desc,
1377  bool bInitial, int i) const
1378 {
1379  desc.resize(0);
1380 }
1381 
1382 void
1383 TotalReaction::SetValue(DataManager *pDM,
1384  VectorHandler& X, VectorHandler& XP,
1385  SimulationEntity::Hints *ph)
1386 {
1387  if (ph) {
1388  for (unsigned int i = 0; i < ph->size(); i++) {
1389  Joint::JointHint *pjh = dynamic_cast<Joint::JointHint *>((*ph)[i]);
1390 
1391  if (pjh) {
1392 
1393  if (dynamic_cast<Joint::OffsetHint<1> *>(pjh)) {
1394  const Mat3x3& R1(pNode1->GetRCurr());
1395  Vec3 fTmp2(pNode2->GetRCurr()*f2);
1396 
1397  f1 = R1.MulTV(pNode2->GetXCurr() + fTmp2 - pNode1->GetXCurr());
1398  tilde_f1 = R1h.MulTV(f1);
1399 
1400 
1401  } else if (dynamic_cast<Joint::OffsetHint<2> *>(pjh)) {
1402  const Mat3x3& R2(pNode2->GetRCurr());
1403  Vec3 fTmp1(pNode1->GetRCurr()*f1);
1404 
1405  f2 = R2.MulTV(pNode1->GetXCurr() + fTmp1 - pNode2->GetXCurr());
1406 
1407  } else if (dynamic_cast<Joint::HingeHint<1> *>(pjh)) {
1408  if (dynamic_cast<Joint::PositionHingeHint<1> *>(pjh)) {
1409  const Mat3x3& R1(pNode1->GetRCurr());
1410  R1h = R1.MulTM(pNode2->GetRCurr()*R2h);
1411 
1412  } else if (dynamic_cast<Joint::OrientationHingeHint<1> *>(pjh)) {
1413  const Mat3x3& R1(pNode1->GetRCurr());
1414  R1hr = R1.MulTM(pNode2->GetRCurr()*R2hr);
1415  }
1416 
1417  } else if (dynamic_cast<Joint::HingeHint<2> *>(pjh)) {
1418  if (dynamic_cast<Joint::PositionHingeHint<2> *>(pjh)) {
1419  const Mat3x3& R2(pNode2->GetRCurr());
1420  R2h = R2.MulTM(pNode1->GetRCurr()*R1h);
1421 
1422  } else if (dynamic_cast<Joint::OrientationHingeHint<2> *>(pjh)) {
1423  const Mat3x3& R2(pNode2->GetRCurr());
1424  R2hr = R2.MulTM(pNode1->GetRCurr()*R1hr);
1425  }
1426 
1427 #if 0
1428  } else if (dynamic_cast<Joint::JointDriveHint<Vec3> *>(pjh)) {
1429  Joint::JointDriveHint<Vec3> *pjdh
1430  = dynamic_cast<Joint::JointDriveHint<Vec3> *>(pjh);
1431  pedantic_cout("TotalReaction(" << uLabel << "): "
1432  "creating drive from hint[" << i << "]..." << std::endl);
1433 
1434  TplDriveCaller<Vec3> *pDC = pjdh->pTDH->pCreateDrive(pDM);
1435  if (pDC == 0) {
1436  silent_cerr("TotalReaction(" << uLabel << "): "
1437  "unable to create drive "
1438  "after hint #" << i << std::endl);
1439  throw ErrGeneric(MBDYN_EXCEPT_ARGS);
1440  }
1441 
1442  if (dynamic_cast<Joint::PositionDriveHint<Vec3> *>(pjdh)) {
1443  XDrv.Set(pDC);
1444 
1445  } else if (dynamic_cast<Joint::VelocityDriveHint<Vec3> *>(pjdh)) {
1446  XPDrv.Set(pDC);
1447 
1448  } else if (dynamic_cast<Joint::AccelerationDriveHint<Vec3> *>(pjdh)) {
1449  XPPDrv.Set(pDC);
1450 
1451  } else if (dynamic_cast<Joint::OrientationDriveHint<Vec3> *>(pjdh)) {
1452  ThetaDrv.Set(pDC);
1453 
1454  } else if (dynamic_cast<Joint::AngularVelocityDriveHint<Vec3> *>(pjdh)) {
1455  OmegaDrv.Set(pDC);
1456 
1457  } else if (dynamic_cast<Joint::AngularAccelerationDriveHint<Vec3> *>(pjdh)) {
1458  OmegaPDrv.Set(pDC);
1459 
1460  } else {
1461  delete pDC;
1462  }
1463 #endif
1464 
1465  } else if (dynamic_cast<Joint::ReactionsHint *>(pjh)) {
1466  /* TODO */
1467  }
1468 
1469  continue;
1470  }
1471  }
1472  }
1473 }
1474 
1475 Hint *
1476 TotalReaction::ParseHint(DataManager *pDM, const char *s) const
1477 {
1478  if (strncasecmp(s, "offset{" /*}*/ , STRLENOF("offset{" /*}*/ )) == 0)
1479  {
1480  s += STRLENOF("offset{" /*}*/ );
1481 
1482  if (strcmp(&s[1], /*{*/ "}") != 0) {
1483  return 0;
1484  }
1485 
1486  switch (s[0]) {
1487  case '1':
1488  return new Joint::OffsetHint<1>;
1489 
1490  case '2':
1491  return new Joint::OffsetHint<2>;
1492  }
1493 
1494  } else if (strncasecmp(s, "position-hinge{" /*}*/, STRLENOF("position-hinge{" /*}*/)) == 0) {
1495  s += STRLENOF("position-hinge{" /*}*/);
1496 
1497  if (strcmp(&s[1], /*{*/ "}") != 0) {
1498  return 0;
1499  }
1500 
1501  switch (s[0]) {
1502  case '1':
1503  return new Joint::HingeHint<1>;
1504 
1505  case '2':
1506  return new Joint::HingeHint<2>;
1507  }
1508 
1509  } else if (strncasecmp(s, "position-drive3{" /*}*/, STRLENOF("position-drive3{" /*}*/)) == 0) {
1510  s += STRLENOF("position-");
1511 
1512  Hint *pH = ::ParseHint(pDM, s);
1513  if (pH) {
1514  TplDriveHint<Vec3> *pTDH = dynamic_cast<TplDriveHint<Vec3> *>(pH);
1515  if (pTDH) {
1516  return new PositionDriveHint<Vec3>(pTDH);
1517  }
1518  }
1519  return 0;
1520 
1521  } else if (strncasecmp(s, "orientation-hinge{" /*}*/, STRLENOF("orientation-hinge{" /*}*/)) == 0) {
1522  s += STRLENOF("orientation-hinge{" /*}*/);
1523 
1524  if (strcmp(&s[1], /*{*/ "}") != 0) {
1525  return 0;
1526  }
1527 
1528  switch (s[0]) {
1529  case '1':
1530  return new Joint::HingeHint<1>;
1531 
1532  case '2':
1533  return new Joint::HingeHint<2>;
1534  }
1535 
1536  } else if (strncasecmp(s, "orientation-drive3{" /*}*/, STRLENOF("orientation-drive3{" /*}*/)) == 0) {
1537  s += STRLENOF("orientation-");
1538 
1539  Hint *pH = ::ParseHint(pDM, s);
1540  if (pH) {
1541  TplDriveHint<Vec3> *pTDH = dynamic_cast<TplDriveHint<Vec3> *>(pH);
1542  if (pTDH) {
1543  return new OrientationDriveHint<Vec3>(pTDH);
1544  }
1545  }
1546  return 0;
1547  }
1548 
1549  return 0;
1550 }
1551 
1552 void
1553 TotalReaction::AfterConvergence(const VectorHandler& /* X */ ,
1554  const VectorHandler& /* XP */ )
1555 {
1556  ThetaDeltaPrev = Unwrap(ThetaDeltaPrev, ThetaDelta);
1557 }
1558 
1559 void
1560 TotalReaction::AfterConvergence(const VectorHandler& /* X */ ,
1561  const VectorHandler& /* XP */, const VectorHandler& /* XP */)
1562 {
1563  ThetaDeltaPrev = Unwrap(ThetaDeltaPrev, ThetaDelta);
1564 }
1565 
1566 /* Contributo al file di restart */
1567 std::ostream&
1568 TotalReaction::Restart(std::ostream& out) const
1569 {
1570  Joint::Restart(out) << ", total equation, "
1571  << pNode1->GetLabel() << ", "
1572  << "position, ";
1573  f1.Write(out, ", ") << ", "
1574  << "position orientation, "
1575  "1 , ";
1576  R1h.GetVec(1).Write(out, ", ") << ", "
1577  "2 , ";
1578  R1h.GetVec(2).Write(out, ", ") << ", "
1579  << "rotation orientation, "
1580  "1 , ";
1581  R1hr.GetVec(1).Write(out, ", ") << ", "
1582  "2 , ";
1583  R1hr.GetVec(2).Write(out, ", ") << ", "
1584  << pNode2->GetLabel() << ", "
1585  << "position, ";
1586  f2.Write(out, ", ") << ", "
1587  << "position orientation, "
1588  "1 , ";
1589  R2h.GetVec(1).Write(out, ", ") << ", "
1590  "2 , ";
1591  R2h.GetVec(2).Write(out, ", ") << ", "
1592  << "rotation orientation, "
1593  "1 , ";
1594  R2hr.GetVec(1).Write(out, ", ") << ", "
1595  "2 , ";
1596  R2hr.GetVec(2).Write(out, ", ");
1597 
1598  if (bPosActive[0] || bPosActive[1] || bPosActive[2]) {
1599  out << ", position constraint";
1600  for (unsigned i = 0; i < 3; i++) {
1601  if (bPosActive[i]) {
1602  out << ", active";
1603  } else {
1604  out << ", inactive";
1605  }
1606  }
1607  }
1608 
1609  if (bRotActive[0] || bRotActive[1] || bRotActive[2]) {
1610  out << ", orientation constraint";
1611  for (unsigned i = 0; i < 3; i++) {
1612  if (bRotActive[i]) {
1613  out << ", active";
1614  } else {
1615  out << ", inactive";
1616  }
1617  }
1618  }
1619 
1620  return out << std::endl;
1621 }
1622 
1623 /* Assemblaggio jacobiano */
1624 VariableSubMatrixHandler&
1625 TotalReaction::AssJac(VariableSubMatrixHandler& WorkMat,
1626  doublereal dCoef,
1627  const VectorHandler& /* XCurr */ ,
1628  const VectorHandler& /* XPrimeCurr */ )
1629 {
1630  /*
1631  * Constraint Equations:
1632  * Position: R1^T(x2 + R2*f2 -x1 - R1*f1) - d = x^delta
1633  * ==> d(Vec3) = imposed displacement in node 1 local R.F.
1634  * ==> x^delta is used to activate/deactivate the constraint
1635  * equation along the corresponding direction. If each
1636  * component is set to 0, all relative displacement
1637  * are forbidden (or imposed by the drive). If a component
1638  * of x^delta is left free, the corrsponding equation is
1639  * dropped
1640  *
1641  * Orientation: Theta - Theta0 = ax(exp^-1(R1^T * R2 * R0^T)) = Theta^delta
1642  * ==> Theta = ax(exp^-1(R1^T * R2)) = Relative orientation in node1 R.F.
1643  * ==> Theta0 = Imposed relative orientation = ax(exp^-1(R0))
1644  * ==> Theta^delta is used to activate/deactivate the constraint
1645  * equation along the corresponding direction. If each
1646  * component is set to 0, all relative rotation
1647  * are forbidden (or imposed by the drive). If a component
1648  * of Theta^delta is left free, the corrsponding equation is
1649  * dropped
1650  *Jacobian Matrix:
1651  * x1 g1 x2 g2 F M
1652  * Q1 | 0 F1X 0 0 -R1 0 | | x1 |
1653  * G1 |-(F1)X (b1)X(F1)X+(M1)X (F1)X -(F1)X(b2)X (b1)X(R1) -R1r | | g1 |
1654  * Q2 | 0 -F1X 0 0 R1 0 | | x2 |
1655  * G2 | 0 -(b2)X(F1)X-(M1)X 0 (F1)X(b2)X (b2)X(R1) R1r | | g2 |
1656  * F |-c*R1^T c*R1^T*[(b1)X] c*R1^T -c*R1^T*[(b2)X] 0 0 | | F |if(bPos)
1657  * M | 0 -c*R1r^T 0 c* R1r^T 0 0 | | M |if(bRot)
1658  * if(bPos) if(bRot)
1659  *with: _ b1 = (x2 + R2*f2 - x1)
1660  * _ b2 = (R2*f2)
1661  * _ R1 = R1*R1h
1662  * _ R2 = R2*R2h
1663  * _ R1r = R1*R1hr
1664  * _ F1 = R1*F
1665  * _ M1 = R1*M
1666  * _ X = "Cross" operator
1667  *
1668  * */
1669 
1670  DEBUGCOUT("Entering TotalReaction::AssJac()" << std::endl);
1671 
1672  FullSubMatrixHandler& WM = WorkMat.SetFull();
1673 
1674  /* Ridimensiona la sottomatrice in base alle esigenze */
1675  integer iNumRows = 0;
1676  integer iNumCols = 0;
1677  WorkSpaceDim(&iNumRows, &iNumCols);
1678  WM.ResizeReset(iNumRows, iNumCols);
1679 
1680  /* Recupera gli indici delle varie incognite */
1681  integer iNode1FirstPosIndex = pNode1->iGetFirstPositionIndex();
1682  integer iNode1FirstMomIndex = pNode1->iGetFirstMomentumIndex();
1683  integer iNode2FirstPosIndex = pNode2->iGetFirstPositionIndex();
1684  integer iNode2FirstMomIndex = pNode2->iGetFirstMomentumIndex();
1685  integer iFirstReactionIndex = total_equation_element->iGetFirstIndex();
1686 
1687  /* Setta gli indici delle equazioni */
1688  for (int iCnt = 1; iCnt <= 6; iCnt++) {
1689  WM.PutRowIndex(iCnt, iNode1FirstMomIndex + iCnt);
1690  WM.PutColIndex(iCnt, iNode1FirstPosIndex + iCnt);
1691  WM.PutRowIndex(6 + iCnt, iNode2FirstMomIndex + iCnt);
1692  WM.PutColIndex(6 + iCnt, iNode2FirstPosIndex + iCnt);
1693  }
1694 
1695  for (unsigned int iCnt = 1; iCnt <= nConstraints; iCnt++) {
1696  WM.PutColIndex(12 + iCnt, iFirstReactionIndex + iCnt);
1697  }
1698 
1699  /* Recupera i dati che servono */
1700  Mat3x3 R1(pNode1->GetRRef()*R1h);
1701  Mat3x3 R1r(pNode1->GetRRef()*R1hr);
1702  Vec3 b2(pNode2->GetRCurr()*f2);
1703  Vec3 b1(pNode2->GetXCurr() + b2 - pNode1->GetXCurr());
1704 
1705  /* Moltiplica il momento e la forza per il coefficiente del metodo */
1706  Vec3 FTmp(R1*(F*dCoef));
1707  Vec3 MTmp(R1r*(M*dCoef));
1708 
1709  /* Equilibrium: ((Phi/q)^T*Lambda)/q */
1710 
1711  Mat3x3 Tmp;
1712 
1713  /* [ F x ] */
1714  Tmp = Mat3x3(MatCross, FTmp);
1715 
1716  /* Lines 1->3: */
1717  WM.Add(1, 3 + 1, Tmp);
1718 
1719  /* Lines 4->6: */
1720  WM.Sub(3 + 1, 1, Tmp);
1721 
1722  WM.Add(3 + 1, 6 + 1, Tmp);
1723 
1724  /* Lines 7->9: */
1725  WM.Sub(6 + 1, 3 + 1, Tmp);
1726 
1727  /* [ F x ] [ b2 x ] */
1728  Tmp = Mat3x3(MatCrossCross, FTmp, b2);
1729 
1730  /* Lines 4->6: */
1731  WM.Sub(3 + 1, 9 + 1, Tmp);
1732 
1733  /* Lines 10->12: */
1734  WM.Add(9 + 1, 9 + 1, Tmp);
1735 
1736  /* [ b1 x ] [ F x ] + [ M x ] */
1737 
1738  /* Lines 4->6: */
1739  WM.Add(3 + 1, 3 + 1, Mat3x3(MatCrossCross, b1, FTmp) + Mat3x3(MatCross, MTmp));
1740 
1741  /* [ b2 x ] [ F x ] + [ M x ] */
1742 
1743  /* Lines 10->12: */
1744  WM.Sub(9 + 1, 3 + 1, Mat3x3(MatCrossCross, b2, FTmp) + Mat3x3(MatCross, MTmp));
1745 
1746 /* Phi/q and (Phi/q)^T */
1747 
1748  Mat3x3 b1Cross_R1(b1.Cross(R1)); // = [ b1 x ] * R1
1749  Mat3x3 b2Cross_R1(b2.Cross(R1)); // = [ b2 x ] * R1
1750 
1751  for (unsigned iCnt = 0 ; iCnt < nPosConstraints; iCnt++) {
1752  Vec3 vR1(R1.GetVec(iPosIncid[iCnt]));
1753  Vec3 vb1Cross_R1(b1Cross_R1.GetVec(iPosIncid[iCnt]));
1754  Vec3 vb2Cross_R1(b2Cross_R1.GetVec(iPosIncid[iCnt]));
1755 
1756  /* Equilibrium, node 1 */
1757  WM.Sub(1, 12 + 1 + iCnt, vR1);
1758  WM.Sub(3 + 1, 12 + 1 + iCnt, vb1Cross_R1);
1759 
1760  /* Equilibrium, node 2 */
1761  WM.Add(6 + 1, 12 + 1 + iCnt, vR1);
1762  WM.Add(9 + 1, 12 + 1 + iCnt, vb2Cross_R1);
1763  }
1764 
1765  for (unsigned iCnt = 0 ; iCnt < nRotConstraints; iCnt++) {
1766  Vec3 vR1(R1r.GetVec(iRotIncid[iCnt]));
1767 
1768  /* Equilibrium, node 1 */
1769  WM.Sub(3 + 1, 12 + 1 + nPosConstraints + iCnt, vR1);
1770 
1771  /* Equilibrium, node 2 */
1772  WM.Add(9 + 1, 12 + 1 + nPosConstraints + iCnt, vR1);
1773  }
1774 
1775  return WorkMat;
1776 }
1777 
1778 /* Assemblaggio residuo */
1779 SubVectorHandler&
1780 TotalReaction::AssRes(SubVectorHandler& WorkVec,
1781  doublereal dCoef,
1782  const VectorHandler& XCurr,
1783  const VectorHandler& /* XPrimeCurr */ )
1784 {
1785  DEBUGCOUT("Entering TotalReaction::AssRes()" << std::endl);
1786 
1787  /* Dimensiona e resetta la matrice di lavoro */
1788  integer iNumRows = 0;
1789  integer iNumCols = 0;
1790  WorkSpaceDim(&iNumRows, &iNumCols);
1791  WorkVec.ResizeReset(iNumRows);
1792 
1793  /* Indici */
1794  integer iNode1FirstMomIndex = pNode1->iGetFirstMomentumIndex();
1795  integer iNode2FirstMomIndex = pNode2->iGetFirstMomentumIndex();
1796  integer iFirstReactionIndex = total_equation_element->iGetFirstIndex();
1797 
1798  /* Indici dei nodi */
1799  for (int iCnt = 1; iCnt <= 6; iCnt++) {
1800  WorkVec.PutRowIndex(iCnt, iNode1FirstMomIndex + iCnt);
1801  WorkVec.PutRowIndex(6+iCnt, iNode2FirstMomIndex + iCnt);
1802  }
1803 
1804  /* Get constraint reactions */
1805 
1806  for (unsigned iCnt = 0; iCnt < nPosConstraints; iCnt++) {
1807  F(iPosIncid[iCnt]) = XCurr(iFirstReactionIndex + 1 + iCnt);
1808  }
1809 
1810  for (unsigned iCnt = 0; iCnt < nRotConstraints; iCnt++) {
1811  M(iRotIncid[iCnt]) = XCurr(iFirstReactionIndex + 1 + nPosConstraints + iCnt);
1812  }
1813 
1814 
1815  Vec3 b2(pNode2->GetRCurr()*f2);
1816  Vec3 b1(pNode2->GetXCurr() + b2 - pNode1->GetXCurr());
1817 
1818  Mat3x3 R1 = pNode1->GetRCurr()*R1h;
1819  Mat3x3 R1r = pNode1->GetRCurr()*R1hr;
1820  Mat3x3 R2r = pNode2->GetRCurr()*R2hr;
1821 
1822  Vec3 FTmp(R1*F);
1823  Vec3 MTmp(R1r*M);
1824 
1825  /* Equilibrium, node 1 */
1826  WorkVec.Add(1, FTmp);
1827  WorkVec.Add(3 + 1, MTmp + b1.Cross(FTmp));
1828 
1829  /* Equilibrium, node 2 */
1830  WorkVec.Sub(6 + 1, FTmp);
1831  WorkVec.Sub(9 + 1, MTmp + b2.Cross(FTmp));
1832 
1833  return WorkVec;
1834 }
1835 
1836 DofOrder::Order
1837 TotalReaction::GetEqType(unsigned int i) const
1838 {
1839  ASSERTMSGBREAK(i < iGetNumDof(),
1840  "INDEX ERROR in TotalReaction::GetEqType");
1841 
1842  return DofOrder::ALGEBRAIC;
1843 }
1844 
1845 /* Output (da mettere a punto), per ora solo reazioni */
1846 void
1847 TotalReaction::Output(OutputHandler& OH) const
1848 {
1849  if (bToBeOutput()) {
1850  const Vec3& X1(pNode1->GetXCurr());
1851  const Vec3& X2(pNode2->GetXCurr());
1852  const Mat3x3& R1(pNode1->GetRCurr());
1853  const Mat3x3& R2(pNode2->GetRCurr());
1854  const Vec3& V1(pNode1->GetVCurr());
1855  const Vec3& V2(pNode2->GetVCurr());
1856  const Vec3& Omega1(pNode1->GetWCurr());
1857  const Vec3& Omega2(pNode2->GetWCurr());
1858 
1859  Mat3x3 R1Tmp(R1*R1h);
1860  Mat3x3 R1rTmp(R1*R1hr);
1861  Mat3x3 R2rTmp(R2*R2hr);
1862  Mat3x3 RTmp(R1rTmp.MulTM(R2rTmp));
1863  Vec3 b2(R2*f2);
1864  Vec3 b1(X2 + b2 - X1);
1865  Vec3 X(R1Tmp.MulTV(b1) - tilde_f1);
1866 
1867  Joint::Output(OH.Joints(), "TotalReaction", GetLabel(),
1868  F, M, R1Tmp*F, R1rTmp*M)
1869  << " " << X
1870  << " " << RotManip::VecRot(RTmp)
1871  << " " << R1Tmp.MulTV(V2 + Omega2.Cross(b2) - V1 - Omega1.Cross(b1))
1872  << " " << R1rTmp.MulTV(Omega2 - Omega1)
1873  << std::endl;
1874  // accelerations?
1875  }
1876 }
1877 
1878 /* Contributo allo jacobiano durante l'assemblaggio iniziale */
1879 VariableSubMatrixHandler&
1880 TotalReaction::InitialAssJac(VariableSubMatrixHandler& WorkMat,
1881  const VectorHandler& XCurr)
1882 {
1883 
1884  /* Per ora usa la matrice piena; eventualmente si puo'
1885  * passare a quella sparsa quando si ottimizza */
1886  FullSubMatrixHandler& WM = WorkMat.SetFull();
1887 
1888  /* Dimensiona e resetta la matrice di lavoro */
1889  integer iNumRows = 0;
1890  integer iNumCols = 0;
1891  InitialWorkSpaceDim(&iNumRows, &iNumCols);
1892  WM.ResizeReset(iNumRows, iNumCols);
1893 
1894  /* Equazioni: vedi joints.dvi */
1895 
1896  /* equazioni ed incognite
1897  * F1 Delta_x1 1
1898  * M1 Delta_g1 3 + 1
1899  * FP1 Delta_xP1 6 + 1
1900  * MP1 Delta_w1 9 + 1
1901  * F2 Delta_x2 12 + 1
1902  * M2 Delta_g2 15 + 1
1903  * FP2 Delta_xP2 18 + 1
1904  * MP2 Delta_w2 21 + 1
1905  * vincolo spostamento Delta_F 24 + 1
1906  * vincolo rotazione Delta_M 24 + nPosConstraints
1907  * derivata vincolo spostamento Delta_FP 24 + nConstraints
1908  * derivata vincolo rotazione Delta_MP 24 + nConstraints + nPosConstraints
1909  */
1910 
1911 
1912  /* Indici */
1913  integer iNode1FirstPosIndex = pNode1->iGetFirstPositionIndex();
1914  integer iNode1FirstVelIndex = iNode1FirstPosIndex + 6;
1915  integer iNode2FirstPosIndex = pNode2->iGetFirstPositionIndex();
1916  integer iNode2FirstVelIndex = iNode2FirstPosIndex + 6;
1917  integer iFirstReactionIndex = total_equation_element->iGetFirstIndex();
1918  integer iReactionPrimeIndex = iFirstReactionIndex + nConstraints;
1919 
1920 
1921  /* Setta gli indici dei nodi */
1922  for (int iCnt = 1; iCnt <= 6; iCnt++) {
1923  WM.PutRowIndex(iCnt, iNode1FirstPosIndex + iCnt);
1924  WM.PutColIndex(iCnt, iNode1FirstPosIndex + iCnt);
1925  WM.PutRowIndex(6 + iCnt, iNode1FirstVelIndex + iCnt);
1926  WM.PutColIndex(6 + iCnt, iNode1FirstVelIndex + iCnt);
1927  WM.PutRowIndex(12 + iCnt, iNode2FirstPosIndex + iCnt);
1928  WM.PutColIndex(12 + iCnt, iNode2FirstPosIndex + iCnt);
1929  WM.PutRowIndex(18 + iCnt, iNode2FirstVelIndex + iCnt);
1930  WM.PutColIndex(18 + iCnt, iNode2FirstVelIndex + iCnt);
1931  }
1932 
1933  /* Setta gli indici delle reazioni */
1934  for (unsigned int iCnt = 1; iCnt <= nConstraints; iCnt++) {
1935  WM.PutRowIndex(24 + iCnt, iFirstReactionIndex + iCnt);
1936  WM.PutColIndex(24 + iCnt, iFirstReactionIndex + iCnt);
1937  WM.PutRowIndex(24 + nConstraints + iCnt, iReactionPrimeIndex + iCnt);
1938  WM.PutColIndex(24 + nConstraints + iCnt, iReactionPrimeIndex + iCnt);
1939  }
1940 
1941  /* Recupera i dati che servono */
1942  Mat3x3 R1(pNode1->GetRRef()*R1h);
1943  Mat3x3 R1r(pNode1->GetRRef()*R1hr);
1944 
1945  Vec3 b2(pNode2->GetRCurr()*f2);
1946  Vec3 b1(pNode2->GetXCurr() + b2 - pNode1->GetXCurr());
1947 
1948  Vec3 Omega1(pNode1->GetWCurr());
1949  Vec3 Omega2(pNode2->GetWCurr());
1950 
1951  Vec3 b1Prime(pNode2->GetVCurr() + Omega2.Cross(b2) - pNode1->GetVCurr());
1952 
1953  /* F ed M sono gia' state aggiornate da InitialAssRes;
1954  * Recupero FPrime e MPrime*/
1955  Vec3 MPrime(Zero3);
1956  Vec3 FPrime(Zero3);
1957 
1958  for (unsigned iCnt = 0; iCnt < nPosConstraints; iCnt++) {
1959  FPrime(iPosIncid[iCnt]) = XCurr(iReactionPrimeIndex + 1 + iCnt);
1960  }
1961 
1962  for (unsigned iCnt = 0; iCnt < nRotConstraints; iCnt++) {
1963  MPrime(iRotIncid[iCnt]) = XCurr(iReactionPrimeIndex + 1 + nPosConstraints + iCnt);
1964  }
1965 
1966  Vec3 FTmp(R1 * F);
1967  Vec3 MTmp(R1r * M);
1968  Vec3 FPrimeTmp(R1 * FPrime);
1969  Vec3 MPrimeTmp(R1r * MPrime);
1970 
1971  Mat3x3 Tmp;
1972 
1973  /* [ F x ] */
1974  Tmp = Mat3x3(MatCross, FTmp);
1975 
1976  /* Force, Node 1, Lines 1->3: */
1977  WM.Add(1, 3 + 1, Tmp); // * Delta_g1
1978 
1979  /* Moment, Node 1, Lines 4->6: */
1980  WM.Sub(3 + 1, 1, Tmp); // * Delta_x1
1981 
1982  WM.Add(3 + 1, 12 + 1, Tmp); // * Delta_x2
1983 
1984  /* Force, Node 2, Lines 13->15: */
1985  WM.Sub(12 + 1, 3 + 1, Tmp); // * Delta_g1
1986 
1987  /* [ FP x ] */
1988  Tmp = Mat3x3(MatCross, FPrimeTmp);
1989 
1990  /* d/dt(Force), Node 1, Lines 7->9: */
1991  WM.Add(6 + 1, 9 + 1, Tmp); // * Delta_W1
1992 
1993  /* d/dt(Moment), Node 1, Lines 10->12: */
1994  WM.Sub(9 + 1, 6 + 1, Tmp); // * Delta_x1P
1995 
1996  WM.Add(9 + 1, 18 + 1, Tmp); // * Delta_x2P
1997 
1998  /* d/dt(Force), Node 2, Lines 19->21: */
1999  WM.Sub(18 + 1 , 9 + 1, Tmp); // * Delta_W1
2000 
2001  /* [ F x ] [ b2 x ] */
2002  Tmp = Mat3x3(MatCrossCross, FTmp, b2);
2003 
2004  /* Moment, Node1, Lines 4->6: */
2005  WM.Sub(3 + 1, 15 + 1, Tmp); // * Delta_g2
2006 
2007  /* Moment, Node2, Lines 16->18: */
2008  WM.Add(15 + 1, 15 + 1, Tmp); // * Delta_g2
2009 
2010  /* [ FP x ] [ b2 x ] */
2011  Tmp = Mat3x3(MatCrossCross, FPrimeTmp, b2);
2012 
2013  /* d/dt(Moment), Node1, Lines 10->12: */
2014  WM.Sub(9 + 1, 21 + 1, Tmp); // * Delta_W2
2015 
2016  /* d/dt(Moment), Node2, Lines 22->24: */
2017  WM.Add(21 + 1, 21 + 1, Tmp); // * Delta_W2
2018 
2019  /* [ b1 x ] [ F x ] + [ M x ] */
2020 
2021  /* Moment, Node1, Lines 4->6: */
2022  WM.Add(3 + 1, 3 + 1, Mat3x3(MatCrossCross, b1, FTmp) + Mat3x3(MatCross, MTmp)); // *Delta_g1
2023 
2024  /* d/dt(Moment), Node1, Lines 10->12: */
2025  WM.Add(9 + 1, 9 + 1, Mat3x3(MatCrossCross, b1, FPrimeTmp) + Mat3x3(MatCross, MPrimeTmp));// *Delta_W1
2026 
2027  /* [ b2 x ] [ F x ] + [ M x ] */
2028 
2029  /* Moment, Node2, Lines 16->18: */
2030  WM.Sub(15 + 1, 3 + 1, Mat3x3(MatCrossCross, b2, FTmp) + Mat3x3(MatCross, MTmp)); // * Delta_g1
2031 
2032  /* d/dt(Moment), Node2, Lines 22->24: */
2033  WM.Sub(21 + 1, 9 + 1, Mat3x3(MatCrossCross, b2, FTmp) + Mat3x3(MatCross, MTmp)); // * Delta_W1
2034 
2035  /* Constraints: Add only active rows/columns*/
2036 
2037  /* Positions contribution:*/
2038 
2039  Mat3x3 b1Cross_R1(b1.Cross(R1)); // = [ b1 x ] * R1
2040  Mat3x3 b2Cross_R1(b2.Cross(R1)); // = [ b2 x ] * R1
2041 
2042  for (unsigned iCnt = 0 ; iCnt < nPosConstraints; iCnt++) {
2043  Vec3 vR1(R1.GetVec(iPosIncid[iCnt]));
2044  Vec3 vb1Cross_R1(b1Cross_R1.GetVec(iPosIncid[iCnt]));
2045  Vec3 vb2Cross_R1(b2Cross_R1.GetVec(iPosIncid[iCnt]));
2046 
2047  /* Equilibrium, node 1 */
2048  WM.Sub(1, 24 + 1 + iCnt, vR1); // * Delta_F
2049  WM.Sub(3 + 1, 24 + 1 + iCnt, vb1Cross_R1); // * Delta_F
2050 
2051  /* Constraint, node 1 */
2052  WM.SubT(24 + 1 + iCnt, 1, vR1); // * Delta_x1
2053  WM.SubT(24 + 1 + iCnt, 3 + 1, vb1Cross_R1); // * Delta_g1
2054 
2055  /* d/dt(Equilibrium), node 1 */
2056  WM.Sub(6 + 1, 24 + 1 + nConstraints + iCnt, vR1); // * Delta_FP
2057  WM.Sub(9 + 1, 24 + 1 + nConstraints + iCnt, vb1Cross_R1); // * Delta_FP
2058 
2059  /* d/dt(Constraint), node 1 */
2060  WM.SubT(24 + 1 + nConstraints + iCnt, 6 + 1, vR1); // * Delta_xP1
2061  WM.SubT(24 + 1 + nConstraints + iCnt, 9 + 1, vb1Cross_R1); // * Delta_W1
2062 
2063  /* Equilibrium, node 2 */
2064  WM.Add(12 + 1, 24 + 1 + iCnt, vR1); // * Delta_F
2065  WM.Add(15 + 1, 24 + 1 + iCnt, vb2Cross_R1); // * Delta_F
2066 
2067  /* Constraint, node 2 */
2068  WM.AddT(24 + 1 + iCnt, 12 + 1, vR1); // * Delta_x2
2069  WM.AddT(24 + 1 + iCnt, 15 + 1, vb2Cross_R1); // * Delta_g2
2070 
2071  /* d/dt(Equilibrium), node 2 */
2072  WM.Add(18 + 1, 24 + 1 + nConstraints + iCnt, vR1); // * Delta_FP
2073  WM.Add(21 + 1, 24 + 1 + nConstraints + iCnt, vb2Cross_R1); // * Delta_FP
2074 
2075  /* d/dt(Constraint), node 2 */
2076  WM.AddT(24 + 1 + nConstraints + iCnt, 18 + 1, vR1); // * Delta_xP2
2077  WM.AddT(24 + 1 + nConstraints + iCnt, 21 + 1, vb2Cross_R1); // * Delta_W2
2078  }
2079 
2080  for (unsigned iCnt = 0 ; iCnt < nRotConstraints; iCnt++) {
2081  Vec3 vR1(R1r.GetVec(iRotIncid[iCnt]));
2082 
2083  /* Equilibrium, node 1 */
2084  WM.Sub(3 + 1, 24 + 1 + nPosConstraints + iCnt, vR1); // * Delta_M
2085 
2086  /* Constraint, node 1 */
2087  WM.SubT(24 + 1 + nPosConstraints + iCnt, 3 + 1, vR1); // * Delta_g1
2088 
2089  /* d/dt(Equilibrium), node 1 */
2090  WM.Sub(9 + 1, 24 + 1 + nConstraints + nPosConstraints + iCnt, vR1); // * Delta_MP
2091 
2092  /* d/dt(Constraint), node 1 */
2093  WM.SubT(24 + 1 + nConstraints + nPosConstraints + iCnt, 9 + 1, vR1); // * Delta_W1
2094 
2095  /* Equilibrium, node 2 */
2096  WM.Add(15 + 1, 24 + 1 + nPosConstraints + iCnt, vR1); // * Delta_M
2097 
2098  /* Constraint, node 2 */
2099  WM.AddT(24 + 1 + nPosConstraints + iCnt, 15 + 1, vR1); // * Delta_g2
2100 
2101  /* d/dt(Equilibrium), node 2 */
2102  WM.Add(21 + 1, 24 + 1 + nConstraints + nPosConstraints + iCnt, vR1); // * Delta_MP
2103 
2104  /* d/dt(Constraint), node 2 */
2105  WM.AddT(24 + 1 + nConstraints + nPosConstraints + iCnt, 21 + 1, vR1); // * Delta_W2
2106  }
2107 
2108  return WorkMat;
2109 }
2110 
2111 
2112 /* Contributo al residuo durante l'assemblaggio iniziale */
2113 SubVectorHandler&
2114 TotalReaction::InitialAssRes(SubVectorHandler& WorkVec,
2115  const VectorHandler& XCurr)
2116 {
2117 
2118  DEBUGCOUT("Entering TotalReaction::InitialAssRes()" << std::endl);
2119 
2120  /* Dimensiona e resetta la matrice di lavoro */
2121  integer iNumRows = 0;
2122  integer iNumCols = 0;
2123  InitialWorkSpaceDim(&iNumRows, &iNumCols);
2124  WorkVec.ResizeReset(iNumRows);
2125 
2126  /* Indici */
2127  integer iNode1FirstPosIndex = pNode1->iGetFirstPositionIndex();
2128  integer iNode1FirstVelIndex = iNode1FirstPosIndex + 6;
2129  integer iNode2FirstPosIndex = pNode2->iGetFirstPositionIndex();
2130  integer iNode2FirstVelIndex = iNode2FirstPosIndex + 6;
2131  integer iFirstReactionIndex = total_equation_element->iGetFirstIndex();
2132  integer iReactionPrimeIndex = iFirstReactionIndex + nConstraints;
2133 
2134  /* Setta gli indici */
2135  for (int iCnt = 1; iCnt <= 6; iCnt++) {
2136  WorkVec.PutRowIndex(iCnt, iNode1FirstPosIndex+iCnt);
2137  WorkVec.PutRowIndex(6 + iCnt, iNode1FirstVelIndex + iCnt);
2138  WorkVec.PutRowIndex(12 + iCnt, iNode2FirstPosIndex + iCnt);
2139  WorkVec.PutRowIndex(18 + iCnt, iNode2FirstVelIndex + iCnt);
2140  }
2141 
2142  for (unsigned int iCnt = 1; iCnt <= nConstraints; iCnt++) {
2143  WorkVec.PutRowIndex(24 + iCnt, iFirstReactionIndex + iCnt);
2144  WorkVec.PutRowIndex(24 + nConstraints + iCnt, iReactionPrimeIndex + iCnt);
2145  }
2146 
2147  /* Recupera i dati */
2148  /* Recupera i dati che servono */
2149  Mat3x3 R1(pNode1->GetRRef()*R1h);
2150  Mat3x3 R1r(pNode1->GetRRef()*R1hr);
2151  Mat3x3 R2r(pNode2->GetRCurr()*R2hr);
2152 
2153  Vec3 b2(pNode2->GetRCurr()*f2);
2154  Vec3 b1(pNode2->GetXCurr() + b2 - pNode1->GetXCurr());
2155 
2156  Vec3 Omega1(pNode1->GetWCurr());
2157  Vec3 Omega2(pNode2->GetWCurr());
2158 
2159  Vec3 b1Prime(pNode2->GetVCurr() + Omega2.Cross(b2) - pNode1->GetVCurr());
2160 
2161  Vec3 FPrime(Zero3);
2162  Vec3 MPrime(Zero3);
2163 
2164  /* Aggiorna F ed M, che restano anche per InitialAssJac */
2165  for (unsigned iCnt = 0; iCnt < nPosConstraints; iCnt++) {
2166  F(iPosIncid[iCnt]) = XCurr(iFirstReactionIndex + 1 + iCnt);
2167  FPrime(iPosIncid[iCnt]) = XCurr(iReactionPrimeIndex + 1 + iCnt);
2168  }
2169 
2170  for (unsigned iCnt = 0; iCnt < nRotConstraints; iCnt++) {
2171  M(iRotIncid[iCnt]) = XCurr(iFirstReactionIndex + 1 + nPosConstraints + iCnt);
2172  MPrime(iRotIncid[iCnt]) = XCurr(iReactionPrimeIndex + 1 + nPosConstraints + iCnt);
2173  }
2174 
2175  Vec3 FTmp(R1 * F);
2176  Vec3 MTmp(R1r * M);
2177  Vec3 FPrimeTmp(R1 * FPrime);
2178  Vec3 MPrimeTmp(R1r * MPrime);
2179 
2180  /* Equilibrium, node 1 */
2181  WorkVec.Add(1, FTmp);
2182  WorkVec.Add(3 + 1, b1.Cross(FTmp) + MTmp);
2183 
2184  /* d/dt(Equilibrium), node 1 */
2185  WorkVec.Add(6 + 1, FPrimeTmp);
2186  WorkVec.Add(9 + 1, b1.Cross(FPrimeTmp) + MPrimeTmp);
2187 
2188  /* Equilibrium, node 2 */
2189  WorkVec.Sub(12 + 1, FTmp);
2190  WorkVec.Sub(15 + 1, b2.Cross(FTmp) + MTmp);
2191 
2192  /* d/dt( Equilibrium ) , node 2 */
2193  WorkVec.Sub(18 + 1, FPrimeTmp);
2194  WorkVec.Sub(21 + 1, b2.Cross(FPrimeTmp) + MPrimeTmp);
2195 
2196  return WorkVec;
2197 }
2198 
2199 
2200 unsigned int
2201 TotalReaction::iGetNumPrivData(void) const
2202 {
2203  return 21;
2204 }
2205 
2206 unsigned int
2207 TotalReaction::iGetPrivDataIdx(const char *s) const
2208 {
2209  ASSERT(s != NULL);
2210 
2211  unsigned int off = 0;
2212 
2213  switch (s[0]) {
2214  case 'p':
2215  /* relative position */
2216  break;
2217 
2218  case 'r':
2219  /* relative orientation */
2220  off += 3;
2221  break;
2222 
2223  case 'F':
2224  /* force */
2225  off += 6;
2226  break;
2227 
2228  case 'M':
2229  /* moment */
2230  off += 9;
2231  break;
2232 
2233  case 'd':
2234  /* imposed relative position */
2235  off += 12;
2236  break;
2237 
2238  case 't':
2239  /* imposed relative orientation */
2240  off += 15;
2241  break;
2242 
2243  case 'v':
2244  /* relative linear velocity */
2245  off += 18;
2246  break;
2247 
2248  default:
2249  return 0;
2250  }
2251 
2252  switch (s[1]) {
2253  case 'x':
2254  return off + 1;
2255 
2256  case 'y':
2257  return off + 2;
2258 
2259  case 'z':
2260  return off + 3;
2261  }
2262 
2263  return 0;
2264 }
2265 
2266 doublereal
2267 TotalReaction::dGetPrivData(unsigned int i) const
2268 {
2269  switch (i) {
2270  case 1:
2271  case 2:
2272  case 3: {
2273  Vec3 x(pNode1->GetRCurr().MulTV(pNode2->GetXCurr() + pNode2->GetRCurr()*f2 - pNode1->GetXCurr()) - f1);
2274  return R1h.GetVec(i)*x;
2275  }
2276 
2277  case 4:
2278  case 5:
2279  case 6: {
2280 #if 0
2281  Vec3 Theta(Unwrap(ThetaDeltaPrev, ThetaDelta) + ThetaDrv.Get());
2282  return Theta(i - 3);
2283 #endif
2284  return 0.;
2285  }
2286 
2287  case 7:
2288  case 8:
2289  case 9:
2290  return F(i - 6);
2291 
2292  case 10:
2293  case 11:
2294  case 12:
2295  return M(i - 9);
2296 
2297  case 13:
2298  case 14:
2299  case 15:
2300  if (!bPosActive[i - 13]) {
2301  return 0.;
2302  }
2303 // return XDrv.Get()(i - 12);
2304  return 0.;
2305 
2306  case 16:
2307  case 17:
2308  case 18:
2309  if (!bRotActive[i - 16]) {
2310  return 0.;
2311  }
2312 // return ThetaDrv.Get()(i - 15);
2313  return 0.;
2314  case 19:
2315  case 20:
2316  case 21:
2317  {
2318  // FIXME: blind fix
2319  Vec3 v(pNode1->GetRCurr().MulTV(
2320  (pNode2->GetVCurr() + pNode2->GetWCurr().Cross(pNode2->GetRCurr()*f2)
2321  - pNode1->GetVCurr()) -
2322  pNode1->GetWCurr().Cross( pNode2->GetXCurr() +
2323  pNode2->GetRCurr()*f2
2324  - pNode1->GetXCurr() -f1)
2325  )
2326  );
2327 
2328  return R1h.GetVec(i-18)*v;
2329  }
2330 
2331 
2332  default:
2333  ASSERT(0);
2334  }
2335 
2336  return 0.;
2337 }
2338 
2339 /* TotalReaction - end */
Hint
Definition: hint.h:38
TotalReaction::bRotActive
bool bRotActive[3]
Definition: totalequation.h:261
TotalEquation::AfterConvergence
virtual void AfterConvergence(const VectorHandler &X, const VectorHandler &XP)
Definition: totalequation.cc:564
TotalEquation::nRotConstraints
unsigned int nRotConstraints
Definition: totalequation.h:73
TotalReaction::pNode1
const StructNode * pNode1
Definition: totalequation.h:252
TotalReaction::InitialAssJac
VariableSubMatrixHandler & InitialAssJac(VariableSubMatrixHandler &WorkMat, const VectorHandler &XCurr)
Definition: totalequation.cc:1880
TotalEquation::iGetNumPrivData
virtual unsigned int iGetNumPrivData(void) const
Definition: totalequation.cc:1075
TotalEquation::bAgvActive
bool bAgvActive[3]
Definition: totalequation.h:61
FullSubMatrixHandler::PutColIndex
void PutColIndex(integer iSubCol, integer iCol)
Definition: submat.h:325
TotalReaction::iPosIncid
unsigned int iPosIncid[3]
Definition: totalequation.h:267
TotalReaction::nRotConstraints
unsigned int nRotConstraints
Definition: totalequation.h:265
Zero3
const Vec3 Zero3(0., 0., 0.)
Vec3::Cross
Vec3 Cross(const Vec3 &v) const
Definition: matvec3.h:218
TotalReaction::InitialWorkSpaceDim
virtual void InitialWorkSpaceDim(integer *piNumRows, integer *piNumCols) const
Definition: totalequation.h:381
ASSERTMSGBREAK
#define ASSERTMSGBREAK(expr, msg)
Definition: myassert.h:222
TotalReaction::Output
void Output(OutputHandler &OH) const
Definition: totalequation.cc:1847
Vec3::Write
std::ostream & Write(std::ostream &out, const char *sFill=" ") const
Definition: matvec3.cc:738
flag
long int flag
Definition: mbdyn.h:43
StructNode::GetRRef
virtual const Mat3x3 & GetRRef(void) const
Definition: strnode.h:1006
ToBeOutput::bToBeOutput
virtual bool bToBeOutput(void) const
Definition: output.cc:890
TotalEquation::bRotActive
bool bRotActive[3]
Definition: totalequation.h:58
TplDriveOwner::bIsDifferentiable
virtual bool bIsDifferentiable(void) const
Definition: tpldrive.h:118
TotalEquation::XDrv
TplDriveOwner< Vec3 > XDrv
Definition: totalequation.h:63
TotalEquation::AssRes
SubVectorHandler & AssRes(SubVectorHandler &WorkVec, doublereal dCoef, const VectorHandler &XCurr, const VectorHandler &XPrimeCurr)
Definition: totalequation.cc:765
MBDYN_EXCEPT_ARGS
#define MBDYN_EXCEPT_ARGS
Definition: except.h:63
Vec3
Definition: matvec3.h:98
TotalReaction::iGetNumDof
virtual unsigned int iGetNumDof(void) const
Definition: totalequation.h:304
TotalEquation::Output
void Output(OutputHandler &OH) const
Definition: totalequation.cc:840
VectorHandler::ResizeReset
virtual void ResizeReset(integer)
Definition: vh.cc:55
TotalReaction::nPosConstraints
unsigned int nPosConstraints
Definition: totalequation.h:264
MatCross
const MatCross_Manip MatCross
Definition: matvec3.cc:639
TotalReaction::AssJac
VariableSubMatrixHandler & AssJac(VariableSubMatrixHandler &WorkMat, doublereal dCoef, const VectorHandler &XCurr, const VectorHandler &XPrimeCurr)
Definition: totalequation.cc:1625
TotalEquation::nConstraints
unsigned int nConstraints
Definition: totalequation.h:71
VariableSubMatrixHandler::SetFull
FullSubMatrixHandler & SetFull(void)
Definition: submat.h:1168
StructNode::GetRCurr
virtual const Mat3x3 & GetRCurr(void) const
Definition: strnode.h:1012
TotalEquation::InitialAssJac
VariableSubMatrixHandler & InitialAssJac(VariableSubMatrixHandler &WorkMat, const VectorHandler &XCurr)
Definition: totalequation.cc:873
TotalEquation::bVelActive
bool bVelActive[3]
Definition: totalequation.h:60
Joint::JointDriveHint::pTDH
TplDriveHint< T > * pTDH
Definition: joint.h:137
TotalReaction::iRotIncid
unsigned int iRotIncid[3]
Definition: totalequation.h:268
TotalEquation::ParseHint
virtual Hint * ParseHint(DataManager *pDM, const char *s) const
Definition: totalequation.cc:487
FullSubMatrixHandler::Add
void Add(integer iRow, integer iCol, const Vec3 &v)
Definition: submat.cc:209
TotalReaction::iGetNumPrivData
virtual unsigned int iGetNumPrivData(void) const
Definition: totalequation.cc:2201
TotalEquation::pNode1
const StructNode * pNode1
Definition: totalequation.h:49
TotalReaction::bPosActive
bool bPosActive[3]
Definition: totalequation.h:260
VectorHandler::Sub
virtual void Sub(integer iRow, const Vec3 &v)
Definition: vh.cc:78
TotalEquation::iRotEqIndex
unsigned int iRotEqIndex[3]
Definition: totalequation.h:83
TotalReaction::WorkSpaceDim
void WorkSpaceDim(integer *piNumRows, integer *piNumCols) const
Definition: totalequation.h:353
TotalReaction::dGetPrivData
virtual doublereal dGetPrivData(unsigned int i) const
Definition: totalequation.cc:2267
TotalReaction::ParseHint
virtual Hint * ParseHint(DataManager *pDM, const char *s) const
Definition: totalequation.cc:1476
TotalEquation::bPosActive
bool bPosActive[3]
Definition: totalequation.h:57
NO_OP
#define NO_OP
Definition: myassert.h:74
TotalEquation::iAgvIncid
unsigned int iAgvIncid[3]
Definition: totalequation.h:80
V2
Definition: matvec3.h:50
SimulationEntity::Hints
std::vector< Hint * > Hints
Definition: simentity.h:89
TotalEquation::iAgvEqIndex
unsigned int iAgvEqIndex[3]
Definition: totalequation.h:85
TotalReaction::DescribeDof
virtual std::ostream & DescribeDof(std::ostream &out, const char *prefix="", bool bInitial=false) const
Definition: totalequation.cc:1284
TotalEquation::nVelConstraints
unsigned int nVelConstraints
Definition: totalequation.h:74
Mat3x3::GetVec
Vec3 GetVec(unsigned short int i) const
Definition: matvec3.h:893
TotalReaction::total_equation_element
TotalEquation * total_equation_element
Definition: totalequation.h:277
TotalEquation::OmegaDrv
TplDriveOwner< Vec3 > OmegaDrv
Definition: totalequation.h:68
TotalReaction::AssRes
SubVectorHandler & AssRes(SubVectorHandler &WorkVec, doublereal dCoef, const VectorHandler &XCurr, const VectorHandler &XPrimeCurr)
Definition: totalequation.cc:1780
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
TotalEquation::OmegaPDrv
TplDriveOwner< Vec3 > OmegaPDrv
Definition: totalequation.h:69
TotalEquation::SetValue
virtual void SetValue(DataManager *pDM, VectorHandler &X, VectorHandler &XP, SimulationEntity::Hints *ph=0)
Definition: totalequation.cc:397
TplDriveOwner::GetP
virtual T GetP(void) const
Definition: tpldrive.h:121
Mat3x3::MulTV
Vec3 MulTV(const Vec3 &v) const
Definition: matvec3.cc:482
TotalReaction::GetEqType
DofOrder::Order GetEqType(unsigned int i) const
Definition: totalequation.cc:1837
SubVectorHandler::PutRowIndex
virtual void PutRowIndex(integer iSubRow, integer iRow)=0
TotalEquation::XPDrv
TplDriveOwner< Vec3 > XPDrv
Definition: totalequation.h:64
TotalEquation::iGetNumDof
virtual unsigned int iGetNumDof(void) const
Definition: totalequation.h:122
TotalEquation::ThetaDrv
TplDriveOwner< Vec3 > ThetaDrv
Definition: totalequation.h:67
TotalEquation::Restart
virtual std::ostream & Restart(std::ostream &out) const
Definition: totalequation.cc:580
TotalEquation::DescribeEq
virtual std::ostream & DescribeEq(std::ostream &out, const char *prefix="", bool bInitial=false) const
Definition: totalequation.cc:253
hint_impl.h
TotalEquation::InitialAssRes
SubVectorHandler & InitialAssRes(SubVectorHandler &WorkVec, const VectorHandler &XCurr)
Definition: totalequation.cc:991
DEBUGCOUT
#define DEBUGCOUT(msg)
Definition: myassert.h:232
TplDriveOwner::Get
T Get(const doublereal &dVar) const
Definition: tpldrive.h:109
TotalEquation::dGetPrivData
virtual doublereal dGetPrivData(unsigned int i) const
Definition: totalequation.cc:1147
RotManip::Rot
Mat3x3 Rot(const Vec3 &phi)
Definition: Rot.cc:62
TotalEquation::iGetInitialNumDof
virtual unsigned int iGetInitialNumDof(void) const
Definition: totalequation.h:209
TotalEquation::iGetPrivDataIdx
virtual unsigned int iGetPrivDataIdx(const char *s) const
Definition: totalequation.cc:1081
StructDispNode::iGetFirstMomentumIndex
virtual integer iGetFirstMomentumIndex(void) const =0
StructDispNode::iGetFirstPositionIndex
virtual integer iGetFirstPositionIndex(void) const
Definition: strnode.h:452
StructNode::GetWCurr
virtual const Vec3 & GetWCurr(void) const
Definition: strnode.h:1030
WithLabel::uLabel
unsigned int uLabel
Definition: withlab.h:44
Joint::Restart
virtual std::ostream & Restart(std::ostream &out) const
Definition: joint.h:195
TotalReaction::pNode2
const StructNode * pNode2
Definition: totalequation.h:253
TotalEquation::AssJac
VariableSubMatrixHandler & AssJac(VariableSubMatrixHandler &WorkMat, doublereal dCoef, const VectorHandler &XCurr, const VectorHandler &XPrimeCurr)
Definition: totalequation.cc:637
OutputHandler::Joints
std::ostream & Joints(void) const
Definition: output.h:443
TotalEquation::WorkSpaceDim
void WorkSpaceDim(integer *piNumRows, integer *piNumCols) const
Definition: totalequation.h:170
TotalEquation::iVelIncid
unsigned int iVelIncid[3]
Definition: totalequation.h:79
ASSERT
#define ASSERT(expression)
Definition: colamd.c:977
TotalReaction::TotalReaction
TotalReaction(unsigned int uL, const DofOwner *pDO, bool bPos[3], bool bRot[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, TotalEquation *t_elm, flag fOut)
Definition: totalequation.cc:1232
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
StructDispNode::GetXCurr
virtual const Vec3 & GetXCurr(void) const
Definition: strnode.h:310
TotalEquation::DescribeDof
virtual std::ostream & DescribeDof(std::ostream &out, const char *prefix="", bool bInitial=false) const
Definition: totalequation.cc:124
VectorHandler::Add
virtual void Add(integer iRow, const Vec3 &v)
Definition: vh.cc:63
FullSubMatrixHandler::ResizeReset
virtual void ResizeReset(integer, integer)
Definition: submat.cc:182
TotalReaction::AfterConvergence
virtual void AfterConvergence(const VectorHandler &X, const VectorHandler &XP)
Definition: totalequation.cc:1553
TotalReaction::SetValue
virtual void SetValue(DataManager *pDM, VectorHandler &X, VectorHandler &XP, SimulationEntity::Hints *ph=0)
Definition: totalequation.cc:1383
TotalEquation::nAgvConstraints
unsigned int nAgvConstraints
Definition: totalequation.h:75
Unwrap
Vec3 Unwrap(const Vec3 &vPrev, const Vec3 &v)
Definition: matvec3.cc:1089
TotalEquation::XPPDrv
TplDriveOwner< Vec3 > XPPDrv
Definition: totalequation.h:65
TotalEquation::pNode2
const StructNode * pNode2
Definition: totalequation.h:50
TotalEquation::iVelEqIndex
unsigned int iVelEqIndex[3]
Definition: totalequation.h:84
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
Elem
Definition: elem.h:75
TotalReaction::nConstraints
unsigned int nConstraints
Definition: totalequation.h:263
TotalReaction::InitialAssRes
SubVectorHandler & InitialAssRes(SubVectorHandler &WorkVec, const VectorHandler &XCurr)
Definition: totalequation.cc:2114
FullSubMatrixHandler::PutRowIndex
void PutRowIndex(integer iSubRow, integer iRow)
Definition: submat.h:311
V1
Definition: matvec3.h:49
TotalEquation::iPosEqIndex
unsigned int iPosEqIndex[3]
Definition: totalequation.h:82
TotalEquation::iPosIncid
unsigned int iPosIncid[3]
Definition: totalequation.h:77
idx2xyz
static const char idx2xyz[]
Definition: totalequation.cc:40
TotalReaction::Restart
virtual std::ostream & Restart(std::ostream &out) const
Definition: totalequation.cc:1568
StructDispNode::GetVCurr
virtual const Vec3 & GetVCurr(void) const
Definition: strnode.h:322
FullSubMatrixHandler::Sub
void Sub(integer iRow, integer iCol, const Vec3 &v)
Definition: submat.cc:215
TotalEquation::TotalEquation
TotalEquation(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: totalequation.cc:44
Joint
Definition: joint.h:50
TotalEquation::nPosConstraints
unsigned int nPosConstraints
Definition: totalequation.h:72
TotalEquation::iRotIncid
unsigned int iRotIncid[3]
Definition: totalequation.h:78
TotalEquation::InitialWorkSpaceDim
virtual void InitialWorkSpaceDim(integer *piNumRows, integer *piNumCols) const
Definition: totalequation.h:213
DofOwnerOwner::iGetFirstIndex
virtual integer iGetFirstIndex(void) const
Definition: dofown.h:127
doublereal
double doublereal
Definition: colamd.c:52
Joint::Output
std::ostream & Output(std::ostream &out, const char *sJointName, unsigned int uLabel, const Vec3 &FLocal, const Vec3 &MLocal, const Vec3 &FGlobal, const Vec3 &MGlobal) const
Definition: joint.cc:138
integer
long int integer
Definition: colamd.c:51
WithLabel::GetLabel
unsigned int GetLabel(void) const
Definition: withlab.cc:62
TotalReaction::iGetPrivDataIdx
virtual unsigned int iGetPrivDataIdx(const char *s) const
Definition: totalequation.cc:2207