MBDyn-1.7.3
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Groups
Public Member Functions | Private Attributes | List of all members
TotalJoint Class Reference

#include <totalj.h>

Inheritance diagram for TotalJoint:
Inheritance graph
[legend]
Collaboration diagram for TotalJoint:
Collaboration graph
[legend]

Public Member Functions

 TotalJoint (unsigned int uL, const DofOwner *pDO, bool bPos[3], bool bVel[3], TplDriveCaller< Vec3 > *const pDCPos[3], bool bRot[3], bool bAgv[3], TplDriveCaller< Vec3 > *const pDCRot[3], const StructNode *pN1, const Vec3 &f1Tmp, const Mat3x3 &R1hTmp, const Mat3x3 &R1hrTmp, const StructNode *pN2, const Vec3 &f2Tmp, const Mat3x3 &R2hTmp, const Mat3x3 &R2hrTmp, flag fOut)
 
 ~TotalJoint (void)
 
virtual std::ostream & Restart (std::ostream &out) const
 
virtual Joint::Type GetJointType (void) const
 
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
 
DofOrder::Order GetDofType (unsigned int i) const
 
virtual void SetValue (DataManager *pDM, VectorHandler &X, VectorHandler &XP, SimulationEntity::Hints *ph=0)
 
virtual Hint * ParseHint (DataManager *pDM, const char *s) const
 
virtual void AfterConvergence (const VectorHandler &X, const VectorHandler &XP)
 
virtual void AfterConvergence (const VectorHandler &X, const VectorHandler &XP, const VectorHandler &XPP)
 
void WorkSpaceDim (integer *piNumRows, integer *piNumCols) const
 
VariableSubMatrixHandler & AssJac (VariableSubMatrixHandler &WorkMat, doublereal dCoef, const VectorHandler &XCurr, const VectorHandler &XPrimeCurr)
 
SubVectorHandler & AssRes (SubVectorHandler &WorkVec, doublereal dCoef, const VectorHandler &XCurr, const VectorHandler &XPrimeCurr)
 
virtual bool bInverseDynamics (void) const
 
VariableSubMatrixHandler & AssJac (VariableSubMatrixHandler &WorkMat, const VectorHandler &XCurr)
 
SubVectorHandler & AssRes (SubVectorHandler &WorkVec, const VectorHandler &XCurr, const VectorHandler &XPrimeCurr, const VectorHandler &XPrimePrimeCurr, InverseDynamics::Order iOrder=InverseDynamics::INVERSE_DYNAMICS)
 
void Update (const VectorHandler &XCurr, InverseDynamics::Order iOrder=InverseDynamics::INVERSE_DYNAMICS)
 
DofOrder::Order GetEqType (unsigned int i) const
 
void OutputPrepare (OutputHandler &OH)
 
void Output (OutputHandler &OH) const
 
virtual unsigned int iGetInitialNumDof (void) const
 
virtual void InitialWorkSpaceDim (integer *piNumRows, integer *piNumCols) const
 
VariableSubMatrixHandler & InitialAssJac (VariableSubMatrixHandler &WorkMat, const VectorHandler &XCurr)
 
SubVectorHandler & InitialAssRes (SubVectorHandler &WorkVec, const VectorHandler &XCurr)
 
virtual unsigned int iGetNumPrivData (void) const
 
virtual unsigned int iGetPrivDataIdx (const char *s) const
 
virtual doublereal dGetPrivData (unsigned int i) const
 
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 void AssMats (VariableSubMatrixHandler &WorkMatA, VariableSubMatrixHandler &WorkMatB, const VectorHandler &XCurr, const VectorHandler &XPrimeCurr)
 
void SetInverseDynamicsFlags (unsigned uIDF)
 
unsigned GetInverseDynamicsFlags (void) const
 
bool bIsErgonomy (void) const
 
bool bIsRightHandSide (void) const
 
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 void BeforePredict (VectorHandler &, VectorHandler &, VectorHandler &, VectorHandler &) const
 
virtual void AfterPredict (VectorHandler &X, VectorHandler &XP)
 
virtual void Update (const VectorHandler &XCurr, const VectorHandler &XPrimeCurr)
 
virtual void DerivativesUpdate (const VectorHandler &XCurr, const VectorHandler &XPrimeCurr)
 
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 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 Joint
 Joint (unsigned int uL, const DofOwner *pD, flag fOut)
 
virtual ~Joint (void)
 
virtual Elem::Type GetElemType (void) const
 
std::ostream & Output (std::ostream &out, const char *sJointName, unsigned int uLabel, const Vec3 &FLocal, const Vec3 &MLocal, const Vec3 &FGlobal, const Vec3 &MGlobal) const
 
virtual void SetInitialValue (VectorHandler &)
 
bool bIsPrescribedMotion (void) const
 
bool bIsTorque (void) const
 
- Public Member Functions inherited from ElemGravityOwner
 ElemGravityOwner (unsigned int uL, flag fOut)
 
virtual ~ElemGravityOwner (void)
 
virtual doublereal dGetM (void) const
 
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 ElemWithDofs
 ElemWithDofs (unsigned int uL, const DofOwner *pDO, flag fOut)
 
virtual ~ElemWithDofs (void)
 
- Public Member Functions inherited from DofOwnerOwner
 DofOwnerOwner (const DofOwner *pDO)
 
virtual ~DofOwnerOwner ()
 
virtual const DofOwner * pGetDofOwner (void) const
 
virtual integer iGetFirstIndex (void) 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 Attributes

const StructNode * pNode1
 
const StructNode * pNode2
 
Vec3 f1
 
Mat3x3 R1h
 
Mat3x3 R1hr
 
Vec3 f2
 
Mat3x3 R2h
 
Mat3x3 R2hr
 
bool bPosActive [3]
 
bool bRotActive [3]
 
bool bVelActive [3]
 
bool bAgvActive [3]
 
TplDriveOwner< Vec3 > XDrv
 
TplDriveOwner< Vec3 > XPDrv
 
TplDriveOwner< Vec3 > XPPDrv
 
TplDriveOwner< Vec3 > ThetaDrv
 
TplDriveOwner< Vec3 > OmegaDrv
 
TplDriveOwner< Vec3 > OmegaPDrv
 
unsigned int nConstraints
 
unsigned int nPosConstraints
 
unsigned int nRotConstraints
 
unsigned int nVelConstraints
 
unsigned int nAgvConstraints
 
unsigned int iPosIncid [3]
 
unsigned int iRotIncid [3]
 
unsigned int iVelIncid [3]
 
unsigned int iAgvIncid [3]
 
unsigned int iPosEqIndex [3]
 
unsigned int iRotEqIndex [3]
 
unsigned int iVelEqIndex [3]
 
unsigned int iAgvEqIndex [3]
 
Vec3 tilde_f1
 
Vec3 M
 
Vec3 F
 
Vec3 ThetaDelta
 
Vec3 ThetaDeltaPrev
 
Vec3 ThetaDeltaRemnant
 
Vec3 ThetaDeltaTrue
 

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 }
 
- Public Types inherited from Joint
enum  Type {
  UNKNOWN = -1, DISTANCE = 0, DISTANCEWITHOFFSET, CLAMP,
  SPHERICALHINGE, PIN, UNIVERSALHINGE, UNIVERSALROTATION,
  UNIVERSALPIN, PLANEHINGE, PLANEROTATION, PLANEPIN,
  AXIALROTATION, PLANEDISP, PLANEDISPPIN, INPLANE,
  INPLANECONTACT, J_INLINE, ROD, RODBEZIER,
  DEFORMABLEHINGE, DEFORMABLEDISPJOINT, DEFORMABLEJOINT, DEFORMABLEAXIALJOINT,
  VISCOUSBODY, LINEARVELOCITY, ANGULARVELOCITY, LINEARACCELERATION,
  ANGULARACCELERATION, PRISMATIC, DRIVEHINGE, DRIVEDISP,
  DRIVEDISPPIN, IMPOSEDORIENTATION, IMPOSEDDISP, IMPOSEDDISPPIN,
  IMPOSEDKINEMATICS, BEAMSLIDER, BRAKE, GIMBAL,
  POINT_SURFACE_CONTACT, TOTALJOINT, TOTALPINJOINT, TOTALEQUATION,
  TOTALREACTION, MODAL, SCREWJOINT, LASTJOINTTYPE
}
 
- Protected Member Functions inherited from Joint
virtual void OutputPrepare_int (const std::string &type, OutputHandler &OH, std::string &name)
 
- Protected Member Functions inherited from ElemGravityOwner
virtual Vec3 GetS_int (void) const
 
virtual Mat3x3 GetJ_int (void) const
 
virtual Vec3 GetB_int (void) const
 
virtual Vec3 GetG_int (void) const
 
- Protected Attributes inherited from WithLabel
unsigned int uLabel
 
std::string sName
 
- Protected Attributes inherited from ToBeOutput
flag fOutput
 
- Protected Attributes inherited from GravityOwner
Gravity * pGravity
 

Detailed Description

Definition at line 45 of file totalj.h.

Constructor & Destructor Documentation

TotalJoint::TotalJoint ( unsigned int  uL,
const DofOwner *  pDO,
bool  bPos[3],
bool  bVel[3],
TplDriveCaller< Vec3 > *const  pDCPos[3],
bool  bRot[3],
bool  bAgv[3],
TplDriveCaller< Vec3 > *const  pDCRot[3],
const StructNode *  pN1,
const Vec3 &  f1Tmp,
const Mat3x3 &  R1hTmp,
const Mat3x3 &  R1hrTmp,
const StructNode *  pN2,
const Vec3 &  f2Tmp,
const Mat3x3 &  R2hTmp,
const Mat3x3 &  R2hrTmp,
flag  fOut 
)

Definition at line 51 of file totalj.cc.

References ASSERT, bAgvActive, bPosActive, bRotActive, bVelActive, StructNode::GetRCurr(), iAgvEqIndex, iAgvIncid, iPosEqIndex, iPosIncid, iRotEqIndex, iRotIncid, iVelEqIndex, iVelIncid, MBDYN_EXCEPT_ARGS, nAgvConstraints, nConstraints, nPosConstraints, nRotConstraints, nVelConstraints, pNode1, pNode2, R1hr, R2hr, ThetaDeltaTrue, and RotManip::VecRot().

61 : Elem(uL, fOut),
62 Joint(uL, pDO, fOut),
63 pNode1(pN1), pNode2(pN2),
64 f1(f1Tmp), R1h(R1hTmp), R1hr(R1hrTmp),
65 f2(f2Tmp), R2h(R2hTmp), R2hr(R2hrTmp),
66 XDrv(pDCPos[0]), XPDrv(pDCPos[1]), XPPDrv(pDCPos[2]),
67 ThetaDrv(pDCRot[0]), OmegaDrv(pDCRot[1]), OmegaPDrv(pDCRot[2]),
68 nConstraints(0), nPosConstraints(0), nRotConstraints(0),
69 nVelConstraints(0), nAgvConstraints(0),
70 tilde_f1(R1h.MulTV(f1)),
71 #ifdef USE_NETCDF
72 Var_X(0),
73 Var_Phi(0),
74 Var_V(0),
75 Var_Omega(0),
76 #endif // USE_NETCDF
77 M(::Zero3), F(::Zero3),
78 ThetaDelta(::Zero3), ThetaDeltaPrev(::Zero3),
79 ThetaDeltaRemnant(::Zero3),
80 ThetaDeltaTrue(::Zero3)
81 {
82  /* Equations 1->3: Positions
83  * Equations 4->6: Rotations */
84 
85  unsigned int index = 0;
86 
87  for (unsigned int i = 0; i < 3; i++) {
88  ASSERT(bPos[i] == false || bVel[i] == false);
89 
90  bPosActive[i] = bPos[i];
91  bVelActive[i] = bVel[i];
92 
93  if (bPosActive[i]) {
94  iPosIncid[nPosConstraints] = i + 1;
95  iPosEqIndex[nPosConstraints] = index;
96  nPosConstraints++;
97  index++;
98  }
99 
100  if (bVelActive[i]) {
101  iVelIncid[nVelConstraints] = i + 1;
102  iVelEqIndex[nVelConstraints] = index;
103  nVelConstraints++;
104  index++;
105  }
106  }
107 
108  index = 0;
109  for (unsigned int i = 0; i < 3; i++) {
110  ASSERT(bRot[i] == false || bAgv[i] == false);
111 
112  bRotActive[i] = bRot[i];
113  bAgvActive[i] = bAgv[i];
114 
115  if (bRotActive[i]) {
116  iRotIncid[nRotConstraints] = i + 1;
117  iRotEqIndex[nRotConstraints] = nPosConstraints + nVelConstraints + index;
118  nRotConstraints++;
119  index++;
120  }
121 
122  if (bAgvActive[i]) {
123  iAgvIncid[nAgvConstraints] = i + 1;
124  iAgvEqIndex[nAgvConstraints] = nPosConstraints + nVelConstraints + index;
125  nAgvConstraints++;
126  index++;
127  }
128  }
129 
130  // if only a fraction of the rotation degrees of freedom
131  // are constrained, store the incidence of the unconstrained ones
132  // in the remainder of array iRotIncid[] for later use
133  if (nRotConstraints > 0 && nRotConstraints < 3) {
134  switch (nRotConstraints) {
135  case 1:
136  switch (iRotIncid[0]) {
137  case 1:
138  iRotIncid[1] = 2;
139  iRotIncid[2] = 3;
140  break;
141 
142  case 2:
143  iRotIncid[1] = 1;
144  iRotIncid[2] = 3;
145  break;
146 
147  case 3:
148  iRotIncid[1] = 1;
149  iRotIncid[2] = 2;
150  break;
151 
152  default:
153  ASSERT(0);
154  throw ErrGeneric(MBDYN_EXCEPT_ARGS);
155  }
156  break;
157 
158  case 2:
159  switch (iRotIncid[0]) {
160  case 1:
161  iRotIncid[2] = 5 - iRotIncid[1];
162  break;
163 
164  case 2:
165  iRotIncid[2] = 1;
166  break;
167  }
168  break;
169 
170  default:
171  ASSERT(0);
172  throw ErrGeneric(MBDYN_EXCEPT_ARGS);
173  }
174  }
175 
176  nConstraints = nPosConstraints + nVelConstraints
177  + nRotConstraints + nAgvConstraints;
178 
179  ThetaDeltaTrue = RotManip::VecRot((pNode1->GetRCurr()*R1hr).MulTM(pNode2->GetRCurr()*R2hr));
180 }
TotalJoint::R2h
Mat3x3 R2h
Definition: totalj.h:54
TotalJoint::R1hr
Mat3x3 R1hr
Definition: totalj.h:52
TotalJoint::iVelEqIndex
unsigned int iVelEqIndex[3]
Definition: totalj.h:83
TotalJoint::R1h
Mat3x3 R1h
Definition: totalj.h:51
TotalJoint::bVelActive
bool bVelActive[3]
Definition: totalj.h:59
Zero3
const Vec3 Zero3(0., 0., 0.)
MBDYN_EXCEPT_ARGS
#define MBDYN_EXCEPT_ARGS
Definition: except.h:63
StructNode::GetRCurr
virtual const Mat3x3 & GetRCurr(void) const
Definition: strnode.h:1012
TotalJoint::f1
Vec3 f1
Definition: totalj.h:50
TotalJoint::OmegaPDrv
TplDriveOwner< Vec3 > OmegaPDrv
Definition: totalj.h:68
TotalJoint::nPosConstraints
unsigned int nPosConstraints
Definition: totalj.h:71
TotalJoint::nVelConstraints
unsigned int nVelConstraints
Definition: totalj.h:73
TotalJoint::ThetaDrv
TplDriveOwner< Vec3 > ThetaDrv
Definition: totalj.h:66
Joint::Joint
Joint(unsigned int uL, const DofOwner *pD, flag fOut)
Definition: joint.cc:83
TotalJoint::iPosIncid
unsigned int iPosIncid[3]
Definition: totalj.h:76
TotalJoint::pNode2
const StructNode * pNode2
Definition: totalj.h:49
TotalJoint::tilde_f1
Vec3 tilde_f1
Definition: totalj.h:86
TotalJoint::XPPDrv
TplDriveOwner< Vec3 > XPPDrv
Definition: totalj.h:64
TotalJoint::iRotEqIndex
unsigned int iRotEqIndex[3]
Definition: totalj.h:82
RotManip::VecRot
Vec3 VecRot(const Mat3x3 &Phi)
Definition: Rot.cc:136
Mat3x3::MulTV
Vec3 MulTV(const Vec3 &v) const
Definition: matvec3.cc:482
TotalJoint::ThetaDelta
Vec3 ThetaDelta
Definition: totalj.h:97
TotalJoint::iAgvIncid
unsigned int iAgvIncid[3]
Definition: totalj.h:79
TotalJoint::bRotActive
bool bRotActive[3]
Definition: totalj.h:57
TotalJoint::nAgvConstraints
unsigned int nAgvConstraints
Definition: totalj.h:74
TotalJoint::ThetaDeltaRemnant
Vec3 ThetaDeltaRemnant
Definition: totalj.h:100
TotalJoint::R2hr
Mat3x3 R2hr
Definition: totalj.h:55
TotalJoint::XPDrv
TplDriveOwner< Vec3 > XPDrv
Definition: totalj.h:63
TotalJoint::iAgvEqIndex
unsigned int iAgvEqIndex[3]
Definition: totalj.h:84
TotalJoint::bAgvActive
bool bAgvActive[3]
Definition: totalj.h:60
TotalJoint::pNode1
const StructNode * pNode1
Definition: totalj.h:48
TotalJoint::ThetaDeltaTrue
Vec3 ThetaDeltaTrue
Definition: totalj.h:101
ASSERT
#define ASSERT(expression)
Definition: colamd.c:977
TotalJoint::OmegaDrv
TplDriveOwner< Vec3 > OmegaDrv
Definition: totalj.h:67
TotalJoint::M
Vec3 M
Definition: totalj.h:95
TotalJoint::nRotConstraints
unsigned int nRotConstraints
Definition: totalj.h:72
TotalJoint::XDrv
TplDriveOwner< Vec3 > XDrv
Definition: totalj.h:62
TotalJoint::ThetaDeltaPrev
Vec3 ThetaDeltaPrev
Definition: totalj.h:98
TotalJoint::iVelIncid
unsigned int iVelIncid[3]
Definition: totalj.h:78
TotalJoint::nConstraints
unsigned int nConstraints
Definition: totalj.h:70
TotalJoint::F
Vec3 F
Definition: totalj.h:96
Elem::Elem
Elem(unsigned int uL, flag fOut)
Definition: elem.cc:41
TotalJoint::iRotIncid
unsigned int iRotIncid[3]
Definition: totalj.h:77
TotalJoint::f2
Vec3 f2
Definition: totalj.h:53
TotalJoint::iPosEqIndex
unsigned int iPosEqIndex[3]
Definition: totalj.h:81
TotalJoint::bPosActive
bool bPosActive[3]
Definition: totalj.h:56

Here is the call graph for this function:

TotalJoint::~TotalJoint ( void  )

Definition at line 182 of file totalj.cc.

References NO_OP.

183 {
184  NO_OP;
185 };
NO_OP
#define NO_OP
Definition: myassert.h:74

Member Function Documentation

void TotalJoint::AfterConvergence ( const VectorHandler &  X,
const VectorHandler &  XP 
)
virtual

Reimplemented from SimulationEntity.

Definition at line 726 of file totalj.cc.

References StructNode::GetRCurr(), iRotIncid, nRotConstraints, pNode1, pNode2, R1hr, R2hr, ThetaDelta, ThetaDeltaPrev, ThetaDeltaRemnant, ThetaDeltaTrue, Unwrap(), and RotManip::VecRot().

Referenced by AfterConvergence().

728 {
729  // "trick": correct ThetaDrv to keep ThetaDelta small
730  // See "A Vectorial Formulation of Generic Pair Constraints"
731  if (nRotConstraints < 3) {
732  Vec3 ThetaDeltaTmp(ThetaDelta);
733  for (unsigned i = 0; i < nRotConstraints; i++) {
734  // zero out constrained components
735  // NOTE: should already be zero within tolerance
736  ThetaDeltaTmp(iRotIncid[i]) = 0.;
737  }
738  ThetaDeltaRemnant += ThetaDeltaTmp;
739  }
740 
741  ThetaDeltaPrev = Unwrap(ThetaDeltaPrev, ThetaDeltaRemnant);
742  ThetaDeltaTrue = Unwrap(ThetaDeltaTrue, RotManip::VecRot((pNode1->GetRCurr()*R1hr).MulTM(pNode2->GetRCurr()*R2hr)));
743 }
TotalJoint::R1hr
Mat3x3 R1hr
Definition: totalj.h:52
Vec3
Definition: matvec3.h:98
StructNode::GetRCurr
virtual const Mat3x3 & GetRCurr(void) const
Definition: strnode.h:1012
TotalJoint::pNode2
const StructNode * pNode2
Definition: totalj.h:49
RotManip::VecRot
Vec3 VecRot(const Mat3x3 &Phi)
Definition: Rot.cc:136
TotalJoint::ThetaDelta
Vec3 ThetaDelta
Definition: totalj.h:97
TotalJoint::ThetaDeltaRemnant
Vec3 ThetaDeltaRemnant
Definition: totalj.h:100
TotalJoint::R2hr
Mat3x3 R2hr
Definition: totalj.h:55
TotalJoint::pNode1
const StructNode * pNode1
Definition: totalj.h:48
TotalJoint::ThetaDeltaTrue
Vec3 ThetaDeltaTrue
Definition: totalj.h:101
TotalJoint::nRotConstraints
unsigned int nRotConstraints
Definition: totalj.h:72
Unwrap
Vec3 Unwrap(const Vec3 &vPrev, const Vec3 &v)
Definition: matvec3.cc:1089
TotalJoint::ThetaDeltaPrev
Vec3 ThetaDeltaPrev
Definition: totalj.h:98
TotalJoint::iRotIncid
unsigned int iRotIncid[3]
Definition: totalj.h:77

