MBDyn-1.7.3
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Groups
body_vm.cc
Go to the documentation of this file.
1 /* $Header: /var/cvs/mbdyn/mbdyn/mbdyn-1.0/mbdyn/struct/body_vm.cc,v 1.3 2011/12/14 18:30:53 masarati Exp $ */
2 /*
3  * MBDyn (C) is a multibody analysis code.
4  * http://www.mbdyn.org
5  *
6  * Copyright (C) 1996-2005
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 /* elementi di massa */
33 
34 #include "mbconfig.h" /* This goes first in every *.c,*.cc file */
35 
36 #include <limits>
37 
38 #include "body_vm.h"
39 #include "dataman.h"
40 
41 /* VariableBody - begin */
42 
44  const StructNode *pNode,
45  const DriveCaller *pDCMass,
46  const TplDriveCaller<Vec3> *pDCXgc,
47  const TplDriveCaller<Mat3x3> *pDCJgc_vm,
48  const TplDriveCaller<Mat3x3> *pDCJgc_vg,
49  flag fOut)
50 : Elem(uL, fOut),
51 ElemGravityOwner(uL, fOut),
52 InitialAssemblyElem(uL, fOut),
53 pNode(pNode),
54 m_Mass(pDCMass),
55 m_Xgc(pDCXgc),
56 m_Jgc_vm(pDCJgc_vm),
57 m_Jgc_vg(pDCJgc_vg)
58 {
59  ASSERT(pNode != 0);
60  ASSERT(pNode->GetNodeType() == Node::STRUCTURAL);
61 }
62 
63 
64 /* distruttore */
66 {
67  NO_OP;
68 }
69 
70 
71 /* momento statico */
72 Vec3
74 {
75  return (pNode->GetXCurr() + pNode->GetRCurr()*m_Xgc.Get())*m_Mass.dGet();
76 }
77 
78 
79 /* momento d'inerzia */
80 Mat3x3
82 {
83  Vec3 x = pNode->GetXCurr() + pNode->GetRCurr()*m_Xgc.Get();
84 
85  return pNode->GetRCurr()*(m_Jgc_vm.Get() + m_Jgc_vg.Get()).MulMT(pNode->GetRCurr())
86  - Mat3x3(MatCrossCross, x, x*m_Mass.dGet());
87 }
88 
89 
90 /* Scrive il contributo dell'elemento al file di restart */
91 std::ostream&
92 VariableBody::Restart(std::ostream& out) const
93 {
94  out << " body: " << GetLabel() << ", "
95  << pNode->GetLabel() << ", variable mass, ", m_Mass.pGetDriveCaller()->Restart(out) << ", "
96  << "reference, node, ", m_Xgc.pGetDriveCaller()->Restart(out) << ", "
97  << "reference, node, ", m_Jgc_vm.pGetDriveCaller()->Restart(out) << ", "
98  << "reference, node, ", m_Jgc_vg.pGetDriveCaller()->Restart(out) << ";" << std::endl;
99 
100  return out;
101 }
102 
103 
104 void
106 {
107  const Mat3x3& R = pNode->GetRRef();
108 
109  dMTmp = m_Mass.dGet();
110  Vec3 x(m_Xgc.Get());
111  STmp = (R*x)*dMTmp;
112  JTmp = R*(m_Jgc_vm.Get() + m_Jgc_vg.Get() + Mat3x3(MatCrossCross, x, x*dMTmp)).MulMT(R);
113 }
114 
115 /* massa totale */
118 {
119  return m_Mass.dGet();
120 }
121 
122 /* momento statico */
123 Vec3
125 {
126  return GetS_int();
127 }
128 
129 /* momento d'inerzia */
130 Mat3x3
132 {
133  return GetJ_int();
134 }
135 
136 /* nodo */
137 const StructNode *
139 {
140  return pNode;
141 }
142 
143 /* Accesso ai dati privati */
144 unsigned int
146 {
147  return 1;
148 }
149 
150 unsigned int
151 VariableBody::iGetPrivDataIdx(const char *s) const
152 {
153  if (strcmp(s, "E") == 0) {
154  // kinetic energy
155  return 1;
156  }
157 
158  if (strcmp(s, "V") == 0) {
159  // potential energy
160  return 2;
161  }
162 
163  return 0;
164 }
165 
167 VariableBody::dGetPrivData(unsigned int i) const
168 {
169  if (i == 1) {
170  // kinetic energy
171  const Mat3x3& Rn = pNode->GetRCurr();
172  const Vec3& Vn = pNode->GetVCurr();
173  const Vec3& Wn = pNode->GetWCurr();
174 
175  Vec3 DXgc = Rn*m_Xgc.Get();
176  Vec3 V = Vn + Wn.Cross(DXgc);
177  Vec3 W = Rn*Wn;
178 
179  return ((V*V)*m_Mass.dGet() + W*((m_Jgc_vm.Get() + m_Jgc_vg.Get())*W))/2.;
180  }
181 
182  if (i == 2) {
183  // potential energy
184  // NOTE: it is only valid for uniform gravity field
185  Vec3 Xgc = pNode->GetXCurr() + pNode->GetRCurr()*m_Xgc.Get();
186 
187  Vec3 GravityAcceleration;
188  if (GravityOwner::bGetGravity(Xgc, GravityAcceleration)) {
189  return -Xgc.Dot(GravityAcceleration)*m_Mass.dGet();
190  }
191  }
192 
193  return 0.;
194 }
195 
196 void
198 {
199  const RigidBodyKinematics *pRBK = pNode->pGetRBK();
200 
201  // NOTE: dMTmp, STmp, JTmp updated earlier
202 
203  Vec3 s0;
204 
205  integer iIdx = 0;
206  if (dynamic_cast<DynamicVariableBody *>(this)) {
207  iIdx = 6;
208  }
209 
210  s0 = pNode->GetXCurr()*m_Mass.dGet() + STmp;
211 
212  // force
213  Vec3 f;
214  f = pRBK->GetXPP()*dMTmp;
215  f += pRBK->GetWP().Cross(s0);
216  f += pRBK->GetW().Cross(pRBK->GetW().Cross(s0));
217 
218  WorkVec.Sub(iIdx + 1, f);
219 
220  // moment
221  Vec3 a;
222  a = pRBK->GetXPP();
223  a += pRBK->GetWP().Cross(pNode->GetXCurr());
224  a += pRBK->GetW().Cross(pRBK->GetW().Cross(pNode->GetXCurr()));
225  a += pRBK->GetW().Cross(pNode->GetVCurr());
226 
227  Vec3 m;
228  m = STmp.Cross(a);
229  m += pRBK->GetW().Cross(JTmp*pRBK->GetW());
230  m += JTmp*pRBK->GetWP();
231  m += pNode->GetWCurr().Cross(JTmp*pRBK->GetW());
232  m -= JTmp*(pNode->GetWCurr().Cross(pRBK->GetW()));
233  m += pNode->GetVCurr().Cross(pRBK->GetW().Cross(STmp));
234 
235  WorkVec.Sub(iIdx + 3 + 1, m);
236 }
237 
238 void
242  const doublereal& dCoef,
243  const Vec3& Sc)
244 {
245  const RigidBodyKinematics *pRBK = pNode->pGetRBK();
246 
247  Mat3x3 MTmp;
248  Vec3 VTmp;
249 
250  integer iIdx = 0;
251  if (dynamic_cast<DynamicVariableBody *>(this)) {
252  iIdx = 6;
253  }
254 
255  // f: delta x
256  MTmp = Mat3x3(MatCross, pRBK->GetWP());
257  MTmp += Mat3x3(MatCrossCross, pRBK->GetW(), pRBK->GetW());
258 
259  WMA.Add(iIdx + 1, 1, MTmp*(dMTmp*dCoef));
260 
261 
262  // f: theta delta
263 
264  WMA.Sub(iIdx + 1, 3 + 1, MTmp*Mat3x3(MatCross, Sc));
265 
266 
267  // m: delta x
268  MTmp = Mat3x3(MatCrossCross, Sc, pRBK->GetWP());
269  MTmp += Sc.Cross(Mat3x3(MatCrossCross, pRBK->GetW(), pRBK->GetW()));
270 
271  WMA.Add(iIdx + 3 + 1, 1, MTmp);
272 
273 
274  // m: theta delta
275 
276  VTmp = pRBK->GetXPP();
277  VTmp += pRBK->GetWP().Cross(pNode->GetXCurr());
278  VTmp += pRBK->GetW().Cross(pRBK->GetW().Cross(pNode->GetXCurr()));
279  VTmp += pRBK->GetW().Cross(pNode->GetVCurr());
280 
281  MTmp = Mat3x3(MatCrossCross, VTmp, Sc);
282 
283  VTmp = (pRBK->GetW() + pNode->GetWCurr())*dCoef;
284 
285  Mat3x3 MTmp2(JTmp*Mat3x3(MatCross, pRBK->GetW()) - Mat3x3(MatCross, JTmp*pRBK->GetW()));
286  MTmp += VTmp.Cross(MTmp2);
287 
288  VTmp = (pRBK->GetWP() + pRBK->GetW().Cross(pNode->GetWCurr()))*dCoef;
289 
290  MTmp += JTmp*Mat3x3(MatCross, VTmp);
291  MTmp -= Mat3x3(MatCross, JTmp*VTmp);
292 
293  MTmp -= pNode->GetVCurr().Cross(Mat3x3(MatCrossCross, pRBK->GetW(), Sc));
294 
295  WMA.Add(iIdx + 3 + 1, 3 + 1, MTmp);
296 
297 
298  // m: delta dot x
299  MTmp = Mat3x3(MatCrossCross, STmp, pRBK->GetW());
300  MTmp -= Mat3x3(MatCross, pRBK->GetW().Cross(STmp));
301 
302  WMB.Add(iIdx + 3 + 1, 1, MTmp);
303 
304  // m: delta omega
305  WMB.Add(iIdx + 3 + 1, 3 + 1, MTmp2);
306 }
307 
308 /* VariableBody - end */
309 
310 
311 /* DynamicVariableBody - begin */
312 
314  const DynamicStructNode* pNode,
315  const DriveCaller *pDCMass,
316  const TplDriveCaller<Vec3> *pDCXgc,
317  const TplDriveCaller<Mat3x3> *pDCJgc_vm,
318  const TplDriveCaller<Mat3x3> *pDCJgc_vg,
319  flag fOut)
320 : Elem(uL, fOut),
321 VariableBody(uL, pNode, pDCMass, pDCXgc, pDCJgc_vm, pDCJgc_vg, fOut)
322 {
323  NO_OP;
324 }
325 
326 
327 /* distruttore */
329 {
330  NO_OP;
331 }
332 
333 
336  doublereal dCoef,
337  const VectorHandler& XCurr,
338  const VectorHandler& XPrimeCurr)
339 {
340  DEBUGCOUTFNAME("DynamicVariableBody::AssJac");
341 
342  /* Casting di WorkMat */
343  FullSubMatrixHandler& WM = WorkMat.SetFull();
344 
345  Vec3 GravityAcceleration;
347  GravityAcceleration);
348 
349  integer iNumRows = 6;
350  if (g || pNode->pGetRBK()) {
351  iNumRows = 12;
352  }
353 
354  /* Dimensiona e resetta la matrice di lavoro */
355  WM.ResizeReset(iNumRows, 6);
356 
357  /* Setta gli indici della matrice - le incognite sono ordinate come:
358  * - posizione (3)
359  * - parametri di rotazione (3)
360  * - quantita' di moto (3)
361  * - momento della quantita' di moto
362  * e gli indici sono consecutivi. La funzione pGetFirstPositionIndex()
363  * ritorna il valore del primo indice -1, in modo che l'indice i-esimo
364  * e' dato da iGetFirstPositionIndex()+i */
365  integer iFirstPositionIndex = pNode->iGetFirstPositionIndex();
366  for (integer iCnt = 1; iCnt <= 6; iCnt++) {
367  WM.PutRowIndex(iCnt, iFirstPositionIndex + iCnt);
368  WM.PutColIndex(iCnt, iFirstPositionIndex + iCnt);
369  }
370 
371  if (iNumRows == 12) {
372  integer iFirstMomentumIndex = pNode->iGetFirstMomentumIndex();
373  for (integer iCnt = 1; iCnt <= 6; iCnt++) {
374  WM.PutRowIndex(6 + iCnt, iFirstMomentumIndex + iCnt);
375  }
376  }
377 
378  AssMats(WM, WM, dCoef, g, GravityAcceleration);
379 
380  return WorkMat;
381 }
382 
383 
384 void
386  VariableSubMatrixHandler& WorkMatB,
387  const VectorHandler& XCurr,
388  const VectorHandler& XPrimeCurr)
389 {
390  DEBUGCOUTFNAME("DynamicVariableBody::AssMats");
391 
392  /* Casting di WorkMat */
393  FullSubMatrixHandler& WMA = WorkMatA.SetFull();
394  FullSubMatrixHandler& WMB = WorkMatB.SetFull();
395 
396  Vec3 GravityAcceleration;
398  GravityAcceleration);
399 
400  integer iNumRows = 6;
401  if (g) {
402  iNumRows = 12;
403  }
404 
405  /* Dimensiona e resetta la matrice di lavoro */
406  WMA.ResizeReset(iNumRows, 6);
407  WMB.ResizeReset(6, 6);
408 
409  /* Setta gli indici della matrice - le incognite sono ordinate come:
410  * - posizione (3)
411  * - parametri di rotazione (3)
412  * - quantita' di moto (3)
413  * - momento della quantita' di moto
414  * e gli indici sono consecutivi. La funzione pGetFirstPositionIndex()
415  * ritorna il valore del primo indice -1, in modo che l'indice i-esimo
416  * e' dato da iGetFirstPositionIndex()+i */
417  integer iFirstPositionIndex = pNode->iGetFirstPositionIndex();
418  for (integer iCnt = 1; iCnt <= 6; iCnt++) {
419  WMA.PutRowIndex(iCnt, iFirstPositionIndex + iCnt);
420  WMA.PutColIndex(iCnt, iFirstPositionIndex + iCnt);
421 
422  WMB.PutRowIndex(iCnt, iFirstPositionIndex + iCnt);
423  WMB.PutColIndex(iCnt, iFirstPositionIndex + iCnt);
424  }
425 
426  if (g) {
427  integer iFirstMomentumIndex = pNode->iGetFirstMomentumIndex();
428  for (integer iCnt = 1; iCnt <= 6; iCnt++) {
429  WMA.PutRowIndex(6 + iCnt, iFirstMomentumIndex + iCnt);
430  }
431  }
432 
433  AssMats(WMA, WMB, 1., g, GravityAcceleration);
434 }
435 
436 
437 void
440  doublereal dCoef,
441  bool bGravity,
442  const Vec3& GravityAcceleration)
443 {
444  DEBUGCOUTFNAME("DynamicVariableBody::AssMats");
445 
446  const Vec3& V(pNode->GetVCurr());
447  const Vec3& W(pNode->GetWCurr());
448 
449  // STmp, JTmp computed by AssRes()
450  // const Mat3x3& R(pNode->GetRCurr());
451  // STmp = R*S0;
452  // JTmp = R*J0.MulMT(R);
453 
454  Mat3x3 SWedge(MatCross, STmp); /* S /\ */
455  Vec3 Sc(STmp*dCoef);
456 
457  /*
458  * momentum:
459  *
460  * m * I DeltaV - S /\ DeltagP + ( S /\ W ) /\ Deltag
461  */
462  WMB.IncCoef(1, 1, dMTmp);
463  WMB.IncCoef(2, 2, dMTmp);
464  WMB.IncCoef(3, 3, dMTmp);
465 
466  WMB.Sub(1, 3 + 1, SWedge);
467  WMA.Add(1, 3 + 1, Mat3x3(MatCross, Sc.Cross(W)));
468 
469  /*
470  * momenta moment:
471  *
472  * S /\ DeltaV + J DeltagP + ( V /\ S /\ - ( J * W ) /\ ) Deltag
473  */
474  WMB.Add(3 + 1, 1, SWedge);
475 
476  WMB.Add(3 + 1, 3 + 1, JTmp);
477  WMA.Add(3 + 1, 3 + 1, Mat3x3(MatCrossCross, V, Sc) - Mat3x3(MatCross, JTmp*(W*dCoef)));
478 
479  if (bGravity) {
480  WMA.Sub(9 + 1, 3 + 1, Mat3x3(MatCrossCross, GravityAcceleration, Sc));
481  }
482 
483  const RigidBodyKinematics *pRBK = pNode->pGetRBK();
484  if (pRBK) {
485  AssMatsRBK_int(WMA, WMB, dCoef, Sc);
486  }
487 }
488 
489 
492  doublereal /* dCoef */ ,
493  const VectorHandler& /* XCurr */ ,
494  const VectorHandler& /* XPrimeCurr */ )
495 {
496  DEBUGCOUTFNAME("VariableBody::DynamicAssRes");
497 
498  /* Se e' definita l'accelerazione di gravita',
499  * la aggiunge (solo al residuo) */
500  Vec3 GravityAcceleration;
502  GravityAcceleration);
503 
504  const RigidBodyKinematics *pRBK = pNode->pGetRBK();
505 
506  integer iNumRows = 12;
507  WorkVec.ResizeReset(iNumRows);
508 
509  integer iFirstPositionIndex = pNode->iGetFirstPositionIndex();
510  for (integer iCnt = 1; iCnt <= iNumRows; iCnt++) {
511  WorkVec.PutRowIndex(iCnt, iFirstPositionIndex + iCnt);
512  }
513 
514  const Vec3& V(pNode->GetVCurr());
515  const Vec3& W(pNode->GetWCurr());
516 
517  /* Aggiorna i suoi dati (saranno pronti anche per AssJac) */
518  const Mat3x3& R(pNode->GetRCurr());
519 
520  dMTmp = m_Mass.dGet();
521  Vec3 DXgc(R*m_Xgc.Get());
522  STmp = DXgc*dMTmp;
523  JTmp = R*(m_Jgc_vm.Get() + m_Jgc_vg.Get() - Mat3x3(MatCrossCross, STmp, DXgc)).MulMT(R);
524  Vec3 DXgcP(R*m_Xgc.GetP());
525  Vec3 XgcPTmp(V + W.Cross(DXgc) + DXgcP);
526 
527  /* Quantita' di moto: R[1] = Q - M * V - W /\ S */
528  WorkVec.Sub(1, XgcPTmp*dMTmp);
529 
530  /* Momento della quantita' di moto: R[2] = G - S /\ V - J * W */
531  WorkVec.Sub(3 + 1, JTmp*W + STmp.Cross(V));
532 
533  Vec3 mp_Xp_cm(XgcPTmp*m_Mass.dGetP());
534 
535  /* Variable mass correction */
536  WorkVec.Add(6 + 1, mp_Xp_cm);
537  WorkVec.Add(9 + 1, m_Jgc_vm.GetP()*W + DXgc.Cross(mp_Xp_cm) + V.Cross(DXgcP*dMTmp));
538 
539  if (g) {
540  WorkVec.Add(6 + 1, GravityAcceleration*dMTmp);
541  /* FIXME: this should go into Jacobian matrix
542  * as Gravity /\ S /\ Delta g */
543  WorkVec.Add(9 + 1, STmp.Cross(GravityAcceleration));
544  }
545 
546  if (pRBK) {
547  AssVecRBK_int(WorkVec);
548  }
549 
550  const DynamicStructNode *pDN = dynamic_cast<const DynamicStructNode *>(pNode);
551  ASSERT(pDN != 0);
552 
553  pDN->AddInertia(dMTmp, STmp, JTmp);
554 
555  return WorkVec;
556 }
557 
558 
559 /* Contributo allo jacobiano durante l'assemblaggio iniziale */
562  const VectorHandler& /* XCurr */ )
563 {
564  DEBUGCOUTFNAME("DynamicVariableBody::InitialAssJac");
565 
566  /* Casting di WorkMat */
567  FullSubMatrixHandler& WM = WorkMat.SetFull();
568 
569  /* Dimensiona e resetta la matrice di lavoro */
570  integer iNumRows = 0;
571  integer iNumCols = 0;
572  InitialWorkSpaceDim(&iNumRows, &iNumCols);
573  WM.ResizeReset(iNumRows, iNumCols);
574 
575  /* Setta gli indici della matrice - le incognite sono ordinate come:
576  * - posizione (3)
577  * - parametri di rotazione (3)
578  * - quantita' di moto (3)
579  * - momento della quantita' di moto
580  * e gli indici sono consecutivi. La funzione pGetFirstPositionIndex()
581  * ritorna il valore del primo indice -1, in modo che l'indice i-esimo
582  * e' dato da iGetFirstPositionIndex()+i */
583  integer iFirstPositionIndex = pNode->iGetFirstPositionIndex();
584  integer iFirstVelocityIndex = iFirstPositionIndex + 6;
585  for (integer iCnt = 1; iCnt <= 6; iCnt++) {
586  WM.PutRowIndex(iCnt, iFirstPositionIndex + iCnt);
587  WM.PutRowIndex(6 + iCnt, iFirstVelocityIndex + iCnt);
588  WM.PutColIndex(iCnt, iFirstPositionIndex + iCnt);
589  }
590 
591  /* Prepara matrici e vettori */
592 
593  /* Velocita' angolare corrente */
594  const Vec3& W(pNode->GetWRef());
595 
596  Vec3 SWedgeW(STmp.Cross(W));
597  Mat3x3 WWedgeSWedge(MatCrossCross, -W, STmp);
598  Mat3x3 WWedge(MatCross, W);
599  Mat3x3 WWedgeWWedgeSWedge(W.Cross(WWedgeSWedge));
600  Mat3x3 FDeltaW(Mat3x3(MatCross, SWedgeW) + WWedgeSWedge);
601 
602  // STmp, JTmp computed by InitialAssRes()
603  Vec3 JW(JTmp*W);
604  Mat3x3 JWWedge(MatCross, JW);
605  Mat3x3 MDeltag(W.Cross(JTmp*WWedge - JWWedge));
606  Mat3x3 MDeltaW(W.Cross(JTmp) - JWWedge);
607 
608  /* Forza */
609  WM.Add(1, 1, WWedgeWWedgeSWedge);
610  WM.Add(1, 4, FDeltaW);
611 
612  /* Momento */
613  WM.Add(4, 1, MDeltag);
614  WM.Add(4, 4, MDeltaW);
615 
616  /* Derivata forza */
617  WM.Add(7, 1, Mat3x3(MatCross, W.Cross(SWedgeW)) + W.Cross(FDeltaW));
618  WM.Add(7, 4, W.Cross(WWedgeWWedgeSWedge));
619 
620  /* Derivata Momento */
621  WM.Add(4, 1, W.Cross(MDeltag));
622  WM.Add(4, 4, W.Cross(MDeltaW) - Mat3x3(MatCross, W.Cross(JW)));
623 
624  return WorkMat;
625 }
626 
627 
628 /* Contributo al residuo durante l'assemblaggio iniziale */
631  const VectorHandler& /* XCurr */ )
632 {
633  DEBUGCOUTFNAME("DynamicVariableBody::InitialAssRes");
634 
635  integer iNumRows;
636  integer iNumCols;
637  InitialWorkSpaceDim(&iNumRows, &iNumCols);
638  WorkVec.ResizeReset(iNumRows);
639 
640  integer iFirstPositionIndex = pNode->iGetFirstPositionIndex();
641  for (integer iCnt = 1; iCnt <= 12; iCnt++) {
642  WorkVec.PutRowIndex(iCnt, iFirstPositionIndex+iCnt);
643  }
644 
645  const Vec3& X(pNode->GetXCurr());
646  const Vec3& W(pNode->GetWCurr());
647 
648  // Aggiorna i suoi dati (saranno pronti anche per InitialAssJac)
649  const Mat3x3& R(pNode->GetRCurr());
650 
651  dMTmp = m_Mass.dGet();
652  Vec3 DXgc(R*m_Xgc.Get());
653  STmp = DXgc*dMTmp;
654  JTmp = R*(m_Jgc_vm.Get() + m_Jgc_vg.Get() - Mat3x3(MatCrossCross, DXgc, STmp)).MulMT(R);
655 
656  Vec3 FC(-W.Cross(W.Cross(STmp)));
657  Vec3 MC(-W.Cross(JTmp*W));
658 
659  /* Forza */
660  WorkVec.Add(1, FC);
661 
662  /* Momento */
663  WorkVec.Add(4, MC);
664 
665  /* Derivata forza */
666  WorkVec.Add(7, W.Cross(FC));
667 
668  /* Derivata momento */
669  WorkVec.Add(10, W.Cross(MC));
670 
671  /* Se e' definita l'accelerazione di gravita',
672  * la aggiunge (solo al residuo) */
673  Vec3 GravityAcceleration;
674  if (GravityOwner::bGetGravity(X + DXgc, GravityAcceleration)) {
675  WorkVec.Add(1, GravityAcceleration*dMTmp);
676  WorkVec.Add(3 + 1, STmp.Cross(GravityAcceleration));
677  WorkVec.Add(9 + 1, (W.Cross(STmp)).Cross(GravityAcceleration));
678  }
679 
680  return WorkVec;
681 }
682 
683 
684 /* Usata per inizializzare la quantita' di moto */
685 void
687  VectorHandler& X, VectorHandler& /* XP */ ,
689 {
690  integer iFirstIndex = pNode->iGetFirstMomentumIndex();
691 
692  // TODO: make configurable
693  const Vec3& V(pNode->GetVCurr());
694  const Vec3& W(pNode->GetWCurr());
695  const Mat3x3& R(pNode->GetRCurr());
696 
697  dMTmp = m_Mass.dGet();
698  Vec3 DXgc(R*m_Xgc.Get());
699  STmp = DXgc*dMTmp;
700  JTmp = R*(m_Jgc_vm.Get() + m_Jgc_vg.Get() - Mat3x3(MatCrossCross, DXgc, STmp)).MulMT(R);
701  X.Add(iFirstIndex + 1, V*dMTmp + W.Cross(STmp));
702  // NOTE: does not start correctly if m_Xgc.GetP() != 0 at initial time...
703  // X.Add(iFirstIndex + 1, (V + R*m_Xgc.GetP())*dMTmp + W.Cross(STmp));
704  X.Add(iFirstIndex + 4, STmp.Cross(V) + JTmp*W);
705 }
706 
707 /* momentum */
708 Vec3
710 {
711  const Vec3& V(pNode->GetVCurr());
712  const Vec3& W(pNode->GetWCurr());
713  const Mat3x3& R(pNode->GetRCurr());
714 
715  return (V + W.Cross(R*m_Xgc.Get()))*m_Mass.dGet();
716 }
717 
718 
719 /* momenta moment */
720 Vec3
722 {
723  const Vec3& X(pNode->GetXCurr());
724  const Mat3x3& R(pNode->GetRCurr());
725  const Vec3& V(pNode->GetVCurr());
726  const Vec3& W(pNode->GetWCurr());
727 
728  Vec3 DXgc(R*m_Xgc.Get());
729 
730  // NOTE: with respect to the origin of the global reference frame!
731  return (X + DXgc).Cross((V + W.Cross(R*DXgc))*m_Mass.dGet()) + (m_Jgc_vm.Get() + m_Jgc_vg.Get())*W;
732 }
733 
734 /* DynamicVariableBody - end */
735 
736 
737 /* StaticVariableBody - begin */
738 
740  const StaticStructNode* pNode,
741  const DriveCaller *pDCMass,
742  const TplDriveCaller<Vec3> *pDCXgc,
743  const TplDriveCaller<Mat3x3> *pDCJgc_vm,
744  const TplDriveCaller<Mat3x3> *pDCJgc_vg,
745  flag fOut)
746 : Elem(uL, fOut),
747 VariableBody(uL, pNode, pDCMass, pDCXgc, pDCJgc_vm, pDCJgc_vg, fOut)
748 {
749  NO_OP;
750 }
751 
752 
753 /* distruttore */
755 {
756  NO_OP;
757 }
758 
759 
762  doublereal dCoef,
763  const VectorHandler& XCurr,
764  const VectorHandler& XPrimeCurr)
765 {
766  DEBUGCOUTFNAME("StaticVariableBody::AssJac");
767 
768  /* Casting di WorkMat */
769  FullSubMatrixHandler& WM = WorkMat.SetFull();
770 
771  /* Dimensiona e resetta la matrice di lavoro */
772  WM.ResizeReset(6, 6);
773 
774  /* Setta gli indici della matrice - le incognite sono ordinate come:
775  * - posizione (3)
776  * - parametri di rotazione (3)
777  * - quantita' di moto (3)
778  * - momento della quantita' di moto
779  * e gli indici sono consecutivi. La funzione pGetFirstPositionIndex()
780  * ritorna il valore del primo indice -1, in modo che l'indice i-esimo
781  * e' dato da iGetFirstPositionIndex() + i */
782  integer iFirstPositionIndex = pNode->iGetFirstPositionIndex();
783  integer iFirstMomentumIndex = pNode->iGetFirstMomentumIndex();
784  for (integer iCnt = 1; iCnt <= 6; iCnt++) {
785  WM.PutRowIndex(iCnt, iFirstMomentumIndex + iCnt);
786  WM.PutColIndex(iCnt, iFirstPositionIndex + iCnt);
787  }
788 
789  if (AssMats(WM, WM, dCoef)) {
790  WorkMat.SetNullMatrix();
791  }
792 
793  return WorkMat;
794 }
795 
796 
797 void
799  VariableSubMatrixHandler& WorkMatB,
800  const VectorHandler& XCurr,
801  const VectorHandler& XPrimeCurr)
802 {
803  DEBUGCOUTFNAME("StaticVariableBody::AssMats");
804 
805  /* Casting di WorkMat */
806  FullSubMatrixHandler& WMA = WorkMatA.SetFull();
807  FullSubMatrixHandler& WMB = WorkMatB.SetFull();
808 
809  /* Dimensiona e resetta la matrice di lavoro */
810  WMA.ResizeReset(6, 6);
811  WMB.ResizeReset(6, 6);
812 
813  /* Setta gli indici della matrice - le incognite sono ordinate come:
814  * - posizione (3)
815  * - parametri di rotazione (3)
816  * - quantita' di moto (3)
817  * - momento della quantita' di moto
818  * e gli indici sono consecutivi. La funzione pGetFirstPositionIndex()
819  * ritorna il valore del primo indice -1, in modo che l'indice i-esimo
820  * e' dato da iGetFirstPositionIndex() + i */
821  integer iFirstPositionIndex = pNode->iGetFirstPositionIndex();
822  integer iFirstMomentumIndex = pNode->iGetFirstMomentumIndex();
823  for (integer iCnt = 1; iCnt <= 6; iCnt++) {
824  WMA.PutRowIndex(iCnt, iFirstMomentumIndex + iCnt);
825  WMA.PutColIndex(iCnt, iFirstPositionIndex + iCnt);
826 
827  WMB.PutRowIndex(iCnt, iFirstMomentumIndex + iCnt);
828  WMB.PutColIndex(iCnt, iFirstPositionIndex + iCnt);
829  }
830 
831  if (AssMats(WMA, WMB, 1.)) {
832  WorkMatA.SetNullMatrix();
833  WorkMatB.SetNullMatrix();
834  }
835 }
836 
837 bool
840  doublereal dCoef)
841 {
842  DEBUGCOUTFNAME("StaticVariableBody::AssMats");
843 
844  /* Se e' definita l'accelerazione di gravita',
845  * la aggiunge (solo al residuo) */
846  Vec3 Acceleration(Zero3);
847  bool g = GravityOwner::bGetGravity(pNode->GetXCurr(), Acceleration);
848 
849  /* TODO: reference */
850  Vec3 W(Zero3);
851 
852  const RigidBodyKinematics *pRBK = pNode->pGetRBK();
853 
854  if (!g && !pRBK) {
855  /* Caller will set WMA & WMB to null matrix */
856  return true;
857  }
858 
859  Vec3 Sc(STmp*dCoef);
860 
861  if (g) {
862  WMA.Add(3 + 1, 3 + 1, Mat3x3(MatCrossCross, Acceleration, Sc));
863  }
864 
865  if (pRBK) {
866  AssMatsRBK_int(WMA, WMB, dCoef, Sc);
867  }
868 
869  return false;
870 }
871 
872 
875  doublereal /* dCoef */ ,
876  const VectorHandler& /* XCurr */ ,
877  const VectorHandler& /* XPrimeCurr */ )
878 {
879  DEBUGCOUTFNAME("StaticVariableBody::AssRes");
880 
881  const Vec3& X(pNode->GetXCurr());
882  const Mat3x3& R(pNode->GetRCurr());
883  Vec3 DXgc(R*m_Xgc.Get());
884 
885  /* Se e' definita l'accelerazione di gravita',
886  * la aggiunge (solo al residuo) */
887  Vec3 Acceleration(Zero3);
888  bool g = GravityOwner::bGetGravity(X + DXgc, Acceleration);
889 
890  /* W is uninitialized because its use is conditioned by w */
891  const RigidBodyKinematics *pRBK = pNode->pGetRBK();
892 
893  if (!g && !pRBK) {
894  WorkVec.Resize(0);
895  return WorkVec;
896  }
897 
898  WorkVec.ResizeReset(6);
899 
900  integer iFirstMomentumIndex = pNode->iGetFirstMomentumIndex();
901  for (integer iCnt = 1; iCnt <= 6; iCnt++) {
902  WorkVec.PutRowIndex(iCnt, iFirstMomentumIndex + iCnt);
903  }
904 
905  dMTmp = m_Mass.dGet();
906  STmp = DXgc*dMTmp;
907  JTmp = R*(m_Jgc_vm.Get() + m_Jgc_vg.Get() - Mat3x3(MatCrossCross, STmp, DXgc)).MulMT(R);
908 
909  if (g) {
910  WorkVec.Add(1, Acceleration*dMTmp);
911  WorkVec.Add(3 + 1, STmp.Cross(Acceleration));
912  }
913 
914  if (pRBK) {
915  AssVecRBK_int(WorkVec);
916  }
917 
918  return WorkVec;
919 }
920 
921 
922 /* inverse dynamics capable element */
923 bool
925 {
926  return true;
927 }
928 
929 
932  const VectorHandler& /* XCurr */ ,
933  const VectorHandler& /* XPrimeCurr */ ,
934  const VectorHandler& /* XPrimePrimeCurr */ ,
935  InverseDynamics::Order iOrder)
936 {
937  DEBUGCOUTFNAME("DynamicVariableBody::AssRes");
938 
940 
941  const Vec3& X(pNode->GetXCurr());
942  const Mat3x3& R(pNode->GetRCurr());
943  Vec3 DXgc(R*m_Xgc.Get());
944 
945  /* Se e' definita l'accelerazione di gravita', la aggiunge */
946  Vec3 GravityAcceleration;
947  bool g = GravityOwner::bGetGravity(X + DXgc, GravityAcceleration);
948 
949  WorkVec.ResizeReset(6);
950 
951  integer iFirstPositionIndex = pNode->iGetFirstPositionIndex();
952  for (integer iCnt = 1; iCnt <= 6; iCnt++) {
953  WorkVec.PutRowIndex(iCnt, iFirstPositionIndex + iCnt);
954  }
955 
956  dMTmp = m_Mass.dGet();
957  STmp = DXgc*dMTmp;
958  JTmp = R*(m_Jgc_vm.Get() + m_Jgc_vg.Get() - Mat3x3(MatCrossCross, STmp, DXgc)).MulMT(R);
959 
960  Vec3 Acceleration = pNode->GetXPPCurr()
961  + pNode->GetWPCurr().Cross(DXgc)
962  + pNode->GetWCurr().Cross(pNode->GetWCurr().Cross(DXgc));
963  if (g) {
964  Acceleration -= GravityAcceleration;
965  }
966 
967  WorkVec.Sub(1, Acceleration*dMTmp);
968 
969  Vec3 M = JTmp*pNode->GetWPCurr()
970  + STmp.Cross(pNode->GetXPPCurr())
971  + STmp.Cross(pNode->GetWCurr().Cross(pNode->GetWCurr().Cross(DXgc)));
972  if (g) {
973  M -= STmp.Cross(GravityAcceleration);
974  }
975 
976  WorkVec.Sub(4, M);
977 
978  return WorkVec;
979 }
980 
981 
982 /* Contributo allo jacobiano durante l'assemblaggio iniziale */
985  const VectorHandler& /* XCurr */ )
986 {
987  DEBUGCOUTFNAME("StaticVariableBody::InitialAssJac");
988 
989  WorkMat.SetNullMatrix();
990 
991  return WorkMat;
992 }
993 
994 
995 /* Contributo al residuo durante l'assemblaggio iniziale */
998  const VectorHandler& /* XCurr */ )
999 {
1000  DEBUGCOUTFNAME("StaticVariableBody::InitialAssRes");
1001 
1002  WorkVec.ResizeReset(0);
1003 
1004  return WorkVec;
1005 }
1006 
1007 
1008 /* Usata per inizializzare la quantita' di moto */
1009 void
1011  VectorHandler& X, VectorHandler& /* XP */ ,
1013 {
1014  NO_OP;
1015 }
1016 
1017 /* StaticVariableBody - end */
1018 
1019 
1020 /* Legge un corpo rigido */
1021 Elem*
1022 ReadVariableBody(DataManager* pDM, MBDynParser& HP, unsigned int uLabel,
1023  const StructNode* pStrNode)
1024 {
1025  DEBUGCOUTFNAME("ReadVariableBody");
1026 
1027  /* may be determined by a special DataManager parameter... */
1028  bool bStaticModel = pDM->bIsStaticModel();
1029  bool bInverseDynamics = pDM->bIsInverseDynamics();
1030 
1031  ReferenceFrame RF(pStrNode);
1032 
1033  const DriveCaller *pDCMass = HP.GetDriveCaller();
1034  if (!pDCMass->bIsDifferentiable()) {
1035  silent_cerr("VariableBody(" << uLabel << "): "
1036  "mass drive caller must be differentiable "
1037  "at line " << HP.GetLineData() << std::endl);
1039  }
1040 
1041  const TplDriveCaller<Vec3> *pDCXgc = HP.GetTplDriveCaller<Vec3>();
1042  if (!pDCXgc->bIsDifferentiable()) {
1043  silent_cerr("VariableBody(" << uLabel << "): "
1044  "center of mass drive caller must be differentiable "
1045  "at line " << HP.GetLineData() << std::endl);
1047  }
1048 
1049  const TplDriveCaller<Mat3x3> *pDCJgc_vm = HP.GetTplDriveCaller<Mat3x3>();
1050  if (!pDCJgc_vm->bIsDifferentiable()) {
1051  silent_cerr("VariableBody(" << uLabel << "): "
1052  "mass-related inertia matrix drive caller must be differentiable "
1053  "at line " << HP.GetLineData() << std::endl);
1055  }
1056 
1057  const TplDriveCaller<Mat3x3> *pDCJgc_vg = HP.GetTplDriveCaller<Mat3x3>();
1058  if (!pDCJgc_vg->bIsDifferentiable()) {
1059  silent_cerr("VariableBody(" << uLabel << "): "
1060  "geometry-related inertia matrix drive caller must be differentiable "
1061  "at line " << HP.GetLineData() << std::endl);
1063  }
1064 
1065  const DynamicStructNode* pDynamicNode = 0;
1066  const StaticStructNode* pStaticNode = 0;
1067 
1068  if (bStaticModel || bInverseDynamics) {
1069  /* static */
1070  pStaticNode = dynamic_cast<const StaticStructNode *>(pStrNode);
1071  if (pStaticNode == 0 || pStaticNode->GetStructNodeType() != StructNode::STATIC) {
1072  silent_cerr("VariableBody(" << uLabel << "): "
1073  "illegal structural node type "
1074  "for StructNode(" << pStrNode->GetLabel() << ") "
1075  "in " << (bStaticModel ? "static model" : "inverse dynamics") << " analysis "
1076  "at line " << HP.GetLineData() << std::endl);
1078  }
1079 
1080  } else {
1081  pDynamicNode = dynamic_cast<const DynamicStructNode*>(pStrNode);
1082  if (pDynamicNode == 0 || pDynamicNode->GetStructNodeType() != StructNode::DYNAMIC) {
1083  silent_cerr("VariableBody(" << uLabel << "): "
1084  "illegal structural node type "
1085  "for StructNode(" << pStrNode->GetLabel() << ") "
1086  "at line " << HP.GetLineData() << std::endl);
1088  }
1089  }
1090 
1091  flag fOut = pDM->fReadOutput(HP, Elem::BODY);
1092 
1093  /* Allocazione e costruzione */
1094  Elem *pEl = 0;
1095  if (bStaticModel || bInverseDynamics) {
1096  /* static */
1098  StaticVariableBody(uLabel, pStaticNode,
1099  pDCMass, pDCXgc, pDCJgc_vm, pDCJgc_vg, fOut));
1100 
1101  } else {
1103  DynamicVariableBody(uLabel, pDynamicNode,
1104  pDCMass, pDCXgc, pDCJgc_vm, pDCJgc_vg, fOut));
1105  }
1106 
1107  pDM->GetLogFile()
1108  << "variable body: " << uLabel
1109  << ' ' << pStrNode->GetLabel()
1110  << ' ' << pDCMass->dGet()
1111  << ' ' << pDCXgc->Get()
1112  << ' ' << pDCJgc_vm->Get()
1113  << ' ' << pDCJgc_vg->Get()
1114  << std::endl;
1115 
1116  /* Se non c'e' il punto e virgola finale */
1117  if (HP.IsArg()) {
1118  silent_cerr("VariableBody(" << uLabel << "): semicolon expected "
1119  "at line " << HP.GetLineData() << std::endl);
1121  }
1122 
1123  return pEl;
1124 } /* End of ReadVariableBody() */
1125 
flag fReadOutput(MBDynParser &HP, const T &t) const
Definition: dataman.h:1064
RFType RF
Definition: mbpar.h:166
virtual void InitialWorkSpaceDim(integer *piNumRows, integer *piNumCols) const
Definition: body_vm.h:177
void PutColIndex(integer iSubCol, integer iCol)
Definition: submat.h:325
TplDriveCaller< T > * GetTplDriveCaller(void)
Definition: mbpar.cc:2112
const Vec3 Zero3(0., 0., 0.)
Vec3 Cross(const Vec3 &v) const
Definition: matvec3.h:218
virtual ~StaticVariableBody(void)
Definition: body_vm.cc:754
long int flag
Definition: mbdyn.h:43
virtual const Mat3x3 & GetRRef(void) const
Definition: strnode.h:1006
#define MBDYN_EXCEPT_ARGS
Definition: except.h:63
Definition: matvec3.h:98
virtual ~VariableBody(void)
Definition: body_vm.cc:65
#define DEBUGCOUTFNAME(fname)
Definition: myassert.h:256
DriveOwner m_Mass
Definition: body_vm.h:50
virtual void ResizeReset(integer)
Definition: vh.cc:55
virtual StructNode::Type GetStructNodeType(void) const
Definition: strnode.cc:3293
const MatCross_Manip MatCross
Definition: matvec3.cc:639
Mat3x3 JTmp
Definition: body_vm.h:57
FullSubMatrixHandler & SetFull(void)
Definition: submat.h:1168
virtual const Mat3x3 & GetRCurr(void) const
Definition: strnode.h:1012
virtual Node::Type GetNodeType(void) const
Definition: strnode.cc:145
virtual T Get(const doublereal &dVar) const =0
doublereal Dot(const Vec3 &v) const
Definition: matvec3.h:243
void Add(integer iRow, integer iCol, const Vec3 &v)
Definition: submat.cc:209
virtual doublereal dGetPrivData(unsigned int i) const
Definition: body_vm.cc:167
virtual void Sub(integer iRow, const Vec3 &v)
Definition: vh.cc:78
virtual const Vec3 & GetXPP(void) const =0
void AssMatsRBK_int(FullSubMatrixHandler &WMA, FullSubMatrixHandler &WMB, const doublereal &dCoef, const Vec3 &Sc)
Definition: body_vm.cc:239
void AssVecRBK_int(SubVectorHandler &WorkVec)
Definition: body_vm.cc:197
Vec3 GetS_int(void) const
Definition: body_vm.cc:73
virtual bool bGetGravity(const Vec3 &X, Vec3 &Acc) const
Definition: gravity.cc:208
virtual SubVectorHandler & InitialAssRes(SubVectorHandler &WorkVec, const VectorHandler &XCurr)
Definition: body_vm.cc:630
TplDriveOwner< Mat3x3 > m_Jgc_vm
Definition: body_vm.h:52
virtual const Vec3 & GetWRef(void) const
Definition: strnode.h:1024
#define NO_OP
Definition: myassert.h:74
virtual VariableSubMatrixHandler & InitialAssJac(VariableSubMatrixHandler &WorkMat, const VectorHandler &XCurr)
Definition: body_vm.cc:984
std::vector< Hint * > Hints
Definition: simentity.h:89
bool bIsStaticModel(void) const
Definition: dataman.h:482
doublereal dGetP(const doublereal &dVar) const
Definition: drive.cc:683
void IncCoef(integer iRow, integer iCol, const doublereal &dCoef)
Definition: submat.h:683
virtual std::ostream & Restart(std::ostream &out) const =0
Mat3x3 GetJ(void) const
Definition: body_vm.cc:131
Mat3x3 GetJ_int(void) const
Definition: body_vm.cc:81
virtual SubVectorHandler & AssRes(SubVectorHandler &WorkVec, doublereal dCoef, const VectorHandler &XCurr, const VectorHandler &XPrimeCurr)
Definition: body_vm.cc:491
virtual T GetP(void) const
Definition: tpldrive.h:121
doublereal dMTmp
Definition: body_vm.h:55
virtual void PutRowIndex(integer iSubRow, integer iRow)=0
Vec3 GetB_int(void) const
Definition: body_vm.cc:709
virtual SubVectorHandler & AssRes(SubVectorHandler &WorkVec, doublereal dCoef, const VectorHandler &XCurr, const VectorHandler &XPrimeCurr)
Definition: body_vm.cc:874
bool AssMats(FullSubMatrixHandler &WorkMatA, FullSubMatrixHandler &WorkMatB, doublereal dCoef)
Definition: body_vm.cc:838
VariableBody(unsigned int uL, const StructNode *pNode, const DriveCaller *pDCMass, const TplDriveCaller< Vec3 > *pDCXgc, const TplDriveCaller< Mat3x3 > *pDCJgc_vm, const TplDriveCaller< Mat3x3 > *pDCJgc_vg, flag fOut)
Definition: body_vm.cc:43
virtual VariableSubMatrixHandler & AssJac(VariableSubMatrixHandler &WorkMat, doublereal dCoef, const VectorHandler &XCurr, const VectorHandler &XPrimeCurr)
Definition: body_vm.cc:761
TplDriveCaller< T > * pGetDriveCaller(void) const
Definition: tpldrive.h:105
TplDriveOwner< Mat3x3 > m_Jgc_vg
Definition: body_vm.h:53
virtual bool bIsDifferentiable(void) const
Definition: drive.h:495
virtual SubVectorHandler & InitialAssRes(SubVectorHandler &WorkVec, const VectorHandler &XCurr)
Definition: body_vm.cc:997
virtual void SetValue(DataManager *pDM, VectorHandler &X, VectorHandler &XP, SimulationEntity::Hints *ph=0)
Definition: body_vm.cc:1010
DataManager * pDM
Definition: mbpar.h:252
virtual bool bIsDifferentiable(void) const
Definition: tpldrive.h:65
void SetNullMatrix(void)
Definition: submat.h:1159
T Get(const doublereal &dVar) const
Definition: tpldrive.h:109
virtual VariableSubMatrixHandler & InitialAssJac(VariableSubMatrixHandler &WorkMat, const VectorHandler &XCurr)
Definition: body_vm.cc:561
virtual void AfterPredict(VectorHandler &X, VectorHandler &XP)
Definition: body_vm.cc:105
DynamicVariableBody(unsigned int uL, const DynamicStructNode *pNodeTmp, const DriveCaller *pDCMass, const TplDriveCaller< Vec3 > *pDCXgc, const TplDriveCaller< Mat3x3 > *pDCJgc_vm, const TplDriveCaller< Mat3x3 > *pDCJgc_vg, flag fOut)
Definition: body_vm.cc:313
virtual integer iGetFirstMomentumIndex(void) const =0
virtual integer iGetFirstPositionIndex(void) const
Definition: strnode.h:452
virtual const Vec3 & GetWCurr(void) const
Definition: strnode.h:1030
virtual bool bInverseDynamics(void) const
Definition: body_vm.cc:924
virtual ~DynamicVariableBody(void)
Definition: body_vm.cc:328
Vec3 GetG_int(void) const
Definition: body_vm.cc:721
virtual const Vec3 & GetWPCurr(void) const
Definition: strnode.h:1042
virtual std::ostream & Restart(std::ostream &out) const =0
#define ASSERT(expression)
Definition: colamd.c:977
const RigidBodyKinematics * pGetRBK(void) const
Definition: strnode.cc:152
VectorExpression< VectorCrossExpr< VectorLhsExpr, VectorRhsExpr >, 3 > Cross(const VectorExpression< VectorLhsExpr, 3 > &u, const VectorExpression< VectorRhsExpr, 3 > &v)
Definition: matvec.h:3248
Vec3 GetS(void) const
Definition: body_vm.cc:124
Vec3 STmp
Definition: body_vm.h:56
#define SAFENEWWITHCONSTRUCTOR(pnt, item, constructor)
Definition: mynewmem.h:698
DriveCaller * pGetDriveCaller(void) const
Definition: drive.cc:658
virtual const Vec3 & GetXCurr(void) const
Definition: strnode.h:310
virtual void Add(integer iRow, const Vec3 &v)
Definition: vh.cc:63
virtual void ResizeReset(integer, integer)
Definition: submat.cc:182
virtual const Vec3 & GetWP(void) const =0
virtual doublereal dGet(const doublereal &dVar) const =0
TplDriveOwner< Vec3 > m_Xgc
Definition: body_vm.h:51
const MatCrossCross_Manip MatCrossCross
Definition: matvec3.cc:640
virtual VariableSubMatrixHandler & AssJac(VariableSubMatrixHandler &WorkMat, doublereal dCoef, const VectorHandler &XCurr, const VectorHandler &XPrimeCurr)
Definition: body_vm.cc:335
virtual bool IsArg(void)
Definition: parser.cc:807
Definition: elem.h:75
const StructNode * pNode
Definition: body_vm.h:48
virtual const Vec3 & GetW(void) const =0
void PutRowIndex(integer iSubRow, integer iRow)
Definition: submat.h:311
virtual std::ostream & Restart(std::ostream &out) const
Definition: body_vm.cc:92
std::ostream & GetLogFile(void) const
Definition: dataman.h:326
doublereal dGet(const doublereal &dVar) const
Definition: drive.cc:664
Elem * ReadVariableBody(DataManager *pDM, MBDynParser &HP, unsigned int uLabel, const StructNode *pStrNode)
Definition: body_vm.cc:1022
virtual const Vec3 & GetVCurr(void) const
Definition: strnode.h:322
void Sub(integer iRow, integer iCol, const Vec3 &v)
Definition: submat.cc:215
virtual unsigned int iGetPrivDataIdx(const char *s) const
Definition: body_vm.cc:151
DriveCaller * GetDriveCaller(bool bDeferred=false)
Definition: mbpar.cc:2033
static const doublereal a
Definition: hfluid_.h:289
doublereal dGetM(void) const
Definition: body_vm.cc:117
virtual const Vec3 & GetXPPCurr(void) const
Definition: strnode.h:334
virtual unsigned int iGetNumPrivData(void) const
Definition: body_vm.cc:145
void AssMats(FullSubMatrixHandler &WorkMatA, FullSubMatrixHandler &WorkMatB, doublereal dCoef, bool bGravity, const Vec3 &GravityAcceleration)
Definition: body_vm.cc:438
StaticVariableBody(unsigned int uL, const StaticStructNode *pNode, const DriveCaller *pDCMass, const TplDriveCaller< Vec3 > *pDCXgc, const TplDriveCaller< Mat3x3 > *pDCJgc_vm, const TplDriveCaller< Mat3x3 > *pDCJgc_vg, flag fOut)
Definition: body_vm.cc:739
double doublereal
Definition: colamd.c:52
long int integer
Definition: colamd.c:51
virtual HighParser::ErrOut GetLineData(void) const
Definition: parsinc.cc:697
virtual StructNode::Type GetStructNodeType(void) const
Definition: strnode.cc:2946
unsigned int GetLabel(void) const
Definition: withlab.cc:62
virtual void SetValue(DataManager *pDM, VectorHandler &X, VectorHandler &XP, SimulationEntity::Hints *ph=0)
Definition: body_vm.cc:686
const StructNode * pGetNode(void) const
Definition: body_vm.cc:138
virtual void Resize(integer iNewSize)=0
Mat3x3 R
virtual void AddInertia(const doublereal &dm, const Vec3 &dS, const Mat3x3 &dJ) const
Definition: strnode.cc:3092
bool bIsInverseDynamics(void) const
Definition: dataman.h:493