MBDyn-1.7.3
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Groups
DynamicVariableBody Class Reference

#include <body_vm.h>

Inheritance diagram for DynamicVariableBody:
Collaboration diagram for DynamicVariableBody:

Public Member Functions

 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)
 
virtual ~DynamicVariableBody (void)
 
void WorkSpaceDim (integer *piNumRows, integer *piNumCols) const
 
virtual VariableSubMatrixHandlerAssJac (VariableSubMatrixHandler &WorkMat, doublereal dCoef, const VectorHandler &XCurr, const VectorHandler &XPrimeCurr)
 
void AssMats (VariableSubMatrixHandler &WorkMatA, VariableSubMatrixHandler &WorkMatB, const VectorHandler &XCurr, const VectorHandler &XPrimeCurr)
 
virtual SubVectorHandlerAssRes (SubVectorHandler &WorkVec, doublereal dCoef, const VectorHandler &XCurr, const VectorHandler &XPrimeCurr)
 
virtual void InitialWorkSpaceDim (integer *piNumRows, integer *piNumCols) const
 
virtual VariableSubMatrixHandlerInitialAssJac (VariableSubMatrixHandler &WorkMat, const VectorHandler &XCurr)
 
virtual SubVectorHandlerInitialAssRes (SubVectorHandler &WorkVec, const VectorHandler &XCurr)
 
virtual void SetValue (DataManager *pDM, VectorHandler &X, VectorHandler &XP, SimulationEntity::Hints *ph=0)
 
virtual void GetConnectedNodes (std::vector< const Node * > &connectedNodes) const
 
- Public Member Functions inherited from Elem
 Elem (unsigned int uL, flag fOut)
 
virtual ~Elem (void)
 
virtual unsigned int iGetNumDof (void) const
 
virtual std::ostream & DescribeDof (std::ostream &out, const char *prefix="", bool bInitial=false) const
 
virtual void DescribeDof (std::vector< std::string > &desc, bool bInitial=false, int i=-1) const
 
virtual std::ostream & DescribeEq (std::ostream &out, const char *prefix="", bool bInitial=false) const
 
virtual void DescribeEq (std::vector< std::string > &desc, bool bInitial=false, int i=-1) const
 
virtual DofOrder::Order GetDofType (unsigned int) const
 
virtual bool bInverseDynamics (void) const
 
void SetInverseDynamicsFlags (unsigned uIDF)
 
unsigned GetInverseDynamicsFlags (void) const
 
bool bIsErgonomy (void) const
 
bool bIsRightHandSide (void) const
 
virtual VariableSubMatrixHandlerAssJac (VariableSubMatrixHandler &WorkMat, const VectorHandler &XCurr)
 
virtual SubVectorHandlerAssRes (SubVectorHandler &WorkVec, const VectorHandler &XCurr, const VectorHandler &XPrimeCurr, const VectorHandler &XPrimePrimeCurr, InverseDynamics::Order iOrder=InverseDynamics::INVERSE_DYNAMICS)
 
virtual int GetNumConnectedNodes (void) const
 
- Public Member Functions inherited from WithLabel
 WithLabel (unsigned int uL=0, const std::string &sN="")
 
virtual ~WithLabel (void)
 
void PutLabel (unsigned int uL)
 
void PutName (const std::string &sN)
 
unsigned int GetLabel (void) const
 
const std::string & GetName (void) const
 
- Public Member Functions inherited from SimulationEntity
 SimulationEntity (void)
 
virtual ~SimulationEntity (void)
 
virtual bool bIsValidIndex (unsigned int i) const
 
virtual DofOrder::Order GetEqType (unsigned int i) const
 
virtual HintParseHint (DataManager *pDM, const char *s) const
 
virtual void BeforePredict (VectorHandler &, VectorHandler &, VectorHandler &, VectorHandler &) const
 
virtual void Update (const VectorHandler &XCurr, const VectorHandler &XPrimeCurr)
 
virtual void DerivativesUpdate (const VectorHandler &XCurr, const VectorHandler &XPrimeCurr)
 
virtual void Update (const VectorHandler &XCurr, InverseDynamics::Order iOrder)
 
virtual void AfterConvergence (const VectorHandler &X, const VectorHandler &XP)
 
virtual void AfterConvergence (const VectorHandler &X, const VectorHandler &XP, const VectorHandler &XPP)
 
virtual std::ostream & OutputAppend (std::ostream &out) const
 