Here is the call graph for this function:

void TotalJoint::AfterConvergence ( const VectorHandler &  X,
const VectorHandler &  XP,
const VectorHandler &  XPP 
)
virtual

Reimplemented from SimulationEntity.

Definition at line 746 of file totalj.cc.

References AfterConvergence().

748 {
749  AfterConvergence(X, XP);
750 }
TotalJoint::AfterConvergence
virtual void AfterConvergence(const VectorHandler &X, const VectorHandler &XP)
Definition: totalj.cc:726

Here is the call graph for this function:

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

Implements Elem.

Definition at line 834 of file totalj.cc.

References FullSubMatrixHandler::Add(), FullSubMatrixHandler::AddT(), Vec3::Cross(), DEBUGCOUT, F, f2, StructNode::GetRCurr(), StructDispNode::GetVCurr(), Mat3x3::GetVec(), StructNode::GetWCurr(), StructDispNode::GetXCurr(), iAgvEqIndex, iAgvIncid, DofOwnerOwner::iGetFirstIndex(), StructDispNode::iGetFirstMomentumIndex(), StructDispNode::iGetFirstPositionIndex(), iGetNumDof(), iPosEqIndex, iPosIncid, iRotEqIndex, iRotIncid, iVelEqIndex, iVelIncid, M, MatCross, MatCrossCross, nAgvConstraints, nConstraints, nPosConstraints, nRotConstraints, nVelConstraints, pNode1, pNode2, FullSubMatrixHandler::PutColIndex(), FullSubMatrixHandler::PutRowIndex(), R1h, R1hr, FullSubMatrixHandler::ResizeReset(), VariableSubMatrixHandler::SetFull(), VariableSubMatrixHandler::SetNullMatrix(), FullSubMatrixHandler::Sub(), FullSubMatrixHandler::SubT(), and WorkSpaceDim().

838 {
839  /*
840  See tecman.pdf for details
841  */
842 
843  DEBUGCOUT("Entering TotalJoint::AssJac()" << std::endl);
844 
845  if (iGetNumDof() == 0) {
846  WorkMat.SetNullMatrix();
847  return WorkMat;
848  }
849 
850  FullSubMatrixHandler& WM = WorkMat.SetFull();
851 
852  /* Ridimensiona la sottomatrice in base alle esigenze */
853  integer iNumRows = 0;
854  integer iNumCols = 0;
855  WorkSpaceDim(&iNumRows, &iNumCols);
856  WM.ResizeReset(iNumRows, iNumCols);
857 
858  /* Recupera gli indici delle varie incognite */
859  integer iNode1FirstPosIndex = pNode1->iGetFirstPositionIndex();
860  integer iNode1FirstMomIndex = pNode1->iGetFirstMomentumIndex();
861  integer iNode2FirstPosIndex = pNode2->iGetFirstPositionIndex();
862  integer iNode2FirstMomIndex = pNode2->iGetFirstMomentumIndex();
863  integer iFirstReactionIndex = iGetFirstIndex();
864 
865  /* Setta gli indici delle equazioni */
866  for (int iCnt = 1; iCnt <= 6; iCnt++) {
867  WM.PutRowIndex(iCnt, iNode1FirstMomIndex + iCnt);
868  WM.PutColIndex(iCnt, iNode1FirstPosIndex + iCnt);
869  WM.PutRowIndex(6 + iCnt, iNode2FirstMomIndex + iCnt);
870  WM.PutColIndex(6 + iCnt, iNode2FirstPosIndex + iCnt);
871  }
872 
873  for (unsigned int iCnt = 1; iCnt <= nConstraints; iCnt++) {
874  WM.PutRowIndex(12 + iCnt, iFirstReactionIndex + iCnt);
875  WM.PutColIndex(12 + iCnt, iFirstReactionIndex + iCnt);
876  }
877 
878  /* Recupera i dati che servono */
879  Mat3x3 R1(pNode1->GetRCurr()*R1h);
880  Mat3x3 R1r(pNode1->GetRCurr()*R1hr);
881  Vec3 b2(pNode2->GetRCurr()*f2);
882  Vec3 b1(pNode2->GetXCurr() + b2 - pNode1->GetXCurr());
883 
884  /* Moltiplica il momento e la forza per il coefficiente del metodo
885  * SOLO se il vincolo è in posizione */
886 
887  Vec3 FTmp(R1*(F*dCoef));
888  Vec3 MTmp(R1r*(M*dCoef));
889 
890  /* Equilibrium: ((Phi/q)^T*Lambda)/q */
891 
892  Mat3x3 Tmp;
893 
894  /* [ F x ] */
895  Tmp = Mat3x3(MatCross, FTmp);
896 
897  /* Lines 1->3: */
898  WM.Add(1, 3 + 1, Tmp);
899 
900  /* Lines 4->6: */
901  WM.Sub(3 + 1, 1, Tmp);
902 
903  WM.Add(3 + 1, 6 + 1, Tmp);
904 
905  /* Lines 7->9: */
906  WM.Sub(6 + 1, 3 + 1, Tmp);
907 
908  /* [ F x ] [ b2 x ] */
909  Tmp = Mat3x3(MatCrossCross, FTmp, b2);
910 
911  /* Lines 4->6: */
912  WM.Sub(3 + 1, 9 + 1, Tmp);
913 
914  /* Lines 10->12: */
915  WM.Add(9 + 1, 9 + 1, Tmp);
916 
917  /* [ b1 x ] [ F x ] + [ M x ] */
918 
919  /* Lines 4->6: */
920  WM.Add(3 + 1, 3 + 1, Mat3x3(MatCrossCross, b1, FTmp) + Mat3x3(MatCross, MTmp));
921 
922  /* [ b2 x ] [ F x ] + [ M x ] */
923 
924  /* Lines 10->12: */
925  WM.Sub(9 + 1, 3 + 1, Mat3x3(MatCrossCross, b2, FTmp) + Mat3x3(MatCross, MTmp));
926 
927  /* Phi/q and (Phi/q)^T */
928 
929  Mat3x3 b1Cross_R1;
930  Mat3x3 b2Cross_R1;
931  if (nPosConstraints > 0 || nVelConstraints > 0) {
932  b1Cross_R1 = b1.Cross(R1); // = [ b1 x ] * R1
933  b2Cross_R1 = b2.Cross(R1); // = [ b2 x ] * R1
934  }
935 
936  if (nPosConstraints > 0) {
937  for (unsigned iCnt = 0 ; iCnt < nPosConstraints; iCnt++) {
938  Vec3 vR1(R1.GetVec(iPosIncid[iCnt]));
939  Vec3 vb1Cross_R1(b1Cross_R1.GetVec(iPosIncid[iCnt]));
940  Vec3 vb2Cross_R1(b2Cross_R1.GetVec(iPosIncid[iCnt]));
941 
942  /* Equilibrium, node 1 */
943  WM.Sub(1, 12 + 1 + iPosEqIndex[iCnt], vR1);
944  WM.Sub(3 + 1, 12 + 1 + iPosEqIndex[iCnt], vb1Cross_R1);
945 
946  /* Constraint, node 1 */
947  WM.SubT(12 + 1 + iPosEqIndex[iCnt], 1, vR1);
948  WM.SubT(12 + 1 + iPosEqIndex[iCnt], 3 + 1, vb1Cross_R1);
949 
950  /* Equilibrium, node 2 */
951  WM.Add(6 + 1, 12 + 1 + iPosEqIndex[iCnt], vR1);
952  WM.Add(9 + 1, 12 + 1 + iPosEqIndex[iCnt], vb2Cross_R1);
953 
954  /* Constraint, node 2 */
955  WM.AddT(12 + 1 + iPosEqIndex[iCnt], 6 + 1, vR1);
956  WM.AddT(12 + 1 + iPosEqIndex[iCnt], 9 + 1, vb2Cross_R1);
957  }
958  }
959 
960  if (nVelConstraints > 0) {
961  Mat3x3 Omega1Cross_R1(pNode1->GetWCurr().Cross(R1));
962  Mat3x3 Tmp12 = (
963  - Mat3x3(MatCrossCross, pNode1->GetWCurr(), b1)
964  + Mat3x3(MatCrossCross, b2, pNode1->GetWCurr())
965  - Mat3x3(MatCross, pNode2->GetVCurr())
966  - Mat3x3(MatCrossCross, pNode2->GetWCurr(), b2)
967  + Mat3x3(MatCross, pNode1->GetVCurr())
968  ) * R1;
969  Mat3x3 Tmp22(MatCrossCross, pNode2->GetWCurr(), b2);
970 
971  for (unsigned iCnt = 0 ; iCnt < nVelConstraints; iCnt++) {
972  Vec3 vR1(R1.GetVec(iVelIncid[iCnt]));
973  Vec3 vb1Cross_R1(b1Cross_R1.GetVec(iVelIncid[iCnt]));
974  Vec3 vb2Cross_R1(b2Cross_R1.GetVec(iVelIncid[iCnt]));
975 
976  Vec3 vOmega1Cross_R1(Omega1Cross_R1.GetVec(iVelIncid[iCnt])*dCoef);
977  Vec3 vTmp12(Tmp12.GetVec(iVelIncid[iCnt]));
978  Vec3 vTmp22(Tmp22.GetVec(iVelIncid[iCnt]));
979 
980  /* Equilibrium, node 1 */
981  /* The same as in position constraint*/
982  WM.Sub(1, 12 + 1 + iVelEqIndex[iCnt], vR1); // delta_F
983  WM.Sub(3 + 1, 12 + 1 + iVelEqIndex[iCnt], vb1Cross_R1); // delta_M
984 
985  /* Constraint, node 1 */
986  /* The same as in position constraint*/
987  WM.SubT(12 + 1 + iVelEqIndex[iCnt], 1, vR1); // delta_v1
988  WM.SubT(12 + 1 + iVelEqIndex[iCnt], 3 + 1, vb1Cross_R1); // delta_W1
989 
990  /* New contributions, related to delta_x1 and delta_g1 */
991  WM.SubT(12 + 1 + iVelEqIndex[iCnt], 1, vOmega1Cross_R1); // delta_x1
992  WM.AddT(12 + 1 + iVelEqIndex[iCnt], 3 + 1, vTmp12 * dCoef); // delta_g1
993 
994  /* Equilibrium, node 2 */
995  /* The same as in position constraint*/
996  WM.Add(6 + 1, 12 + 1 + iVelEqIndex[iCnt], vR1); // delta_F
997  WM.Add(9 + 1, 12 + 1 + iVelEqIndex[iCnt], vb2Cross_R1); // delta_M
998 
999  /* Constraint, node 2 */
1000  /* The same as in position constraint*/
1001  WM.AddT(12 + 1 + iVelEqIndex[iCnt], 6 + 1, vR1); // delta_v2
1002  WM.AddT(12 + 1 + iVelEqIndex[iCnt], 9 + 1, vb2Cross_R1); // delta_W2
1003 
1004  /* New contributions, related to delta_x1 and delta_g1 */
1005  WM.AddT(12 + 1 + iVelEqIndex[iCnt], 6 + 1, vOmega1Cross_R1); // delta_x2
1006  WM.AddT(12 + 1 + iVelEqIndex[iCnt], 9 + 1, vTmp22 * dCoef); // delta_g2
1007  }
1008  }
1009 
1010  if (nRotConstraints > 0) {
1011  for (unsigned iCnt = 0 ; iCnt < nRotConstraints; iCnt++) {
1012  Vec3 vR1(R1r.GetVec(iRotIncid[iCnt]));
1013 
1014  /* Equilibrium, node 1 */
1015  WM.Sub(3 + 1, 12 + 1 + iRotEqIndex[iCnt], vR1);
1016 
1017  /* Constraint, node 1 */
1018  WM.SubT(12 + 1 + iRotEqIndex[iCnt], 3 + 1, vR1);
1019 
1020  /* Equilibrium, node 2 */
1021  WM.Add(9 + 1, 12 + 1 + iRotEqIndex[iCnt], vR1);
1022 
1023  /* Constraint, node 2 */
1024  WM.AddT(12 + 1 + iRotEqIndex[iCnt], 9 + 1, vR1);
1025  }
1026  }
1027 
1028  if (nAgvConstraints > 0) {
1029  Mat3x3 W2_Cross_R1(pNode2->GetWCurr().Cross(R1r));
1030 
1031  for (unsigned iCnt = 0 ; iCnt < nAgvConstraints; iCnt++) {
1032  Vec3 vR1(R1r.GetVec(iAgvIncid[iCnt]));
1033  Vec3 vW2_Cross_R1(W2_Cross_R1.GetVec(iAgvIncid[iCnt])*dCoef);
1034 
1035  /* Equilibrium, node 1 */
1036  WM.Sub(3 + 1, 12 + 1 + iAgvEqIndex[iCnt], vR1); // delta_M
1037 
1038  /* Constraint, node 1 */
1039  WM.SubT(12 + 1 + iAgvEqIndex[iCnt], 3 + 1, vR1); // delta_W1
1040 
1041  /* New contribution, related to delta_g1 */
1042  WM.SubT(12 + 1 + iAgvEqIndex[iCnt], 3 + 1, vW2_Cross_R1); // delta_g1
1043 
1044  /* Equilibrium, node 2 */
1045  WM.Add(9 + 1, 12 + 1 + iAgvEqIndex[iCnt], vR1); // delta_M
1046 
1047  /* Constraint, node 2 */
1048  WM.AddT(12 + 1 + iAgvEqIndex[iCnt], 9 + 1, vR1);// delta_W2
1049 
1050  /* New contribution, related to delta_g2 */
1051  WM.AddT(12 + 1 + iAgvEqIndex[iCnt], 9 + 1, vW2_Cross_R1); // delta_g2
1052  }
1053  }
1054 
1055  return WorkMat;
1056 }
TotalJoint::R1hr
Mat3x3 R1hr
Definition: totalj.h:52
TotalJoint::iVelEqIndex
unsigned int iVelEqIndex[3]
Definition: totalj.h:83
TotalJoint::R1h
Mat3x3 R1h
Definition: totalj.h:51
FullSubMatrixHandler::PutColIndex
void PutColIndex(integer iSubCol, integer iCol)
Definition: submat.h:325
Vec3::Cross
Vec3 Cross(const Vec3 &v) const
Definition: matvec3.h:218
TotalJoint::iGetNumDof
virtual unsigned int iGetNumDof(void) const
Definition: totalj.h:129
Vec3
Definition: matvec3.h:98
MatCross
const MatCross_Manip MatCross
Definition: matvec3.cc:639
VariableSubMatrixHandler::SetFull
FullSubMatrixHandler & SetFull(void)
Definition: submat.h:1168
StructNode::GetRCurr
virtual const Mat3x3 & GetRCurr(void) const
Definition: strnode.h:1012
FullSubMatrixHandler::Add
void Add(integer iRow, integer iCol, const Vec3 &v)
Definition: submat.cc:209
TotalJoint::nPosConstraints
unsigned int nPosConstraints
Definition: totalj.h:71
TotalJoint::nVelConstraints
unsigned int nVelConstraints
Definition: totalj.h:73
TotalJoint::iPosIncid
unsigned int iPosIncid[3]
Definition: totalj.h:76
TotalJoint::pNode2
const StructNode * pNode2
Definition: totalj.h:49
Mat3x3::GetVec
Vec3 GetVec(unsigned short int i) const
Definition: matvec3.h:893
TotalJoint::iRotEqIndex
unsigned int iRotEqIndex[3]
Definition: totalj.h:82
FullSubMatrixHandler::AddT
void AddT(integer iRow, integer iCol, const Vec3 &v)
Definition: submat.cc:227
TotalJoint::iAgvIncid
unsigned int iAgvIncid[3]
Definition: totalj.h:79
VariableSubMatrixHandler::SetNullMatrix
void SetNullMatrix(void)
Definition: submat.h:1159
TotalJoint::nAgvConstraints
unsigned int nAgvConstraints
Definition: totalj.h:74
DEBUGCOUT
#define DEBUGCOUT(msg)
Definition: myassert.h:232
StructDispNode::iGetFirstMomentumIndex
virtual integer iGetFirstMomentumIndex(void) const =0
StructDispNode::iGetFirstPositionIndex
virtual integer iGetFirstPositionIndex(void) const
Definition: strnode.h:452
StructNode::GetWCurr
virtual const Vec3 & GetWCurr(void) const
Definition: strnode.h:1030
TotalJoint::iAgvEqIndex
unsigned int iAgvEqIndex[3]
Definition: totalj.h:84
TotalJoint::pNode1
const StructNode * pNode1
Definition: totalj.h:48
StructDispNode::GetXCurr
virtual const Vec3 & GetXCurr(void) const
Definition: strnode.h:310
FullSubMatrixHandler::ResizeReset
virtual void ResizeReset(integer, integer)
Definition: submat.cc:182
TotalJoint::M
Vec3 M
Definition: totalj.h:95
TotalJoint::nRotConstraints
unsigned int nRotConstraints
Definition: totalj.h:72
TotalJoint::WorkSpaceDim
void WorkSpaceDim(integer *piNumRows, integer *piNumCols) const
Definition: totalj.h:177
MatCrossCross
const MatCrossCross_Manip MatCrossCross
Definition: matvec3.cc:640
FullSubMatrixHandler::SubT
void SubT(integer iRow, integer iCol, const Vec3 &v)
Definition: submat.cc:233
FullSubMatrixHandler::PutRowIndex
void PutRowIndex(integer iSubRow, integer iRow)
Definition: submat.h:311
StructDispNode::GetVCurr
virtual const Vec3 & GetVCurr(void) const
Definition: strnode.h:322
FullSubMatrixHandler::Sub
void Sub(integer iRow, integer iCol, const Vec3 &v)
Definition: submat.cc:215
TotalJoint::iVelIncid
unsigned int iVelIncid[3]
Definition: totalj.h:78
TotalJoint::nConstraints
unsigned int nConstraints
Definition: totalj.h:70
DofOwnerOwner::iGetFirstIndex
virtual integer iGetFirstIndex(void) const
Definition: dofown.h:127
TotalJoint::F
Vec3 F
Definition: totalj.h:96
TotalJoint::iRotIncid
unsigned int iRotIncid[3]
Definition: totalj.h:77
integer
long int integer
Definition: colamd.c:51
TotalJoint::f2
Vec3 f2
Definition: totalj.h:53
TotalJoint::iPosEqIndex
unsigned int iPosEqIndex[3]
Definition: totalj.h:81

Here is the call graph for this function:

VariableSubMatrixHandler & TotalJoint::AssJac ( VariableSubMatrixHandler &  WorkMat,
const VectorHandler &  XCurr 
)
virtual

Reimplemented from Elem.

Definition at line 1207 of file totalj.cc.

References FullSubMatrixHandler::AddT(), DEBUGCOUT, f2, StructNode::GetRCurr(), StructNode::GetRRef(), StructDispNode::GetXCurr(), DofOwnerOwner::iGetFirstIndex(), StructDispNode::iGetFirstPositionIndex(), iGetNumDof(), iPosIncid, iRotIncid, nConstraints, nPosConstraints, nRotConstraints, pNode1, pNode2, FullSubMatrixHandler::PutColIndex(), FullSubMatrixHandler::PutRowIndex(), R1h, R1hr, FullSubMatrixHandler::ResizeReset(), VariableSubMatrixHandler::SetFull(), VariableSubMatrixHandler::SetNullMatrix(), FullSubMatrixHandler::SubT(), and WorkSpaceDim().

