#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 VariableSubMatrixHandler &	AssJac (VariableSubMatrixHandler &WorkMat, doublereal dCoef, const VectorHandler &XCurr, const VectorHandler &XPrimeCurr)

void	AssMats (VariableSubMatrixHandler &WorkMatA, VariableSubMatrixHandler &WorkMatB, const VectorHandler &XCurr, const VectorHandler &XPrimeCurr)

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

virtual void	InitialWorkSpaceDim (integer piNumRows, integer piNumCols) const

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

virtual SubVectorHandler &	InitialAssRes (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 VariableSubMatrixHandler &	AssJac (VariableSubMatrixHandler &WorkMat, const VectorHandler &XCurr)

virtual SubVectorHandler &	AssRes (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 Hint *	ParseHint (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 StructNode *	pGetNode (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 StructNode *	pNode

DriveOwner	m_Mass

TplDriveOwner< Vec3 >	m_Xgc

TplDriveOwner< Mat3x3 >	m_Jgc_vm

TplDriveOwner< Mat3x3 >	m_Jgc_vg

doublereal	dMTmp

Vec3	STmp

Mat3x3	JTmp

Protected Attributes inherited from GravityOwner
Gravity *	pGravity

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.

 : Elem(uL, fOut),
 VariableBody(uL, pNode, pDCMass, pDCXgc, pDCJgc_vm, pDCJgc_vg, fOut)
 {
         NO_OP;
 }

DynamicVariableBody::~DynamicVariableBody ( void )

virtual

Definition at line 328 of file body_vm.cc.

References NO_OP.

 {
         NO_OP;
 }

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().

 {
         DEBUGCOUTFNAME("DynamicVariableBody::AssJac");
 
         /* Casting di WorkMat */
         FullSubMatrixHandler& WM = WorkMat.SetFull();
 
         Vec3 GravityAcceleration;
         bool g = GravityOwner::bGetGravity(pNode->GetXCurr(),
                 GravityAcceleration);
 
         integer iNumRows = 6;
         if (g || pNode->pGetRBK()) {
                 iNumRows = 12;
         }
 
         /* Dimensiona e resetta la matrice di lavoro */
         WM.ResizeReset(iNumRows, 6);
 
         /* Setta gli indici della matrice - le incognite sono ordinate come:
          *   - posizione (3)
          *   - parametri di rotazione (3)
          *   - quantita' di moto (3)
          *   - momento della quantita' di moto
          * e gli indici sono consecutivi. La funzione pGetFirstPositionIndex()
          * ritorna il valore del primo indice -1, in modo che l'indice i-esimo
          * e' dato da iGetFirstPositionIndex()+i */
         integer iFirstPositionIndex = pNode->iGetFirstPositionIndex();
         for (integer iCnt = 1; iCnt <= 6; iCnt++) {
                 WM.PutRowIndex(iCnt, iFirstPositionIndex + iCnt);
                 WM.PutColIndex(iCnt, iFirstPositionIndex + iCnt);
         }
 
         if (iNumRows == 12) {
                 integer iFirstMomentumIndex = pNode->iGetFirstMomentumIndex();
                 for (integer iCnt = 1; iCnt <= 6; iCnt++) {
                         WM.PutRowIndex(6 + iCnt, iFirstMomentumIndex + iCnt);
                 }
         }
 
         AssMats(WM, WM, dCoef, g, GravityAcceleration);
 
         return WorkMat;
 }

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().

 {
         DEBUGCOUTFNAME("DynamicVariableBody::AssMats");
 
         const Vec3& V(pNode->GetVCurr());
         const Vec3& W(pNode->GetWCurr());
 
         // STmp, JTmp computed by AssRes()
         // const Mat3x3& R(pNode->GetRCurr());
         // STmp = R*S0;
         // JTmp = R*J0.MulMT(R);
 
         Mat3x3 SWedge(MatCross, STmp);                  /* S /\ */
         Vec3 Sc(STmp*dCoef);
 
         /*
          * momentum:
          *
          * m * I DeltaV - S /\ DeltagP + ( S /\ W ) /\ Deltag
          */
         WMB.IncCoef(1, 1, dMTmp);
         WMB.IncCoef(2, 2, dMTmp);
         WMB.IncCoef(3, 3, dMTmp);
 
         WMB.Sub(1, 3 + 1, SWedge);
         WMA.Add(1, 3 + 1, Mat3x3(MatCross, Sc.Cross(W)));
 
         /*
          * momenta moment:
          *
          * S /\ DeltaV + J DeltagP + ( V /\ S /\ - ( J * W ) /\ ) Deltag
          */
         WMB.Add(3 + 1, 1, SWedge);
 
         WMB.Add(3 + 1, 3 + 1, JTmp);
         WMA.Add(3 + 1, 3 + 1, Mat3x3(MatCrossCross, V, Sc) - Mat3x3(MatCross, JTmp*(W*dCoef)));
 
         if (bGravity) {
                 WMA.Sub(9 + 1, 3 + 1, Mat3x3(MatCrossCross, GravityAcceleration, Sc));
         }
 
         const RigidBodyKinematics *pRBK = pNode->pGetRBK();
         if (pRBK) {
                 AssMatsRBK_int(WMA, WMB, dCoef, Sc);
         }
 }

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().

 {
         DEBUGCOUTFNAME("DynamicVariableBody::AssMats");
 
         /* Casting di WorkMat */
         FullSubMatrixHandler& WMA = WorkMatA.SetFull();
         FullSubMatrixHandler& WMB = WorkMatB.SetFull();
 
         Vec3 GravityAcceleration;
         bool g = GravityOwner::bGetGravity(pNode->GetXCurr(),
                 GravityAcceleration);
 
         integer iNumRows = 6;
         if (g) {
                 iNumRows = 12;
         }
 
         /* Dimensiona e resetta la matrice di lavoro */
         WMA.ResizeReset(iNumRows, 6);
         WMB.ResizeReset(6, 6);
 
         /* Setta gli indici della matrice - le incognite sono ordinate come:
          *   - posizione (3)
          *   - parametri di rotazione (3)
          *   - quantita' di moto (3)
          *   - momento della quantita' di moto
          * e gli indici sono consecutivi. La funzione pGetFirstPositionIndex()
          * ritorna il valore del primo indice -1, in modo che l'indice i-esimo
          * e' dato da iGetFirstPositionIndex()+i */
         integer iFirstPositionIndex = pNode->iGetFirstPositionIndex();
         for (integer iCnt = 1; iCnt <= 6; iCnt++) {
                 WMA.PutRowIndex(iCnt, iFirstPositionIndex + iCnt);
                 WMA.PutColIndex(iCnt, iFirstPositionIndex + iCnt);
 
                 WMB.PutRowIndex(iCnt, iFirstPositionIndex + iCnt);
                 WMB.PutColIndex(iCnt, iFirstPositionIndex + iCnt);
         }
 
         if (g) {
                 integer iFirstMomentumIndex = pNode->iGetFirstMomentumIndex();
                 for (integer iCnt = 1; iCnt <= 6; iCnt++) {
                         WMA.PutRowIndex(6 + iCnt, iFirstMomentumIndex + iCnt);
                 }
         }
 
         AssMats(WMA, WMB, 1., g, GravityAcceleration);
 }

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().

 {
         DEBUGCOUTFNAME("VariableBody::DynamicAssRes");
 
         /* Se e' definita l'accelerazione di gravita',
          * la aggiunge (solo al residuo) */
         Vec3 GravityAcceleration;
         bool g = GravityOwner::bGetGravity(pNode->GetXCurr(),
                 GravityAcceleration);
 
         const RigidBodyKinematics *pRBK = pNode->pGetRBK();
 
         integer iNumRows = 12;
         WorkVec.ResizeReset(iNumRows);
 
         integer iFirstPositionIndex = pNode->iGetFirstPositionIndex();
         for (integer iCnt = 1; iCnt <= iNumRows; iCnt++) {
                 WorkVec.PutRowIndex(iCnt, iFirstPositionIndex + iCnt);
         }
 
         const Vec3& V(pNode->GetVCurr());
         const Vec3& W(pNode->GetWCurr());
 
         /* Aggiorna i suoi dati (saranno pronti anche per AssJac) */
         const Mat3x3& R(pNode->GetRCurr());
 
         dMTmp = m_Mass.dGet();
         Vec3 DXgc(R*m_Xgc.Get());
         STmp = DXgc*dMTmp;
         JTmp = R*(m_Jgc_vm.Get() + m_Jgc_vg.Get() - Mat3x3(MatCrossCross, STmp, DXgc)).MulMT(R);
         Vec3 DXgcP(R*m_Xgc.GetP());
         Vec3 XgcPTmp(V + W.Cross(DXgc) + DXgcP);
 
         /* Quantita' di moto: R[1] = Q - M * V - W /\ S */
         WorkVec.Sub(1, XgcPTmp*dMTmp);
 
         /* Momento della quantita' di moto: R[2] = G - S /\ V - J * W */
         WorkVec.Sub(3 + 1, JTmp*W + STmp.Cross(V));
 
         Vec3 mp_Xp_cm(XgcPTmp*m_Mass.dGetP());
 
         /* Variable mass correction */
         WorkVec.Add(6 + 1, mp_Xp_cm);
         WorkVec.Add(9 + 1, m_Jgc_vm.GetP()*W + DXgc.Cross(mp_Xp_cm) + V.Cross(DXgcP*dMTmp));
 
         if (g) {
                 WorkVec.Add(6 + 1, GravityAcceleration*dMTmp);
                 /* FIXME: this should go into Jacobian matrix
                  * as Gravity /\ S /\ Delta g */
                 WorkVec.Add(9 + 1, STmp.Cross(GravityAcceleration));
         }
 
         if (pRBK) {
                 AssVecRBK_int(WorkVec);
         }
 
         const DynamicStructNode *pDN = dynamic_cast<const DynamicStructNode *>(pNode);
         ASSERT(pDN != 0);
 
         pDN->AddInertia(dMTmp, STmp, JTmp);
 
         return WorkVec;
 }

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.

 {
         const Vec3& V(pNode->GetVCurr());
         const Vec3& W(pNode->GetWCurr());
         const Mat3x3& R(pNode->GetRCurr());
 
         return (V + W.Cross(R*m_Xgc.Get()))*m_Mass.dGet();
 }

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.

                                                                          {
                 connectedNodes.resize(1);
                 connectedNodes[0] = pNode;
         };

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.

 {
         const Vec3& X(pNode->GetXCurr());
         const Mat3x3& R(pNode->GetRCurr());
         const Vec3& V(pNode->GetVCurr());
         const Vec3& W(pNode->GetWCurr());
 
         Vec3 DXgc(R*m_Xgc.Get());
 
         // NOTE: with respect to the origin of the global reference frame!
         return (X + DXgc).Cross((V + W.Cross(R*DXgc))*m_Mass.dGet()) + (m_Jgc_vm.Get() + m_Jgc_vg.Get())*W;
 }

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.

 {
         DEBUGCOUTFNAME("DynamicVariableBody::InitialAssJac");
 
         /* Casting di WorkMat */
         FullSubMatrixHandler& WM = WorkMat.SetFull();
 
         /* Dimensiona e resetta la matrice di lavoro */
         integer iNumRows = 0;
         integer iNumCols = 0;
         InitialWorkSpaceDim(&iNumRows, &iNumCols);
         WM.ResizeReset(iNumRows, iNumCols);
 
         /* Setta gli indici della matrice - le incognite sono ordinate come:
          *   - posizione (3)
          *   - parametri di rotazione (3)
          *   - quantita' di moto (3)
          *   - momento della quantita' di moto
          * e gli indici sono consecutivi. La funzione pGetFirstPositionIndex()
          * ritorna il valore del primo indice -1, in modo che l'indice i-esimo
          * e' dato da iGetFirstPositionIndex()+i */
         integer iFirstPositionIndex = pNode->iGetFirstPositionIndex();
         integer iFirstVelocityIndex = iFirstPositionIndex + 6;
         for (integer iCnt = 1; iCnt <= 6; iCnt++) {
                 WM.PutRowIndex(iCnt, iFirstPositionIndex + iCnt);
                 WM.PutRowIndex(6 + iCnt, iFirstVelocityIndex + iCnt);
                 WM.PutColIndex(iCnt, iFirstPositionIndex + iCnt);
         }
 
         /* Prepara matrici e vettori */
 
         /* Velocita' angolare corrente */
         const Vec3& W(pNode->GetWRef());
 
         Vec3 SWedgeW(STmp.Cross(W));
         Mat3x3 WWedgeSWedge(MatCrossCross, -W, STmp);
         Mat3x3 WWedge(MatCross, W);
         Mat3x3 WWedgeWWedgeSWedge(W.Cross(WWedgeSWedge));
         Mat3x3 FDeltaW(Mat3x3(MatCross, SWedgeW) + WWedgeSWedge);
 
         // STmp, JTmp computed by InitialAssRes()
         Vec3 JW(JTmp*W);
         Mat3x3 JWWedge(MatCross, JW);
         Mat3x3 MDeltag(W.Cross(JTmp*WWedge - JWWedge));
         Mat3x3 MDeltaW(W.Cross(JTmp) - JWWedge);
 
         /* Forza */
         WM.Add(1, 1, WWedgeWWedgeSWedge);
         WM.Add(1, 4, FDeltaW);
 
         /* Momento */
         WM.Add(4, 1, MDeltag);
         WM.Add(4, 4, MDeltaW);
 
         /* Derivata forza */
         WM.Add(7, 1, Mat3x3(MatCross, W.Cross(SWedgeW)) + W.Cross(FDeltaW));
         WM.Add(7, 4, W.Cross(WWedgeWWedgeSWedge));
 
         /* Derivata Momento */
         WM.Add(4, 1, W.Cross(MDeltag));
         WM.Add(4, 4, W.Cross(MDeltaW) - Mat3x3(MatCross, W.Cross(JW)));
 
         return WorkMat;
 }

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.

 {
         DEBUGCOUTFNAME("DynamicVariableBody::InitialAssRes");
 
         integer iNumRows;
         integer iNumCols;
         InitialWorkSpaceDim(&iNumRows, &iNumCols);
         WorkVec.ResizeReset(iNumRows);
 
         integer iFirstPositionIndex = pNode->iGetFirstPositionIndex();
         for (integer iCnt = 1; iCnt <= 12; iCnt++) {
                 WorkVec.PutRowIndex(iCnt, iFirstPositionIndex+iCnt);
         }
 
         const Vec3& X(pNode->GetXCurr());
         const Vec3& W(pNode->GetWCurr());
 
         // Aggiorna i suoi dati (saranno pronti anche per InitialAssJac)
         const Mat3x3& R(pNode->GetRCurr());
 
         dMTmp = m_Mass.dGet();
         Vec3 DXgc(R*m_Xgc.Get());
         STmp = DXgc*dMTmp;
         JTmp = R*(m_Jgc_vm.Get() + m_Jgc_vg.Get() - Mat3x3(MatCrossCross, DXgc, STmp)).MulMT(R);
 
         Vec3 FC(-W.Cross(W.Cross(STmp)));
         Vec3 MC(-W.Cross(JTmp*W));
 
         /* Forza */
         WorkVec.Add(1, FC);
 
         /* Momento */
         WorkVec.Add(4, MC);
 
         /* Derivata forza */
         WorkVec.Add(7, W.Cross(FC));
 
         /* Derivata momento */
         WorkVec.Add(10, W.Cross(MC));
 
         /* Se e' definita l'accelerazione di gravita',
          * la aggiunge (solo al residuo) */
         Vec3 GravityAcceleration;
         if (GravityOwner::bGetGravity(X + DXgc, GravityAcceleration)) {
                 WorkVec.Add(1, GravityAcceleration*dMTmp);
                 WorkVec.Add(3 + 1, STmp.Cross(GravityAcceleration));
                 WorkVec.Add(9 + 1, (W.Cross(STmp)).Cross(GravityAcceleration));
         }
 
         return WorkVec;
 }

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().

                                                                           { 
                 *piNumRows = 12; 
                 *piNumCols = 6; 
         };

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.

 {
         integer iFirstIndex = pNode->iGetFirstMomentumIndex();
 
         // TODO: make configurable
         const Vec3& V(pNode->GetVCurr());
         const Vec3& W(pNode->GetWCurr());
         const Mat3x3& R(pNode->GetRCurr());
 
         dMTmp = m_Mass.dGet();
         Vec3 DXgc(R*m_Xgc.Get());
         STmp = DXgc*dMTmp;
         JTmp = R*(m_Jgc_vm.Get() + m_Jgc_vg.Get() - Mat3x3(MatCrossCross, DXgc, STmp)).MulMT(R);
         X.Add(iFirstIndex + 1, V*dMTmp + W.Cross(STmp));
         // NOTE: does not start correctly if m_Xgc.GetP() != 0 at initial time...
         // X.Add(iFirstIndex + 1, (V + R*m_Xgc.GetP())*dMTmp + W.Cross(STmp));
         X.Add(iFirstIndex + 4, STmp.Cross(V) + JTmp*W);
 }

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.

                                                                         { 
                 *piNumRows = 12; 
                 *piNumCols = 6; 
         };

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

Public Member Functions

Private Member Functions

Additional Inherited Members

Detailed Description

Constructor & Destructor Documentation

Member Function Documentation