virtual void ReadInitialState (MBDynParser &HP)
 
- Public Member Functions inherited from ToBeOutput
 ToBeOutput (flag fOut=fDefaultOut)
 
virtual ~ToBeOutput (void)
 
virtual void OutputPrepare (OutputHandler &OH)
 
virtual void Output (OutputHandler &OH) const
 
virtual void Output (OutputHandler &OH, const VectorHandler &X, const VectorHandler &XP) const
 
virtual flag fToBeOutput (void) const
 
virtual bool bToBeOutput (void) const
 
virtual void SetOutputFlag (flag f=flag(1))
 
- Public Member Functions inherited from VariableBody
 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)
 
virtual ~VariableBody (void)
 
virtual std::ostream & Restart (std::ostream &out) const
 
doublereal dGetM (void) const
 
Vec3 GetS (void) const
 
Mat3x3 GetJ (void) const
 
const StructNodepGetNode (void) const
 
virtual Elem::Type GetElemType (void) const
 
virtual unsigned int iGetInitialNumDof (void) const
 
virtual void AfterPredict (VectorHandler &X, VectorHandler &XP)
 
virtual unsigned int iGetNumPrivData (void) const
 
virtual unsigned int iGetPrivDataIdx (const char *s) const
 
virtual doublereal dGetPrivData (unsigned int i) const
 
- Public Member Functions inherited from ElemGravityOwner
 ElemGravityOwner (unsigned int uL, flag fOut)
 
virtual ~ElemGravityOwner (void)
 
Vec3 GetS (void) const
 
Mat3x3 GetJ (void) const
 
Vec3 GetB (void) const
 
Vec3 GetG (void) const
 
- Public Member Functions inherited from GravityOwner
 GravityOwner (void)
 
virtual ~GravityOwner (void)
 
void PutGravity (const Gravity *pG)
 
virtual bool bGetGravity (const Vec3 &X, Vec3 &Acc) const
 
- Public Member Functions inherited from InitialAssemblyElem
 InitialAssemblyElem (unsigned int uL, flag fOut)
 
virtual ~InitialAssemblyElem (void)
 
- Public Member Functions inherited from SubjectToInitialAssembly
 SubjectToInitialAssembly (void)
 
virtual ~SubjectToInitialAssembly (void)
 

Private Member Functions

Vec3 GetB_int (void) const
 
Vec3 GetG_int (void) const
 
void AssMats (FullSubMatrixHandler &WorkMatA, FullSubMatrixHandler &WorkMatB, doublereal dCoef, bool bGravity, const Vec3 &GravityAcceleration)
 

Additional Inherited Members

- Public Types inherited from Elem
enum  Type {
  UNKNOWN = -1, AIRPROPERTIES = 0, INDUCEDVELOCITY, AUTOMATICSTRUCTURAL,
  GRAVITY, BODY, JOINT, JOINT_REGULARIZATION,
  BEAM, PLATE, FORCE, INERTIA,
  ELECTRICBULK, ELECTRIC, THERMAL, HYDRAULIC,
  BULK, LOADABLE, DRIVEN, EXTERNAL,
  AEROMODAL, AERODYNAMIC, GENEL, SOCKETSTREAM_OUTPUT,
  RTAI_OUTPUT = SOCKETSTREAM_OUTPUT, LASTELEMTYPE
}
 
- Public Types inherited from SimulationEntity
typedef std::vector< Hint * > Hints
 
- Public Types inherited from ToBeOutput
enum  { OUTPUT = 0x1U, OUTPUT_MASK = 0xFU, OUTPUT_PRIVATE = 0x10U, OUTPUT_PRIVATE_MASK = ~OUTPUT_MASK }
 
- Protected Member Functions inherited from VariableBody
Vec3 GetS_int (void) const
 
Mat3x3 GetJ_int (void) const
 
void AssVecRBK_int (SubVectorHandler &WorkVec)
 
void AssMatsRBK_int (FullSubMatrixHandler &WMA, FullSubMatrixHandler &WMB, const doublereal &dCoef, const Vec3 &Sc)
 
- Protected Attributes inherited from WithLabel
unsigned int uLabel
 
std::string sName
 
- Protected Attributes inherited from ToBeOutput
flag fOutput
 
- Protected Attributes inherited from VariableBody
const StructNodepNode
 
DriveOwner m_Mass
 
TplDriveOwner< Vec3m_Xgc
 