1209 {
1210  /*
1211  * identical to regular AssJac's lower-left block
1212  */
1213  DEBUGCOUT("Entering TotalJoint::AssJac()" << std::endl);
1214 
1215  if (iGetNumDof() == 0) {
1216  WorkMat.SetNullMatrix();
1217  return WorkMat;
1218  }
1219 
1220  /* Ridimensiona la sottomatrice in base alle esigenze */
1221  integer iNumRows = 0;
1222  integer iNumCols = 0;
1223  WorkSpaceDim(&iNumRows, &iNumCols);
1224 
1225  FullSubMatrixHandler& WM = WorkMat.SetFull();
1226 
1227  /* original - nodes, nodes */
1228  WM.ResizeReset(iNumRows - 12, 12);
1229 
1230  /* Recupera gli indici delle varie incognite */
1231  integer iNode1FirstPosIndex = pNode1->iGetFirstPositionIndex();
1232  integer iNode2FirstPosIndex = pNode2->iGetFirstPositionIndex();
1233  integer iFirstReactionIndex = iGetFirstIndex();
1234 
1235  /* Setta gli indici delle equazioni */
1236  for (int iCnt = 1; iCnt <= 6; iCnt++) {
1237  WM.PutColIndex(iCnt, iNode1FirstPosIndex + iCnt);
1238  WM.PutColIndex(6 + iCnt, iNode2FirstPosIndex + iCnt);
1239  }
1240 
1241  for (unsigned int iCnt = 1; iCnt <= nConstraints; iCnt++) {
1242  WM.PutRowIndex(iCnt, iFirstReactionIndex + iCnt);
1243  }
1244 
1245  /* Recupera i dati che servono */
1246  Mat3x3 R1(pNode1->GetRRef()*R1h);
1247 
1248  if (nPosConstraints > 0) {
1249  Vec3 b2(pNode2->GetRCurr()*f2);
1250  Vec3 b1(pNode2->GetXCurr() + b2 - pNode1->GetXCurr());
1251 
1252  Mat3x3 b1Cross_R1(b1.Cross(R1)); // = [ b1 x ] * R1
1253  Mat3x3 b2Cross_R1(b2.Cross(R1)); // = [ b2 x ] * R1
1254 
1255  for (unsigned iCnt = 0 ; iCnt < nPosConstraints; iCnt++) {
1256  Vec3 vR1(R1.GetVec(iPosIncid[iCnt]));
1257  Vec3 vb1Cross_R1(b1Cross_R1.GetVec(iPosIncid[iCnt]));
1258  Vec3 vb2Cross_R1(b2Cross_R1.GetVec(iPosIncid[iCnt]));
1259 
1260  /* Constraint, node 1 */
1261  WM.SubT(1 + iCnt, 1, vR1);
1262  WM.SubT(1 + iCnt, 3 + 1, vb1Cross_R1);
1263 
1264  /* Constraint, node 2 */
1265  WM.AddT(1 + iCnt, 6 + 1, vR1);
1266  WM.AddT(1 + iCnt, 9 + 1, vb2Cross_R1);
1267  }
1268  }
1269 
1270  if (nRotConstraints > 0) {
1271  Mat3x3 R1r(pNode1->GetRRef()*R1hr);
1272  for (unsigned iCnt = 0 ; iCnt < nRotConstraints; iCnt++) {
1273  Vec3 vR1(R1r.GetVec(iRotIncid[iCnt]));
1274 
1275  /* Constraint, node 1 */
1276  WM.SubT(1 + nPosConstraints + iCnt, 3 + 1, vR1);
1277 
1278  /* Constraint, node 2 */
1279  WM.AddT(1 + nPosConstraints + iCnt, 9 + 1, vR1);
1280  }
1281  }
1282 
1283  return WorkMat;
1284 }
TotalJoint::R1hr
Mat3x3 R1hr
Definition: totalj.h:52
TotalJoint::R1h
Mat3x3 R1h
Definition: totalj.h:51
FullSubMatrixHandler::PutColIndex
void PutColIndex(integer iSubCol, integer iCol)
Definition: submat.h:325
TotalJoint::iGetNumDof
virtual unsigned int iGetNumDof(void) const
Definition: totalj.h:129
StructNode::GetRRef
virtual const Mat3x3 & GetRRef(void) const
Definition: strnode.h:1006
Vec3
Definition: matvec3.h:98
VariableSubMatrixHandler::SetFull
FullSubMatrixHandler & SetFull(void)
Definition: submat.h:1168
StructNode::GetRCurr
virtual const Mat3x3 & GetRCurr(void) const
Definition: strnode.h:1012
TotalJoint::nPosConstraints
unsigned int nPosConstraints
Definition: totalj.h:71
TotalJoint::iPosIncid
unsigned int iPosIncid[3]
Definition: totalj.h:76
TotalJoint::pNode2
const StructNode * pNode2
Definition: totalj.h:49
FullSubMatrixHandler::AddT
void AddT(integer iRow, integer iCol, const Vec3 &v)
Definition: submat.cc:227
VariableSubMatrixHandler::SetNullMatrix
void SetNullMatrix(void)
Definition: submat.h:1159
DEBUGCOUT
#define DEBUGCOUT(msg)
Definition: myassert.h:232
StructDispNode::iGetFirstPositionIndex
virtual integer iGetFirstPositionIndex(void) const
Definition: strnode.h:452
TotalJoint::pNode1
const StructNode * pNode1
Definition: totalj.h:48
StructDispNode::GetXCurr
virtual const Vec3 & GetXCurr(void) const
Definition: strnode.h:310
FullSubMatrixHandler::ResizeReset
virtual void ResizeReset(integer, integer)
Definition: submat.cc:182
TotalJoint::nRotConstraints
unsigned int nRotConstraints
Definition: totalj.h:72
TotalJoint::WorkSpaceDim
void WorkSpaceDim(integer *piNumRows, integer *piNumCols) const
Definition: totalj.h:177
FullSubMatrixHandler::SubT
void SubT(integer iRow, integer iCol, const Vec3 &v)
Definition: submat.cc:233
FullSubMatrixHandler::PutRowIndex
void PutRowIndex(integer iSubRow, integer iRow)
Definition: submat.h:311
TotalJoint::nConstraints
unsigned int nConstraints
Definition: totalj.h:70
DofOwnerOwner::iGetFirstIndex
virtual integer iGetFirstIndex(void) const
Definition: dofown.h:127
TotalJoint::iRotIncid
unsigned int iRotIncid[3]
Definition: totalj.h:77
integer
long int integer
Definition: colamd.c:51
TotalJoint::f2
Vec3 f2
Definition: totalj.h:53

Here is the call graph for this function:

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

Implements Elem.

Definition at line 1060 of file totalj.cc.

References VectorHandler::Add(), ASSERT, Vec3::Cross(), DEBUGCOUT, F, f2, TplDriveOwner< T >::Get(), StructNode::GetRCurr(), StructDispNode::GetVCurr(), StructNode::GetWCurr(), StructDispNode::GetXCurr(), iAgvEqIndex, iAgvIncid, DofOwnerOwner::iGetFirstIndex(), StructDispNode::iGetFirstMomentumIndex(), iGetNumDof(), iPosEqIndex, iPosIncid, iRotEqIndex, iRotIncid, iVelEqIndex, iVelIncid, M, Mat3x3::MulTM(), Mat3x3::MulTV(), nAgvConstraints, nConstraints, nPosConstraints, nRotConstraints, nVelConstraints, pNode1, pNode2, VectorHandler::PutCoef(), SubVectorHandler::PutRowIndex(), R1h, R1hr, R2hr, VectorHandler::ResizeReset(), RotManip::Rot(), VectorHandler::Sub(), ThetaDelta, ThetaDeltaRemnant, ThetaDrv, tilde_f1, RotManip::VecRot(), WorkSpaceDim(), and XDrv.

1064 {
1065  DEBUGCOUT("Entering TotalJoint::AssRes()" << std::endl);
1066 
1067  if (iGetNumDof() == 0) {
1068  WorkVec.ResizeReset(0);
1069  return WorkVec;
1070  }
1071 
1072  /* Dimensiona e resetta la matrice di lavoro */
1073  integer iNumRows = 0;
1074  integer iNumCols = 0;
1075  WorkSpaceDim(&iNumRows, &iNumCols);
1076  WorkVec.ResizeReset(iNumRows);
1077 
1078  /* Indici */
1079  integer iNode1FirstMomIndex = pNode1->iGetFirstMomentumIndex();
1080  integer iNode2FirstMomIndex = pNode2->iGetFirstMomentumIndex();
1081  integer iFirstReactionIndex = iGetFirstIndex();
1082 
1083  /* Indici dei nodi */
1084  for (int iCnt = 1; iCnt <= 6; iCnt++) {
1085  WorkVec.PutRowIndex(iCnt, iNode1FirstMomIndex + iCnt);
1086  WorkVec.PutRowIndex(6 + iCnt, iNode2FirstMomIndex + iCnt);
1087  }
1088 
1089  /* Indici del vincolo */
1090  for (unsigned int iCnt = 1; iCnt <= nConstraints; iCnt++) {
1091  WorkVec.PutRowIndex(12 + iCnt, iFirstReactionIndex + iCnt);
1092  }
1093 
1094  /* Get constraint reactions */
1095 
1096  for (unsigned iCnt = 0; iCnt < nPosConstraints; iCnt++) {
1097  F(iPosIncid[iCnt]) = XCurr(iFirstReactionIndex + 1 + iPosEqIndex[iCnt]);
1098  }
1099 
1100  for (unsigned iCnt = 0; iCnt < nRotConstraints; iCnt++) {
1101  M(iRotIncid[iCnt]) = XCurr(iFirstReactionIndex + 1 + iRotEqIndex[iCnt]);
1102  }
1103 
1104  for (unsigned iCnt = 0; iCnt < nVelConstraints; iCnt++) {
1105  F(iVelIncid[iCnt]) = XCurr(iFirstReactionIndex + 1 + iVelEqIndex[iCnt]);
1106  }
1107 
1108  for (unsigned iCnt = 0; iCnt < nAgvConstraints; iCnt++) {
1109  M(iAgvIncid[iCnt]) = XCurr(iFirstReactionIndex + 1 + iAgvEqIndex[iCnt]);
1110  }
1111 
1112  Vec3 b2(pNode2->GetRCurr()*f2);
1113  Vec3 b1(pNode2->GetXCurr() + b2 - pNode1->GetXCurr());
1114 
1115  Mat3x3 R1 = pNode1->GetRCurr()*R1h;
1116  Mat3x3 R1r = pNode1->GetRCurr()*R1hr;
1117 
1118  Vec3 XDelta;
1119  if (nPosConstraints) {
1120  XDelta = R1.MulTV(b1) - tilde_f1 - XDrv.Get();
1121  }
1122 
1123  Vec3 VDelta;
1124  if (nVelConstraints) {
1125  VDelta = R1.MulTV(
1126  b1.Cross(pNode1->GetWCurr())
1127  + pNode2->GetVCurr()
1128  - b2.Cross(pNode2->GetWCurr())
1129  - pNode1->GetVCurr()
1130  )
1131  - XDrv.Get();
1132  }
1133 
1134  if (nRotConstraints) {
1135  Mat3x3 R2r(pNode2->GetRCurr()*R2hr);
1136 
1137  Vec3 ThetaDrvTmp(ThetaDrv.Get());
1138  if (nRotConstraints < 3) {
1139  for (int i = nRotConstraints; i < 3; i++) {
1140  // zero out unconstrained components of drive
1141  ThetaDrvTmp(iRotIncid[i]) = 0.;
1142  }
1143  // add remnant to make ThetaDelta as close to zero
1144  // as possible
1145  ThetaDrvTmp += ThetaDeltaRemnant;
1146  }
1147  Mat3x3 R0(RotManip::Rot(ThetaDrvTmp));
1148  Mat3x3 RDelta(R1r.MulTM(R2r.MulMT(R0)));
1149  ThetaDelta = RotManip::VecRot(RDelta);
1150  }
1151 
1152  Vec3 WDelta;
1153  if (nAgvConstraints) {
1154  WDelta = R1r.MulTV(pNode2->GetWCurr() - pNode1->GetWCurr()) - ThetaDrv.Get();
1155  }
1156 
1157  Vec3 FTmp(R1*F);
1158  Vec3 MTmp(R1r*M);
1159 
1160  /* Equilibrium, node 1 */
1161  WorkVec.Add(1, FTmp);
1162  WorkVec.Add(3 + 1, MTmp + b1.Cross(FTmp));
1163 
1164  /* Equilibrium, node 2 */
1165  WorkVec.Sub(6 + 1, FTmp);
1166  WorkVec.Sub(9 + 1, MTmp + b2.Cross(FTmp));
1167 
1168  /* Holonomic constraint equations are divided by dCoef */
1169  ASSERT(dCoef != 0.);
1170 
1171  /* Position constraint: */
1172  for (unsigned iCnt = 0; iCnt < nPosConstraints; iCnt++) {
1173  WorkVec.PutCoef(12 + 1 + iPosEqIndex[iCnt],
1174  -XDelta(iPosIncid[iCnt])/dCoef);
1175  }
1176 
1177  /* Rotation constraints: */
1178  for (unsigned iCnt = 0; iCnt < nRotConstraints; iCnt++) {
1179  WorkVec.PutCoef(12 + 1 + iRotEqIndex[iCnt],
1180  -ThetaDelta(iRotIncid[iCnt])/dCoef);
1181  }
1182 
1183  /* Linear Velocity Constraint */
1184  for (unsigned iCnt = 0; iCnt < nVelConstraints; iCnt++) {
1185  WorkVec.PutCoef(12 + 1 + iVelEqIndex[iCnt],
1186  -VDelta(iVelIncid[iCnt]));
1187  }
1188 
1189  /* Angular Velocity Constraint */
1190  for (unsigned iCnt = 0; iCnt < nAgvConstraints; iCnt++) {
1191  WorkVec.PutCoef(12 + 1 + iAgvEqIndex[iCnt],
1192  -WDelta(iAgvIncid[iCnt]));
1193  }
1194 
1195  return WorkVec;
1196 }
TotalJoint::R1hr
Mat3x3 R1hr
Definition: totalj.h:52
TotalJoint::iVelEqIndex
unsigned int iVelEqIndex[3]
Definition: totalj.h:83
TotalJoint::R1h
Mat3x3 R1h
Definition: totalj.h:51
Vec3::Cross
Vec3 Cross(const Vec3 &v) const
Definition: matvec3.h:218
TotalJoint::iGetNumDof
virtual unsigned int iGetNumDof(void) const
Definition: totalj.h:129
Vec3
Definition: matvec3.h:98
VectorHandler::ResizeReset
virtual void ResizeReset(integer)
Definition: vh.cc:55
StructNode::GetRCurr
virtual const Mat3x3 & GetRCurr(void) const
Definition: strnode.h:1012
TotalJoint::nPosConstraints
unsigned int nPosConstraints
Definition: totalj.h:71
TotalJoint::nVelConstraints
unsigned int nVelConstraints
Definition: totalj.h:73
VectorHandler::Sub
virtual void Sub(integer iRow, const Vec3 &v)
Definition: vh.cc:78
TotalJoint::ThetaDrv
TplDriveOwner< Vec3 > ThetaDrv
Definition: totalj.h:66
TotalJoint::iPosIncid
unsigned int iPosIncid[3]
Definition: totalj.h:76
TotalJoint::pNode2
const StructNode * pNode2
Definition: totalj.h:49
TotalJoint::tilde_f1
Vec3 tilde_f1
Definition: totalj.h:86
TotalJoint::iRotEqIndex
unsigned int iRotEqIndex[3]
Definition: totalj.h:82
RotManip::VecRot
Vec3 VecRot(const Mat3x3 &Phi)
Definition: Rot.cc:136
Mat3x3::MulTV
Vec3 MulTV(const Vec3 &v) const
Definition: matvec3.cc:482
TotalJoint::ThetaDelta
Vec3 ThetaDelta
Definition: totalj.h:97
SubVectorHandler::PutRowIndex
virtual void PutRowIndex(integer iSubRow, integer iRow)=0
TotalJoint::iAgvIncid
unsigned int iAgvIncid[3]
Definition: totalj.h:79
TotalJoint::nAgvConstraints
unsigned int nAgvConstraints
Definition: totalj.h:74
TotalJoint::ThetaDeltaRemnant
Vec3 ThetaDeltaRemnant
Definition: totalj.h:100
DEBUGCOUT
#define DEBUGCOUT(msg)
Definition: myassert.h:232
TplDriveOwner::Get
T Get(const doublereal &dVar) const
Definition: tpldrive.h:109
TotalJoint::R2hr
Mat3x3 R2hr
Definition: totalj.h:55
RotManip::Rot
Mat3x3 Rot(const Vec3 &phi)
Definition: Rot.cc:62
StructDispNode::iGetFirstMomentumIndex
virtual integer iGetFirstMomentumIndex(void) const =0
StructNode::GetWCurr
virtual const Vec3 & GetWCurr(void) const
Definition: strnode.h:1030
TotalJoint::iAgvEqIndex
unsigned int iAgvEqIndex[3]
Definition: totalj.h:84
TotalJoint::pNode1
const StructNode * pNode1
Definition: totalj.h:48
ASSERT
#define ASSERT(expression)
Definition: colamd.c:977
Mat3x3::MulTM
Mat3x3 MulTM(const Mat3x3 &m) const
Definition: matvec3.cc:500
VectorHandler::PutCoef
virtual void PutCoef(integer iRow, const doublereal &dCoef)=0
StructDispNode::GetXCurr
virtual const Vec3 & GetXCurr(void) const
Definition: strnode.h:310
VectorHandler::Add
virtual void Add(integer iRow, const Vec3 &v)
Definition: vh.cc:63
TotalJoint::M
Vec3 M
Definition: totalj.h:95
TotalJoint::nRotConstraints
unsigned int nRotConstraints
Definition: totalj.h:72
TotalJoint::WorkSpaceDim
void WorkSpaceDim(integer *piNumRows, integer *piNumCols) const
Definition: totalj.h:177
TotalJoint::XDrv
TplDriveOwner< Vec3 > XDrv
Definition: totalj.h:62
StructDispNode::GetVCurr
virtual const Vec3 & GetVCurr(void) const
Definition: strnode.h:322
TotalJoint::iVelIncid
unsigned int iVelIncid[3]
Definition: totalj.h:78
TotalJoint::nConstraints
unsigned int nConstraints
Definition: totalj.h:70
DofOwnerOwner::iGetFirstIndex
virtual integer iGetFirstIndex(void) const
Definition: dofown.h:127
TotalJoint::F
Vec3 F
Definition: totalj.h:96
TotalJoint::iRotIncid
unsigned int iRotIncid[3]
Definition: totalj.h:77
integer
long int integer
Definition: colamd.c:51
TotalJoint::f2
Vec3 f2
Definition: totalj.h:53
TotalJoint::iPosEqIndex
unsigned int iPosEqIndex[3]
Definition: totalj.h:81

Here is the call graph for this function:

SubVectorHandler & TotalJoint::AssRes ( SubVectorHandler &  WorkVec,
const VectorHandler &  XCurr,
const VectorHandler &  XPrimeCurr,
const VectorHandler &  XPrimePrimeCurr,
InverseDynamics::Order  iOrder = InverseDynamics::INVERSE_DYNAMICS 
)
virtual

Reimplemented from Elem.

Definition at line 1288 of file totalj.cc.

References InverseDynamics::ACCELERATION, ASSERT, Vec3::Cross(), grad::Cross(), DEBUGCOUT, VectorHandler::DecCoef(), f2, TplDriveOwner< T >::Get(), StructNode::GetRCurr(), StructDispNode::GetVCurr(), StructNode::GetWCurr(), StructDispNode::GetXCurr(), DofOwnerOwner::iGetFirstIndex(), iGetNumDof(), invdyn2str(), InverseDynamics::INVERSE_DYNAMICS, iPosEqIndex, iPosIncid, iRotEqIndex, iRotIncid, MBDYN_EXCEPT_ARGS, Mat3x3::MulMT(), Mat3x3::MulTM(), Mat3x3::MulTV(), nConstraints, nPosConstraints, nRotConstraints, OmegaDrv, OmegaPDrv, pNode1, pNode2, InverseDynamics::POSITION, VectorHandler::PutCoef(), SubVectorHandler::PutRowIndex(), R1h, R1hr, R2hr, VectorHandler::ResizeReset(), RotManip::Rot(), ThetaDelta, ThetaDeltaRemnant, ThetaDrv, tilde_f1, RotManip::VecRot(), InverseDynamics::VELOCITY, WorkSpaceDim(), XDrv, XPDrv, and XPPDrv.

