94 out <<
" body: " <<
GetLabel() <<
", "
153 if (strcmp(s,
"E") == 0) {
158 if (strcmp(s,
"V") == 0) {
187 Vec3 GravityAcceleration;
206 if (dynamic_cast<DynamicVariableBody *>(
this)) {
218 WorkVec.
Sub(iIdx + 1, f);
235 WorkVec.
Sub(iIdx + 3 + 1, m);
251 if (dynamic_cast<DynamicVariableBody *>(
this)) {
259 WMA.
Add(iIdx + 1, 1, MTmp*(
dMTmp*dCoef));
271 WMA.
Add(iIdx + 3 + 1, 1, MTmp);
286 MTmp += VTmp.
Cross(MTmp2);
295 WMA.
Add(iIdx + 3 + 1, 3 + 1, MTmp);
302 WMB.
Add(iIdx + 3 + 1, 1, MTmp);
305 WMB.
Add(iIdx + 3 + 1, 3 + 1, MTmp2);
321 VariableBody(uL, pNode, pDCMass, pDCXgc, pDCJgc_vm, pDCJgc_vg, fOut)
345 Vec3 GravityAcceleration;
347 GravityAcceleration);
366 for (
integer iCnt = 1; iCnt <= 6; iCnt++) {
371 if (iNumRows == 12) {
373 for (
integer iCnt = 1; iCnt <= 6; iCnt++) {
374 WM.
PutRowIndex(6 + iCnt, iFirstMomentumIndex + iCnt);
378 AssMats(WM, WM, dCoef, g, GravityAcceleration);
396 Vec3 GravityAcceleration;
398 GravityAcceleration);
418 for (
integer iCnt = 1; iCnt <= 6; iCnt++) {
428 for (
integer iCnt = 1; iCnt <= 6; iCnt++) {
429 WMA.
PutRowIndex(6 + iCnt, iFirstMomentumIndex + iCnt);
433 AssMats(WMA, WMB, 1., g, GravityAcceleration);
442 const Vec3& GravityAcceleration)
466 WMB.
Sub(1, 3 + 1, SWedge);
474 WMB.
Add(3 + 1, 1, SWedge);
500 Vec3 GravityAcceleration;
502 GravityAcceleration);
510 for (
integer iCnt = 1; iCnt <= iNumRows; iCnt++) {
511 WorkVec.
PutRowIndex(iCnt, iFirstPositionIndex + iCnt);
525 Vec3 XgcPTmp(V + W.Cross(DXgc) + DXgcP);
528 WorkVec.
Sub(1, XgcPTmp*dMTmp);
531 WorkVec.
Sub(3 + 1,
JTmp*W + STmp.Cross(V));
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));
540 WorkVec.
Add(6 + 1, GravityAcceleration*dMTmp);
543 WorkVec.
Add(9 + 1, STmp.Cross(GravityAcceleration));
584 integer iFirstVelocityIndex = iFirstPositionIndex + 6;
585 for (
integer iCnt = 1; iCnt <= 6; iCnt++) {
587 WM.
PutRowIndex(6 + iCnt, iFirstVelocityIndex + iCnt);
599 Mat3x3 WWedgeWWedgeSWedge(W.Cross(WWedgeSWedge));
605 Mat3x3 MDeltag(W.Cross(
JTmp*WWedge - JWWedge));
609 WM.
Add(1, 1, WWedgeWWedgeSWedge);
610 WM.
Add(1, 4, FDeltaW);
613 WM.
Add(4, 1, MDeltag);
614 WM.
Add(4, 4, MDeltaW);
618 WM.
Add(7, 4, W.Cross(WWedgeWWedgeSWedge));
621 WM.
Add(4, 1, W.Cross(MDeltag));
641 for (
integer iCnt = 1; iCnt <= 12; iCnt++) {
642 WorkVec.
PutRowIndex(iCnt, iFirstPositionIndex+iCnt);
656 Vec3 FC(-W.Cross(W.Cross(STmp)));
666 WorkVec.
Add(7, W.Cross(FC));
669 WorkVec.
Add(10, W.Cross(MC));
673 Vec3 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));
701 X.
Add(iFirstIndex + 1, V*dMTmp + W.Cross(STmp));
704 X.
Add(iFirstIndex + 4, STmp.Cross(V) +
JTmp*W);
747 VariableBody(uL, pNode, pDCMass, pDCXgc, pDCJgc_vm, pDCJgc_vg, fOut)
784 for (
integer iCnt = 1; iCnt <= 6; iCnt++) {
823 for (
integer iCnt = 1; iCnt <= 6; iCnt++) {
901 for (
integer iCnt = 1; iCnt <= 6; iCnt++) {
902 WorkVec.
PutRowIndex(iCnt, iFirstMomentumIndex + iCnt);
910 WorkVec.
Add(1, Acceleration*dMTmp);
946 Vec3 GravityAcceleration;
952 for (
integer iCnt = 1; iCnt <= 6; iCnt++) {
953 WorkVec.
PutRowIndex(iCnt, iFirstPositionIndex + iCnt);
964 Acceleration -= GravityAcceleration;
967 WorkVec.
Sub(1, Acceleration*dMTmp);
1035 silent_cerr(
"VariableBody(" << uLabel <<
"): "
1036 "mass drive caller must be differentiable "
1043 silent_cerr(
"VariableBody(" << uLabel <<
"): "
1044 "center of mass drive caller must be differentiable "
1051 silent_cerr(
"VariableBody(" << uLabel <<
"): "
1052 "mass-related inertia matrix drive caller must be differentiable "
1059 silent_cerr(
"VariableBody(" << uLabel <<
"): "
1060 "geometry-related inertia matrix drive caller must be differentiable "
1068 if (bStaticModel || bInverseDynamics) {
1072 silent_cerr(
"VariableBody(" << uLabel <<
"): "
1073 "illegal structural node type "
1074 "for StructNode(" << pStrNode->
GetLabel() <<
") "
1075 "in " << (bStaticModel ?
"static model" :
"inverse dynamics") <<
" analysis "
1083 silent_cerr(
"VariableBody(" << uLabel <<
"): "
1084 "illegal structural node type "
1085 "for StructNode(" << pStrNode->
GetLabel() <<
") "
1095 if (bStaticModel || bInverseDynamics) {
1099 pDCMass, pDCXgc, pDCJgc_vm, pDCJgc_vg, fOut));
1104 pDCMass, pDCXgc, pDCJgc_vm, pDCJgc_vg, fOut));
1108 <<
"variable body: " << uLabel
1110 <<
' ' << pDCMass->
dGet()
1111 <<
' ' << pDCXgc->
Get()
1112 <<
' ' << pDCJgc_vm->
Get()
1113 <<
' ' << pDCJgc_vg->
Get()
1118 silent_cerr(
"VariableBody(" << uLabel <<
"): semicolon expected "
flag fReadOutput(MBDynParser &HP, const T &t) const
virtual void InitialWorkSpaceDim(integer *piNumRows, integer *piNumCols) const
void PutColIndex(integer iSubCol, integer iCol)
TplDriveCaller< T > * GetTplDriveCaller(void)
const Vec3 Zero3(0., 0., 0.)
Vec3 Cross(const Vec3 &v) const
virtual ~StaticVariableBody(void)
virtual const Mat3x3 & GetRRef(void) const
#define MBDYN_EXCEPT_ARGS
virtual ~VariableBody(void)
#define DEBUGCOUTFNAME(fname)
virtual void ResizeReset(integer)
virtual StructNode::Type GetStructNodeType(void) const
const MatCross_Manip MatCross
FullSubMatrixHandler & SetFull(void)
virtual const Mat3x3 & GetRCurr(void) const
virtual Node::Type GetNodeType(void) const
virtual T Get(const doublereal &dVar) const =0
doublereal Dot(const Vec3 &v) const
void Add(integer iRow, integer iCol, const Vec3 &v)
virtual doublereal dGetPrivData(unsigned int i) const
virtual void Sub(integer iRow, const Vec3 &v)
virtual const Vec3 & GetXPP(void) const =0
void AssMatsRBK_int(FullSubMatrixHandler &WMA, FullSubMatrixHandler &WMB, const doublereal &dCoef, const Vec3 &Sc)
void AssVecRBK_int(SubVectorHandler &WorkVec)
Vec3 GetS_int(void) const
virtual bool bGetGravity(const Vec3 &X, Vec3 &Acc) const
virtual SubVectorHandler & InitialAssRes(SubVectorHandler &WorkVec, const VectorHandler &XCurr)
TplDriveOwner< Mat3x3 > m_Jgc_vm
virtual const Vec3 & GetWRef(void) const
virtual VariableSubMatrixHandler & InitialAssJac(VariableSubMatrixHandler &WorkMat, const VectorHandler &XCurr)
std::vector< Hint * > Hints
bool bIsStaticModel(void) const
doublereal dGetP(const doublereal &dVar) const
void IncCoef(integer iRow, integer iCol, const doublereal &dCoef)
virtual std::ostream & Restart(std::ostream &out) const =0
Mat3x3 GetJ_int(void) const
virtual SubVectorHandler & AssRes(SubVectorHandler &WorkVec, doublereal dCoef, const VectorHandler &XCurr, const VectorHandler &XPrimeCurr)
virtual T GetP(void) const
virtual void PutRowIndex(integer iSubRow, integer iRow)=0
Vec3 GetB_int(void) const
virtual SubVectorHandler & AssRes(SubVectorHandler &WorkVec, doublereal dCoef, const VectorHandler &XCurr, const VectorHandler &XPrimeCurr)
bool AssMats(FullSubMatrixHandler &WorkMatA, FullSubMatrixHandler &WorkMatB, doublereal dCoef)
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 VariableSubMatrixHandler & AssJac(VariableSubMatrixHandler &WorkMat, doublereal dCoef, const VectorHandler &XCurr, const VectorHandler &XPrimeCurr)
TplDriveCaller< T > * pGetDriveCaller(void) const
TplDriveOwner< Mat3x3 > m_Jgc_vg
virtual bool bIsDifferentiable(void) const
virtual SubVectorHandler & InitialAssRes(SubVectorHandler &WorkVec, const VectorHandler &XCurr)
virtual void SetValue(DataManager *pDM, VectorHandler &X, VectorHandler &XP, SimulationEntity::Hints *ph=0)
virtual bool bIsDifferentiable(void) const
T Get(const doublereal &dVar) const
virtual VariableSubMatrixHandler & InitialAssJac(VariableSubMatrixHandler &WorkMat, const VectorHandler &XCurr)
virtual void AfterPredict(VectorHandler &X, VectorHandler &XP)
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 integer iGetFirstMomentumIndex(void) const =0
virtual integer iGetFirstPositionIndex(void) const
virtual const Vec3 & GetWCurr(void) const
virtual bool bInverseDynamics(void) const
virtual ~DynamicVariableBody(void)
Vec3 GetG_int(void) const
virtual const Vec3 & GetWPCurr(void) const
virtual std::ostream & Restart(std::ostream &out) const =0
#define ASSERT(expression)
const RigidBodyKinematics * pGetRBK(void) const
VectorExpression< VectorCrossExpr< VectorLhsExpr, VectorRhsExpr >, 3 > Cross(const VectorExpression< VectorLhsExpr, 3 > &u, const VectorExpression< VectorRhsExpr, 3 > &v)
#define SAFENEWWITHCONSTRUCTOR(pnt, item, constructor)
DriveCaller * pGetDriveCaller(void) const
virtual const Vec3 & GetXCurr(void) const
virtual void Add(integer iRow, const Vec3 &v)
virtual void ResizeReset(integer, integer)
virtual const Vec3 & GetWP(void) const =0
virtual doublereal dGet(const doublereal &dVar) const =0
TplDriveOwner< Vec3 > m_Xgc
const MatCrossCross_Manip MatCrossCross
virtual VariableSubMatrixHandler & AssJac(VariableSubMatrixHandler &WorkMat, doublereal dCoef, const VectorHandler &XCurr, const VectorHandler &XPrimeCurr)
virtual const Vec3 & GetW(void) const =0
void PutRowIndex(integer iSubRow, integer iRow)
virtual std::ostream & Restart(std::ostream &out) const
std::ostream & GetLogFile(void) const
doublereal dGet(const doublereal &dVar) const
Elem * ReadVariableBody(DataManager *pDM, MBDynParser &HP, unsigned int uLabel, const StructNode *pStrNode)
virtual const Vec3 & GetVCurr(void) const
void Sub(integer iRow, integer iCol, const Vec3 &v)
virtual unsigned int iGetPrivDataIdx(const char *s) const
DriveCaller * GetDriveCaller(bool bDeferred=false)
static const doublereal a
doublereal dGetM(void) const
virtual const Vec3 & GetXPPCurr(void) const
virtual unsigned int iGetNumPrivData(void) const
void AssMats(FullSubMatrixHandler &WorkMatA, FullSubMatrixHandler &WorkMatB, doublereal dCoef, bool bGravity, const Vec3 &GravityAcceleration)
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)
virtual HighParser::ErrOut GetLineData(void) const
virtual StructNode::Type GetStructNodeType(void) const
unsigned int GetLabel(void) const
virtual void SetValue(DataManager *pDM, VectorHandler &X, VectorHandler &XP, SimulationEntity::Hints *ph=0)
const StructNode * pGetNode(void) const
virtual void Resize(integer iNewSize)=0
virtual void AddInertia(const doublereal &dm, const Vec3 &dS, const Mat3x3 &dJ) const
bool bIsInverseDynamics(void) const