TplDriveOwner< Mat3x3m_Jgc_vm
 
TplDriveOwner< Mat3x3m_Jgc_vg
 
doublereal dMTmp
 
Vec3 STmp
 
Mat3x3 JTmp
 
- Protected Attributes inherited from GravityOwner
GravitypGravity
 

Detailed Description

Definition at line 123 of file body_vm.h.

Constructor & Destructor Documentation

DynamicVariableBody::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 at line 313 of file body_vm.cc.

References NO_OP.

320 : Elem(uL, fOut),
321 VariableBody(uL, pNode, pDCMass, pDCXgc, pDCJgc_vm, pDCJgc_vg, fOut)
322 {
323  NO_OP;
324 }
#define NO_OP
Definition: myassert.h:74
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
const StructNode * pNode
Definition: body_vm.h:48
Elem(unsigned int uL, flag fOut)
Definition: elem.cc:41
DynamicVariableBody::~DynamicVariableBody ( void  )
virtual

Definition at line 328 of file body_vm.cc.

References NO_OP.

329 {
330  NO_OP;
331 }
#define NO_OP
Definition: myassert.h:74

Member Function Documentation

VariableSubMatrixHandler & DynamicVariableBody::AssJac ( VariableSubMatrixHandler WorkMat,
doublereal  dCoef,
const VectorHandler XCurr,
const VectorHandler XPrimeCurr 
)
virtual

Implements Elem.

Definition at line 335 of file body_vm.cc.

References AssMats(), GravityOwner::bGetGravity(), DEBUGCOUTFNAME, StructDispNode::GetXCurr(), StructDispNode::iGetFirstMomentumIndex(), StructDispNode::iGetFirstPositionIndex(), StructDispNode::pGetRBK(), VariableBody::pNode, FullSubMatrixHandler::PutColIndex(), FullSubMatrixHandler::PutRowIndex(), FullSubMatrixHandler::ResizeReset(), and VariableSubMatrixHandler::SetFull().

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 }
void PutColIndex(integer iSubCol, integer iCol)
Definition: submat.h:325
Definition: matvec3.h:98
#define DEBUGCOUTFNAME(fname)
Definition: myassert.h:256
FullSubMatrixHandler & SetFull(void)
Definition: submat.h:1168
virtual bool bGetGravity(const Vec3 &X, Vec3 &Acc) const
Definition: gravity.cc:208
virtual integer iGetFirstMomentumIndex(void) const =0
virtual integer iGetFirstPositionIndex(void) const
Definition: strnode.h:452
const RigidBodyKinematics * pGetRBK(void) const
Definition: strnode.cc:152
virtual const Vec3 & GetXCurr(void) const
Definition: strnode.h:310
virtual void ResizeReset(integer, integer)
Definition: submat.cc:182
const StructNode * pNode
Definition: body_vm.h:48
void PutRowIndex(integer iSubRow, integer iRow)
Definition: submat.h:311
void AssMats(FullSubMatrixHandler &WorkMatA, FullSubMatrixHandler &WorkMatB, doublereal dCoef, bool bGravity, const Vec3 &GravityAcceleration)
Definition: body_vm.cc:438
long int integer
Definition: colamd.c:51

Here is the call graph for this function:

void DynamicVariableBody::AssMats ( FullSubMatrixHandler WorkMatA,
FullSubMatrixHandler WorkMatB,
doublereal  dCoef,
bool  bGravity,
const Vec3 GravityAcceleration 
)
private

Definition at line 438 of file body_vm.cc.

References FullSubMatrixHandler::Add(), VariableBody::AssMatsRBK_int(), Vec3::Cross(), DEBUGCOUTFNAME, VariableBody::dMTmp, StructDispNode::GetVCurr(), StructNode::GetWCurr(), FullSubMatrixHandler::IncCoef(), VariableBody::JTmp, MatCross, MatCrossCross, StructDispNode::pGetRBK(), VariableBody::pNode, VariableBody::STmp, and FullSubMatrixHandler::Sub().