1293 {
1294  DEBUGCOUT("Entering TotalJoint::AssRes(" << invdyn2str(iOrder) << ")" << std::endl);
1295 
1296  if (iGetNumDof() == 0 || iOrder == InverseDynamics::INVERSE_DYNAMICS) {
1297  WorkVec.ResizeReset(0);
1298  return WorkVec;
1299  }
1300 
1301  /* Dimensiona e resetta la matrice di lavoro */
1302  integer iNumRows = 0;
1303  integer iNumCols = 0;
1304  WorkSpaceDim(&iNumRows, &iNumCols);
1305 
1306  /* original - node equations (6 * 2) */
1307  WorkVec.ResizeReset(iNumRows - 12);
1308 
1309  /* Indici */
1310  integer iFirstReactionIndex = iGetFirstIndex();
1311 
1312  /* Indici del vincolo */
1313  for (unsigned int iCnt = 1; iCnt <= nConstraints; iCnt++) {
1314  WorkVec.PutRowIndex(iCnt, iFirstReactionIndex + iCnt);
1315  }
1316 
1317  switch (iOrder) {
1318  case InverseDynamics::POSITION: // Position - Orientation
1319  /*
1320  * identical to regular AssRes' lower block
1321  */
1322 
1323  if (nPosConstraints > 0) {
1324  Mat3x3 R1 = pNode1->GetRCurr()*R1h;
1325  Vec3 b2(pNode2->GetRCurr()*f2);
1326  Vec3 b1(pNode2->GetXCurr() + b2 - pNode1->GetXCurr());
1327 
1328  Vec3 XDelta = R1.MulTV(b1) - tilde_f1 - XDrv.Get();
1329 
1330  /* Position constraint */
1331  for (unsigned iCnt = 0; iCnt < nPosConstraints; iCnt++) {
1332  WorkVec.DecCoef(1 + iPosEqIndex[iCnt], XDelta(iPosIncid[iCnt]));
1333  }
1334  }
1335 
1336  if (nRotConstraints > 0) {
1337  Mat3x3 R1r = pNode1->GetRCurr()*R1hr;
1338  Mat3x3 R2r = pNode2->GetRCurr()*R2hr;
1339 
1340  Vec3 ThetaDrvTmp(ThetaDrv.Get());
1341  if (nRotConstraints < 3) {
1342  for (int i = nRotConstraints; i < 3; i++) {
1343  // zero out unconstrained components of drive
1344  ThetaDrvTmp(iRotIncid[i]) = 0.;
1345  }
1346  // add remnant to make ThetaDelta as close to zero
1347  // as possible
1348  ThetaDrvTmp += ThetaDeltaRemnant;
1349  }
1350  Mat3x3 R0(RotManip::Rot(ThetaDrvTmp));
1351  Mat3x3 RDelta = R1r.MulTM(R2r.MulMT(R0));
1352  ThetaDelta = RotManip::VecRot(RDelta);
1353 
1354  /* Rotation constraint */
1355  for (unsigned iCnt = 0; iCnt < nRotConstraints; iCnt++) {
1356  WorkVec.DecCoef(1 + iRotEqIndex[iCnt], ThetaDelta(iRotIncid[iCnt]));
1357  }
1358  }
1359 
1360  break;
1361 
1362  case InverseDynamics::VELOCITY: // Velocity
1363  /*
1364  * first derivative of regular AssRes' lower block
1365  */
1366  if (nPosConstraints > 0) {
1367  Vec3 Tmp = XPDrv.Get();
1368 
1369  /* Position constraint derivative */
1370  for (unsigned iCnt = 0; iCnt < nPosConstraints; iCnt++) {
1371  WorkVec.PutCoef(1 + iPosEqIndex[iCnt], Tmp(iPosIncid[iCnt]));
1372  }
1373  }
1374 
1375  if (nRotConstraints > 0) {
1376  Mat3x3 R1r = pNode1->GetRCurr()*R1hr;
1377  Mat3x3 R2r = pNode2->GetRCurr()*R2hr;
1378 
1379  Mat3x3 R0 = RotManip::Rot(ThetaDrv.Get());
1380  Mat3x3 RDelta = R1r.MulTM(R2r.MulMT(R0));
1381 
1382  /*This name is only for clarity...*/
1383  Vec3 WDelta = RDelta * OmegaDrv.Get();
1384 
1385  /* Rotation constraint derivative */
1386  for (unsigned iCnt = 0; iCnt < nRotConstraints; iCnt++) {
1387  WorkVec.PutCoef(1 + iRotEqIndex[iCnt], WDelta(iRotIncid[iCnt]));
1388  }
1389  }
1390 
1391  break;
1392 
1393  case InverseDynamics::ACCELERATION: // Acceleration
1394  /*
1395  * second derivative of regular AssRes' lower block
1396  */
1397  if (nPosConstraints > 0) {
1398  Vec3 b2(pNode2->GetRCurr()*f2);
1399  Vec3 b1(pNode2->GetXCurr() + b2 - pNode1->GetXCurr());
1400 
1401  Mat3x3 R1 = pNode1->GetRCurr()*R1h;
1402 
1403  Vec3 b1Prime(pNode2->GetVCurr() + pNode2->GetWCurr().Cross(b2) - pNode1->GetVCurr());
1404 
1405  Vec3 Tmp2 = (- pNode1->GetWCurr().Cross(b1) + b1Prime).Cross(pNode1->GetWCurr());
1406  Tmp2 += (
1407  pNode1->GetWCurr().Cross(- pNode2->GetVCurr()
1408  + b2.Cross(pNode2->GetWCurr())
1409  + pNode1->GetVCurr())
1410  -pNode2->GetWCurr().Cross(b2.Cross(pNode2->GetWCurr()))
1411  );
1412  Vec3 Tmp = R1.MulTV(Tmp2) - XPPDrv.Get();
1413 
1414  /* Position constraint second derivative */
1415  for (unsigned iCnt = 0; iCnt < nPosConstraints; iCnt++) {
1416  WorkVec.DecCoef(1 + iPosEqIndex[iCnt], Tmp(iPosIncid[iCnt]));
1417  }
1418  }
1419 
1420  if (nRotConstraints > 0) {
1421  Mat3x3 R1r = pNode1->GetRCurr()*R1hr;
1422  Mat3x3 R2r = pNode2->GetRCurr()*R2hr;
1423  Mat3x3 R0 = RotManip::Rot(ThetaDrv.Get());
1424  Mat3x3 RDelta = R1r.MulTM(R2r.MulMT(R0));
1425 
1426  Vec3 Tmp = R0.MulTV(OmegaDrv.Get());
1427  Vec3 Tmp2 = R2r * Tmp;
1428  Tmp = (pNode2->GetWCurr() - pNode1->GetWCurr()).Cross(Tmp2);
1429  Tmp += pNode1->GetWCurr().Cross(pNode2->GetWCurr());
1430  Tmp2 = R1r.MulTV(Tmp);
1431  Tmp2 += RDelta * OmegaPDrv.Get();
1432 
1433  /* Rotation constraint second derivative */
1434  for (unsigned iCnt = 0; iCnt < nRotConstraints; iCnt++) {
1435  WorkVec.PutCoef(1 + iRotEqIndex[iCnt], Tmp2(iRotIncid[iCnt]));
1436  }
1437  }
1438 
1439  break;
1440 
1441  default:
1442  /*
1443  * higher-order derivatives make no sense right now
1444  */
1445 
1446  ASSERT(0);
1447  throw ErrGeneric(MBDYN_EXCEPT_ARGS);
1448  }
1449 
1450  return WorkVec;
1451 }
TotalJoint::R1hr
Mat3x3 R1hr
Definition: totalj.h:52
TotalJoint::R1h
Mat3x3 R1h
Definition: totalj.h:51
Vec3::Cross
Vec3 Cross(const Vec3 &v) const
Definition: matvec3.h:218
TotalJoint::iGetNumDof
virtual unsigned int iGetNumDof(void) const
Definition: totalj.h:129
MBDYN_EXCEPT_ARGS
#define MBDYN_EXCEPT_ARGS
Definition: except.h:63
Vec3
Definition: matvec3.h:98
VectorHandler::ResizeReset
virtual void ResizeReset(integer)
Definition: vh.cc:55
StructNode::GetRCurr
virtual const Mat3x3 & GetRCurr(void) const
Definition: strnode.h:1012
TotalJoint::OmegaPDrv
TplDriveOwner< Vec3 > OmegaPDrv
Definition: totalj.h:68
TotalJoint::nPosConstraints
unsigned int nPosConstraints
Definition: totalj.h:71
TotalJoint::ThetaDrv
TplDriveOwner< Vec3 > ThetaDrv
Definition: totalj.h:66
TotalJoint::iPosIncid
unsigned int iPosIncid[3]
Definition: totalj.h:76
TotalJoint::pNode2
const StructNode * pNode2
Definition: totalj.h:49
TotalJoint::tilde_f1
Vec3 tilde_f1
Definition: totalj.h:86
TotalJoint::XPPDrv
TplDriveOwner< Vec3 > XPPDrv
Definition: totalj.h:64
TotalJoint::iRotEqIndex
unsigned int iRotEqIndex[3]
Definition: totalj.h:82
RotManip::VecRot
Vec3 VecRot(const Mat3x3 &Phi)
Definition: Rot.cc:136
Mat3x3::MulTV
Vec3 MulTV(const Vec3 &v) const
Definition: matvec3.cc:482
TotalJoint::ThetaDelta
Vec3 ThetaDelta
Definition: totalj.h:97
SubVectorHandler::PutRowIndex
virtual void PutRowIndex(integer iSubRow, integer iRow)=0
VectorHandler::DecCoef
virtual void DecCoef(integer iRow, const doublereal &dCoef)=0
TotalJoint::ThetaDeltaRemnant
Vec3 ThetaDeltaRemnant
Definition: totalj.h:100
DEBUGCOUT
#define DEBUGCOUT(msg)
Definition: myassert.h:232
TplDriveOwner::Get
T Get(const doublereal &dVar) const
Definition: tpldrive.h:109
TotalJoint::R2hr
Mat3x3 R2hr
Definition: totalj.h:55
RotManip::Rot
Mat3x3 Rot(const Vec3 &phi)
Definition: Rot.cc:62
TotalJoint::XPDrv
TplDriveOwner< Vec3 > XPDrv
Definition: totalj.h:63
StructNode::GetWCurr
virtual const Vec3 & GetWCurr(void) const
Definition: strnode.h:1030
TotalJoint::pNode1
const StructNode * pNode1
Definition: totalj.h:48
ASSERT
#define ASSERT(expression)
Definition: colamd.c:977
TotalJoint::OmegaDrv
TplDriveOwner< Vec3 > OmegaDrv
Definition: totalj.h:67
Mat3x3::MulTM
Mat3x3 MulTM(const Mat3x3 &m) const
Definition: matvec3.cc:500
VectorHandler::PutCoef
virtual void PutCoef(integer iRow, const doublereal &dCoef)=0
grad::Cross
VectorExpression< VectorCrossExpr< VectorLhsExpr, VectorRhsExpr >, 3 > Cross(const VectorExpression< VectorLhsExpr, 3 > &u, const VectorExpression< VectorRhsExpr, 3 > &v)
Definition: matvec.h:3248
StructDispNode::GetXCurr
virtual const Vec3 & GetXCurr(void) const
Definition: strnode.h:310
TotalJoint::nRotConstraints
unsigned int nRotConstraints
Definition: totalj.h:72
TotalJoint::WorkSpaceDim
void WorkSpaceDim(integer *piNumRows, integer *piNumCols) const
Definition: totalj.h:177
TotalJoint::XDrv
TplDriveOwner< Vec3 > XDrv
Definition: totalj.h:62
StructDispNode::GetVCurr
virtual const Vec3 & GetVCurr(void) const
Definition: strnode.h:322
invdyn2str
const char * invdyn2str(InverseDynamics::Order iOrder)
Definition: invdyn.cc:36
TotalJoint::nConstraints
unsigned int nConstraints
Definition: totalj.h:70
Mat3x3::MulMT
Mat3x3 MulMT(const Mat3x3 &m) const
Definition: matvec3.cc:444
DofOwnerOwner::iGetFirstIndex
virtual integer iGetFirstIndex(void) const
Definition: dofown.h:127
TotalJoint::iRotIncid
unsigned int iRotIncid[3]
Definition: totalj.h:77
integer
long int integer
Definition: colamd.c:51
TotalJoint::f2
Vec3 f2
Definition: totalj.h:53
TotalJoint::iPosEqIndex
unsigned int iPosEqIndex[3]
Definition: totalj.h:81

Here is the call graph for this function:

bool TotalJoint::bInverseDynamics ( void  ) const
virtual

Reimplemented from Elem.

Definition at line 1200 of file totalj.cc.

1201 {
1202  return true;
1203 }
std::ostream & TotalJoint::DescribeDof ( std::ostream &  out,
const char *  prefix = "",
bool  bInitial = false 
) const
virtual

Reimplemented from Elem.

Definition at line 188 of file totalj.cc.

References bAgvActive, bPosActive, bRotActive, bVelActive, idx2xyz, DofOwnerOwner::iGetFirstIndex(), nAgvConstraints, nConstraints, nPosConstraints, nRotConstraints, and nVelConstraints.

190 {
191  integer iIndex = iGetFirstIndex();
192 
193  if (nPosConstraints > 0 || nVelConstraints > 0) {
194  out << prefix << iIndex + 1;
195  if (nPosConstraints + nVelConstraints > 1) {
196  out << "->" << iIndex + nPosConstraints + nVelConstraints;
197  }
198  out << ": "
199  "reaction force(s) [";
200 
201  for (unsigned int i = 0, cnt = 0; i < 3; i++) {
202  if (bPosActive[i] || bVelActive[i]) {
203  cnt++;
204  if (cnt > 1) {
205  out << ",";
206  }
207  out << "F" << idx2xyz[i];
208  }
209  }
210  out << "]" << std::endl;
211  }
212 
213  if (nRotConstraints > 0 || nAgvConstraints > 0) {
214  out << prefix << iIndex + nPosConstraints + nVelConstraints + 1;
215  if (nRotConstraints + nAgvConstraints > 1) {
216  out << "->" << iIndex + nConstraints;
217  }
218  out << ": "
219  "reaction couple(s) [";
220 
221  for (unsigned int i = 0, cnt = 0; i < 3; i++) {
222  if (bRotActive[i] || bAgvActive[i]) {
223  cnt++;
224  if (cnt > 1) {
225  out << ",";
226  }
227  out << "m" << idx2xyz[i];
228  }
229  }
230  out << "]" << std::endl;
231  }
232 
233  if (bInitial) {
234  iIndex += nConstraints;
235 
236  if (nPosConstraints > 0) {
237  out << prefix << iIndex + 1;
238  if (nPosConstraints > 1) {
239  out << "->" << iIndex + nPosConstraints;
240  }
241  out << ": "
242  "reaction force(s) derivative(s) [";
243 
244  for (unsigned int i = 0, cnt = 0; i < 3; i++) {
245  if (bPosActive[i]) {
246  cnt++;
247  if (cnt > 1) {
248  out << ",";
249  }
250  out << "FP" << idx2xyz[i];
251  }
252  }
253  out << "]" << std::endl;
254  }
255 
256 
257  if (nRotConstraints > 0) {
258  out << prefix << iIndex + nPosConstraints + 1;
259  if (nRotConstraints > 1) {
260  out << "->" << iIndex + nConstraints;
261  }
262  out << ": "
263  "reaction couple(s) derivative(s) [";
264 
265  for (unsigned int i = 0, cnt = 0; i < 3; i++) {
266  if (bRotActive[i]) {
267  cnt++;
268  if (cnt > 1) {
269  out << ",";
270  }
271  out << "mP" << idx2xyz[i];
272  }
273  }
274  out << "]" << std::endl;
275  }
276  }
277 
278  return out;
279 }
TotalJoint::bVelActive
bool bVelActive[3]
Definition: totalj.h:59
TotalJoint::nPosConstraints
unsigned int nPosConstraints
Definition: totalj.h:71
TotalJoint::nVelConstraints
unsigned int nVelConstraints
Definition: totalj.h:73
TotalJoint::bRotActive
bool bRotActive[3]
Definition: totalj.h:57
TotalJoint::nAgvConstraints
unsigned int nAgvConstraints
Definition: totalj.h:74
TotalJoint::bAgvActive
bool bAgvActive[3]
Definition: totalj.h:60
TotalJoint::nRotConstraints
unsigned int nRotConstraints
Definition: totalj.h:72
TotalJoint::nConstraints
unsigned int nConstraints
Definition: totalj.h:70
DofOwnerOwner::iGetFirstIndex
virtual integer iGetFirstIndex(void) const
Definition: dofown.h:127
integer
long int integer
Definition: colamd.c:51
idx2xyz
static const char idx2xyz[]
Definition: totalj.cc:47
TotalJoint::bPosActive
bool bPosActive[3]
Definition: totalj.h:56

Here is the call graph for this function:

void TotalJoint::DescribeDof ( std::vector< std::string > &  desc,
bool  bInitial = false,
int  i = -1 
) const
virtual

Reimplemented from Elem.

Definition at line 282 of file totalj.cc.

References ASSERT, bAgvActive, bPosActive, bRotActive, bVelActive, WithLabel::GetLabel(), idx2xyz, iGetInitialNumDof(), iGetNumDof(), nAgvConstraints, nPosConstraints, nRotConstraints, and nVelConstraints.

284 {
285  int ndof = 1;
286  if (i == -1) {
287  if (bInitial) {
288  ndof = iGetInitialNumDof();
289 
290  } else {
291  ndof = iGetNumDof();
292  }
293  }
294  desc.resize(ndof);
295 
296  std::ostringstream os;
297  os << "TotalJoint(" << GetLabel() << ")";
298 
299  if (i == -1) {
300  std::string name(os.str());
301  unsigned int cnt = 0;
302 
303  if (nPosConstraints > 0 || nVelConstraints > 0) {
304  for (unsigned int i = 0; i < 3; i++) {
305  if (bPosActive[i] || bVelActive[i]) {
306  os.str(name);
307  os.seekp(0, std::ios_base::end);
308  os << ": dof(" << cnt + 1 << ") F" << idx2xyz[i];
309  desc[cnt] = os.str();
310  cnt++;
311  }
312  }
313  }
314 
315  if (nRotConstraints > 0 || nAgvConstraints > 0) {
316  for (unsigned int i = 0; i < 3; i++) {
317  if (bRotActive[i] || bAgvActive[i]) {
318  os.str(name);
319  os.seekp(0, std::ios_base::end);
320  os << ": dof(" << cnt + 1 << ") M" << idx2xyz[i];
321  desc[cnt] = os.str();
322  cnt++;
323  }
324  }
325  }
326 
327  if (bInitial) {
328  if (nPosConstraints > 0) {
329  for (unsigned int i = 0; i < 3; i++) {
330  if (bPosActive[i]) {
331  os.str(name);
332  os.seekp(0, std::ios_base::end);
333  os << ": dof(" << cnt + 1 << ") FP" << idx2xyz[i];
334  desc[cnt] = os.str();
335  cnt++;
336  }
337  }
338  }
339 
340  if (nRotConstraints > 0) {
341  for (unsigned int i = 0; i < 3; i++) {
342  if (bRotActive[i]) {
343  os.str(name);
344  os.seekp(0, std::ios_base::end);
345  os << ": dof(" << cnt + 1 << ") MP" << idx2xyz[i];
346  desc[cnt] = os.str();
347  cnt++;
348  }
349  }
350  }
351  }
352 
353  ASSERT(cnt == ndof);
354 
355  } else {
356  os << ": dof(" << i + 1 << ")";
357  desc[0] = os.str();
358  }
359 }
TotalJoint::iGetInitialNumDof
virtual unsigned int iGetInitialNumDof(void) const
Definition: totalj.h:220
TotalJoint::bVelActive
bool bVelActive[3]
Definition: totalj.h:59
TotalJoint::iGetNumDof
virtual unsigned int iGetNumDof(void) const
Definition: totalj.h:129
TotalJoint::nPosConstraints
unsigned int nPosConstraints
Definition: totalj.h:71
TotalJoint::nVelConstraints
unsigned int nVelConstraints
Definition: totalj.h:73
TotalJoint::bRotActive
bool bRotActive[3]
Definition: totalj.h:57
TotalJoint::nAgvConstraints
unsigned int nAgvConstraints
Definition: totalj.h:74
TotalJoint::bAgvActive
bool bAgvActive[3]
Definition: totalj.h:60
ASSERT
#define ASSERT(expression)
Definition: colamd.c:977
TotalJoint::nRotConstraints
unsigned int nRotConstraints
Definition: totalj.h:72
WithLabel::GetLabel
unsigned int GetLabel(void) const
Definition: withlab.cc:62
idx2xyz
static const char idx2xyz[]
Definition: totalj.cc:47
TotalJoint::bPosActive
bool bPosActive[3]
Definition: totalj.h:56

Here is the call graph for this function:

std::ostream & TotalJoint::DescribeEq ( std::ostream &  out,
const char *  prefix = "",
bool  bInitial = false 
) const
virtual

Reimplemented from Elem.

Definition at line 362 of file totalj.cc.

References bAgvActive, bPosActive, bRotActive, bVelActive, idx2xyz, DofOwnerOwner::iGetFirstIndex(), nAgvConstraints, nConstraints, nPosConstraints, nRotConstraints, and nVelConstraints.

364 {
365  integer iIndex = iGetFirstIndex();
366 
367  if (nPosConstraints > 0 || nVelConstraints > 0) {
368  out << prefix << iIndex + 1;
369  if (nPosConstraints + nVelConstraints > 1) {
370  out << "->" << iIndex + nPosConstraints + nVelConstraints;
371  }
372  out << ": "
373  "position/velocity constraint(s) [";
374 
375  for (unsigned int i = 0, cnt = 0; i < 3; i++) {
376  if (bPosActive[i] || bVelActive[i]) {
377  cnt++;
378  if (cnt > 1) {
379  out << ",";
380  }
381 
382  if (bPosActive[i]) {
383  out << "P" << idx2xyz[i] << "1=P" << idx2xyz[i] << "2";
384  } else {
385  out << "V" << idx2xyz[i] << "1=V" << idx2xyz[i] << "2";
386  }
387  }
388  }
389 
390  out << "]" << std::endl;
391  }
392 
393  if (nRotConstraints > 0 || nAgvConstraints > 0) {
394  out << prefix << iIndex + nPosConstraints + nVelConstraints + 1;
395  if (nRotConstraints + nAgvConstraints > 1) {
396  out << "->" << iIndex + nConstraints ;
397  }
398  out << ": "
399  "orientation/angular velocity constraint(s) [";
400 
401  for (unsigned int i = 0, cnt = 0; i < 3; i++) {
402  if (bRotActive[i] || bAgvActive[i]) {
403  cnt++;
404  if (cnt > 1) {
405  out << ",";
406  }
407 
408  if (bRotActive[i]) {
409  out << "theta" << idx2xyz[i] << "1=theta" << idx2xyz[i] << "2";
410  } else {
411  out << "W" << idx2xyz[i] << "1=W" << idx2xyz[i] << "2";
412  }
413  }
414  }
415 
416  out << "]" << std::endl;
417  }
418 
419  if (bInitial) {
420  iIndex += nConstraints;
421 
422  if (nPosConstraints > 0) {
423  out << prefix << iIndex + 1;
424  if (nPosConstraints > 1) {
425  out << "->" << iIndex + nPosConstraints;
426  }
427  out << ": "
428  "velocity constraint(s) [";
429 
430  for (unsigned int i = 0, cnt = 0; i < 3; i++) {
431  if (bPosActive[i]) {
432  cnt++;
433  if (cnt > 1) {
434  out << ",";
435  }
436  out << "v" << idx2xyz[i] << "1=v" << idx2xyz[i] << "2";
437  }
438  }
439 
440  out << "]" << std::endl;
441  }
442 
443  if (nRotConstraints > 0) {
444  out << prefix << iIndex + nPosConstraints + 1;
445  if (nRotConstraints > 1) {
446  out << "->" << iIndex + nConstraints ;
447  }
448  out << ": "
449  "angular velocity constraint(s) [";
450 
451  for (unsigned int i = 0, cnt = 0; i < 3; i++) {
452  if (bRotActive[i]) {
453  cnt++;
454  if (cnt > 1) {
455  out << ",";
456  }
457  out << "w" << idx2xyz[i] << "1=w" << idx2xyz[i] << "2";
458  }
459  }
460 
461  out << "]" << std::endl;
462  }
463  }
464 
465  return out;
466 }
TotalJoint::bVelActive
bool bVelActive[3]
Definition: totalj.h:59
TotalJoint::nPosConstraints
unsigned int nPosConstraints
Definition: totalj.h:71
TotalJoint::nVelConstraints
unsigned int nVelConstraints
Definition: totalj.h:73
TotalJoint::bRotActive
bool bRotActive[3]
Definition: totalj.h:57
TotalJoint::nAgvConstraints
unsigned int nAgvConstraints
Definition: totalj.h:74
TotalJoint::bAgvActive
bool bAgvActive[3]
Definition: totalj.h:60
TotalJoint::nRotConstraints
unsigned int nRotConstraints
Definition: totalj.h:72
TotalJoint::nConstraints
unsigned int nConstraints
Definition: totalj.h:70
DofOwnerOwner::iGetFirstIndex
virtual integer iGetFirstIndex(void) const
Definition: dofown.h:127
integer
long int integer
Definition: colamd.c:51
idx2xyz
static const char idx2xyz[]
Definition: totalj.cc:47
TotalJoint::bPosActive
bool bPosActive[3]
Definition: totalj.h:56

Here is the call graph for this function:

void TotalJoint::DescribeEq ( std::vector< std::string > &  desc,
bool  bInitial = false,
int  i = -1 
) const
virtual

Reimplemented from Elem.

Definition at line 469 of file totalj.cc.

References ASSERT, bAgvActive, bPosActive, bRotActive, bVelActive, WithLabel::GetLabel(), idx2xyz, iGetInitialNumDof(), iGetNumDof(), nAgvConstraints, nPosConstraints, nRotConstraints, and nVelConstraints.

471 {
472  int ndof = 1;
473  if (i == -1) {
474  if (bInitial) {
475  ndof = iGetInitialNumDof();
476 
477  } else {
478  ndof = iGetNumDof();
479  }
480  }
481  desc.resize(ndof);
482 
483  std::ostringstream os;
484  os << "TotalJoint(" << GetLabel() << ")";
485 
486  if (i == -1) {
487  std::string name(os.str());
488  unsigned int cnt = 0;
489 
490  if (nPosConstraints > 0 || nVelConstraints > 0) {
491  for (unsigned int i = 0; i < 3; i++) {
492  if (bPosActive[i]) {
493  os.str(name);
494  os.seekp(0, std::ios_base::end);
495  os << ": equation(" << cnt + 1 << ") P" << idx2xyz[i] << "1=P" << idx2xyz[i] << "2";
496  desc[cnt] = os.str();
497  cnt++;
498 
499  } else if (bVelActive[i]) {
500  os.str(name);
501  os.seekp(0, std::ios_base::end);
502  os << ": equation(" << cnt + 1 << ") V" << idx2xyz[i] << "1=V" << idx2xyz[i] << "2";
503  desc[cnt] = os.str();
504  cnt++;
505  }
506  }
507  }
508 
509  if (nRotConstraints > 0 || nAgvConstraints > 0) {
510  for (unsigned int i = 0; i < 3; i++) {
511  if (bRotActive[i]) {
512  os.str(name);
513  os.seekp(0, std::ios_base::end);
514  os << ": equation(" << cnt + 1 << ") theta" << idx2xyz[i] << "1=theta" << idx2xyz[i] << "2";
515  desc[cnt] = os.str();
516  cnt++;
517 
518  } else if (bAgvActive[i]) {
519  os.str(name);
520  os.seekp(0, std::ios_base::end);
521  os << ": equation(" << cnt + 1 << ") W" << idx2xyz[i] << "1=W" << idx2xyz[i] << "2";
522  desc[cnt] = os.str();
523  cnt++;
524  }
525  }
526  }
527 
528  if (bInitial) {
529  if (nPosConstraints > 0) {
530  for (unsigned int i = 0; i < 3; i++) {
531  if (bPosActive[i]) {
532  os.str(name);
533  os.seekp(0, std::ios_base::end);
534  os << ": equation(" << cnt + 1 << ") v" << idx2xyz[i] << "1=v" << idx2xyz[i] << "2";
535  desc[cnt] = os.str();
536  cnt++;
537  }
538  }
539  }
540 
541  if (nRotConstraints > 0) {
542  for (unsigned int i = 0; i < 3; i++) {
543  if (bRotActive[i]) {
544  os.str(name);
545  os.seekp(0, std::ios_base::end);
546  os << ": equation(" << cnt + 1 << ") w" << idx2xyz[i] << "1=w" << idx2xyz[i] << "2";
547  desc[cnt] = os.str();
548  cnt++;
549  }
550  }
551  }
552  }
553 
554  ASSERT(cnt == ndof);
555 
556  } else {
557  os << ": equation(" << i + 1 << ")";
558  desc[0] = os.str();
559  }
560 }
TotalJoint::iGetInitialNumDof
virtual unsigned int iGetInitialNumDof(void) const
Definition: totalj.h:220
TotalJoint::bVelActive
bool bVelActive[3]
Definition: totalj.h:59
TotalJoint::iGetNumDof
virtual unsigned int iGetNumDof(void) const
Definition: totalj.h:129
TotalJoint::nPosConstraints
unsigned int nPosConstraints
Definition: totalj.h:71
TotalJoint::nVelConstraints
unsigned int nVelConstraints
Definition: totalj.h:73
TotalJoint::bRotActive
bool bRotActive[3]
Definition: totalj.h:57
TotalJoint::nAgvConstraints
unsigned int nAgvConstraints
Definition: totalj.h:74
TotalJoint::bAgvActive
bool bAgvActive[3]
Definition: totalj.h:60
ASSERT
#define ASSERT(expression)
Definition: colamd.c:977
TotalJoint::nRotConstraints
unsigned int nRotConstraints
Definition: totalj.h:72
WithLabel::GetLabel
unsigned int GetLabel(void) const
Definition: withlab.cc:62
idx2xyz
static const char idx2xyz[]
Definition: totalj.cc:47
TotalJoint::bPosActive
bool bPosActive[3]
Definition: totalj.h:56

Here is the call graph for this function:

doublereal TotalJoint::dGetPrivData ( unsigned int  i) const
virtual

Reimplemented from SimulationEntity.

Definition at line 1993 of file totalj.cc.

References ASSERT, bPosActive, bRotActive, Vec3::Cross(), F, f1, f2, TplDriveOwner< T >::Get(), StructNode::GetRCurr(), StructDispNode::GetVCurr(), Mat3x3::GetVec(), StructNode::GetWCurr(), StructDispNode::GetXCurr(), M, Mat3x3::MulTV(), pNode1, pNode2, R1h, R1hr, R2hr, ThetaDeltaTrue, ThetaDrv, Unwrap(), RotManip::VecRot(), and XDrv.

1994 {
1995  switch (i) {
1996  case 1:
1997  case 2:
1998  case 3: {
1999  Vec3 x(pNode1->GetRCurr().MulTV(
2000  pNode2->GetXCurr() + pNode2->GetRCurr()*f2
2001  - pNode1->GetXCurr()) - f1);
2002  return R1h.GetVec(i)*x;
2003  }
2004 
2005  case 4:
2006  case 5:
2007  case 6: {
2008  Vec3 Theta(Unwrap(ThetaDeltaTrue, RotManip::VecRot((pNode1->GetRCurr()*R1hr).MulTM(pNode2->GetRCurr()*R2hr))));
2009  return Theta(i - 3);
2010  }
2011 
2012  case 7:
2013  case 8:
2014  case 9:
2015  return F(i - 6);
2016 
2017  case 10:
2018  case 11:
2019  case 12:
2020  return M(i - 9);
2021 
2022  case 13:
2023  case 14:
2024  case 15:
2025  if (!bPosActive[i - 13]) {
2026  return 0.;
2027  }
2028  return XDrv.Get()(i - 12);
2029 
2030  case 16:
2031  case 17:
2032  case 18:
2033  if (!bRotActive[i - 16]) {
2034  return 0.;
2035  }
2036  return ThetaDrv.Get()(i - 15);
2037 
2038  case 19:
2039  case 20:
2040  case 21:
2041  {
2042  Vec3 v( pNode1->GetRCurr().MulTV(
2043  (pNode2->GetVCurr() + pNode2->GetWCurr().Cross(pNode2->GetRCurr()*f2)
2044  - pNode1->GetVCurr()) -
2045  pNode1->GetWCurr().Cross( pNode2->GetXCurr() +
2046  pNode2->GetRCurr()*f2
2047  - pNode1->GetXCurr() - f1)
2048  )
2049  );
2050 
2051  return R1h.GetVec(i-18)*v;
2052  }
2053 
2054  case 22:
2055  case 23:
2056  case 24:
2057  {
2058  Vec3 W(pNode1->GetRCurr().MulTV(pNode2->GetWCurr() - pNode1->GetWCurr()))
2059  ;
2060 
2061  return R1hr.GetVec(i-21)*W;
2062  }
2063 
2064  default:
2065  ASSERT(0);
2066  }
2067 
2068  return 0.;
2069 }
TotalJoint::R1hr
Mat3x3 R1hr
Definition: totalj.h:52
TotalJoint::R1h
Mat3x3 R1h
Definition: totalj.h:51
Vec3::Cross
Vec3 Cross(const Vec3 &v) const
Definition: matvec3.h:218
Vec3
Definition: matvec3.h:98
StructNode::GetRCurr
virtual const Mat3x3 & GetRCurr(void) const
Definition: strnode.h:1012
TotalJoint::f1
Vec3 f1
Definition: totalj.h:50
TotalJoint::ThetaDrv
TplDriveOwner< Vec3 > ThetaDrv
Definition: totalj.h:66
TotalJoint::pNode2
const StructNode * pNode2
Definition: totalj.h:49
Mat3x3::GetVec
Vec3 GetVec(unsigned short int i) const
Definition: matvec3.h:893
RotManip::VecRot
Vec3 VecRot(const Mat3x3 &Phi)
Definition: Rot.cc:136
Mat3x3::MulTV
Vec3 MulTV(const Vec3 &v) const
Definition: matvec3.cc:482
TotalJoint::bRotActive
bool bRotActive[3]
Definition: totalj.h:57
TplDriveOwner::Get
T Get(const doublereal &dVar) const
Definition: tpldrive.h:109
TotalJoint::R2hr
Mat3x3 R2hr
Definition: totalj.h:55
StructNode::GetWCurr
virtual const Vec3 & GetWCurr(void) const
Definition: strnode.h:1030
TotalJoint::pNode1
const StructNode * pNode1
Definition: totalj.h:48
TotalJoint::ThetaDeltaTrue
Vec3 ThetaDeltaTrue
Definition: totalj.h:101
ASSERT
#define ASSERT(expression)
Definition: colamd.c:977
StructDispNode::GetXCurr
virtual const Vec3 & GetXCurr(void) const
Definition: strnode.h:310
TotalJoint::M
Vec3 M
Definition: totalj.h:95
Unwrap
Vec3 Unwrap(const Vec3 &vPrev, const Vec3 &v)
Definition: matvec3.cc:1089
TotalJoint::XDrv
TplDriveOwner< Vec3 > XDrv
Definition: totalj.h:62
StructDispNode::GetVCurr
virtual const Vec3 & GetVCurr(void) const
Definition: strnode.h:322
TotalJoint::F
Vec3 F
Definition: totalj.h:96
TotalJoint::f2
Vec3 f2
Definition: totalj.h:53
TotalJoint::bPosActive
bool bPosActive[3]
Definition: totalj.h:56

Here is the call graph for this function:

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

Reimplemented from Elem.

Definition at line 247 of file totalj.h.

References pNode1, and pNode2.

247  {
248  connectedNodes.resize(2);
249  connectedNodes[0] = pNode1;
250  connectedNodes[1] = pNode2;
251  };
TotalJoint::pNode2
const StructNode * pNode2
Definition: totalj.h:49
TotalJoint::pNode1
const StructNode * pNode1
Definition: totalj.h:48
DofOrder::Order TotalJoint::GetDofType ( unsigned int  i) const
inlinevirtual

Reimplemented from Elem.

Definition at line 154 of file totalj.h.

References DofOrder::ALGEBRAIC, ASSERT, and nConstraints.

154  {
155  ASSERT(i >= 0 && i < nConstraints);
156  return DofOrder::ALGEBRAIC;
157  };
ASSERT
#define ASSERT(expression)
Definition: colamd.c:977
TotalJoint::nConstraints
unsigned int nConstraints
Definition: totalj.h:70
DofOrder::Order TotalJoint::GetEqType ( unsigned int  i) const
virtual

Reimplemented from SimulationEntity.

Definition at line 1469 of file totalj.cc.

References DofOrder::ALGEBRAIC, ASSERTMSGBREAK, and iGetNumDof().

1470 {
1471  ASSERTMSGBREAK(i < iGetNumDof(),
1472  "INDEX ERROR in TotalJoint::GetEqType");
1473 
1474  return DofOrder::ALGEBRAIC;
1475 }
ASSERTMSGBREAK
#define ASSERTMSGBREAK(expr, msg)
Definition: myassert.h:222
TotalJoint::iGetNumDof
virtual unsigned int iGetNumDof(void) const
Definition: totalj.h:129

Here is the call graph for this function:

virtual Joint::Type TotalJoint::GetJointType ( void  ) const
inlinevirtual

Implements Joint.

Definition at line 124 of file totalj.h.

References Joint::TOTALJOINT.

124  {
125  return Joint::TOTALJOINT;
126  };
virtual unsigned int TotalJoint::iGetInitialNumDof ( void  ) const
inlinevirtual

Implements SubjectToInitialAssembly.

Definition at line 220 of file totalj.h.

References nConstraints.

Referenced by DescribeDof(), DescribeEq(), InitialAssJac(), and InitialAssRes().

220  {
221  return 2*nConstraints;
222  };
TotalJoint::nConstraints
unsigned int nConstraints
Definition: totalj.h:70
virtual unsigned int TotalJoint::iGetNumDof ( void  ) const
inlinevirtual

Reimplemented from Elem.

Definition at line 129 of file totalj.h.

References nConstraints.

Referenced by AssJac(), AssRes(), DescribeDof(), DescribeEq(), and GetEqType().

129  {
130  return nConstraints;
131  };
TotalJoint::nConstraints
unsigned int nConstraints
Definition: totalj.h:70
unsigned int TotalJoint::iGetNumPrivData ( void  ) const
virtual

Reimplemented from SimulationEntity.

Definition at line 1918 of file totalj.cc.

1919 {
1920  return 24;
1921 }
unsigned int TotalJoint::iGetPrivDataIdx ( const char *  s) const
virtual

Reimplemented from SimulationEntity.

Definition at line 1924 of file totalj.cc.

References ASSERT.

1925 {
1926  ASSERT(s != NULL);
1927 
1928  if (strlen(s) != 2) {
1929  return 0;
1930  }
1931 
1932  unsigned int off = 0;
1933 
1934  switch (s[0]) {
1935  case 'p':
1936  /* relative position */
1937  break;
1938 
1939  case 'r':
1940  /* relative orientation */
1941  off += 3;
1942  break;
1943 
1944  case 'F':
1945  /* force */
1946  off += 6;
1947  break;
1948 
1949  case 'M':
1950  /* moment */
1951  off += 9;
1952  break;
1953 
1954  case 'd':
1955  /* imposed relative position */
1956  off += 12;
1957  break;
1958 
1959  case 't':
1960  /* imposed relative orientation */
1961  off += 15;
1962  break;
1963 
1964  case 'v':
1965  /* relative linear velocity */
1966  off += 18;
1967  break;
1968 
1969  case 'w':
1970  /* relative angular velocity */
1971  off += 21;
1972  break;
1973 
1974  default:
1975  return 0;
1976  }
1977 
1978  switch (s[1]) {
1979  case 'x':
1980  return off + 1;
1981 
1982  case 'y':
1983  return off + 2;
1984 
1985  case 'z':
1986  return off + 3;
1987  }
1988 
1989  return 0;
1990 }
ASSERT
#define ASSERT(expression)
Definition: colamd.c:977
VariableSubMatrixHandler & TotalJoint::InitialAssJac ( VariableSubMatrixHandler &  WorkMat,
const VectorHandler &  XCurr 
)
virtual

Implements SubjectToInitialAssembly.

Definition at line 1559 of file totalj.cc.

References FullSubMatrixHandler::Add(), FullSubMatrixHandler::AddT(), Vec3::Cross(), F, f2, StructNode::GetRCurr(), StructNode::GetRRef(), StructDispNode::GetVCurr(), StructNode::GetWCurr(), StructDispNode::GetXCurr(), DofOwnerOwner::iGetFirstIndex(), StructDispNode::iGetFirstPositionIndex(), iGetInitialNumDof(), InitialWorkSpaceDim(), iPosIncid, iRotIncid, M, MatCross, MatCrossCross, nConstraints, nPosConstraints, nRotConstraints, pNode1, pNode2, FullSubMatrixHandler::PutColIndex(), FullSubMatrixHandler::PutRowIndex(), R1h, R1hr, FullSubMatrixHandler::ResizeReset(), VariableSubMatrixHandler::SetFull(), VariableSubMatrixHandler::SetNullMatrix(), FullSubMatrixHandler::Sub(), FullSubMatrixHandler::SubT(), and Zero3.

1561 {
1562  if (iGetInitialNumDof() == 0) {
1563  WorkMat.SetNullMatrix();
1564  return WorkMat;
1565  }
1566 
1567  /* Per ora usa la matrice piena; eventualmente si puo'
1568  * passare a quella sparsa quando si ottimizza */
1569  FullSubMatrixHandler& WM = WorkMat.SetFull();
1570 
1571  /* Dimensiona e resetta la matrice di lavoro */
1572  integer iNumRows = 0;
1573  integer iNumCols = 0;
1574  InitialWorkSpaceDim(&iNumRows, &iNumCols);
1575  WM.ResizeReset(iNumRows, iNumCols);
1576 
1577  /* Equazioni: vedi joints.dvi */
1578 
1579  /* equazioni ed incognite
1580  * F1 Delta_x1 1
1581  * M1 Delta_g1 3 + 1
1582  * FP1 Delta_xP1 6 + 1
1583  * MP1 Delta_w1 9 + 1
1584  * F2 Delta_x2 12 + 1
1585  * M2 Delta_g2 15 + 1
1586  * FP2 Delta_xP2 18 + 1
1587  * MP2 Delta_w2 21 + 1
1588  * vincolo spostamento Delta_F 24 + 1
1589  * vincolo rotazione Delta_M 24 + nPosConstraints
1590  * derivata vincolo spostamento Delta_FP 24 + nConstraints
1591  * derivata vincolo rotazione Delta_MP 24 + nConstraints + nPosConstraints
1592  */
1593 
1594 
1595  /* Indici */
1596  integer iNode1FirstPosIndex = pNode1->iGetFirstPositionIndex();
1597  integer iNode1FirstVelIndex = iNode1FirstPosIndex + 6;
1598  integer iNode2FirstPosIndex = pNode2->iGetFirstPositionIndex();
1599  integer iNode2FirstVelIndex = iNode2FirstPosIndex + 6;
1600  integer iFirstReactionIndex = iGetFirstIndex();
1601  integer iReactionPrimeIndex = iFirstReactionIndex + nConstraints;
1602 
1603 
1604  /* Setta gli indici dei nodi */
1605  for (int iCnt = 1; iCnt <= 6; iCnt++) {
1606  WM.PutRowIndex(iCnt, iNode1FirstPosIndex + iCnt);
1607  WM.PutColIndex(iCnt, iNode1FirstPosIndex + iCnt);
1608  WM.PutRowIndex(6 + iCnt, iNode1FirstVelIndex + iCnt);
1609  WM.PutColIndex(6 + iCnt, iNode1FirstVelIndex + iCnt);
1610  WM.PutRowIndex(12 + iCnt, iNode2FirstPosIndex + iCnt);
1611  WM.PutColIndex(12 + iCnt, iNode2FirstPosIndex + iCnt);
1612  WM.PutRowIndex(18 + iCnt, iNode2FirstVelIndex + iCnt);
1613  WM.PutColIndex(18 + iCnt, iNode2FirstVelIndex + iCnt);
1614  }
1615 
1616  /* Setta gli indici delle reazioni */
1617  for (unsigned int iCnt = 1; iCnt <= nConstraints; iCnt++) {
1618  WM.PutRowIndex(24 + iCnt, iFirstReactionIndex + iCnt);
1619  WM.PutColIndex(24 + iCnt, iFirstReactionIndex + iCnt);
1620  WM.PutRowIndex(24 + nConstraints + iCnt, iReactionPrimeIndex + iCnt);
1621  WM.PutColIndex(24 + nConstraints + iCnt, iReactionPrimeIndex + iCnt);
1622  }
1623 
1624  /* Recupera i dati che servono */
1625  Mat3x3 R1(pNode1->GetRRef()*R1h);
1626  Mat3x3 R1r(pNode1->GetRRef()*R1hr);
1627 
1628  Vec3 b2(pNode2->GetRCurr()*f2);
1629  Vec3 b1(pNode2->GetXCurr() + b2 - pNode1->GetXCurr());
1630 
1631  const Vec3& Omega2(pNode2->GetWCurr());
1632 
1633  Vec3 b1Prime(pNode2->GetVCurr() + Omega2.Cross(b2) - pNode1->GetVCurr());
1634 
1635  /* F ed M sono gia' state aggiornate da InitialAssRes;
1636  * Recupero FPrime e MPrime*/
1637  Vec3 MPrime(::Zero3);
1638  Vec3 FPrime(::Zero3);
1639 
1640  for (unsigned iCnt = 0; iCnt < nPosConstraints; iCnt++) {
1641  FPrime(iPosIncid[iCnt]) = XCurr(iReactionPrimeIndex + 1 + iCnt);
1642  }
1643 
1644  for (unsigned iCnt = 0; iCnt < nRotConstraints; iCnt++) {
1645  MPrime(iRotIncid[iCnt]) = XCurr(iReactionPrimeIndex + 1 + nPosConstraints + iCnt);
1646  }
1647 
1648  Vec3 FTmp(R1 * F);
1649  Vec3 MTmp(R1r * M);
1650  Vec3 FPrimeTmp(R1 * FPrime);
1651  Vec3 MPrimeTmp(R1r * MPrime);
1652 
1653  Mat3x3 Tmp;
1654 
1655  /* [ F x ] */
1656  Tmp = Mat3x3(MatCross, FTmp);
1657 
1658  /* Force, Node 1, Lines 1->3: */
1659  WM.Add(1, 3 + 1, Tmp); // * Delta_g1
1660 
1661  /* Moment, Node 1, Lines 4->6: */
1662  WM.Sub(3 + 1, 1, Tmp); // * Delta_x1
1663 
1664  WM.Add(3 + 1, 12 + 1, Tmp); // * Delta_x2
1665 
1666  /* Force, Node 2, Lines 13->15: */
1667  WM.Sub(12 + 1, 3 + 1, Tmp); // * Delta_g1
1668 
1669  /* [ FP x ] */
1670  Tmp = Mat3x3(MatCross, FPrimeTmp);
1671 
1672  /* d/dt(Force), Node 1, Lines 7->9: */
1673  WM.Add(6 + 1, 9 + 1, Tmp); // * Delta_W1
1674 
1675  /* d/dt(Moment), Node 1, Lines 10->12: */
1676  WM.Sub(9 + 1, 6 + 1, Tmp); // * Delta_x1P
1677 
1678  WM.Add(9 + 1, 18 + 1, Tmp); // * Delta_x2P
1679 
1680  /* d/dt(Force), Node 2, Lines 19->21: */
1681  WM.Sub(18 + 1 , 9 + 1, Tmp); // * Delta_W1
1682 
1683  /* [ F x ] [ b2 x ] */
1684  Tmp = Mat3x3(MatCrossCross, FTmp, b2);
1685 
1686  /* Moment, Node1, Lines 4->6: */
1687  WM.Sub(3 + 1, 15 + 1, Tmp); // * Delta_g2
1688 
1689  /* Moment, Node2, Lines 16->18: */
1690  WM.Add(15 + 1, 15 + 1, Tmp); // * Delta_g2
1691 
1692  /* [ FP x ] [ b2 x ] */
1693  Tmp = Mat3x3(MatCrossCross, FPrimeTmp, b2);
1694 
1695  /* d/dt(Moment), Node1, Lines 10->12: */
1696  WM.Sub(9 + 1, 21 + 1, Tmp); // * Delta_W2
1697 
1698  /* d/dt(Moment), Node2, Lines 22->24: */
1699  WM.Add(21 + 1, 21 + 1, Tmp); // * Delta_W2
1700 
1701  /* [ b1 x ] [ F x ] + [ M x ] */
1702 
1703  /* Moment, Node1, Lines 4->6: */
1704  WM.Add(3 + 1, 3 + 1, Mat3x3(MatCrossCross, b1, FTmp) + Mat3x3(MatCross, MTmp)); // *Delta_g1
1705 
1706  /* d/dt(Moment), Node1, Lines 10->12: */
1707  WM.Add(9 + 1, 9 + 1, Mat3x3(MatCrossCross, b1, FPrimeTmp) + Mat3x3(MatCross, MPrimeTmp));// *Delta_W1
1708 
1709  /* [ b2 x ] [ F x ] + [ M x ] */
1710 
1711  /* Moment, Node2, Lines 16->18: */
1712  WM.Sub(15 + 1, 3 + 1, Mat3x3(MatCrossCross, b2, FTmp) + Mat3x3(MatCross, MTmp)); // * Delta_g1
1713 
1714  /* d/dt(Moment), Node2, Lines 22->24: */
1715  WM.Sub(21 + 1, 9 + 1, Mat3x3(MatCrossCross, b2, FTmp) + Mat3x3(MatCross, MTmp)); // * Delta_W1
1716 
1717  /* Constraints: Add only active rows/columns*/
1718 
1719  /* Positions contribution:*/
1720 
1721  Mat3x3 b1Cross_R1(b1.Cross(R1)); // = [ b1 x ] * R1
1722  Mat3x3 b2Cross_R1(b2.Cross(R1)); // = [ b2 x ] * R1
1723 
1724  for (unsigned iCnt = 0 ; iCnt < nPosConstraints; iCnt++) {
1725  Vec3 vR1(R1.GetVec(iPosIncid[iCnt]));
1726  Vec3 vb1Cross_R1(b1Cross_R1.GetVec(iPosIncid[iCnt]));
1727  Vec3 vb2Cross_R1(b2Cross_R1.GetVec(iPosIncid[iCnt]));
1728 
1729  /* Equilibrium, node 1 */
1730  WM.Sub(1, 24 + 1 + iCnt, vR1); // * Delta_F
1731  WM.Sub(3 + 1, 24 + 1 + iCnt, vb1Cross_R1); // * Delta_F
1732 
1733  /* Constraint, node 1 */
1734  WM.SubT(24 + 1 + iCnt, 1, vR1); // * Delta_x1
1735  WM.SubT(24 + 1 + iCnt, 3 + 1, vb1Cross_R1); // * Delta_g1
1736 
1737  /* d/dt(Equilibrium), node 1 */
1738  WM.Sub(6 + 1, 24 + 1 + nConstraints + iCnt, vR1); // * Delta_FP
1739  WM.Sub(9 + 1, 24 + 1 + nConstraints + iCnt, vb1Cross_R1); // * Delta_FP
1740 
1741  /* d/dt(Constraint), node 1 */
1742  WM.SubT(24 + 1 + nConstraints + iCnt, 6 + 1, vR1); // * Delta_xP1
1743  WM.SubT(24 + 1 + nConstraints + iCnt, 9 + 1, vb1Cross_R1); // * Delta_W1
1744 
1745  /* Equilibrium, node 2 */
1746  WM.Add(12 + 1, 24 + 1 + iCnt, vR1); // * Delta_F
1747  WM.Add(15 + 1, 24 + 1 + iCnt, vb2Cross_R1); // * Delta_F
1748 
1749  /* Constraint, node 2 */
1750  WM.AddT(24 + 1 + iCnt, 12 + 1, vR1); // * Delta_x2
1751  WM.AddT(24 + 1 + iCnt, 15 + 1, vb2Cross_R1); // * Delta_g2
1752 
1753  /* d/dt(Equilibrium), node 2 */
1754  WM.Add(18 + 1, 24 + 1 + nConstraints + iCnt, vR1); // * Delta_FP
1755  WM.Add(21 + 1, 24 + 1 + nConstraints + iCnt, vb2Cross_R1); // * Delta_FP
1756 
1757  /* d/dt(Constraint), node 2 */
1758  WM.AddT(24 + 1 + nConstraints + iCnt, 18 + 1, vR1); // * Delta_xP2
1759  WM.AddT(24 + 1 + nConstraints + iCnt, 21 + 1, vb2Cross_R1); // * Delta_W2
1760  }
1761 
1762  for (unsigned iCnt = 0 ; iCnt < nRotConstraints; iCnt++) {
1763  Vec3 vR1(R1r.GetVec(iRotIncid[iCnt]));
1764 
1765  /* Equilibrium, node 1 */
1766  WM.Sub(3 + 1, 24 + 1 + nPosConstraints + iCnt, vR1); // * Delta_M
1767 
1768  /* Constraint, node 1 */
1769  WM.SubT(24 + 1 + nPosConstraints + iCnt, 3 + 1, vR1); // * Delta_g1
1770 
1771  /* d/dt(Equilibrium), node 1 */
1772  WM.Sub(9 + 1, 24 + 1 + nConstraints + nPosConstraints + iCnt, vR1); // * Delta_MP
1773 
1774  /* d/dt(Constraint), node 1 */
1775  WM.SubT(24 + 1 + nConstraints + nPosConstraints + iCnt, 9 + 1, vR1); // * Delta_W1
1776 
1777  /* Equilibrium, node 2 */
1778  WM.Add(15 + 1, 24 + 1 + nPosConstraints + iCnt, vR1); // * Delta_M
1779 
1780  /* Constraint, node 2 */
1781  WM.AddT(24 + 1 + nPosConstraints + iCnt, 15 + 1, vR1); // * Delta_g2
1782 
1783  /* d/dt(Equilibrium), node 2 */
1784  WM.Add(21 + 1, 24 + 1 + nConstraints + nPosConstraints + iCnt, vR1); // * Delta_MP
1785 
1786  /* d/dt(Constraint), node 2 */
1787  WM.AddT(24 + 1 + nConstraints + nPosConstraints + iCnt, 21 + 1, vR1); // * Delta_W2
1788  }
1789 
1790  return WorkMat;
1791 }
TotalJoint::R1hr
Mat3x3 R1hr
Definition: totalj.h:52
TotalJoint::iGetInitialNumDof
virtual unsigned int iGetInitialNumDof(void) const
Definition: totalj.h:220
TotalJoint::R1h
Mat3x3 R1h
Definition: totalj.h:51
FullSubMatrixHandler::PutColIndex
void PutColIndex(integer iSubCol, integer iCol)
Definition: submat.h:325
Zero3
const Vec3 Zero3(0., 0., 0.)
Vec3::Cross
Vec3 Cross(const Vec3 &v) const
Definition: matvec3.h:218
StructNode::GetRRef
virtual const Mat3x3 & GetRRef(void) const
Definition: strnode.h:1006
Vec3
Definition: matvec3.h:98
MatCross
const MatCross_Manip MatCross
Definition: matvec3.cc:639
VariableSubMatrixHandler::SetFull
FullSubMatrixHandler & SetFull(void)
Definition: submat.h:1168
StructNode::GetRCurr
virtual const Mat3x3 & GetRCurr(void) const
Definition: strnode.h:1012
FullSubMatrixHandler::Add
void Add(integer iRow, integer iCol, const Vec3 &v)
Definition: submat.cc:209
TotalJoint::nPosConstraints
unsigned int nPosConstraints
Definition: totalj.h:71
TotalJoint::iPosIncid
unsigned int iPosIncid[3]
Definition: totalj.h:76
TotalJoint::pNode2
const StructNode * pNode2
Definition: totalj.h:49
FullSubMatrixHandler::AddT
void AddT(integer iRow, integer iCol, const Vec3 &v)
Definition: submat.cc:227
VariableSubMatrixHandler::SetNullMatrix
void SetNullMatrix(void)
Definition: submat.h:1159
StructDispNode::iGetFirstPositionIndex
virtual integer iGetFirstPositionIndex(void) const
Definition: strnode.h:452
StructNode::GetWCurr
virtual const Vec3 & GetWCurr(void) const
Definition: strnode.h:1030
TotalJoint::pNode1
const StructNode * pNode1
Definition: totalj.h:48
StructDispNode::GetXCurr
virtual const Vec3 & GetXCurr(void) const
Definition: strnode.h:310
FullSubMatrixHandler::ResizeReset
virtual void ResizeReset(integer, integer)
Definition: submat.cc:182
TotalJoint::M
Vec3 M
Definition: totalj.h:95
TotalJoint::nRotConstraints
unsigned int nRotConstraints
Definition: totalj.h:72
MatCrossCross
const MatCrossCross_Manip MatCrossCross
Definition: matvec3.cc:640
FullSubMatrixHandler::SubT
void SubT(integer iRow, integer iCol, const Vec3 &v)
Definition: submat.cc:233
FullSubMatrixHandler::PutRowIndex
void PutRowIndex(integer iSubRow, integer iRow)
Definition: submat.h:311
StructDispNode::GetVCurr
virtual const Vec3 & GetVCurr(void) const
Definition: strnode.h:322
FullSubMatrixHandler::Sub
void Sub(integer iRow, integer iCol, const Vec3 &v)
Definition: submat.cc:215
TotalJoint::nConstraints
unsigned int nConstraints
Definition: totalj.h:70
TotalJoint::InitialWorkSpaceDim
virtual void InitialWorkSpaceDim(integer *piNumRows, integer *piNumCols) const
Definition: totalj.h:224
DofOwnerOwner::iGetFirstIndex
virtual integer iGetFirstIndex(void) const
Definition: dofown.h:127
TotalJoint::F
Vec3 F
Definition: totalj.h:96
TotalJoint::iRotIncid
unsigned int iRotIncid[3]
Definition: totalj.h:77
integer
long int integer
Definition: colamd.c:51
TotalJoint::f2
Vec3 f2
Definition: totalj.h:53

Here is the call graph for this function:

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

Implements SubjectToInitialAssembly.

Definition at line 1796 of file totalj.cc.

References VectorHandler::Add(), TplDriveOwner< T >::bIsDifferentiable(), Vec3::Cross(), DEBUGCOUT, F, f2, TplDriveOwner< T >::Get(), TplDriveOwner< T >::GetP(), StructNode::GetRCurr(), StructNode::GetRRef(), StructDispNode::GetVCurr(), StructNode::GetWCurr(), StructDispNode::GetXCurr(), DofOwnerOwner::iGetFirstIndex(), StructDispNode::iGetFirstPositionIndex(), iGetInitialNumDof(), InitialWorkSpaceDim(), iPosIncid, iRotIncid, M, Mat3x3::MulTM(), nConstraints, nPosConstraints, nRotConstraints, pNode1, pNode2, VectorHandler::PutCoef(), SubVectorHandler::PutRowIndex(), R1h, R1hr, R2hr, VectorHandler::ResizeReset(), RotManip::Rot(), VectorHandler::Sub(), ThetaDelta, ThetaDrv, tilde_f1, RotManip::VecRot(), XDrv, and Zero3.

1798 {
1799  if (iGetInitialNumDof() == 0) {
1800  WorkVec.ResizeReset(0);
1801  return WorkVec;
1802  }
1803 
1804  DEBUGCOUT("Entering TotalJoint::InitialAssRes()" << std::endl);
1805 
1806  /* Dimensiona e resetta la matrice di lavoro */
1807  integer iNumRows = 0;
1808  integer iNumCols = 0;
1809  InitialWorkSpaceDim(&iNumRows, &iNumCols);
1810  WorkVec.ResizeReset(iNumRows);
1811 
1812  /* Indici */
1813  integer iNode1FirstPosIndex = pNode1->iGetFirstPositionIndex();
1814  integer iNode1FirstVelIndex = iNode1FirstPosIndex + 6;
1815  integer iNode2FirstPosIndex = pNode2->iGetFirstPositionIndex();
1816  integer iNode2FirstVelIndex = iNode2FirstPosIndex + 6;
1817  integer iFirstReactionIndex = iGetFirstIndex();
1818  integer iReactionPrimeIndex = iFirstReactionIndex + nConstraints;
1819 
1820  /* Setta gli indici */
1821  for (int iCnt = 1; iCnt <= 6; iCnt++) {
1822  WorkVec.PutRowIndex(iCnt, iNode1FirstPosIndex+iCnt);
1823  WorkVec.PutRowIndex(6 + iCnt, iNode1FirstVelIndex + iCnt);
1824  WorkVec.PutRowIndex(12 + iCnt, iNode2FirstPosIndex + iCnt);
1825  WorkVec.PutRowIndex(18 + iCnt, iNode2FirstVelIndex + iCnt);
1826  }
1827 
1828  for (unsigned int iCnt = 1; iCnt <= nConstraints; iCnt++) {
1829  WorkVec.PutRowIndex(24 + iCnt, iFirstReactionIndex + iCnt);
1830  WorkVec.PutRowIndex(24 + nConstraints + iCnt, iReactionPrimeIndex + iCnt);
1831  }
1832 
1833  /* Recupera i dati */
1834  /* Recupera i dati che servono */
1835  Mat3x3 R1(pNode1->GetRRef()*R1h);
1836  Mat3x3 R1r(pNode1->GetRRef()*R1hr);
1837  Mat3x3 R2r(pNode2->GetRCurr()*R2hr);
1838 
1839  Vec3 b2(pNode2->GetRCurr()*f2);
1840  Vec3 b1(pNode2->GetXCurr() + b2 - pNode1->GetXCurr());
1841 
1842  const Vec3& Omega1(pNode1->GetWCurr());
1843  const Vec3& Omega2(pNode2->GetWCurr());
1844 
1845  Vec3 b1Prime(pNode2->GetVCurr() + Omega2.Cross(b2) - pNode1->GetVCurr());
1846 
1847  Vec3 FPrime(::Zero3);
1848  Vec3 MPrime(::Zero3);
1849 
1850  /* Aggiorna F ed M, che restano anche per InitialAssJac */
1851  for (unsigned iCnt = 0; iCnt < nPosConstraints; iCnt++) {
1852  F(iPosIncid[iCnt]) = XCurr(iFirstReactionIndex + 1 + iCnt);
1853  FPrime(iPosIncid[iCnt]) = XCurr(iReactionPrimeIndex + 1 + iCnt);
1854  }
1855 
1856  for (unsigned iCnt = 0; iCnt < nRotConstraints; iCnt++) {
1857  M(iRotIncid[iCnt]) = XCurr(iFirstReactionIndex + 1 + nPosConstraints + iCnt);
1858  MPrime(iRotIncid[iCnt]) = XCurr(iReactionPrimeIndex + 1 + nPosConstraints + iCnt);
1859  }
1860 
1861  Vec3 FTmp(R1 * F);
1862  Vec3 MTmp(R1r * M);
1863  Vec3 FPrimeTmp(R1 * FPrime);
1864  Vec3 MPrimeTmp(R1r * MPrime);
1865 
1866  /* Equilibrium, node 1 */
1867  WorkVec.Add(1, FTmp);
1868  WorkVec.Add(3 + 1, b1.Cross(FTmp) + MTmp);
1869 
1870  /* d/dt(Equilibrium), node 1 */
1871  WorkVec.Add(6 + 1, FPrimeTmp);
1872  WorkVec.Add(9 + 1, b1.Cross(FPrimeTmp) + MPrimeTmp);
1873 
1874  /* Equilibrium, node 2 */
1875  WorkVec.Sub(12 + 1, FTmp);
1876  WorkVec.Sub(15 + 1, b2.Cross(FTmp) + MTmp);
1877 
1878  /* d/dt( Equilibrium ) , node 2 */
1879  WorkVec.Sub(18 + 1, FPrimeTmp);
1880  WorkVec.Sub(21 + 1, b2.Cross(FPrimeTmp) + MPrimeTmp);
1881 
1882  /* Constraint Equations */
1883 
1884  Vec3 XDelta = R1.MulTV(b1) - tilde_f1 - XDrv.Get();
1885  Vec3 XDeltaPrime = R1.MulTV(b1Prime + b1.Cross(Omega1));
1886 
1887  if(XDrv.bIsDifferentiable()) {
1888  XDeltaPrime -= XDrv.GetP();
1889  }
1890 
1891  Mat3x3 R0 = RotManip::Rot(ThetaDrv.Get());
1892  Mat3x3 RDelta = R1r.MulTM(R2r.MulMT(R0));
1893  ThetaDelta = RotManip::VecRot(RDelta);
1894  Vec3 ThetaDeltaPrime = R1r.MulTV(Omega2 - Omega1);
1895 
1896  if(ThetaDrv.bIsDifferentiable()) {
1897  ThetaDeltaPrime -= RDelta*ThetaDrv.GetP();
1898  }
1899 
1900 
1901  /* Position constraint: */
1902  for (unsigned iCnt = 0; iCnt < nPosConstraints; iCnt++) {
1903  WorkVec.PutCoef(24 + 1 + iCnt, -XDelta(iPosIncid[iCnt]));
1904  WorkVec.PutCoef(24 + 1 + nConstraints + iCnt, -XDeltaPrime(iPosIncid[iCnt]));
1905  }
1906 
1907  /* Rotation constraints: */
1908  for (unsigned iCnt = 0; iCnt < nRotConstraints; iCnt++) {
1909  WorkVec.PutCoef(24 + 1 + nPosConstraints + iCnt, -ThetaDelta(iRotIncid[iCnt]));
1910  WorkVec.PutCoef(24 + 1 + nPosConstraints + nConstraints + iCnt, -ThetaDeltaPrime(iRotIncid[iCnt]));
1911  }
1912 
1913  return WorkVec;
1914 }
TotalJoint::R1hr
Mat3x3 R1hr
Definition: totalj.h:52
TotalJoint::iGetInitialNumDof
virtual unsigned int iGetInitialNumDof(void) const
Definition: totalj.h:220
TotalJoint::R1h
Mat3x3 R1h
Definition: totalj.h:51
Zero3
const Vec3 Zero3(0., 0., 0.)
Vec3::Cross
Vec3 Cross(const Vec3 &v) const
Definition: matvec3.h:218
StructNode::GetRRef
virtual const Mat3x3 & GetRRef(void) const
Definition: strnode.h:1006
TplDriveOwner::bIsDifferentiable
virtual bool bIsDifferentiable(void) const
Definition: tpldrive.h:118
Vec3
Definition: matvec3.h:98
VectorHandler::ResizeReset
virtual void ResizeReset(integer)
Definition: vh.cc:55
StructNode::GetRCurr
virtual const Mat3x3 & GetRCurr(void) const
Definition: strnode.h:1012
TotalJoint::nPosConstraints
unsigned int nPosConstraints
Definition: totalj.h:71
VectorHandler::Sub
virtual void Sub(integer iRow, const Vec3 &v)
Definition: vh.cc:78
TotalJoint::ThetaDrv
TplDriveOwner< Vec3 > ThetaDrv
Definition: totalj.h:66
TotalJoint::iPosIncid
unsigned int iPosIncid[3]
Definition: totalj.h:76
TotalJoint::pNode2
const StructNode * pNode2
Definition: totalj.h:49
TotalJoint::tilde_f1
Vec3 tilde_f1
Definition: totalj.h:86
RotManip::VecRot
Vec3 VecRot(const Mat3x3 &Phi)
Definition: Rot.cc:136
TplDriveOwner::GetP
virtual T GetP(void) const
Definition: tpldrive.h:121
TotalJoint::ThetaDelta
Vec3 ThetaDelta
Definition: totalj.h:97
SubVectorHandler::PutRowIndex
virtual void PutRowIndex(integer iSubRow, integer iRow)=0
DEBUGCOUT
#define DEBUGCOUT(msg)
Definition: myassert.h:232
TplDriveOwner::Get
T Get(const doublereal &dVar) const
Definition: tpldrive.h:109
TotalJoint::R2hr
Mat3x3 R2hr
Definition: totalj.h:55
RotManip::Rot
Mat3x3 Rot(const Vec3 &phi)
Definition: Rot.cc:62
StructDispNode::iGetFirstPositionIndex
virtual integer iGetFirstPositionIndex(void) const
Definition: strnode.h:452
StructNode::GetWCurr
virtual const Vec3 & GetWCurr(void) const
Definition: strnode.h:1030
TotalJoint::pNode1
const StructNode * pNode1
Definition: totalj.h:48
Mat3x3::MulTM
Mat3x3 MulTM(const Mat3x3 &m) const
Definition: matvec3.cc:500
VectorHandler::PutCoef
virtual void PutCoef(integer iRow, const doublereal &dCoef)=0
StructDispNode::GetXCurr
virtual const Vec3 & GetXCurr(void) const
Definition: strnode.h:310
VectorHandler::Add
virtual void Add(integer iRow, const Vec3 &v)
Definition: vh.cc:63
TotalJoint::M
Vec3 M
Definition: totalj.h:95
TotalJoint::nRotConstraints
unsigned int nRotConstraints
Definition: totalj.h:72
TotalJoint::XDrv
TplDriveOwner< Vec3 > XDrv
Definition: totalj.h:62
StructDispNode::GetVCurr
virtual const Vec3 & GetVCurr(void) const
Definition: strnode.h:322
TotalJoint::nConstraints
unsigned int nConstraints
Definition: totalj.h:70
TotalJoint::InitialWorkSpaceDim
virtual void InitialWorkSpaceDim(integer *piNumRows, integer *piNumCols) const
Definition: totalj.h:224
DofOwnerOwner::iGetFirstIndex
virtual integer iGetFirstIndex(void) const
Definition: dofown.h:127
TotalJoint::F
Vec3 F
Definition: totalj.h:96
TotalJoint::iRotIncid
unsigned int iRotIncid[3]
Definition: totalj.h:77
integer
long int integer
Definition: colamd.c:51
TotalJoint::f2
Vec3 f2
Definition: totalj.h:53

Here is the call graph for this function:

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

Implements SubjectToInitialAssembly.

Definition at line 224 of file totalj.h.

References nConstraints.

Referenced by InitialAssJac(), and InitialAssRes().

224  {
225  *piNumCols = *piNumRows = 24 + 2*nConstraints;
226  };
TotalJoint::nConstraints
unsigned int nConstraints
Definition: totalj.h:70
void TotalJoint::Output ( OutputHandler &  OH) const
virtual

Reimplemented from ToBeOutput.

Definition at line 1504 of file totalj.cc.

References ToBeOutput::bToBeOutput(), F, f2, OutputHandler::GetCurrentStep(), WithLabel::GetLabel(), StructNode::GetRCurr(), StructDispNode::GetVCurr(), StructNode::GetWCurr(), StructDispNode::GetXCurr(), OutputHandler::JOINTS, OutputHandler::Joints(), M, Mat3x3::MulTM(), Mat3x3::MulTV(), Joint::Output(), Vec3::pGetVec(), pNode1, pNode2, R1h, R1hr, R2hr, tilde_f1, OutputHandler::UseNetCDF(), OutputHandler::UseText(), V1, V2, and RotManip::VecRot().

1505 {
1506  if (bToBeOutput()) {
1507  const Vec3& X1(pNode1->GetXCurr());
1508  const Vec3& X2(pNode2->GetXCurr());
1509  const Mat3x3& R1(pNode1->GetRCurr());
1510  const Mat3x3& R2(pNode2->GetRCurr());
1511  const Vec3& V1(pNode1->GetVCurr());
1512  const Vec3& V2(pNode2->GetVCurr());
1513  const Vec3& Omega1(pNode1->GetWCurr());
1514  const Vec3& Omega2(pNode2->GetWCurr());
1515 
1516  Mat3x3 R1Tmp(R1*R1h);
1517  Mat3x3 R1rTmp(R1*R1hr);
1518  Mat3x3 R2rTmp(R2*R2hr);
1519  Mat3x3 RTmp(R1rTmp.MulTM(R2rTmp));
1520  Vec3 ThetaTmp(RotManip::VecRot(RTmp));
1521  Vec3 b2(R2*f2);
1522  Vec3 b1(X2 + b2 - X1);
1523  Vec3 XTmp(R1Tmp.MulTV(b1) - tilde_f1);
1524  Vec3 VTmp(R1Tmp.MulTV(V2 + Omega2.Cross(b2) - V1 - Omega1.Cross(b1)));
1525  Vec3 OmegaTmp(R1rTmp.MulTV(Omega2 - Omega1));
1526 
1527  Vec3 FTmp(R1Tmp*F);
1528  Vec3 MTmp(R1rTmp*M);
1529 
1530 #ifdef USE_NETCDF
1531  if (OH.UseNetCDF(OutputHandler::JOINTS)) {
1532  Var_F_local->put_rec(F.pGetVec(), OH.GetCurrentStep());
1533  Var_M_local->put_rec(M.pGetVec(), OH.GetCurrentStep());
1534  Var_F_global->put_rec(FTmp.pGetVec(), OH.GetCurrentStep());
1535  Var_M_global->put_rec(MTmp.pGetVec(), OH.GetCurrentStep());
1536 
1537  Var_X->put_rec(XTmp.pGetVec(), OH.GetCurrentStep());
1538  Var_Phi->put_rec(ThetaTmp.pGetVec(), OH.GetCurrentStep());
1539  Var_V->put_rec(VTmp.pGetVec(), OH.GetCurrentStep());
1540  Var_Omega->put_rec(OmegaTmp.pGetVec(), OH.GetCurrentStep());
1541  }
1542 #endif // USE_NETCDF
1543 
1544  if (OH.UseText(OutputHandler::JOINTS)) {
1545  Joint::Output(OH.Joints(), "TotalJoint", GetLabel(),
1546  F, M, FTmp, MTmp)
1547  << " " << XTmp
1548  << " " << ThetaTmp
1549  << " " << VTmp
1550  << " " << OmegaTmp
1551  << std::endl;
1552  // accelerations?
1553  }
1554  }
1555 }
TotalJoint::R1hr
Mat3x3 R1hr
Definition: totalj.h:52
TotalJoint::R1h
Mat3x3 R1h
Definition: totalj.h:51
ToBeOutput::bToBeOutput
virtual bool bToBeOutput(void) const
Definition: output.cc:890
Vec3
Definition: matvec3.h:98
OutputHandler::UseNetCDF
bool UseNetCDF(int out) const
Definition: output.cc:491
StructNode::GetRCurr
virtual const Mat3x3 & GetRCurr(void) const
Definition: strnode.h:1012
TotalJoint::pNode2
const StructNode * pNode2
Definition: totalj.h:49
V2
Definition: matvec3.h:50
TotalJoint::tilde_f1
Vec3 tilde_f1
Definition: totalj.h:86
RotManip::VecRot
Vec3 VecRot(const Mat3x3 &Phi)
Definition: Rot.cc:136
OutputHandler::GetCurrentStep
long GetCurrentStep(void) const
Definition: output.h:116
TotalJoint::R2hr
Mat3x3 R2hr
Definition: totalj.h:55
StructNode::GetWCurr
virtual const Vec3 & GetWCurr(void) const
Definition: strnode.h:1030
TotalJoint::pNode1
const StructNode * pNode1
Definition: totalj.h:48
OutputHandler::Joints
std::ostream & Joints(void) const
Definition: output.h:443
StructDispNode::GetXCurr
virtual const Vec3 & GetXCurr(void) const
Definition: strnode.h:310
TotalJoint::M
Vec3 M
Definition: totalj.h:95
V1
Definition: matvec3.h:49
Vec3::pGetVec
const doublereal * pGetVec(void) const
Definition: matvec3.h:192
StructDispNode::GetVCurr
virtual const Vec3 & GetVCurr(void) const
Definition: strnode.h:322
TotalJoint::F
Vec3 F
Definition: totalj.h:96
Joint::Output
std::ostream & Output(std::ostream &out, const char *sJointName, unsigned int uLabel, const Vec3 &FLocal, const Vec3 &MLocal, const Vec3 &FGlobal, const Vec3 &MGlobal) const
Definition: joint.cc:138
WithLabel::GetLabel
unsigned int GetLabel(void) const
Definition: withlab.cc:62
TotalJoint::f2
Vec3 f2
Definition: totalj.h:53
OutputHandler::UseText
bool UseText(int out) const
Definition: output.cc:446

Here is the call graph for this function:

void TotalJoint::OutputPrepare ( OutputHandler &  OH)
virtual

Reimplemented from ToBeOutput.

Definition at line 1478 of file totalj.cc.

References ToBeOutput::bToBeOutput(), OutputHandler::JOINTS, ORIENTATION_VECTOR, Joint::OutputPrepare_int(), and OutputHandler::UseNetCDF().

1479 {
1480  if (bToBeOutput()) {
1481 #ifdef USE_NETCDF
1482  if (OH.UseNetCDF(OutputHandler::JOINTS)) {
1483  std::string name;
1484  OutputPrepare_int("total", OH, name);
1485 
1486  Var_X = OH.CreateVar<Vec3>(name + "X", "m",
1487  "local relative position (x, y, z)");
1488 
1489  // NOTE: by now, force ORIENTATION_VECTOR
1490  Var_Phi = OH.CreateRotationVar(name, "", ORIENTATION_VECTOR, "local relative");
1491 
1492  Var_V = OH.CreateVar<Vec3>(name + "V", "m/s",
1493  "local relative velocity (x, y, z)");
1494 
1495  Var_Omega = OH.CreateVar<Vec3>(name + "Omega", "radian/s",
1496  "local relative angular velocity (x, y, z)");
1497  }
1498 #endif // USE_NETCDF
1499  }
1500 }
ToBeOutput::bToBeOutput
virtual bool bToBeOutput(void) const
Definition: output.cc:890
Vec3
Definition: matvec3.h:98
OutputHandler::UseNetCDF
bool UseNetCDF(int out) const
Definition: output.cc:491
Joint::OutputPrepare_int
virtual void OutputPrepare_int(const std::string &type, OutputHandler &OH, std::string &name)
Definition: joint.cc:107

Here is the call graph for this function:

Hint * TotalJoint::ParseHint ( DataManager *  pDM,
const char *  s 
) const
virtual

Reimplemented from SimulationEntity.

Definition at line 649 of file totalj.cc.

References STRLENOF.

650 {
651  if (strncasecmp(s, "offset{" /*}*/ , STRLENOF("offset{" /*}*/ )) == 0)
652  {
653  s += STRLENOF("offset{" /*}*/ );
654 
655  if (strcmp(&s[1], /*{*/ "}") != 0) {
656  return 0;
657  }
658 
659  switch (s[0]) {
660  case '1':
661  return new Joint::OffsetHint<1>;
662 
663  case '2':
664  return new Joint::OffsetHint<2>;
665  }
666 
667  } else if (strncasecmp(s, "position-hinge{" /*}*/, STRLENOF("position-hinge{" /*}*/)) == 0) {
668  s += STRLENOF("position-hinge{" /*}*/);
669 
670  if (strcmp(&s[1], /*{*/ "}") != 0) {
671  return 0;
672  }
673 
674  switch (s[0]) {
675  case '1':
676  return new Joint::HingeHint<1>;
677 
678  case '2':
679  return new Joint::HingeHint<2>;
680  }
681 
682  } else if (strncasecmp(s, "position-drive3{" /*}*/, STRLENOF("position-drive3{" /*}*/)) == 0) {
683  s += STRLENOF("position-");
684 
685  Hint *pH = ::ParseHint(pDM, s);
686  if (pH) {
687  TplDriveHint<Vec3> *pTDH = dynamic_cast<TplDriveHint<Vec3> *>(pH);
688  if (pTDH) {
689  return new PositionDriveHint<Vec3>(pTDH);
690  }
691  }
692  return 0;
693 
694  } else if (strncasecmp(s, "orientation-hinge{" /*}*/, STRLENOF("orientation-hinge{" /*}*/)) == 0) {
695  s += STRLENOF("orientation-hinge{" /*}*/);
696 
697  if (strcmp(&s[1], /*{*/ "}") != 0) {
698  return 0;
699  }
700 
701  switch (s[0]) {
702  case '1':
703  return new Joint::HingeHint<1>;
704 
705  case '2':
706  return new Joint::HingeHint<2>;
707  }
708 
709  } else if (strncasecmp(s, "orientation-drive3{" /*}*/, STRLENOF("orientation-drive3{" /*}*/)) == 0) {
710  s += STRLENOF("orientation-");
711 
712  Hint *pH = ::ParseHint(pDM, s);
713  if (pH) {
714  TplDriveHint<Vec3> *pTDH = dynamic_cast<TplDriveHint<Vec3> *>(pH);
715  if (pTDH) {
716  return new OrientationDriveHint<Vec3>(pTDH);
717  }
718  }
719  return 0;
720  }
721 
722  return 0;
723 }
Hint
Definition: hint.h:38
TotalJoint::ParseHint
virtual Hint * ParseHint(DataManager *pDM, const char *s) const
Definition: totalj.cc:649
STRLENOF
#define STRLENOF(s)
Definition: mbdyn.h:166
std::ostream & TotalJoint::Restart ( std::ostream &  out) const
virtual

Implements Elem.

Definition at line 753 of file totalj.cc.

References bAgvActive, bPosActive, bRotActive, bVelActive, f1, f2, WithLabel::GetLabel(), Mat3x3::GetVec(), OmegaDrv, OmegaPDrv, TplDriveOwner< T >::pGetDriveCaller(), pNode1, pNode2, R1h, R1hr, R2h, R2hr, TplDriveCaller< T >::Restart(), Joint::Restart(), ThetaDrv, Vec3::Write(), XDrv, XPDrv, and XPPDrv.

754 {
755  Joint::Restart(out) << ", total joint, "
756  << pNode1->GetLabel() << ", "
757  << "position, ";
758  f1.Write(out, ", ") << ", "
759  << "position orientation, "
760  "1 , ";
761  R1h.GetVec(1).Write(out, ", ") << ", "
762  "2 , ";
763  R1h.GetVec(2).Write(out, ", ") << ", "
764  << "rotation orientation, "
765  "1 , ";
766  R1hr.GetVec(1).Write(out, ", ") << ", "
767  "2 , ";
768  R1hr.GetVec(2).Write(out, ", ") << ", "
769  << pNode2->GetLabel() << ", "
770  << "position, ";
771  f2.Write(out, ", ") << ", "
772  << "position orientation, "
773  "1 , ";
774  R2h.GetVec(1).Write(out, ", ") << ", "
775  "2 , ";
776  R2h.GetVec(2).Write(out, ", ") << ", "
777  << "rotation orientation, "
778  "1 , ";
779  R2hr.GetVec(1).Write(out, ", ") << ", "
780  "2 , ";
781  R2hr.GetVec(2).Write(out, ", ");
782 
783  if (bPosActive[0] || bPosActive[1] || bPosActive[2]
784  || bVelActive[0] || bVelActive[1] || bVelActive[2])
785  {
786  out << ", position constraint";
787  for (unsigned i = 0; i < 3; i++) {
788  if (bPosActive[i]) {
789  out << ", active";
790 
791  } else if (bVelActive[i]) {
792  out << ", velocity";
793 
794  } else {
795  out << ", inactive";
796  }
797  }
798 
799  out << ", ", XDrv.pGetDriveCaller()->Restart(out);
800  if (XPDrv.pGetDriveCaller()) {
801  out << ", ", XPDrv.pGetDriveCaller()->Restart(out)
802  << ", ", XPPDrv.pGetDriveCaller()->Restart(out);
803  }
804  }
805 
806  if (bRotActive[0] || bRotActive[1] || bRotActive[2]
807  || bAgvActive[0] || bAgvActive[1] || bAgvActive[2])
808  {
809  out << ", orientation constraint";
810  for (unsigned i = 0; i < 3; i++) {
811  if (bRotActive[i]) {
812  out << ", active";
813 
814  } else if (bAgvActive[i]) {
815  out << ", angular velocity";
816 
817  } else {
818  out << ", inactive";
819  }
820  }
821 
822  out << ", ", ThetaDrv.pGetDriveCaller()->Restart(out);
823  if (OmegaDrv.pGetDriveCaller()) {
824  out << ", ", OmegaDrv.pGetDriveCaller()->Restart(out)
825  << ", ", OmegaPDrv.pGetDriveCaller()->Restart(out);
826  }
827  }
828 
829  return out << ";" << std::endl;
830 }
TotalJoint::R2h
Mat3x3 R2h
Definition: totalj.h:54
TotalJoint::R1hr
Mat3x3 R1hr
Definition: totalj.h:52
TotalJoint::R1h
Mat3x3 R1h
Definition: totalj.h:51
TotalJoint::bVelActive
bool bVelActive[3]
Definition: totalj.h:59
Vec3::Write
std::ostream & Write(std::ostream &out, const char *sFill=" ") const
Definition: matvec3.cc:738
TotalJoint::f1
Vec3 f1
Definition: totalj.h:50
TotalJoint::OmegaPDrv
TplDriveOwner< Vec3 > OmegaPDrv
Definition: totalj.h:68
TotalJoint::ThetaDrv
TplDriveOwner< Vec3 > ThetaDrv
Definition: totalj.h:66
TotalJoint::pNode2
const StructNode * pNode2
Definition: totalj.h:49
Mat3x3::GetVec
Vec3 GetVec(unsigned short int i) const
Definition: matvec3.h:893
TotalJoint::XPPDrv
TplDriveOwner< Vec3 > XPPDrv
Definition: totalj.h:64
TplDriveOwner::pGetDriveCaller
TplDriveCaller< T > * pGetDriveCaller(void) const
Definition: tpldrive.h:105
TotalJoint::bRotActive
bool bRotActive[3]
Definition: totalj.h:57
TotalJoint::R2hr
Mat3x3 R2hr
Definition: totalj.h:55
TotalJoint::XPDrv
TplDriveOwner< Vec3 > XPDrv
Definition: totalj.h:63
Joint::Restart
virtual std::ostream & Restart(std::ostream &out) const
Definition: joint.h:195
TotalJoint::bAgvActive
bool bAgvActive[3]
Definition: totalj.h:60
TotalJoint::pNode1
const StructNode * pNode1
Definition: totalj.h:48
TplDriveCaller::Restart
virtual std::ostream & Restart(std::ostream &out) const =0
TotalJoint::OmegaDrv
TplDriveOwner< Vec3 > OmegaDrv
Definition: totalj.h:67
TotalJoint::XDrv
TplDriveOwner< Vec3 > XDrv
Definition: totalj.h:62
WithLabel::GetLabel
unsigned int GetLabel(void) const
Definition: withlab.cc:62
TotalJoint::f2
Vec3 f2
Definition: totalj.h:53
TotalJoint::bPosActive
bool bPosActive[3]
Definition: totalj.h:56

Here is the call graph for this function:

void TotalJoint::SetValue ( DataManager *  pDM,
VectorHandler &  X,
VectorHandler &  XP,
SimulationEntity::Hints *  ph = 0 
)
virtual

Reimplemented from Joint.

Definition at line 563 of file totalj.cc.

References f1, f2, TplDriveOwner< T >::Get(), StructNode::GetRCurr(), StructDispNode::GetXCurr(), MBDYN_EXCEPT_ARGS, Mat3x3::MulTM(), Mat3x3::MulTV(), OmegaDrv, OmegaPDrv, pNode1, pNode2, Joint::JointDriveHint< T >::pTDH, R1h, R1hr, R2h, R2hr, TplDriveOwner< T >::Set(), ThetaDrv, tilde_f1, Mat3x3::Transpose(), WithLabel::uLabel, XDrv, XPDrv, and XPPDrv.

566 {
567  if (ph) {
568  for (unsigned int i = 0; i < ph->size(); i++) {
569  Joint::JointHint *pjh = dynamic_cast<Joint::JointHint *>((*ph)[i]);
570 
571  if (pjh) {
572 
573  if (dynamic_cast<Joint::OffsetHint<1> *>(pjh)) {
574  Mat3x3 R1T(pNode1->GetRCurr().Transpose());
575  Vec3 fTmp2(pNode2->GetRCurr()*f2);
576 
577  f1 = R1T*(pNode2->GetXCurr() + fTmp2 - pNode1->GetXCurr());
578  tilde_f1 = R1h.MulTV(f1) - XDrv.Get();
579 
580  } else if (dynamic_cast<Joint::OffsetHint<2> *>(pjh)) {
581  Mat3x3 R2T(pNode2->GetRCurr().Transpose());
582  Mat3x3 R1(pNode1->GetRCurr()*R1h);
583  Vec3 fTmp1(pNode1->GetRCurr()*f1);
584 
585  f2 = R2T*(pNode1->GetXCurr() + fTmp1 - pNode2->GetXCurr() + R1*XDrv.Get());
586 
587  } else if (dynamic_cast<Joint::HingeHint<1> *>(pjh)) {
588  if (dynamic_cast<Joint::PositionHingeHint<1> *>(pjh)) {
589  R1h = pNode1->GetRCurr().MulTM(pNode2->GetRCurr()*R2h);
590 
591  } else if (dynamic_cast<Joint::OrientationHingeHint<1> *>(pjh)) {
592  R1hr = pNode1->GetRCurr().MulTM(pNode2->GetRCurr()*R2hr);
593  }
594 
595  } else if (dynamic_cast<Joint::HingeHint<2> *>(pjh)) {
596  if (dynamic_cast<Joint::PositionHingeHint<2> *>(pjh)) {
597  R2h = pNode2->GetRCurr().MulTM(pNode1->GetRCurr()*R1h);
598 
599  } else if (dynamic_cast<Joint::OrientationHingeHint<2> *>(pjh)) {
600  R2hr = pNode2->GetRCurr().MulTM(pNode1->GetRCurr()*R1hr);
601  }
602 
603  } else if (dynamic_cast<Joint::JointDriveHint<Vec3> *>(pjh)) {
604  Joint::JointDriveHint<Vec3> *pjdh
605  = dynamic_cast<Joint::JointDriveHint<Vec3> *>(pjh);
606  pedantic_cout("TotalJoint(" << uLabel << "): "
607  "creating drive from hint[" << i << "]..." << std::endl);
608 
609  TplDriveCaller<Vec3> *pDC = pjdh->pTDH->pCreateDrive(pDM);
610  if (pDC == 0) {
611  silent_cerr("TotalJoint(" << uLabel << "): "
612  "unable to create drive "
613  "after hint #" << i << std::endl);
614  throw ErrGeneric(MBDYN_EXCEPT_ARGS);
615  }
616 
617  if (dynamic_cast<Joint::PositionDriveHint<Vec3> *>(pjdh)) {
618  XDrv.Set(pDC);
619 
620  } else if (dynamic_cast<Joint::VelocityDriveHint<Vec3> *>(pjdh)) {
621  XPDrv.Set(pDC);
622 
623  } else if (dynamic_cast<Joint::AccelerationDriveHint<Vec3> *>(pjdh)) {
624  XPPDrv.Set(pDC);
625 
626  } else if (dynamic_cast<Joint::OrientationDriveHint<Vec3> *>(pjdh)) {
627  ThetaDrv.Set(pDC);
628 
629  } else if (dynamic_cast<Joint::AngularVelocityDriveHint<Vec3> *>(pjdh)) {
630  OmegaDrv.Set(pDC);
631 
632  } else if (dynamic_cast<Joint::AngularAccelerationDriveHint<Vec3> *>(pjdh)) {
633  OmegaPDrv.Set(pDC);
634 
635  } else {
636  delete pDC;
637  }
638 
639  } else if (dynamic_cast<Joint::ReactionsHint *>(pjh)) {
640  /* TODO */
641  }
642  continue;
643  }
644  }
645  }
646 }
TotalJoint::R2h
Mat3x3 R2h
Definition: totalj.h:54
TotalJoint::R1hr
Mat3x3 R1hr
Definition: totalj.h:52
TotalJoint::R1h
Mat3x3 R1h
Definition: totalj.h:51
MBDYN_EXCEPT_ARGS
#define MBDYN_EXCEPT_ARGS
Definition: except.h:63
Vec3
Definition: matvec3.h:98
StructNode::GetRCurr
virtual const Mat3x3 & GetRCurr(void) const
Definition: strnode.h:1012
Joint::JointDriveHint::pTDH
TplDriveHint< T > * pTDH
Definition: joint.h:137
TotalJoint::f1
Vec3 f1
Definition: totalj.h:50
TotalJoint::OmegaPDrv
TplDriveOwner< Vec3 > OmegaPDrv
Definition: totalj.h:68
TotalJoint::ThetaDrv
TplDriveOwner< Vec3 > ThetaDrv
Definition: totalj.h:66
TotalJoint::pNode2
const StructNode * pNode2
Definition: totalj.h:49
TotalJoint::tilde_f1
Vec3 tilde_f1
Definition: totalj.h:86
TotalJoint::XPPDrv
TplDriveOwner< Vec3 > XPPDrv
Definition: totalj.h:64
TplDriveOwner::Set
void Set(const TplDriveCaller< T > *pDC)
Definition: tpldrive.h:97
Mat3x3::MulTV
Vec3 MulTV(const Vec3 &v) const
Definition: matvec3.cc:482
TplDriveOwner::Get
T Get(const doublereal &dVar) const
Definition: tpldrive.h:109
TotalJoint::R2hr
Mat3x3 R2hr
Definition: totalj.h:55
TotalJoint::XPDrv
TplDriveOwner< Vec3 > XPDrv
Definition: totalj.h:63
WithLabel::uLabel
unsigned int uLabel
Definition: withlab.h:44
TotalJoint::pNode1
const StructNode * pNode1
Definition: totalj.h:48
TotalJoint::OmegaDrv
TplDriveOwner< Vec3 > OmegaDrv
Definition: totalj.h:67
Mat3x3::MulTM
Mat3x3 MulTM(const Mat3x3 &m) const
Definition: matvec3.cc:500
StructDispNode::GetXCurr
virtual const Vec3 & GetXCurr(void) const
Definition: strnode.h:310
Mat3x3::Transpose
Mat3x3 Transpose(void) const
Definition: matvec3.h:816
TotalJoint::XDrv
TplDriveOwner< Vec3 > XDrv
Definition: totalj.h:62
TotalJoint::f2
Vec3 f2
Definition: totalj.h:53

Here is the call graph for this function:

void TotalJoint::Update ( const VectorHandler &  XCurr,
InverseDynamics::Order  iOrder = InverseDynamics::INVERSE_DYNAMICS 
)
virtual

Reimplemented from Joint.

Definition at line 1455 of file totalj.cc.

References F, DofOwnerOwner::iGetFirstIndex(), iPosEqIndex, iPosIncid, iRotEqIndex, iRotIncid, M, nPosConstraints, and nRotConstraints.

1456 {
1457  integer iFirstReactionIndex = iGetFirstIndex();
1458 
1459  /* Get constraint reactions */
1460  for (unsigned iCnt = 0; iCnt < nPosConstraints; iCnt++) {
1461  F(iPosIncid[iCnt]) = XCurr(iFirstReactionIndex + 1 + iPosEqIndex[iCnt]);
1462  }
1463  for (unsigned iCnt = 0; iCnt < nRotConstraints; iCnt++) {
1464  M(iRotIncid[iCnt]) = XCurr(iFirstReactionIndex + 1 + iRotEqIndex[iCnt]);
1465  }
1466 };
TotalJoint::nPosConstraints
unsigned int nPosConstraints
Definition: totalj.h:71
TotalJoint::iPosIncid
unsigned int iPosIncid[3]
Definition: totalj.h:76
TotalJoint::iRotEqIndex
unsigned int iRotEqIndex[3]
Definition: totalj.h:82
TotalJoint::M
Vec3 M
Definition: totalj.h:95
TotalJoint::nRotConstraints
unsigned int nRotConstraints
Definition: totalj.h:72
DofOwnerOwner::iGetFirstIndex
virtual integer iGetFirstIndex(void) const
Definition: dofown.h:127
TotalJoint::F
Vec3 F
Definition: totalj.h:96
TotalJoint::iRotIncid
unsigned int iRotIncid[3]
Definition: totalj.h:77
integer
long int integer
Definition: colamd.c:51
TotalJoint::iPosEqIndex
unsigned int iPosEqIndex[3]
Definition: totalj.h:81

Here is the call graph for this function:

void TotalJoint::WorkSpaceDim ( integer *  piNumRows,
integer *  piNumCols 
) const
inlinevirtual

Implements Elem.

Definition at line 177 of file totalj.h.

References nConstraints.

Referenced by AssJac(), and AssRes().

177  {
178  *piNumCols = *piNumRows = 12 + nConstraints ;
179  };
TotalJoint::nConstraints
unsigned int nConstraints
Definition: totalj.h:70

Member Data Documentation

bool TotalJoint::bAgvActive[3]
private

Definition at line 60 of file totalj.h.

Referenced by DescribeDof(), DescribeEq(), Restart(), and TotalJoint().

bool TotalJoint::bPosActive[3]
private

Definition at line 56 of file totalj.h.

Referenced by DescribeDof(), DescribeEq(), dGetPrivData(), Restart(), and TotalJoint().

bool TotalJoint::bRotActive[3]
private

Definition at line 57 of file totalj.h.

Referenced by DescribeDof(), DescribeEq(), dGetPrivData(), Restart(), and TotalJoint().

bool TotalJoint::bVelActive[3]
private

Definition at line 59 of file totalj.h.

Referenced by DescribeDof(), DescribeEq(), Restart(), and TotalJoint().

Vec3 TotalJoint::F
mutableprivate

Definition at line 96 of file totalj.h.

Referenced by AssJac(), AssRes(), dGetPrivData(), InitialAssJac(), InitialAssRes(), Output(), and Update().

Vec3 TotalJoint::f1
private

Definition at line 50 of file totalj.h.

Referenced by dGetPrivData(), Restart(), and SetValue().

Vec3 TotalJoint::f2
private

Definition at line 53 of file totalj.h.

Referenced by AssJac(), AssRes(), dGetPrivData(), InitialAssJac(), InitialAssRes(), Output(), Restart(), and SetValue().

unsigned int TotalJoint::iAgvEqIndex[3]
private

Definition at line 84 of file totalj.h.

Referenced by AssJac(), AssRes(), and TotalJoint().

unsigned int TotalJoint::iAgvIncid[3]
private

Definition at line 79 of file totalj.h.

Referenced by AssJac(), AssRes(), and TotalJoint().

unsigned int TotalJoint::iPosEqIndex[3]
private

Definition at line 81 of file totalj.h.

Referenced by AssJac(), AssRes(), TotalJoint(), and Update().

unsigned int TotalJoint::iPosIncid[3]
private

Definition at line 76 of file totalj.h.

Referenced by AssJac(), AssRes(), InitialAssJac(), InitialAssRes(), TotalJoint(), and Update().

unsigned int TotalJoint::iRotEqIndex[3]
private

Definition at line 82 of file totalj.h.

Referenced by AssJac(), AssRes(), TotalJoint(), and Update().

unsigned int TotalJoint::iRotIncid[3]
private

Definition at line 77 of file totalj.h.

Referenced by AfterConvergence(), AssJac(), AssRes(), InitialAssJac(), InitialAssRes(), TotalJoint(), and Update().

unsigned int TotalJoint::iVelEqIndex[3]
private

Definition at line 83 of file totalj.h.

Referenced by AssJac(), AssRes(), and TotalJoint().

unsigned int TotalJoint::iVelIncid[3]
private

Definition at line 78 of file totalj.h.

Referenced by AssJac(), AssRes(), and TotalJoint().

Vec3 TotalJoint::M
mutableprivate

Definition at line 95 of file totalj.h.

Referenced by AssJac(), AssRes(), dGetPrivData(), InitialAssJac(), InitialAssRes(), Output(), and Update().

unsigned int TotalJoint::nAgvConstraints
private

Definition at line 74 of file totalj.h.

Referenced by AssJac(), AssRes(), DescribeDof(), DescribeEq(), and TotalJoint().

unsigned int TotalJoint::nConstraints
private

Definition at line 70 of file totalj.h.

Referenced by AssJac(), AssRes(), DescribeDof(), DescribeEq(), GetDofType(), iGetInitialNumDof(), iGetNumDof(), InitialAssJac(), InitialAssRes(), InitialWorkSpaceDim(), TotalJoint(), and WorkSpaceDim().

unsigned int TotalJoint::nPosConstraints
private

Definition at line 71 of file totalj.h.

Referenced by AssJac(), AssRes(), DescribeDof(), DescribeEq(), InitialAssJac(), InitialAssRes(), TotalJoint(), and Update().

unsigned int TotalJoint::nRotConstraints
private

Definition at line 72 of file totalj.h.

Referenced by AfterConvergence(), AssJac(), AssRes(), DescribeDof(), DescribeEq(), InitialAssJac(), InitialAssRes(), TotalJoint(), and Update().

unsigned int TotalJoint::nVelConstraints
private

Definition at line 73 of file totalj.h.

Referenced by AssJac(), AssRes(), DescribeDof(), DescribeEq(), and TotalJoint().

TplDriveOwner<Vec3> TotalJoint::OmegaDrv
private

Definition at line 67 of file totalj.h.

Referenced by AssRes(), Restart(), and SetValue().

TplDriveOwner<Vec3> TotalJoint::OmegaPDrv
private

Definition at line 68 of file totalj.h.

Referenced by AssRes(), Restart(), and SetValue().

const StructNode* TotalJoint::pNode1
private

Definition at line 48 of file totalj.h.

Referenced by AfterConvergence(), AssJac(), AssRes(), dGetPrivData(), GetConnectedNodes(), InitialAssJac(), InitialAssRes(), Output(), Restart(), SetValue(), and TotalJoint().

const StructNode* TotalJoint::pNode2
private

Definition at line 49 of file totalj.h.

Referenced by AfterConvergence(), AssJac(), AssRes(), dGetPrivData(), GetConnectedNodes(), InitialAssJac(), InitialAssRes(), Output(), Restart(), SetValue(), and TotalJoint().

Mat3x3 TotalJoint::R1h
private

Definition at line 51 of file totalj.h.

Referenced by AssJac(), AssRes(), dGetPrivData(), InitialAssJac(), InitialAssRes(), Output(), Restart(), and SetValue().

Mat3x3 TotalJoint::R1hr
private

Definition at line 52 of file totalj.h.

Referenced by AfterConvergence(), AssJac(), AssRes(), dGetPrivData(), InitialAssJac(), InitialAssRes(), Output(), Restart(), SetValue(), and TotalJoint().

Mat3x3 TotalJoint::R2h
private

Definition at line 54 of file totalj.h.

Referenced by Restart(), and SetValue().

Mat3x3 TotalJoint::R2hr
private

Definition at line 55 of file totalj.h.

Referenced by AfterConvergence(), AssRes(), dGetPrivData(), InitialAssRes(), Output(), Restart(), SetValue(), and TotalJoint().

Vec3 TotalJoint::ThetaDelta
mutableprivate

Definition at line 97 of file totalj.h.

Referenced by AfterConvergence(), AssRes(), and InitialAssRes().

Vec3 TotalJoint::ThetaDeltaPrev
mutableprivate

Definition at line 98 of file totalj.h.

Referenced by AfterConvergence().

Vec3 TotalJoint::ThetaDeltaRemnant
private

Definition at line 100 of file totalj.h.

Referenced by AfterConvergence(), and AssRes().

Vec3 TotalJoint::ThetaDeltaTrue
private

Definition at line 101 of file totalj.h.

Referenced by AfterConvergence(), dGetPrivData(), and TotalJoint().

TplDriveOwner<Vec3> TotalJoint::ThetaDrv
private

Definition at line 66 of file totalj.h.

Referenced by AssRes(), dGetPrivData(), InitialAssRes(), Restart(), and SetValue().

Vec3 TotalJoint::tilde_f1
private

Definition at line 86 of file totalj.h.

Referenced by AssRes(), InitialAssRes(), Output(), and SetValue().

TplDriveOwner<Vec3> TotalJoint::XDrv
private

Definition at line 62 of file totalj.h.

Referenced by AssRes(), dGetPrivData(), InitialAssRes(), Restart(), and SetValue().

TplDriveOwner<Vec3> TotalJoint::XPDrv
private

Definition at line 63 of file totalj.h.

Referenced by AssRes(), Restart(), and SetValue().

TplDriveOwner<Vec3> TotalJoint::XPPDrv
private

Definition at line 64 of file totalj.h.

Referenced by AssRes(), Restart(), and SetValue().

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