Referenced by AssJac(), and AssMats().

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 }
Definition: matvec3.h:98
#define DEBUGCOUTFNAME(fname)
Definition: myassert.h:256
const MatCross_Manip MatCross
Definition: matvec3.cc:639
Mat3x3 JTmp
Definition: body_vm.h:57
void AssMatsRBK_int(FullSubMatrixHandler &WMA, FullSubMatrixHandler &WMB, const doublereal &dCoef, const Vec3 &Sc)
Definition: body_vm.cc:239
doublereal dMTmp
Definition: body_vm.h:55
virtual const Vec3 & GetWCurr(void) const
Definition: strnode.h:1030
const RigidBodyKinematics * pGetRBK(void) const
Definition: strnode.cc:152
Vec3 STmp
Definition: body_vm.h:56
const MatCrossCross_Manip MatCrossCross
Definition: matvec3.cc:640
const StructNode * pNode
Definition: body_vm.h:48
virtual const Vec3 & GetVCurr(void) const
Definition: strnode.h:322

Here is the call graph for this function:

void DynamicVariableBody::AssMats ( VariableSubMatrixHandler WorkMatA,
VariableSubMatrixHandler WorkMatB,
const VectorHandler XCurr,
const VectorHandler XPrimeCurr 
)
virtual

Reimplemented from Elem.

Definition at line 385 of file body_vm.cc.

References AssMats(), GravityOwner::bGetGravity(), DEBUGCOUTFNAME, StructDispNode::GetXCurr(), StructDispNode::iGetFirstMomentumIndex(), StructDispNode::iGetFirstPositionIndex(), VariableBody::pNode, FullSubMatrixHandler::PutColIndex(), FullSubMatrixHandler::PutRowIndex(), FullSubMatrixHandler::ResizeReset(), and VariableSubMatrixHandler::SetFull().

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 }
void PutColIndex(integer iSubCol, integer iCol)
Definition: submat.h:325
Definition: matvec3.h:98
#define DEBUGCOUTFNAME(fname)
Definition: myassert.h:256
FullSubMatrixHandler & SetFull(void)
Definition: submat.h:1168
virtual bool bGetGravity(const Vec3 &X, Vec3 &Acc) const
Definition: gravity.cc:208
virtual integer iGetFirstMomentumIndex(void) const =0
virtual integer iGetFirstPositionIndex(void) const
Definition: strnode.h:452
virtual const Vec3 & GetXCurr(void) const
Definition: strnode.h:310
virtual void ResizeReset(integer, integer)
Definition: submat.cc:182
const StructNode * pNode
Definition: body_vm.h:48
void PutRowIndex(integer iSubRow, integer iRow)
Definition: submat.h:311
void AssMats(FullSubMatrixHandler &WorkMatA, FullSubMatrixHandler &WorkMatB, doublereal dCoef, bool bGravity, const Vec3 &GravityAcceleration)
Definition: body_vm.cc:438
long int integer
Definition: colamd.c:51

Here is the call graph for this function:

SubVectorHandler & DynamicVariableBody::AssRes ( SubVectorHandler WorkVec,
doublereal  dCoef,
const VectorHandler XCurr,
const VectorHandler XPrimeCurr 
)
virtual

Implements Elem.

Definition at line 491 of file body_vm.cc.

References VectorHandler::Add(), DynamicStructNode::AddInertia(), ASSERT, VariableBody::AssVecRBK_int(), GravityOwner::bGetGravity(), DEBUGCOUTFNAME, DriveOwner::dGet(), DriveOwner::dGetP(), VariableBody::dMTmp, TplDriveOwner< T >::Get(), TplDriveOwner< T >::GetP(), StructNode::GetRCurr(), StructDispNode::GetVCurr(), StructNode::GetWCurr(), StructDispNode::GetXCurr(), StructDispNode::iGetFirstPositionIndex(), VariableBody::JTmp, VariableBody::m_Jgc_vg, VariableBody::m_Jgc_vm, VariableBody::m_Mass, VariableBody::m_Xgc, MatCrossCross, StructDispNode::pGetRBK(), VariableBody::pNode, SubVectorHandler::PutRowIndex(), R, VectorHandler::ResizeReset(), VariableBody::STmp, and VectorHandler::Sub().

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 }
Definition: matvec3.h:98
#define DEBUGCOUTFNAME(fname)
Definition: myassert.h:256
DriveOwner m_Mass
Definition: body_vm.h:50
virtual void ResizeReset(integer)
Definition: vh.cc:55
Mat3x3 JTmp
Definition: body_vm.h:57
virtual const Mat3x3 & GetRCurr(void) const
Definition: strnode.h:1012
virtual void Sub(integer iRow, const Vec3 &v)
Definition: vh.cc:78
void AssVecRBK_int(SubVectorHandler &WorkVec)
Definition: body_vm.cc:197
virtual bool bGetGravity(const Vec3 &X, Vec3 &Acc) const
Definition: gravity.cc:208
TplDriveOwner< Mat3x3 > m_Jgc_vm
Definition: body_vm.h:52
doublereal dGetP(const doublereal &dVar) const
Definition: drive.cc:683
virtual T GetP(void) const
Definition: tpldrive.h:121
doublereal dMTmp
Definition: body_vm.h:55
virtual void PutRowIndex(integer iSubRow, integer iRow)=0
TplDriveOwner< Mat3x3 > m_Jgc_vg
Definition: body_vm.h:53
T Get(const doublereal &dVar) const
Definition: tpldrive.h:109
virtual integer iGetFirstPositionIndex(void) const
Definition: strnode.h:452
virtual const Vec3 & GetWCurr(void) const
Definition: strnode.h:1030
#define ASSERT(expression)
Definition: colamd.c:977
const RigidBodyKinematics * pGetRBK(void) const
Definition: strnode.cc:152
Vec3 STmp
Definition: body_vm.h:56
virtual const Vec3 & GetXCurr(void) const
Definition: strnode.h:310
virtual void Add(integer iRow, const Vec3 &v)
Definition: vh.cc:63
TplDriveOwner< Vec3 > m_Xgc
Definition: body_vm.h:51
const MatCrossCross_Manip MatCrossCross
Definition: matvec3.cc:640
const StructNode * pNode
Definition: body_vm.h:48
doublereal dGet(const doublereal &dVar) const
Definition: drive.cc:664
virtual const Vec3 & GetVCurr(void) const
Definition: strnode.h:322
long int integer
Definition: colamd.c:51
Mat3x3 R
virtual void AddInertia(const doublereal &dm, const Vec3 &dS, const Mat3x3 &dJ) const
Definition: strnode.cc:3092

Here is the call graph for this function:

Vec3 DynamicVariableBody::GetB_int ( void  ) const
privatevirtual

Reimplemented from ElemGravityOwner.

Definition at line 709 of file body_vm.cc.

References DriveOwner::dGet(), TplDriveOwner< T >::Get(), StructNode::GetRCurr(), StructDispNode::GetVCurr(), StructNode::GetWCurr(), VariableBody::m_Mass, VariableBody::m_Xgc, VariableBody::pNode, and R.

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 }
Definition: matvec3.h:98
DriveOwner m_Mass
Definition: body_vm.h:50
virtual const Mat3x3 & GetRCurr(void) const
Definition: strnode.h:1012
T Get(const doublereal &dVar) const
Definition: tpldrive.h:109
virtual const Vec3 & GetWCurr(void) const
Definition: strnode.h:1030
TplDriveOwner< Vec3 > m_Xgc
Definition: body_vm.h:51
const StructNode * pNode
Definition: body_vm.h:48
doublereal dGet(const doublereal &dVar) const
Definition: drive.cc:664
virtual const Vec3 & GetVCurr(void) const
Definition: strnode.h:322
Mat3x3 R

Here is the call graph for this function:

virtual void DynamicVariableBody::GetConnectedNodes ( std::vector< const Node * > &  connectedNodes) const
inlinevirtual

Reimplemented from Elem.

Definition at line 200 of file body_vm.h.

References VariableBody::pNode.

200  {
201  connectedNodes.resize(1);
202  connectedNodes[0] = pNode;
203  };
const StructNode * pNode
Definition: body_vm.h:48
Vec3 DynamicVariableBody::GetG_int ( void  ) const
privatevirtual

Reimplemented from ElemGravityOwner.

Definition at line 721 of file body_vm.cc.

References Vec3::Cross(), DriveOwner::dGet(), TplDriveOwner< T >::Get(), StructNode::GetRCurr(), StructDispNode::GetVCurr(), StructNode::GetWCurr(), StructDispNode::GetXCurr(), VariableBody::m_Jgc_vg, VariableBody::m_Jgc_vm, VariableBody::m_Mass, VariableBody::m_Xgc, VariableBody::pNode, and R.

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 }
Vec3 Cross(const Vec3 &v) const
Definition: matvec3.h:218
Definition: matvec3.h:98
DriveOwner m_Mass
Definition: body_vm.h:50
virtual const Mat3x3 & GetRCurr(void) const
Definition: strnode.h:1012
TplDriveOwner< Mat3x3 > m_Jgc_vm
Definition: body_vm.h:52
TplDriveOwner< Mat3x3 > m_Jgc_vg
Definition: body_vm.h:53
T Get(const doublereal &dVar) const
Definition: tpldrive.h:109
virtual const Vec3 & GetWCurr(void) const
Definition: strnode.h:1030
virtual const Vec3 & GetXCurr(void) const
Definition: strnode.h:310
TplDriveOwner< Vec3 > m_Xgc
Definition: body_vm.h:51
const StructNode * pNode
Definition: body_vm.h:48
doublereal dGet(const doublereal &dVar) const
Definition: drive.cc:664
virtual const Vec3 & GetVCurr(void) const
Definition: strnode.h:322
Mat3x3 R

Here is the call graph for this function:

VariableSubMatrixHandler & DynamicVariableBody::InitialAssJac ( VariableSubMatrixHandler WorkMat,
const VectorHandler XCurr 
)
virtual

Implements SubjectToInitialAssembly.

Definition at line 561 of file body_vm.cc.

References FullSubMatrixHandler::Add(), Vec3::Cross(), DEBUGCOUTFNAME, StructNode::GetWRef(), StructDispNode::iGetFirstPositionIndex(), InitialWorkSpaceDim(), VariableBody::JTmp, MatCross, MatCrossCross, VariableBody::pNode, FullSubMatrixHandler::PutColIndex(), FullSubMatrixHandler::PutRowIndex(), FullSubMatrixHandler::ResizeReset(), VariableSubMatrixHandler::SetFull(), and VariableBody::STmp.

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 }
virtual void InitialWorkSpaceDim(integer *piNumRows, integer *piNumCols) const
Definition: body_vm.h:177
void PutColIndex(integer iSubCol, integer iCol)
Definition: submat.h:325
Vec3 Cross(const Vec3 &v) const
Definition: matvec3.h:218
Definition: matvec3.h:98
#define DEBUGCOUTFNAME(fname)
Definition: myassert.h:256
const MatCross_Manip MatCross
Definition: matvec3.cc:639
Mat3x3 JTmp
Definition: body_vm.h:57
FullSubMatrixHandler & SetFull(void)
Definition: submat.h:1168
void Add(integer iRow, integer iCol, const Vec3 &v)
Definition: submat.cc:209
virtual const Vec3 & GetWRef(void) const
Definition: strnode.h:1024
virtual integer iGetFirstPositionIndex(void) const
Definition: strnode.h:452
Vec3 STmp
Definition: body_vm.h:56
virtual void ResizeReset(integer, integer)
Definition: submat.cc:182
const MatCrossCross_Manip MatCrossCross
Definition: matvec3.cc:640
const StructNode * pNode
Definition: body_vm.h:48
void PutRowIndex(integer iSubRow, integer iRow)
Definition: submat.h:311
long int integer
Definition: colamd.c:51

Here is the call graph for this function:

SubVectorHandler & DynamicVariableBody::InitialAssRes ( SubVectorHandler WorkVec,
const VectorHandler XCurr 
)
virtual

Implements SubjectToInitialAssembly.

Definition at line 630 of file body_vm.cc.

References VectorHandler::Add(), GravityOwner::bGetGravity(), grad::Cross(), DEBUGCOUTFNAME, DriveOwner::dGet(), VariableBody::dMTmp, TplDriveOwner< T >::Get(), StructNode::GetRCurr(), StructNode::GetWCurr(), StructDispNode::GetXCurr(), StructDispNode::iGetFirstPositionIndex(), InitialWorkSpaceDim(), VariableBody::JTmp, VariableBody::m_Jgc_vg, VariableBody::m_Jgc_vm, VariableBody::m_Mass, VariableBody::m_Xgc, MatCrossCross, VariableBody::pNode, SubVectorHandler::PutRowIndex(), R, VectorHandler::ResizeReset(), and VariableBody::STmp.

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 }
virtual void InitialWorkSpaceDim(integer *piNumRows, integer *piNumCols) const
Definition: body_vm.h:177
Definition: matvec3.h:98
#define DEBUGCOUTFNAME(fname)
Definition: myassert.h:256
DriveOwner m_Mass
Definition: body_vm.h:50
virtual void ResizeReset(integer)
Definition: vh.cc:55
Mat3x3 JTmp
Definition: body_vm.h:57
virtual const Mat3x3 & GetRCurr(void) const
Definition: strnode.h:1012
virtual bool bGetGravity(const Vec3 &X, Vec3 &Acc) const
Definition: gravity.cc:208
TplDriveOwner< Mat3x3 > m_Jgc_vm
Definition: body_vm.h:52
doublereal dMTmp
Definition: body_vm.h:55
virtual void PutRowIndex(integer iSubRow, integer iRow)=0
TplDriveOwner< Mat3x3 > m_Jgc_vg
Definition: body_vm.h:53
T Get(const doublereal &dVar) const
Definition: tpldrive.h:109
virtual integer iGetFirstPositionIndex(void) const
Definition: strnode.h:452
virtual const Vec3 & GetWCurr(void) const
Definition: strnode.h:1030
VectorExpression< VectorCrossExpr< VectorLhsExpr, VectorRhsExpr >, 3 > Cross(const VectorExpression< VectorLhsExpr, 3 > &u, const VectorExpression< VectorRhsExpr, 3 > &v)
Definition: matvec.h:3248
Vec3 STmp
Definition: body_vm.h:56
virtual const Vec3 & GetXCurr(void) const
Definition: strnode.h:310
virtual void Add(integer iRow, const Vec3 &v)
Definition: vh.cc:63
TplDriveOwner< Vec3 > m_Xgc
Definition: body_vm.h:51
const MatCrossCross_Manip MatCrossCross
Definition: matvec3.cc:640
const StructNode * pNode
Definition: body_vm.h:48
doublereal dGet(const doublereal &dVar) const
Definition: drive.cc:664
long int integer
Definition: colamd.c:51
Mat3x3 R

Here is the call graph for this function:

virtual void DynamicVariableBody::InitialWorkSpaceDim ( integer piNumRows,
integer piNumCols 
) const
inlinevirtual

Implements SubjectToInitialAssembly.

Definition at line 177 of file body_vm.h.

Referenced by InitialAssJac(), and InitialAssRes().

177  {
178  *piNumRows = 12;
179  *piNumCols = 6;
180  };
void DynamicVariableBody::SetValue ( DataManager pDM,
VectorHandler X,
VectorHandler XP,
SimulationEntity::Hints ph = 0 
)
virtual

Reimplemented from SimulationEntity.

Definition at line 686 of file body_vm.cc.

References VectorHandler::Add(), DriveOwner::dGet(), VariableBody::dMTmp, TplDriveOwner< T >::Get(), StructNode::GetRCurr(), StructDispNode::GetVCurr(), StructNode::GetWCurr(), StructDispNode::iGetFirstMomentumIndex(), VariableBody::JTmp, VariableBody::m_Jgc_vg, VariableBody::m_Jgc_vm, VariableBody::m_Mass, VariableBody::m_Xgc, MatCrossCross, VariableBody::pNode, R, and VariableBody::STmp.

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 }
Definition: matvec3.h:98
DriveOwner m_Mass
Definition: body_vm.h:50
Mat3x3 JTmp
Definition: body_vm.h:57
virtual const Mat3x3 & GetRCurr(void) const
Definition: strnode.h:1012
TplDriveOwner< Mat3x3 > m_Jgc_vm
Definition: body_vm.h:52
doublereal dMTmp
Definition: body_vm.h:55
TplDriveOwner< Mat3x3 > m_Jgc_vg
Definition: body_vm.h:53
T Get(const doublereal &dVar) const
Definition: tpldrive.h:109
virtual integer iGetFirstMomentumIndex(void) const =0
virtual const Vec3 & GetWCurr(void) const
Definition: strnode.h:1030
Vec3 STmp
Definition: body_vm.h:56
virtual void Add(integer iRow, const Vec3 &v)
Definition: vh.cc:63
TplDriveOwner< Vec3 > m_Xgc
Definition: body_vm.h:51
const MatCrossCross_Manip MatCrossCross
Definition: matvec3.cc:640
const StructNode * pNode
Definition: body_vm.h:48
doublereal dGet(const doublereal &dVar) const
Definition: drive.cc:664
virtual const Vec3 & GetVCurr(void) const
Definition: strnode.h:322
long int integer
Definition: colamd.c:51
Mat3x3 R

Here is the call graph for this function:

void DynamicVariableBody::WorkSpaceDim ( integer piNumRows,
integer piNumCols 
) const
inlinevirtual

Implements Elem.

Definition at line 149 of file body_vm.h.

149  {
150  *piNumRows = 12;
151  *piNumCols = 6;
152  };

The documentation for this class was generated from the following files: