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
TotalPinJoint Class Reference

#include <totalj.h>

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

Public Member Functions

 TotalPinJoint (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 Vec3 &XcTmp, const Mat3x3 &RchTmp, const Mat3x3 &RchrTmp, const StructNode *pN, const Vec3 &fnTmp, const Mat3x3 &RnhTmp, const Mat3x3 &RnhrTmp, flag fOut)
 
 ~TotalPinJoint (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)
 
virtual 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 * pNode
 
Vec3 Xc
 
Mat3x3 Rch
 
Mat3x3 Rchr
 
Vec3 tilde_fn
 
Mat3x3 tilde_Rnh
 
Mat3x3 tilde_Rnhr
 
bool bPosActive [3]
 
bool bRotActive [3]
 
bool bVelActive [3]
 
bool bAgvActive [3]
 
Mat3x3 RchT
 
Vec3 tilde_Xc
 
Mat3x3 RchrT
 
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 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 259 of file totalj.h.

Constructor & Destructor Documentation

TotalPinJoint::TotalPinJoint ( 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 Vec3 &  XcTmp,
const Mat3x3 &  RchTmp,
const Mat3x3 &  RchrTmp,
const StructNode *  pN,
const Vec3 &  fnTmp,
const Mat3x3 &  RnhTmp,
const Mat3x3 &  RnhrTmp,
flag  fOut 
)

Definition at line 2075 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, pNode, RchT, ThetaDeltaTrue, tilde_Rnhr, and RotManip::VecRot().

2084 : Elem(uL, fOut),
2085 Joint(uL, pDO, fOut),
2086 pNode(pN),
2087 Xc(XcTmp), Rch(RchTmp), Rchr(RchrTmp),
2088 tilde_fn(fnTmp), tilde_Rnh(RnhTmp), tilde_Rnhr(RnhrTmp),
2089 RchT(Rch.Transpose()), tilde_Xc(RchT*Xc), RchrT(Rchr.Transpose()),
2090 XDrv(pDCPos[0]), XPDrv(pDCPos[1]), XPPDrv(pDCPos[2]),
2091 ThetaDrv(pDCRot[0]), OmegaDrv(pDCRot[1]), OmegaPDrv(pDCRot[2]),
2092 nConstraints(0), nPosConstraints(0), nRotConstraints(0),
2093 nVelConstraints(0), nAgvConstraints(0),
2094 #ifdef USE_NETCDF
2095 Var_X(0),
2096 Var_Phi(0),
2097 Var_V(0),
2098 Var_Omega(0),
2099 #endif // USE_NETCDF
2100 M(::Zero3), F(::Zero3),
2101 ThetaDelta(::Zero3),
2102 ThetaDeltaPrev(::Zero3),
2103 ThetaDeltaRemnant(::Zero3),
2104 ThetaDeltaTrue(::Zero3)
2105 {
2106  /* Equations 1->3: Positions
2107  * Equations 4->6: Rotations */
2108 
2109  unsigned int index = 0;
2110 
2111  for (unsigned int i = 0; i < 3; i++) {
2112  bPosActive[i] = bPos[i];
2113  bVelActive[i] = bVel[i];
2114 
2115  if (bPosActive[i]) {
2116  iPosIncid[nPosConstraints] = i + 1;
2117  iPosEqIndex[nPosConstraints] = index;
2118  nPosConstraints++;
2119  index++;
2120  }
2121 
2122  if (bVelActive[i]) {
2123  iVelIncid[nVelConstraints] = i + 1;
2124  iVelEqIndex[nVelConstraints] = index;
2125  nVelConstraints++;
2126  index++;
2127  }
2128  }
2129 
2130  index = 0;
2131  for (unsigned int i = 0; i < 3; i++) {
2132  bRotActive[i] = bRot[i];
2133  bAgvActive[i] = bAgv[i];
2134 
2135  if (bRotActive[i]) {
2136  iRotIncid[nRotConstraints] = i + 1;
2137  iRotEqIndex[nRotConstraints] = nPosConstraints + nVelConstraints + index;
2138  nRotConstraints++;
2139  index++;
2140  }
2141 
2142  if (bAgvActive[i]) {
2143  iAgvIncid[nAgvConstraints] = i + 1;
2144  iAgvEqIndex[nAgvConstraints] = nPosConstraints + nVelConstraints + index;
2145  nAgvConstraints++;
2146  index++;
2147  }
2148  }
2149 
2150  // if only a fraction of the rotation degrees of freedom
2151  // are constrained, store the incidence of the unconstrained ones
2152  // in the remainder of array iRotIncid[] for later use
2153  if (nRotConstraints > 0 && nRotConstraints < 3) {
2154  switch (nRotConstraints) {
2155  case 1:
2156  switch (iRotIncid[0]) {
2157  case 1:
2158  iRotIncid[1] = 2;
2159  iRotIncid[2] = 3;
2160  break;
2161 
2162  case 2:
2163  iRotIncid[1] = 1;
2164  iRotIncid[2] = 3;
2165  break;
2166 
2167  case 3:
2168  iRotIncid[1] = 1;
2169  iRotIncid[2] = 2;
2170  break;
2171 
2172  default:
2173  ASSERT(0);
2174  throw ErrGeneric(MBDYN_EXCEPT_ARGS);
2175  }
2176  break;
2177 
2178  case 2:
2179  switch (iRotIncid[0]) {
2180  case 1:
2181  iRotIncid[2] = 5 - iRotIncid[1];
2182  break;
2183 
2184  case 2:
2185  iRotIncid[2] = 1;
2186  break;
2187  }
2188  break;
2189 
2190  default:
2191  ASSERT(0);
2192  throw ErrGeneric(MBDYN_EXCEPT_ARGS);
2193  }
2194  }
2195 
2196  nConstraints = nPosConstraints + nVelConstraints
2197  + nRotConstraints + nAgvConstraints;
2198 
2199  ThetaDeltaTrue = RotManip::VecRot(RchT*pNode->GetRCurr()*tilde_Rnhr);
2200 }
TotalPinJoint::iVelEqIndex
unsigned int iVelEqIndex[3]
Definition: totalj.h:299
TotalPinJoint::Rchr
Mat3x3 Rchr
Definition: totalj.h:265
Zero3
const Vec3 Zero3(0., 0., 0.)
TotalPinJoint::iAgvIncid
unsigned int iAgvIncid[3]
Definition: totalj.h:295
TotalPinJoint::bPosActive
bool bPosActive[3]
Definition: totalj.h:269
TotalPinJoint::Rch
Mat3x3 Rch
Definition: totalj.h:264
MBDYN_EXCEPT_ARGS
#define MBDYN_EXCEPT_ARGS
Definition: except.h:63
TotalPinJoint::XPDrv
TplDriveOwner< Vec3 > XPDrv
Definition: totalj.h:279
StructNode::GetRCurr
virtual const Mat3x3 & GetRCurr(void) const
Definition: strnode.h:1012
TotalPinJoint::bRotActive
bool bRotActive[3]
Definition: totalj.h:270
TotalPinJoint::nRotConstraints
unsigned int nRotConstraints
Definition: totalj.h:288
TotalPinJoint::nAgvConstraints
unsigned int nAgvConstraints
Definition: totalj.h:290
Joint::Joint
Joint(unsigned int uL, const DofOwner *pD, flag fOut)
Definition: joint.cc:83
TotalPinJoint::tilde_Rnh
Mat3x3 tilde_Rnh
Definition: totalj.h:267
TotalPinJoint::nVelConstraints
unsigned int nVelConstraints
Definition: totalj.h:289
TotalPinJoint::bVelActive
bool bVelActive[3]
Definition: totalj.h:271
TotalPinJoint::tilde_fn
Vec3 tilde_fn
Definition: totalj.h:266
RotManip::VecRot
Vec3 VecRot(const Mat3x3 &Phi)
Definition: Rot.cc:136
TotalPinJoint::iPosIncid
unsigned int iPosIncid[3]
Definition: totalj.h:292
TotalPinJoint::ThetaDeltaRemnant
Vec3 ThetaDeltaRemnant
Definition: totalj.h:314
TotalPinJoint::iVelIncid
unsigned int iVelIncid[3]
Definition: totalj.h:294
TotalPinJoint::iPosEqIndex
unsigned int iPosEqIndex[3]
Definition: totalj.h:297
TotalPinJoint::OmegaPDrv
TplDriveOwner< Vec3 > OmegaPDrv
Definition: totalj.h:284
TotalPinJoint::tilde_Rnhr
Mat3x3 tilde_Rnhr
Definition: totalj.h:268
TotalPinJoint::OmegaDrv
TplDriveOwner< Vec3 > OmegaDrv
Definition: totalj.h:283
TotalPinJoint::ThetaDeltaPrev
Vec3 ThetaDeltaPrev
Definition: totalj.h:312
TotalPinJoint::pNode
const StructNode * pNode
Definition: totalj.h:262
TotalPinJoint::iAgvEqIndex
unsigned int iAgvEqIndex[3]
Definition: totalj.h:300
TotalPinJoint::XPPDrv
TplDriveOwner< Vec3 > XPPDrv
Definition: totalj.h:280
TotalPinJoint::iRotEqIndex
unsigned int iRotEqIndex[3]
Definition: totalj.h:298
ASSERT
#define ASSERT(expression)
Definition: colamd.c:977
TotalPinJoint::tilde_Xc
Vec3 tilde_Xc
Definition: totalj.h:275
TotalPinJoint::ThetaDelta
Vec3 ThetaDelta
Definition: totalj.h:311
Mat3x3::Transpose
Mat3x3 Transpose(void) const
Definition: matvec3.h:816
TotalPinJoint::nPosConstraints
unsigned int nPosConstraints
Definition: totalj.h:287
TotalPinJoint::RchT
Mat3x3 RchT
Definition: totalj.h:274
TotalPinJoint::bAgvActive
bool bAgvActive[3]
Definition: totalj.h:272
TotalPinJoint::iRotIncid
unsigned int iRotIncid[3]
Definition: totalj.h:293
TotalPinJoint::ThetaDeltaTrue
Vec3 ThetaDeltaTrue
Definition: totalj.h:315
TotalPinJoint::nConstraints
unsigned int nConstraints
Definition: totalj.h:286
TotalPinJoint::ThetaDrv
TplDriveOwner< Vec3 > ThetaDrv
Definition: totalj.h:282
Elem::Elem
Elem(unsigned int uL, flag fOut)
Definition: elem.cc:41
TotalPinJoint::XDrv
TplDriveOwner< Vec3 > XDrv
Definition: totalj.h:278
TotalPinJoint::RchrT
Mat3x3 RchrT
Definition: totalj.h:276

Here is the call graph for this function:

TotalPinJoint::~TotalPinJoint ( void  )

Definition at line 2202 of file totalj.cc.

References NO_OP.

2203 {
2204  NO_OP;
2205 }
NO_OP
#define NO_OP
Definition: myassert.h:74

Member Function Documentation

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

Reimplemented from SimulationEntity.

Definition at line 2744 of file totalj.cc.

References StructNode::GetRCurr(), iRotIncid, nRotConstraints, pNode, RchT, ThetaDelta, ThetaDeltaPrev, ThetaDeltaRemnant, ThetaDeltaTrue, tilde_Rnhr, Unwrap(), and RotManip::VecRot().

Referenced by AfterConvergence().

2746 {
2747  // "trick": correct ThetaDrv to keep ThetaDelta small
2748  // See "A Vectorial Formulation of Generic Pair Constraints"
2749  if (nRotConstraints < 3) {
2750  Vec3 ThetaDeltaTmp(ThetaDelta);
2751  for (unsigned i = 0; i < nRotConstraints; i++) {
2752  // zero out constrained components
2753  // NOTE: should already be zero within tolerance
2754  ThetaDeltaTmp(iRotIncid[i]) = 0.;
2755  }
2756  ThetaDeltaRemnant += ThetaDeltaTmp;
2757  }
2758 
2759  ThetaDeltaPrev = Unwrap(ThetaDeltaPrev, ThetaDeltaRemnant);
2760  ThetaDeltaTrue = Unwrap(ThetaDeltaTrue, RotManip::VecRot(RchT*pNode->GetRCurr()*tilde_Rnhr));
2761 }
Vec3
Definition: matvec3.h:98
StructNode::GetRCurr
virtual const Mat3x3 & GetRCurr(void) const
Definition: strnode.h:1012
TotalPinJoint::nRotConstraints
unsigned int nRotConstraints
Definition: totalj.h:288
RotManip::VecRot
Vec3 VecRot(const Mat3x3 &Phi)
Definition: Rot.cc:136
TotalPinJoint::ThetaDeltaRemnant
Vec3 ThetaDeltaRemnant
Definition: totalj.h:314
TotalPinJoint::tilde_Rnhr
Mat3x3 tilde_Rnhr
Definition: totalj.h:268
TotalPinJoint::ThetaDeltaPrev
Vec3 ThetaDeltaPrev
Definition: totalj.h:312
TotalPinJoint::pNode
const StructNode * pNode
Definition: totalj.h:262
TotalPinJoint::ThetaDelta
Vec3 ThetaDelta
Definition: totalj.h:311
Unwrap
Vec3 Unwrap(const Vec3 &vPrev, const Vec3 &v)
Definition: matvec3.cc:1089
TotalPinJoint::RchT
Mat3x3 RchT
Definition: totalj.h:274
TotalPinJoint::iRotIncid
unsigned int iRotIncid[3]
Definition: totalj.h:293
TotalPinJoint::ThetaDeltaTrue
Vec3 ThetaDeltaTrue
Definition: totalj.h:315

Here is the call graph for this function:

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

Reimplemented from SimulationEntity.

Definition at line 2765 of file totalj.cc.

References AfterConvergence().

2767 {
2768  AfterConvergence(X, XP);
2769 }
TotalPinJoint::AfterConvergence
virtual void AfterConvergence(const VectorHandler &X, const VectorHandler &XP)
Definition: totalj.cc:2744

Here is the call graph for this function:

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

Implements Elem.

Definition at line 2850 of file totalj.cc.

References FullSubMatrixHandler::Add(), FullSubMatrixHandler::AddT(), DEBUGCOUT, F, StructNode::GetRCurr(), Mat3x3::GetVec(), DofOwnerOwner::iGetFirstIndex(), StructDispNode::iGetFirstMomentumIndex(), StructDispNode::iGetFirstPositionIndex(), iGetNumDof(), iPosIncid, iRotIncid, MatCrossCross, nConstraints, nPosConstraints, nRotConstraints, pNode, FullSubMatrixHandler::PutColIndex(), FullSubMatrixHandler::PutRowIndex(), Rch, Rchr, FullSubMatrixHandler::ResizeReset(), VariableSubMatrixHandler::SetFull(), VariableSubMatrixHandler::SetNullMatrix(), tilde_fn, and WorkSpaceDim().

2854 {
2855  /*
2856  * See tecman.pdf
2857  */
2858 
2859  DEBUGCOUT("Entering TotalPinJoint::AssJac()" << std::endl);
2860 
2861  if (iGetNumDof() == 0) {
2862  WorkMat.SetNullMatrix();
2863  return WorkMat;
2864  }
2865 
2866  FullSubMatrixHandler& WM = WorkMat.SetFull();
2867 
2868  /* Ridimensiona la sottomatrice in base alle esigenze */
2869  integer iNumRows = 0;
2870  integer iNumCols = 0;
2871  WorkSpaceDim(&iNumRows, &iNumCols);
2872  WM.ResizeReset(iNumRows, iNumCols);
2873 
2874  /* Recupera gli indici delle varie incognite */
2875  integer iNodeFirstPosIndex = pNode->iGetFirstPositionIndex();
2876  integer iNodeFirstMomIndex = pNode->iGetFirstMomentumIndex();
2877  integer iFirstReactionIndex = iGetFirstIndex();
2878 
2879  /* Setta gli indici delle equazioni */
2880  for (int iCnt = 1; iCnt <= 6; iCnt++) {
2881  WM.PutRowIndex(iCnt, iNodeFirstMomIndex + iCnt);
2882  WM.PutColIndex(iCnt, iNodeFirstPosIndex + iCnt);
2883  }
2884 
2885  for (unsigned int iCnt = 1; iCnt <= nConstraints; iCnt++) {
2886  WM.PutRowIndex(6 + iCnt, iFirstReactionIndex + iCnt);
2887  WM.PutColIndex(6 + iCnt, iFirstReactionIndex + iCnt);
2888  }
2889 
2890  /* Recupera i dati che servono */
2891  Vec3 fn(pNode->GetRCurr()*tilde_fn);
2892 
2893  /* Moltiplica la forza per il coefficiente del metodo */
2894  Vec3 FTmp(Rch*(F*dCoef));
2895 
2896  /* Equilibrium: ((Phi/q)^T*Lambda)/q */
2897 
2898  /* Lines 4->6: */
2899  /* [ F x ] [ b2 x ] */
2900  WM.Add(3 + 1, 3 + 1, Mat3x3(MatCrossCross, FTmp, fn));
2901 
2902 /* Phi/q and (Phi/q)^T */
2903 
2904  Mat3x3 fnCross_Rch(fn.Cross(Rch)); // = [ b2 x ] * R1
2905 
2906  for (unsigned iCnt = 0 ; iCnt < nPosConstraints; iCnt++) {
2907  Vec3 vRch(Rch.GetVec(iPosIncid[iCnt]));
2908  Vec3 vfnCross_Rch(fnCross_Rch.GetVec(iPosIncid[iCnt]));
2909 
2910  /* Equilibrium, node */
2911  WM.Add(1, 6 + 1 + iCnt, vRch);
2912  WM.Add(3 + 1, 6 + 1 + iCnt, vfnCross_Rch);
2913 
2914  /* Constraint, node */
2915  WM.AddT(6 + 1 + iCnt, 1, vRch);
2916  WM.AddT(6 + 1 + iCnt, 3 + 1, vfnCross_Rch);
2917  }
2918 
2919  for (unsigned iCnt = 0 ; iCnt < nRotConstraints; iCnt++) {
2920  Vec3 vRchr(Rchr.GetVec(iRotIncid[iCnt]));
2921 
2922  /* Equilibrium, node */
2923  WM.Add(3 + 1, 6 + 1 + nPosConstraints + iCnt, vRchr);
2924 
2925  /* Constraint, node */
2926  WM.AddT(6 + 1 + nPosConstraints + iCnt, 3 + 1, vRchr);
2927  }
2928 
2929  // TODO: velocity & angular velocity
2930 
2931  return WorkMat;
2932 }
TotalPinJoint::Rchr
Mat3x3 Rchr
Definition: totalj.h:265
FullSubMatrixHandler::PutColIndex
void PutColIndex(integer iSubCol, integer iCol)
Definition: submat.h:325
TotalPinJoint::Rch
Mat3x3 Rch
Definition: totalj.h:264
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
FullSubMatrixHandler::Add
void Add(integer iRow, integer iCol, const Vec3 &v)
Definition: submat.cc:209
TotalPinJoint::nRotConstraints
unsigned int nRotConstraints
Definition: totalj.h:288
TotalPinJoint::tilde_fn
Vec3 tilde_fn
Definition: totalj.h:266
Mat3x3::GetVec
Vec3 GetVec(unsigned short int i) const
Definition: matvec3.h:893
FullSubMatrixHandler::AddT
void AddT(integer iRow, integer iCol, const Vec3 &v)
Definition: submat.cc:227
TotalPinJoint::iPosIncid
unsigned int iPosIncid[3]
Definition: totalj.h:292
TotalPinJoint::pNode
const StructNode * pNode
Definition: totalj.h:262
VariableSubMatrixHandler::SetNullMatrix
void SetNullMatrix(void)
Definition: submat.h:1159
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
FullSubMatrixHandler::ResizeReset
virtual void ResizeReset(integer, integer)
Definition: submat.cc:182
TotalPinJoint::iGetNumDof
virtual unsigned int iGetNumDof(void) const
Definition: totalj.h:342
MatCrossCross
const MatCrossCross_Manip MatCrossCross
Definition: matvec3.cc:640
TotalPinJoint::nPosConstraints
unsigned int nPosConstraints
Definition: totalj.h:287
FullSubMatrixHandler::PutRowIndex
void PutRowIndex(integer iSubRow, integer iRow)
Definition: submat.h:311
TotalPinJoint::iRotIncid
unsigned int iRotIncid[3]
Definition: totalj.h:293
TotalPinJoint::WorkSpaceDim
void WorkSpaceDim(integer *piNumRows, integer *piNumCols) const
Definition: totalj.h:389
DofOwnerOwner::iGetFirstIndex
virtual integer iGetFirstIndex(void) const
Definition: dofown.h:127
TotalPinJoint::nConstraints
unsigned int nConstraints
Definition: totalj.h:286
integer
long int integer
Definition: colamd.c:51

Here is the call graph for this function:

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

Reimplemented from Elem.

Definition at line 3068 of file totalj.cc.

References FullSubMatrixHandler::AddT(), DEBUGCOUT, StructNode::GetRCurr(), Mat3x3::GetVec(), DofOwnerOwner::iGetFirstIndex(), StructDispNode::iGetFirstPositionIndex(), iGetNumDof(), iPosIncid, iRotIncid, nConstraints, nPosConstraints, nRotConstraints, pNode, FullSubMatrixHandler::PutColIndex(), FullSubMatrixHandler::PutRowIndex(), Rch, Rchr, FullSubMatrixHandler::ResizeReset(), VariableSubMatrixHandler::SetFull(), VariableSubMatrixHandler::SetNullMatrix(), tilde_fn, and WorkSpaceDim().

3071 {
3072  /*
3073  * See tecman.pdf
3074  */
3075 
3076  DEBUGCOUT("Entering TotalPinJoint::AssJac()" << std::endl);
3077 
3078  if (iGetNumDof() == 0) {
3079  WorkMat.SetNullMatrix();
3080  return WorkMat;
3081  }
3082 
3083  FullSubMatrixHandler& WM = WorkMat.SetFull();
3084 
3085  /* Ridimensiona la sottomatrice in base alle esigenze */
3086  integer iNumRows = 0;
3087  integer iNumCols = 0;
3088  WorkSpaceDim(&iNumRows, &iNumCols);
3089  WM.ResizeReset(iNumRows - 6, 6);
3090 
3091  /* Recupera gli indici delle varie incognite */
3092  integer iNodeFirstPosIndex = pNode->iGetFirstPositionIndex();
3093  integer iFirstReactionIndex = iGetFirstIndex();
3094 
3095  /* Setta gli indici delle equazioni */
3096  for (int iCnt = 1; iCnt <= 6; iCnt++) {
3097  WM.PutColIndex(iCnt, iNodeFirstPosIndex + iCnt);
3098  }
3099 
3100  for (unsigned int iCnt = 1; iCnt <= nConstraints; iCnt++) {
3101  WM.PutRowIndex(iCnt, iFirstReactionIndex + iCnt);
3102  }
3103 
3104  /* Recupera i dati che servono */
3105  Vec3 fn(pNode->GetRCurr()*tilde_fn);
3106 
3107  Mat3x3 fnCross_Rch(fn.Cross(Rch)); // = [ b2 x ] * R1
3108 
3109  for (unsigned iCnt = 0 ; iCnt < nPosConstraints; iCnt++) {
3110  Vec3 vRch(Rch.GetVec(iPosIncid[iCnt]));
3111  Vec3 vfnCross_Rch(fnCross_Rch.GetVec(iPosIncid[iCnt]));
3112 
3113  /* Constraint, node */
3114  WM.AddT(1 + iCnt, 1, vRch);
3115  WM.AddT(3 + 1 + iCnt, 3 + 1, vfnCross_Rch);
3116  }
3117 
3118  for (unsigned iCnt = 0 ; iCnt < nRotConstraints; iCnt++) {
3119  Vec3 vRchr(Rchr.GetVec(iRotIncid[iCnt]));
3120 
3121  /* Constraint, node */
3122  WM.AddT(1 + nPosConstraints + iCnt, 3 + 1, vRchr);
3123  }
3124 
3125  return WorkMat;
3126 }
TotalPinJoint::Rchr
Mat3x3 Rchr
Definition: totalj.h:265
FullSubMatrixHandler::PutColIndex
void PutColIndex(integer iSubCol, integer iCol)
Definition: submat.h:325
TotalPinJoint::Rch
Mat3x3 Rch
Definition: totalj.h:264
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
TotalPinJoint::nRotConstraints
unsigned int nRotConstraints
Definition: totalj.h:288
TotalPinJoint::tilde_fn
Vec3 tilde_fn
Definition: totalj.h:266
Mat3x3::GetVec
Vec3 GetVec(unsigned short int i) const
Definition: matvec3.h:893
FullSubMatrixHandler::AddT
void AddT(integer iRow, integer iCol, const Vec3 &v)
Definition: submat.cc:227
TotalPinJoint::iPosIncid
unsigned int iPosIncid[3]
Definition: totalj.h:292
TotalPinJoint::pNode
const StructNode * pNode
Definition: totalj.h:262
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
FullSubMatrixHandler::ResizeReset
virtual void ResizeReset(integer, integer)
Definition: submat.cc:182
TotalPinJoint::iGetNumDof
virtual unsigned int iGetNumDof(void) const
Definition: totalj.h:342
TotalPinJoint::nPosConstraints
unsigned int nPosConstraints
Definition: totalj.h:287
FullSubMatrixHandler::PutRowIndex
void PutRowIndex(integer iSubRow, integer iRow)
Definition: submat.h:311
TotalPinJoint::iRotIncid
unsigned int iRotIncid[3]
Definition: totalj.h:293
TotalPinJoint::WorkSpaceDim
void WorkSpaceDim(integer *piNumRows, integer *piNumCols) const
Definition: totalj.h:389
DofOwnerOwner::iGetFirstIndex
virtual integer iGetFirstIndex(void) const
Definition: dofown.h:127
TotalPinJoint::nConstraints
unsigned int nConstraints
Definition: totalj.h:286
integer
long int integer
Definition: colamd.c:51

Here is the call graph for this function:

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

Implements Elem.

Definition at line 2936 of file totalj.cc.

References ASSERT, Vec3::Cross(), DEBUGCOUT, F, 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::MulMT(), nAgvConstraints, nConstraints, nPosConstraints, nRotConstraints, nVelConstraints, pNode, VectorHandler::PutCoef(), SubVectorHandler::PutRowIndex(), Rch, Rchr, RchrT, RchT, VectorHandler::Resize(), VectorHandler::ResizeReset(), RotManip::Rot(), VectorHandler::Sub(), ThetaDelta, ThetaDeltaRemnant, ThetaDrv, tilde_fn, tilde_Rnhr, tilde_Xc, RotManip::VecRot(), WorkSpaceDim(), and XDrv.

2940 {
2941  DEBUGCOUT("Entering TotalPinJoint::AssRes()" << std::endl);
2942 
2943  if (iGetNumDof() == 0) {
2944  WorkVec.Resize(0);
2945  return WorkVec;
2946  }
2947 
2948  /* Dimensiona e resetta la matrice di lavoro */
2949  integer iNumRows = 0;
2950  integer iNumCols = 0;
2951  WorkSpaceDim(&iNumRows, &iNumCols);
2952  WorkVec.ResizeReset(iNumRows);
2953 
2954  /* Indici */
2955  integer iNodeFirstMomIndex = pNode->iGetFirstMomentumIndex();
2956  integer iFirstReactionIndex = iGetFirstIndex();
2957 
2958  /* Indici dei nodi */
2959  for (int iCnt = 1; iCnt <= 6; iCnt++) {
2960  WorkVec.PutRowIndex(iCnt, iNodeFirstMomIndex + iCnt);
2961  }
2962 
2963  /* Indici del vincolo */
2964  for (unsigned int iCnt = 1; iCnt <= nConstraints; iCnt++) {
2965  WorkVec.PutRowIndex(6 + iCnt, iFirstReactionIndex + iCnt);
2966  }
2967 
2968  /* Get constraint reactions */
2969 
2970  for (unsigned iCnt = 0; iCnt < nPosConstraints; iCnt++) {
2971  F(iPosIncid[iCnt]) = XCurr(iFirstReactionIndex + 1 + iPosEqIndex[iCnt]);
2972  }
2973 
2974  for (unsigned iCnt = 0; iCnt < nRotConstraints; iCnt++) {
2975  M(iRotIncid[iCnt]) = XCurr(iFirstReactionIndex + 1 + iRotEqIndex[iCnt]);
2976  }
2977 
2978  for (unsigned iCnt = 0; iCnt < nVelConstraints; iCnt++) {
2979  F(iVelIncid[iCnt]) = XCurr(iFirstReactionIndex + 1 + iVelEqIndex[iCnt]);
2980  }
2981 
2982  for (unsigned iCnt = 0; iCnt < nAgvConstraints; iCnt++) {
2983  M(iAgvIncid[iCnt]) = XCurr(iFirstReactionIndex + 1 + iAgvEqIndex[iCnt]);
2984  }
2985 
2986  Vec3 fn(pNode->GetRCurr()*tilde_fn);
2987 
2988  Vec3 XDelta;
2989  if (nPosConstraints > 0) {
2990  XDelta = RchT*(pNode->GetXCurr() + fn) - tilde_Xc - XDrv.Get();
2991  }
2992 
2993  Vec3 VDelta;
2994  if (nVelConstraints > 0) {
2995  VDelta = RchT*(pNode->GetVCurr() - fn.Cross(pNode->GetWCurr())) - XDrv.Get();
2996  }
2997 
2998  if (nRotConstraints > 0) {
2999  Mat3x3 Rnhr = pNode->GetRCurr()*tilde_Rnhr;
3000 
3001  Vec3 ThetaDrvTmp(ThetaDrv.Get());
3002  if (nRotConstraints < 3) {
3003  for (int i = nRotConstraints; i < 3; i++) {
3004  // zero out unconstrained components of drive
3005  ThetaDrvTmp(iRotIncid[i]) = 0.;
3006  }
3007  // add remnant to make ThetaDelta as close to zero
3008  // as possible
3009  ThetaDrvTmp += ThetaDeltaRemnant;
3010  }
3011  Mat3x3 R0(RotManip::Rot(ThetaDrvTmp));
3012  Mat3x3 RDelta = RchrT*Rnhr.MulMT(R0);
3013  ThetaDelta = RotManip::VecRot(RDelta);
3014  }
3015 
3016  Vec3 WDelta;
3017  if (nAgvConstraints > 0) {
3018  WDelta = RchT*(pNode->GetWCurr()) - ThetaDrv.Get();
3019  }
3020 
3021  Vec3 FTmp(Rch*F);
3022  Vec3 MTmp(Rchr*M);
3023 
3024  /* Equilibrium, node 2 */
3025  WorkVec.Sub(1, FTmp);
3026  WorkVec.Sub(3 + 1, MTmp + fn.Cross(FTmp));
3027 
3028  /* Holonomic constraint equations are divided by dCoef */
3029  ASSERT(dCoef != 0.);
3030 
3031  /* Position constraint: */
3032  for (unsigned iCnt = 0; iCnt < nPosConstraints; iCnt++) {
3033  WorkVec.PutCoef(6 + 1 + iPosEqIndex[iCnt],
3034  -XDelta(iPosIncid[iCnt])/dCoef);
3035  }
3036 
3037  /* Rotation constraints: */
3038  for (unsigned iCnt = 0; iCnt < nRotConstraints; iCnt++) {
3039  WorkVec.PutCoef(6 + 1 + iRotEqIndex[iCnt],
3040  -ThetaDelta(iRotIncid[iCnt])/dCoef);
3041  }
3042 
3043  /* Linear Velocity Constraint */
3044  for (unsigned iCnt = 0; iCnt < nVelConstraints; iCnt++) {
3045  WorkVec.PutCoef(6 + 1 + iVelEqIndex[iCnt],
3046  -VDelta(iVelIncid[iCnt]));
3047  }
3048 
3049  /* Angular Velocity Constraint */
3050  for (unsigned iCnt = 0; iCnt < nAgvConstraints; iCnt++) {
3051  WorkVec.PutCoef(6 + 1 + iAgvEqIndex[iCnt],
3052  -WDelta(iAgvIncid[iCnt]));
3053  }
3054 
3055  return WorkVec;
3056 }
TotalPinJoint::iVelEqIndex
unsigned int iVelEqIndex[3]
Definition: totalj.h:299
TotalPinJoint::Rchr
Mat3x3 Rchr
Definition: totalj.h:265
Vec3::Cross
Vec3 Cross(const Vec3 &v) const
Definition: matvec3.h:218
TotalPinJoint::iAgvIncid
unsigned int iAgvIncid[3]
Definition: totalj.h:295
TotalPinJoint::Rch
Mat3x3 Rch
Definition: totalj.h:264
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
TotalPinJoint::nRotConstraints
unsigned int nRotConstraints
Definition: totalj.h:288
TotalPinJoint::nAgvConstraints
unsigned int nAgvConstraints
Definition: totalj.h:290
VectorHandler::Sub
virtual void Sub(integer iRow, const Vec3 &v)
Definition: vh.cc:78
TotalPinJoint::nVelConstraints
unsigned int nVelConstraints
Definition: totalj.h:289
TotalPinJoint::tilde_fn
Vec3 tilde_fn
Definition: totalj.h:266
RotManip::VecRot
Vec3 VecRot(const Mat3x3 &Phi)
Definition: Rot.cc:136
TotalPinJoint::iPosIncid
unsigned int iPosIncid[3]
Definition: totalj.h:292
TotalPinJoint::ThetaDeltaRemnant
Vec3 ThetaDeltaRemnant
Definition: totalj.h:314
TotalPinJoint::iVelIncid
unsigned int iVelIncid[3]
Definition: totalj.h:294
SubVectorHandler::PutRowIndex
virtual void PutRowIndex(integer iSubRow, integer iRow)=0
TotalPinJoint::iPosEqIndex
unsigned int iPosEqIndex[3]
Definition: totalj.h:297
TotalPinJoint::tilde_Rnhr
Mat3x3 tilde_Rnhr
Definition: totalj.h:268
TotalPinJoint::pNode
const StructNode * pNode
Definition: totalj.h:262
TotalPinJoint::iAgvEqIndex
unsigned int iAgvEqIndex[3]
Definition: totalj.h:300
DEBUGCOUT
#define DEBUGCOUT(msg)
Definition: myassert.h:232
TplDriveOwner::Get
T Get(const doublereal &dVar) const
Definition: tpldrive.h:109
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
TotalPinJoint::iRotEqIndex
unsigned int iRotEqIndex[3]
Definition: totalj.h:298
ASSERT
#define ASSERT(expression)
Definition: colamd.c:977
VectorHandler::PutCoef
virtual void PutCoef(integer iRow, const doublereal &dCoef)=0
StructDispNode::GetXCurr
virtual const Vec3 & GetXCurr(void) const
Definition: strnode.h:310
TotalPinJoint::tilde_Xc
Vec3 tilde_Xc
Definition: totalj.h:275
TotalPinJoint::ThetaDelta
Vec3 ThetaDelta
Definition: totalj.h:311
TotalPinJoint::iGetNumDof
virtual unsigned int iGetNumDof(void) const
Definition: totalj.h:342
TotalPinJoint::nPosConstraints
unsigned int nPosConstraints
Definition: totalj.h:287
TotalPinJoint::RchT
Mat3x3 RchT
Definition: totalj.h:274
StructDispNode::GetVCurr
virtual const Vec3 & GetVCurr(void) const
Definition: strnode.h:322
TotalPinJoint::iRotIncid
unsigned int iRotIncid[3]
Definition: totalj.h:293
TotalPinJoint::WorkSpaceDim
void WorkSpaceDim(integer *piNumRows, integer *piNumCols) const
Definition: totalj.h:389
Mat3x3::MulMT
Mat3x3 MulMT(const Mat3x3 &m) const
Definition: matvec3.cc:444
DofOwnerOwner::iGetFirstIndex
virtual integer iGetFirstIndex(void) const
Definition: dofown.h:127
TotalPinJoint::nConstraints
unsigned int nConstraints
Definition: totalj.h:286
TotalPinJoint::ThetaDrv
TplDriveOwner< Vec3 > ThetaDrv
Definition: totalj.h:282
integer
long int integer
Definition: colamd.c:51
TotalPinJoint::XDrv
TplDriveOwner< Vec3 > XDrv
Definition: totalj.h:278
VectorHandler::Resize
virtual void Resize(integer iNewSize)=0
TotalPinJoint::RchrT
Mat3x3 RchrT
Definition: totalj.h:276

Here is the call graph for this function:

SubVectorHandler & TotalPinJoint::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 3130 of file totalj.cc.

References InverseDynamics::ACCELERATION, ASSERT, Vec3::Cross(), DEBUGCOUT, TplDriveOwner< T >::Get(), StructNode::GetRCurr(), StructNode::GetWCurr(), StructDispNode::GetXCurr(), DofOwnerOwner::iGetFirstIndex(), iGetNumDof(), InverseDynamics::INVERSE_DYNAMICS, iPosEqIndex, iPosIncid, iRotEqIndex, iRotIncid, MBDYN_EXCEPT_ARGS, Mat3x3::MulMT(), Mat3x3::MulTV(), nConstraints, nPosConstraints, nRotConstraints, OmegaDrv, OmegaPDrv, pNode, InverseDynamics::POSITION, VectorHandler::PutCoef(), SubVectorHandler::PutRowIndex(), RchrT, RchT, VectorHandler::ResizeReset(), RotManip::Rot(), ThetaDelta, ThetaDeltaRemnant, ThetaDrv, tilde_fn, tilde_Rnhr, tilde_Xc, RotManip::VecRot(), InverseDynamics::VELOCITY, WorkSpaceDim(), XDrv, XPDrv, and XPPDrv.

3135 {
3136  DEBUGCOUT("Entering TotalPinJoint::AssRes()" << std::endl);
3137 
3138  if (iGetNumDof() == 0 || iOrder == InverseDynamics::INVERSE_DYNAMICS) {
3139  WorkVec.ResizeReset(0);
3140  return WorkVec;
3141  }
3142 
3143  /* Dimensiona e resetta la matrice di lavoro */
3144  integer iNumRows = 0;
3145  integer iNumCols = 0;
3146  WorkSpaceDim(&iNumRows, &iNumCols);
3147 
3148  /* original - node equations */
3149  WorkVec.ResizeReset(iNumRows - 6);
3150 
3151  /* Indici */
3152  integer iFirstReactionIndex = iGetFirstIndex();
3153 
3154  /* Indici del vincolo */
3155  for (unsigned int iCnt = 1; iCnt <= nConstraints; iCnt++) {
3156  WorkVec.PutRowIndex(iCnt, iFirstReactionIndex + iCnt);
3157  }
3158 
3159  switch (iOrder) {
3160  case InverseDynamics::POSITION: {
3161 
3162  /* Position constraint: */
3163  if (nPosConstraints > 0) {
3164  Vec3 fn(pNode->GetRCurr()*tilde_fn);
3165  Vec3 XDelta = RchT*(pNode->GetXCurr() + fn) - tilde_Xc - XDrv.Get();
3166 
3167  for (unsigned iCnt = 0; iCnt < nPosConstraints; iCnt++) {
3168  WorkVec.PutCoef(1 + iPosEqIndex[iCnt], -XDelta(iPosIncid[iCnt]));
3169  }
3170  }
3171 
3172  /* Rotation constraints: */
3173  if (nRotConstraints > 0) {
3174  Mat3x3 Rnhr = pNode->GetRCurr()*tilde_Rnhr;
3175  Vec3 ThetaDrvTmp(ThetaDrv.Get());
3176  if (nRotConstraints < 3) {
3177  for (int i = nRotConstraints; i < 3; i++) {
3178  // zero out unconstrained components of drive
3179  ThetaDrvTmp(iRotIncid[i]) = 0.;
3180  }
3181  // add remnant to make ThetaDelta as close to zero
3182  // as possible
3183  ThetaDrvTmp += ThetaDeltaRemnant;
3184  }
3185  Mat3x3 R0(RotManip::Rot(ThetaDrvTmp));
3186  Mat3x3 RDelta = RchrT*Rnhr.MulMT(R0);
3187  ThetaDelta = RotManip::VecRot(RDelta);
3188 
3189  for (unsigned iCnt = 0; iCnt < nRotConstraints; iCnt++) {
3190  WorkVec.PutCoef(1 + iRotEqIndex[iCnt], -(ThetaDelta(iRotIncid[iCnt])));
3191  }
3192  }
3193  } break;
3194 
3195  case InverseDynamics::VELOCITY: {
3196  /* Position constraint derivative */
3197  if (nPosConstraints > 0) {
3198  // First derivative of regular AssRes' lower block
3199  Vec3 Tmp = XPDrv.Get();
3200 
3201  for (unsigned iCnt = 0; iCnt < nPosConstraints; iCnt++) {
3202  WorkVec.PutCoef(1 + iPosEqIndex[iCnt], Tmp(iPosIncid[iCnt]));
3203  }
3204  }
3205 
3206  /* Rotation constraint derivative */
3207  if (nRotConstraints > 0) {
3208  Mat3x3 Rnhr = pNode->GetRCurr()*tilde_Rnhr;
3209  Mat3x3 R0 = RotManip::Rot(ThetaDrv.Get());
3210  Mat3x3 RDelta = RchrT*Rnhr.MulMT(R0);
3211 
3212  /* This name is only for clarity... */
3213  Vec3 Tmp = RDelta * OmegaDrv.Get();
3214 
3215  for (unsigned iCnt = 0; iCnt < nRotConstraints; iCnt++) {
3216  WorkVec.PutCoef(1 + iRotEqIndex[iCnt], Tmp(iRotIncid[iCnt]));
3217  }
3218  }
3219  } break;
3220 
3221  case InverseDynamics::ACCELERATION: {
3222  /* Position constraint second derivative */
3223  if (nPosConstraints > 0) {
3224  // Second derivative of regular AssRes' lower block
3225  Vec3 Tmp = XPPDrv.Get();
3226 
3227  Vec3 fn(pNode->GetRCurr()*tilde_fn);
3228 
3229  Tmp -= pNode->GetWCurr().Cross(pNode->GetWCurr().Cross(fn));
3230 
3231  for (unsigned iCnt = 0; iCnt < nPosConstraints; iCnt++) {
3232  WorkVec.PutCoef(1 + iPosEqIndex[iCnt], Tmp(iPosIncid[iCnt]));
3233  }
3234  }
3235 
3236  /* Rotation constraint second derivative */
3237  if (nRotConstraints > 0) {
3238  Mat3x3 Rnhr = pNode->GetRCurr()*tilde_Rnhr;
3239  Mat3x3 R0 = RotManip::Rot(ThetaDrv.Get());
3240  Mat3x3 RDelta = RchrT*Rnhr.MulMT(R0);
3241 
3242  Vec3 Tmp = R0.MulTV(OmegaDrv.Get());
3243  Vec3 Tmp2 = Rnhr * Tmp;
3244 
3245  Tmp = pNode->GetWCurr().Cross(Tmp2);
3246 
3247  Tmp2 = RchrT*Tmp;
3248  Tmp2 += RDelta * OmegaPDrv.Get();
3249 
3250  for (unsigned iCnt = 0; iCnt < nRotConstraints; iCnt++) {
3251  WorkVec.PutCoef(1 + iRotEqIndex[iCnt], Tmp2(iRotIncid[iCnt]));
3252  }
3253  }
3254  } break;
3255 
3256  default:
3257  ASSERT(0);
3258  throw ErrGeneric(MBDYN_EXCEPT_ARGS);
3259  }
3260 
3261  return WorkVec;
3262 }
Vec3::Cross
Vec3 Cross(const Vec3 &v) const
Definition: matvec3.h:218
MBDYN_EXCEPT_ARGS
#define MBDYN_EXCEPT_ARGS
Definition: except.h:63
TotalPinJoint::XPDrv
TplDriveOwner< Vec3 > XPDrv
Definition: totalj.h:279
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
TotalPinJoint::nRotConstraints
unsigned int nRotConstraints
Definition: totalj.h:288
TotalPinJoint::tilde_fn
Vec3 tilde_fn
Definition: totalj.h:266
RotManip::VecRot
Vec3 VecRot(const Mat3x3 &Phi)
Definition: Rot.cc:136
TotalPinJoint::iPosIncid
unsigned int iPosIncid[3]
Definition: totalj.h:292
TotalPinJoint::ThetaDeltaRemnant
Vec3 ThetaDeltaRemnant
Definition: totalj.h:314
Mat3x3::MulTV
Vec3 MulTV(const Vec3 &v) const
Definition: matvec3.cc:482
SubVectorHandler::PutRowIndex
virtual void PutRowIndex(integer iSubRow, integer iRow)=0
TotalPinJoint::iPosEqIndex
unsigned int iPosEqIndex[3]
Definition: totalj.h:297
TotalPinJoint::OmegaPDrv
TplDriveOwner< Vec3 > OmegaPDrv
Definition: totalj.h:284
TotalPinJoint::tilde_Rnhr
Mat3x3 tilde_Rnhr
Definition: totalj.h:268
TotalPinJoint::OmegaDrv
TplDriveOwner< Vec3 > OmegaDrv
Definition: totalj.h:283
TotalPinJoint::pNode
const StructNode * pNode
Definition: totalj.h:262
DEBUGCOUT
#define DEBUGCOUT(msg)
Definition: myassert.h:232
TplDriveOwner::Get
T Get(const doublereal &dVar) const
Definition: tpldrive.h:109
RotManip::Rot
Mat3x3 Rot(const Vec3 &phi)
Definition: Rot.cc:62
TotalPinJoint::XPPDrv
TplDriveOwner< Vec3 > XPPDrv
Definition: totalj.h:280
StructNode::GetWCurr
virtual const Vec3 & GetWCurr(void) const
Definition: strnode.h:1030
TotalPinJoint::iRotEqIndex
unsigned int iRotEqIndex[3]
Definition: totalj.h:298
ASSERT
#define ASSERT(expression)
Definition: colamd.c:977
VectorHandler::PutCoef
virtual void PutCoef(integer iRow, const doublereal &dCoef)=0
StructDispNode::GetXCurr
virtual const Vec3 & GetXCurr(void) const
Definition: strnode.h:310
TotalPinJoint::tilde_Xc
Vec3 tilde_Xc
Definition: totalj.h:275
TotalPinJoint::ThetaDelta
Vec3 ThetaDelta
Definition: totalj.h:311
TotalPinJoint::iGetNumDof
virtual unsigned int iGetNumDof(void) const
Definition: totalj.h:342
TotalPinJoint::nPosConstraints
unsigned int nPosConstraints
Definition: totalj.h:287
TotalPinJoint::RchT
Mat3x3 RchT
Definition: totalj.h:274
TotalPinJoint::iRotIncid
unsigned int iRotIncid[3]
Definition: totalj.h:293
TotalPinJoint::WorkSpaceDim
void WorkSpaceDim(integer *piNumRows, integer *piNumCols) const
Definition: totalj.h:389
Mat3x3::MulMT
Mat3x3 MulMT(const Mat3x3 &m) const
Definition: matvec3.cc:444
DofOwnerOwner::iGetFirstIndex
virtual integer iGetFirstIndex(void) const
Definition: dofown.h:127
TotalPinJoint::nConstraints
unsigned int nConstraints
Definition: totalj.h:286
TotalPinJoint::ThetaDrv
TplDriveOwner< Vec3 > ThetaDrv
Definition: totalj.h:282
integer
long int integer
Definition: colamd.c:51
TotalPinJoint::XDrv
TplDriveOwner< Vec3 > XDrv
Definition: totalj.h:278
TotalPinJoint::RchrT
Mat3x3 RchrT
Definition: totalj.h:276

Here is the call graph for this function:

bool TotalPinJoint::bInverseDynamics ( void  ) const
virtual

Reimplemented from Elem.

Definition at line 3060 of file totalj.cc.

3061 {
3062  return true;
3063 }
std::ostream & TotalPinJoint::DescribeDof ( std::ostream &  out,
const char *  prefix = "",
bool  bInitial = false 
) const
virtual

Reimplemented from Elem.

Definition at line 2208 of file totalj.cc.

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

2210 {
2211  integer iIndex = iGetFirstIndex();
2212 
2213  if (nPosConstraints > 0 || nVelConstraints > 0) {
2214  out << prefix << iIndex + 1;
2215  if (nPosConstraints + nVelConstraints > 1) {
2216  out << "->" << iIndex + nPosConstraints + nVelConstraints;
2217  }
2218  out << ": ";
2219  }
2220  out << "reaction force(s) [";
2221 
2222  for (unsigned int i = 0, cnt = 0; i < 3; i++) {
2223  if (bPosActive[i] || bVelActive[i]) {
2224  cnt++;
2225  if (cnt > 1) {
2226  out << ",";
2227  }
2228  out << "F" << idx2xyz[i];
2229  }
2230  }
2231  out << "]" << std::endl;
2232 
2233  if (nRotConstraints > 0 || nAgvConstraints > 0) {
2234  out << prefix << iIndex + nPosConstraints + nVelConstraints + 1;
2235  if (nRotConstraints + nAgvConstraints > 1) {
2236  out << "->" << iIndex + nConstraints;
2237  }
2238  out << ": ";
2239  }
2240  out << "reaction couple(s) [";
2241 
2242  for (unsigned int i = 0, cnt = 0; i < 3; i++) {
2243  if (bRotActive[i] || bAgvActive[i]) {
2244  cnt++;
2245  if (cnt > 1) {
2246  out << ",";
2247  }
2248  out << "m" << idx2xyz[i];
2249  }
2250  }
2251  out << "]" << std::endl;
2252 
2253  if (bInitial) {
2254  iIndex += nConstraints;
2255 
2256  if (nPosConstraints > 0) {
2257  out << prefix << iIndex + 1;
2258  if (nPosConstraints > 1) {
2259  out << "->" << iIndex + nPosConstraints;
2260  }
2261  out << ": ";
2262  }
2263  out << "reaction force(s) derivative(s) [";
2264 
2265  for (unsigned int i = 0, cnt = 0; i < 3; i++) {
2266  if (bPosActive[i]) {
2267  cnt++;
2268  if (cnt > 1) {
2269  out << ",";
2270  }
2271  out << "FP" << idx2xyz[i];
2272  }
2273  }
2274  out << "]" << std::endl;
2275 
2276 
2277  if (nRotConstraints > 0) {
2278  out << prefix << iIndex + nPosConstraints + 1;
2279  if (nRotConstraints > 1) {
2280  out << "->" << iIndex + nConstraints;
2281  }
2282  out << ": ";
2283  }
2284  out << "reaction couple(s) derivative(s) [";
2285 
2286  for (unsigned int i = 0, cnt = 0; i < 3; i++) {
2287  if (bRotActive[i]) {
2288  cnt++;
2289  if (cnt > 1) {
2290  out << ",";
2291  }
2292  out << "mP" << idx2xyz[i];
2293  }
2294  }
2295  out << "]" << std::endl;
2296  }
2297 
2298  return out;
2299 }
TotalPinJoint::bPosActive
bool bPosActive[3]
Definition: totalj.h:269
TotalPinJoint::bRotActive
bool bRotActive[3]
Definition: totalj.h:270
TotalPinJoint::nRotConstraints
unsigned int nRotConstraints
Definition: totalj.h:288
TotalPinJoint::nAgvConstraints
unsigned int nAgvConstraints
Definition: totalj.h:290
TotalPinJoint::nVelConstraints
unsigned int nVelConstraints
Definition: totalj.h:289
TotalPinJoint::bVelActive
bool bVelActive[3]
Definition: totalj.h:271
TotalPinJoint::nPosConstraints
unsigned int nPosConstraints
Definition: totalj.h:287
TotalPinJoint::bAgvActive
bool bAgvActive[3]
Definition: totalj.h:272
DofOwnerOwner::iGetFirstIndex
virtual integer iGetFirstIndex(void) const
Definition: dofown.h:127
TotalPinJoint::nConstraints
unsigned int nConstraints
Definition: totalj.h:286
integer
long int integer
Definition: colamd.c:51
idx2xyz
static const char idx2xyz[]
Definition: totalj.cc:47

Here is the call graph for this function:

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

Reimplemented from Elem.

Definition at line 2302 of file totalj.cc.

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

2304 {
2305  int ndof = 1;
2306  if (i == -1) {
2307  if (bInitial) {
2308  ndof = iGetInitialNumDof();
2309 
2310  } else {
2311  ndof = iGetNumDof();
2312  }
2313  }
2314  desc.resize(ndof);
2315 
2316  std::ostringstream os;
2317  os << "TotalPinJoint(" << GetLabel() << ")";
2318 
2319  if (i == -1) {
2320  std::string name(os.str());
2321  unsigned int cnt = 0;
2322 
2323  if (nPosConstraints > 0 || nVelConstraints > 0) {
2324  for (unsigned int i = 0; i < 3; i++) {
2325  if (bPosActive[i] || bVelActive[i]) {
2326  os.str(name);
2327  os.seekp(0, std::ios_base::end);
2328  os << ": dof(" << cnt + 1 << ") F" << idx2xyz[i];
2329  desc[cnt] = os.str();
2330  cnt++;
2331  }
2332  }
2333  }
2334 
2335  if (nRotConstraints > 0 || nAgvConstraints > 0) {
2336  for (unsigned int i = 0; i < 3; i++) {
2337  if (bRotActive[i] || bAgvActive[i]) {
2338  os.str(name);
2339  os.seekp(0, std::ios_base::end);
2340  os << ": dof(" << cnt + 1 << ") M" << idx2xyz[i];
2341  desc[cnt] = os.str();
2342  cnt++;
2343  }
2344  }
2345  }
2346 
2347  if (bInitial) {
2348  if (nPosConstraints > 0) {
2349  for (unsigned int i = 0; i < 3; i++) {
2350  if (bPosActive[i]) {
2351  os.str(name);
2352  os.seekp(0, std::ios_base::end);
2353  os << ": dof(" << cnt + 1 << ") FP" << idx2xyz[i];
2354  desc[cnt] = os.str();
2355  cnt++;
2356  }
2357  }
2358  }
2359 
2360  if (nRotConstraints > 0) {
2361  for (unsigned int i = 0; i < 3; i++) {
2362  if (bRotActive[i]) {
2363  os.str(name);
2364  os.seekp(0, std::ios_base::end);
2365  os << ": dof(" << cnt + 1 << ") MP" << idx2xyz[i];
2366  desc[cnt] = os.str();
2367  cnt++;
2368  }
2369  }
2370  }
2371  }
2372 
2373  ASSERT(cnt == ndof);
2374 
2375  } else {
2376  os << ": dof(" << i + 1 << ")";
2377  desc[0] = os.str();
2378  }
2379 }
TotalPinJoint::bPosActive
bool bPosActive[3]
Definition: totalj.h:269
TotalPinJoint::bRotActive
bool bRotActive[3]
Definition: totalj.h:270
TotalPinJoint::nRotConstraints
unsigned int nRotConstraints
Definition: totalj.h:288
TotalPinJoint::nAgvConstraints
unsigned int nAgvConstraints
Definition: totalj.h:290
TotalPinJoint::nVelConstraints
unsigned int nVelConstraints
Definition: totalj.h:289
TotalPinJoint::bVelActive
bool bVelActive[3]
Definition: totalj.h:271
ASSERT
#define ASSERT(expression)
Definition: colamd.c:977
TotalPinJoint::iGetInitialNumDof
virtual unsigned int iGetInitialNumDof(void) const
Definition: totalj.h:434
TotalPinJoint::iGetNumDof
virtual unsigned int iGetNumDof(void) const
Definition: totalj.h:342
TotalPinJoint::nPosConstraints
unsigned int nPosConstraints
Definition: totalj.h:287
TotalPinJoint::bAgvActive
bool bAgvActive[3]
Definition: totalj.h:272
WithLabel::GetLabel
unsigned int GetLabel(void) const
Definition: withlab.cc:62
idx2xyz
static const char idx2xyz[]
Definition: totalj.cc:47

Here is the call graph for this function:

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

Reimplemented from Elem.

Definition at line 2382 of file totalj.cc.

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

2384 {
2385  integer iIndex = iGetFirstIndex();
2386 
2387  if (nPosConstraints > 0 || nVelConstraints > 0) {
2388  out << prefix << iIndex + 1;
2389  if (nPosConstraints + nVelConstraints > 1) {
2390  out << "->" << iIndex + nPosConstraints + nVelConstraints;
2391  }
2392  out << ": "
2393  "position/velocity constraint(s) [";
2394 
2395  for (unsigned int i = 0, cnt = 0; i < 3; i++) {
2396  if (bPosActive[i] || bVelActive[i]) {
2397  cnt++;
2398  if (cnt > 1) {
2399  out << ",";
2400  }
2401 
2402  if (bPosActive[i]) {
2403  out << "P" << idx2xyz[i] << "=P" << idx2xyz[i] << "0";
2404  } else {
2405  out << "V" << idx2xyz[i] << "=V" << idx2xyz[i] << "0";
2406  }
2407  }
2408  }
2409 
2410  out << "]" << std::endl;
2411  }
2412 
2413  if (nRotConstraints > 0 || nAgvConstraints > 0) {
2414  out << prefix << iIndex + nPosConstraints + nVelConstraints + 1;
2415  if (nRotConstraints + nAgvConstraints > 1) {
2416  out << "->" << iIndex + nConstraints ;
2417  }
2418  out << ": "
2419  "orientation/angular velocity constraint(s) [";
2420 
2421  for (unsigned int i = 0, cnt = 0; i < 3; i++) {
2422  if (bRotActive[i] || bAgvActive[i]) {
2423  cnt++;
2424  if (cnt > 1) {
2425  out << ",";
2426  }
2427 
2428  if (bRotActive[i]) {
2429  out << "theta" << idx2xyz[i] << "=theta" << idx2xyz[i] << "0";
2430  } else {
2431  out << "W" << idx2xyz[i] << "=W" << idx2xyz[i] << "0";
2432  }
2433  }
2434  }
2435 
2436  out << "]" << std::endl;
2437  }
2438 
2439  if (bInitial) {
2440  iIndex += nConstraints;
2441 
2442  if (nPosConstraints > 0) {
2443  out << prefix << iIndex + 1;
2444  out << "->" << iIndex + nPosConstraints;
2445  out << ": "
2446  "velocity constraint(s) [";
2447 
2448  for (unsigned int i = 0, cnt = 0; i < 3; i++) {
2449  if (bPosActive[i]) {
2450  cnt++;
2451  if (cnt > 1) {
2452  out << ",";
2453  }
2454  out << "v" << idx2xyz[i] << "=v" << idx2xyz[i] << "0";
2455  }
2456  }
2457 
2458  out << "]" << std::endl;
2459  }
2460 
2461  if (nRotConstraints > 0) {
2462  out << prefix << iIndex + nPosConstraints + 1;
2463  if (nRotConstraints > 1) {
2464  out << "->" << iIndex + nConstraints ;
2465  }
2466  out << ": "
2467  "angular velocity constraint(s) [";
2468 
2469  for (unsigned int i = 0, cnt = 0; i < 3; i++) {
2470  if (bRotActive[i]) {
2471  cnt++;
2472  if (cnt > 1) {
2473  out << ",";
2474  }
2475  out << "w" << idx2xyz[i] << "=w" << idx2xyz[i] << "0";
2476  }
2477  }
2478 
2479  out << "]" << std::endl;
2480  }
2481  }
2482 
2483  return out;
2484 }
TotalPinJoint::bPosActive
bool bPosActive[3]
Definition: totalj.h:269
TotalPinJoint::bRotActive
bool bRotActive[3]
Definition: totalj.h:270
TotalPinJoint::nRotConstraints
unsigned int nRotConstraints
Definition: totalj.h:288
TotalPinJoint::nAgvConstraints
unsigned int nAgvConstraints
Definition: totalj.h:290
TotalPinJoint::nVelConstraints
unsigned int nVelConstraints
Definition: totalj.h:289
TotalPinJoint::bVelActive
bool bVelActive[3]
Definition: totalj.h:271
TotalPinJoint::nPosConstraints
unsigned int nPosConstraints
Definition: totalj.h:287
TotalPinJoint::bAgvActive
bool bAgvActive[3]
Definition: totalj.h:272
DofOwnerOwner::iGetFirstIndex
virtual integer iGetFirstIndex(void) const
Definition: dofown.h:127
TotalPinJoint::nConstraints
unsigned int nConstraints
Definition: totalj.h:286
integer
long int integer
Definition: colamd.c:51
idx2xyz
static const char idx2xyz[]
Definition: totalj.cc:47

Here is the call graph for this function:

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

Reimplemented from Elem.

Definition at line 2487 of file totalj.cc.

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

2489 {
2490  int ndof = 1;
2491  if (i == -1) {
2492  if (bInitial) {
2493  ndof = iGetInitialNumDof();
2494 
2495  } else {
2496  ndof = iGetNumDof();
2497  }
2498  }
2499  desc.resize(ndof);
2500 
2501  std::ostringstream os;
2502  os << "TotalPinJoint(" << GetLabel() << ")";
2503 
2504  if (i == -1) {
2505  std::string name(os.str());
2506  unsigned int cnt = 0;
2507 
2508  if (nPosConstraints > 0 || nVelConstraints > 0) {
2509  for (unsigned int i = 0; i < 3; i++) {
2510  if (bPosActive[i]) {
2511  os.str(name);
2512  os.seekp(0, std::ios_base::end);
2513  os << ": equation(" << cnt + 1 << ") P" << idx2xyz[i] << "1=P" << idx2xyz[i] << "2";
2514  desc[cnt] = os.str();
2515  cnt++;
2516 
2517  } else if (bVelActive[i]) {
2518  os.str(name);
2519  os.seekp(0, std::ios_base::end);
2520  os << ": equation(" << cnt + 1 << ") V" << idx2xyz[i] << "1=V" << idx2xyz[i] << "2";
2521  desc[cnt] = os.str();
2522  cnt++;
2523  }
2524  }
2525  }
2526 
2527  if (nRotConstraints > 0 || nAgvConstraints > 0) {
2528  for (unsigned int i = 0; i < 3; i++) {
2529  if (bRotActive[i]) {
2530  os.str(name);
2531  os.seekp(0, std::ios_base::end);
2532  os << ": equation(" << cnt + 1 << ") theta" << idx2xyz[i] << "1=theta" << idx2xyz[i] << "2";
2533  desc[cnt] = os.str();
2534  cnt++;
2535 
2536  } else if (bAgvActive[i]) {
2537  os.str(name);
2538  os.seekp(0, std::ios_base::end);
2539  os << ": equation(" << cnt + 1 << ") W" << idx2xyz[i] << "1=W" << idx2xyz[i] << "2";
2540  desc[cnt] = os.str();
2541  cnt++;
2542  }
2543  }
2544  }
2545 
2546  if (bInitial) {
2547  if (nPosConstraints > 0) {
2548  for (unsigned int i = 0; i < 3; i++) {
2549  if (bPosActive[i]) {
2550  os.str(name);
2551  os.seekp(0, std::ios_base::end);
2552  os << ": equation(" << cnt + 1 << ") v" << idx2xyz[i] << "1=v" << idx2xyz[i] << "2";
2553  desc[cnt] = os.str();
2554  cnt++;
2555  }
2556  }
2557  }
2558 
2559  if (nRotConstraints > 0) {
2560  for (unsigned int i = 0; i < 3; i++) {
2561  if (bRotActive[i]) {
2562  os.str(name);
2563  os.seekp(0, std::ios_base::end);
2564  os << ": equation(" << cnt + 1 << ") w" << idx2xyz[i] << "1=w" << idx2xyz[i] << "2";
2565  desc[cnt] = os.str();
2566  cnt++;
2567  }
2568  }
2569  }
2570  }
2571 
2572  ASSERT(cnt == ndof);
2573 
2574  } else {
2575  os << ": equation(" << i + 1 << ")";
2576  desc[0] = os.str();
2577  }
2578 }
TotalPinJoint::bPosActive
bool bPosActive[3]
Definition: totalj.h:269
TotalPinJoint::bRotActive
bool bRotActive[3]
Definition: totalj.h:270
TotalPinJoint::nRotConstraints
unsigned int nRotConstraints
Definition: totalj.h:288
TotalPinJoint::nAgvConstraints
unsigned int nAgvConstraints
Definition: totalj.h:290
TotalPinJoint::nVelConstraints
unsigned int nVelConstraints
Definition: totalj.h:289
TotalPinJoint::bVelActive
bool bVelActive[3]
Definition: totalj.h:271
ASSERT
#define ASSERT(expression)
Definition: colamd.c:977
TotalPinJoint::iGetInitialNumDof
virtual unsigned int iGetInitialNumDof(void) const
Definition: totalj.h:434
TotalPinJoint::iGetNumDof
virtual unsigned int iGetNumDof(void) const
Definition: totalj.h:342
TotalPinJoint::nPosConstraints
unsigned int nPosConstraints
Definition: totalj.h:287
TotalPinJoint::bAgvActive
bool bAgvActive[3]
Definition: totalj.h:272
WithLabel::GetLabel
unsigned int GetLabel(void) const
Definition: withlab.cc:62
idx2xyz
static const char idx2xyz[]
Definition: totalj.cc:47

Here is the call graph for this function:

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

Reimplemented from SimulationEntity.

Definition at line 3675 of file totalj.cc.

References ASSERT, bPosActive, bRotActive, Vec3::Cross(), F, TplDriveOwner< T >::Get(), StructNode::GetRCurr(), StructDispNode::GetVCurr(), StructNode::GetWCurr(), StructDispNode::GetXCurr(), M, pNode, RchrT, RchT, ThetaDeltaTrue, ThetaDrv, tilde_fn, tilde_Rnhr, tilde_Xc, Unwrap(), RotManip::VecRot(), and XDrv.

3676 {
3677  switch (i) {
3678  case 1:
3679  case 2:
3680  case 3: {
3681  Vec3 x(RchT*(pNode->GetXCurr() + pNode->GetRCurr()*tilde_fn) - tilde_Xc);
3682  return x(i);
3683  }
3684 
3685  case 4:
3686  case 5:
3687  case 6: {
3688  Vec3 Theta(Unwrap(ThetaDeltaTrue, RotManip::VecRot(RchT*pNode->GetRCurr()*tilde_Rnhr)));
3689  return Theta(i - 3);
3690  }
3691 
3692  case 7:
3693  case 8:
3694  case 9:
3695  return F(i - 6);
3696 
3697  case 10:
3698  case 11:
3699  case 12:
3700  return M(i - 9);
3701 
3702  case 13:
3703  case 14:
3704  case 15:
3705  if (!bPosActive[i - 13]) {
3706  return 0.;
3707  }
3708  return XDrv.Get()(i - 12);
3709 
3710  case 16:
3711  case 17:
3712  case 18:
3713  if (!bRotActive[i - 16]) {
3714  return 0.;
3715  }
3716  return ThetaDrv.Get()(i - 15);
3717 
3718  case 19:
3719  case 20:
3720  case 21:
3721  {
3722  Vec3 v(RchrT*(pNode->GetVCurr() + pNode->GetWCurr().Cross(pNode->GetRCurr()*tilde_fn)));
3723 
3724  return v(i-18);
3725  }
3726 
3727  case 22:
3728  case 23:
3729  case 24:
3730  {
3731  Vec3 W(RchrT*pNode->GetWCurr());
3732  return W(i-21);
3733  }
3734 
3735  default:
3736  ASSERT(0);
3737  }
3738 
3739  return 0.;
3740 }
Vec3::Cross
Vec3 Cross(const Vec3 &v) const
Definition: matvec3.h:218
TotalPinJoint::bPosActive
bool bPosActive[3]
Definition: totalj.h:269
Vec3
Definition: matvec3.h:98
StructNode::GetRCurr
virtual const Mat3x3 & GetRCurr(void) const
Definition: strnode.h:1012
TotalPinJoint::bRotActive
bool bRotActive[3]
Definition: totalj.h:270
TotalPinJoint::tilde_fn
Vec3 tilde_fn
Definition: totalj.h:266
RotManip::VecRot
Vec3 VecRot(const Mat3x3 &Phi)
Definition: Rot.cc:136
TotalPinJoint::tilde_Rnhr
Mat3x3 tilde_Rnhr
Definition: totalj.h:268
TotalPinJoint::pNode
const StructNode * pNode
Definition: totalj.h:262
TplDriveOwner::Get
T Get(const doublereal &dVar) const
Definition: tpldrive.h:109
StructNode::GetWCurr
virtual const Vec3 & GetWCurr(void) const
Definition: strnode.h:1030
ASSERT
#define ASSERT(expression)
Definition: colamd.c:977
StructDispNode::GetXCurr
virtual const Vec3 & GetXCurr(void) const
Definition: strnode.h:310
TotalPinJoint::tilde_Xc
Vec3 tilde_Xc
Definition: totalj.h:275
Unwrap
Vec3 Unwrap(const Vec3 &vPrev, const Vec3 &v)
Definition: matvec3.cc:1089
TotalPinJoint::RchT
Mat3x3 RchT
Definition: totalj.h:274
StructDispNode::GetVCurr
virtual const Vec3 & GetVCurr(void) const
Definition: strnode.h:322
TotalPinJoint::ThetaDeltaTrue
Vec3 ThetaDeltaTrue
Definition: totalj.h:315
TotalPinJoint::ThetaDrv
TplDriveOwner< Vec3 > ThetaDrv
Definition: totalj.h:282
TotalPinJoint::XDrv
TplDriveOwner< Vec3 > XDrv
Definition: totalj.h:278
TotalPinJoint::RchrT
Mat3x3 RchrT
Definition: totalj.h:276

Here is the call graph for this function:

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

Reimplemented from Elem.

Definition at line 461 of file totalj.h.

References pNode.

461  {
462  connectedNodes.resize(1);
463  connectedNodes[0] = pNode;
464  };
TotalPinJoint::pNode
const StructNode * pNode
Definition: totalj.h:262
DofOrder::Order TotalPinJoint::GetDofType ( unsigned int  i) const
inlinevirtual

Reimplemented from Elem.

Definition at line 367 of file totalj.h.

References DofOrder::ALGEBRAIC, ASSERT, and nConstraints.

367  {
368  ASSERT(i >= 0 && i < nConstraints);
369  return DofOrder::ALGEBRAIC;
370  };
ASSERT
#define ASSERT(expression)
Definition: colamd.c:977
TotalPinJoint::nConstraints
unsigned int nConstraints
Definition: totalj.h:286
DofOrder::Order TotalPinJoint::GetEqType ( unsigned int  i) const
virtual

Reimplemented from SimulationEntity.

Definition at line 3284 of file totalj.cc.

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

3285 {
3286  ASSERTMSGBREAK(i < iGetNumDof(),
3287  "INDEX ERROR in TotalPinJoint::GetEqType");
3288 
3289  return DofOrder::ALGEBRAIC;
3290 }
ASSERTMSGBREAK
#define ASSERTMSGBREAK(expr, msg)
Definition: myassert.h:222
TotalPinJoint::iGetNumDof
virtual unsigned int iGetNumDof(void) const
Definition: totalj.h:342

Here is the call graph for this function:

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

Implements Joint.

Definition at line 337 of file totalj.h.

References Joint::TOTALPINJOINT.

337  {
338  return Joint::TOTALPINJOINT;
339  };
virtual unsigned int TotalPinJoint::iGetInitialNumDof ( void  ) const
inlinevirtual

Implements SubjectToInitialAssembly.

Definition at line 434 of file totalj.h.

References nConstraints.

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

434  {
435  return 2*nConstraints;
436  };
TotalPinJoint::nConstraints
unsigned int nConstraints
Definition: totalj.h:286
virtual unsigned int TotalPinJoint::iGetNumDof ( void  ) const
inlinevirtual

Reimplemented from Elem.

Definition at line 342 of file totalj.h.

References nConstraints.

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

342  {
343  return nConstraints;
344  };
TotalPinJoint::nConstraints
unsigned int nConstraints
Definition: totalj.h:286
unsigned int TotalPinJoint::iGetNumPrivData ( void  ) const
virtual

Reimplemented from SimulationEntity.

Definition at line 3600 of file totalj.cc.

3601 {
3602  return 24;
3603 }
unsigned int TotalPinJoint::iGetPrivDataIdx ( const char *  s) const
virtual

Reimplemented from SimulationEntity.

Definition at line 3606 of file totalj.cc.

References ASSERT.

3607 {
3608  ASSERT(s != NULL);
3609 
3610  if (strlen(s) != 2) {
3611  return 0;
3612  }
3613 
3614  unsigned int off = 0;
3615 
3616  switch (s[0]) {
3617  case 'p':
3618  /* relative position */
3619  break;
3620 
3621  case 'r':
3622  /* relative orientation */
3623  off += 3;
3624  break;
3625 
3626  case 'F':
3627  /* force */
3628  off += 6;
3629  break;
3630 
3631  case 'M':
3632  /* moment */
3633  off += 9;
3634  break;
3635 
3636  case 'd':
3637  /* imposed relative position */
3638  off += 12;
3639  break;
3640 
3641  case 't':
3642  /* imposed relative orientation */
3643  off += 15;
3644  break;
3645 
3646  case 'v':
3647  /* relative linear velocity */
3648  off += 18;
3649  break;
3650 
3651  case 'w':
3652  /* relative angular velocity */
3653  off += 21;
3654  break;
3655 
3656  default:
3657  return 0;
3658  }
3659 
3660  switch (s[1]) {
3661  case 'x':
3662  return off + 1;
3663 
3664  case 'y':
3665  return off + 2;
3666 
3667  case 'z':
3668  return off + 3;
3669  }
3670 
3671  return 0;
3672 }
ASSERT
#define ASSERT(expression)
Definition: colamd.c:977
VariableSubMatrixHandler & TotalPinJoint::InitialAssJac ( VariableSubMatrixHandler &  WorkMat,
const VectorHandler &  XCurr 
)
virtual

Implements SubjectToInitialAssembly.

Definition at line 3367 of file totalj.cc.

References FullSubMatrixHandler::Add(), FullSubMatrixHandler::AddT(), Vec3::Cross(), F, StructNode::GetRCurr(), StructDispNode::GetVCurr(), Mat3x3::GetVec(), StructNode::GetWCurr(), DofOwnerOwner::iGetFirstIndex(), StructDispNode::iGetFirstPositionIndex(), iGetInitialNumDof(), InitialWorkSpaceDim(), iPosIncid, iRotIncid, MatCrossCross, nConstraints, nPosConstraints, nRotConstraints, pNode, FullSubMatrixHandler::PutColIndex(), FullSubMatrixHandler::PutRowIndex(), Rch, Rchr, FullSubMatrixHandler::ResizeReset(), VariableSubMatrixHandler::SetFull(), VariableSubMatrixHandler::SetNullMatrix(), tilde_fn, and Zero3.

3369 {
3370  if (iGetInitialNumDof() == 0) {
3371  WorkMat.SetNullMatrix();
3372  return WorkMat;
3373  }
3374 
3375  /* Per ora usa la matrice piena; eventualmente si puo'
3376  * passare a quella sparsa quando si ottimizza */
3377  FullSubMatrixHandler& WM = WorkMat.SetFull();
3378 
3379  /* Dimensiona e resetta la matrice di lavoro */
3380  integer iNumRows = 0;
3381  integer iNumCols = 0;
3382  InitialWorkSpaceDim(&iNumRows, &iNumCols);
3383  WM.ResizeReset(iNumRows, iNumCols);
3384 
3385  /* Equazioni: vedi joints.dvi */
3386 
3387  /* equazioni ed incognite
3388  * F1 Delta_x1 1
3389  * M1 Delta_g1 3 + 1
3390  * FP1 Delta_xP1 6 + 1
3391  * MP1 Delta_w1 9 + 1
3392  * F2 Delta_x2 12 + 1
3393  * M2 Delta_g2 15 + 1
3394  * FP2 Delta_xP2 18 + 1
3395  * MP2 Delta_w2 21 + 1
3396  * vincolo spostamento Delta_F 24 + 1
3397  * vincolo rotazione Delta_M 24 + nPosConstraints
3398  * derivata vincolo spostamento Delta_FP 24 + nConstraints
3399  * derivata vincolo rotazione Delta_MP 24 + nConstraints + nPosConstraints
3400  */
3401 
3402 
3403  /* Indici */
3404  integer iNodeFirstPosIndex = pNode->iGetFirstPositionIndex();
3405  integer iNodeFirstVelIndex = iNodeFirstPosIndex + 6;
3406  integer iFirstReactionIndex = iGetFirstIndex();
3407  integer iReactionPrimeIndex = iFirstReactionIndex + nConstraints;
3408 
3409 
3410  /* Setta gli indici dei nodi */
3411  for (int iCnt = 1; iCnt <= 6; iCnt++) {
3412  WM.PutRowIndex(iCnt, iNodeFirstPosIndex + iCnt);
3413  WM.PutColIndex(iCnt, iNodeFirstPosIndex + iCnt);
3414  WM.PutRowIndex(6 + iCnt, iNodeFirstVelIndex + iCnt);
3415  WM.PutColIndex(6 + iCnt, iNodeFirstVelIndex + iCnt);
3416  }
3417 
3418  /* Setta gli indici delle reazioni */
3419  for (unsigned int iCnt = 1; iCnt <= nConstraints; iCnt++) {
3420  WM.PutRowIndex(12 + iCnt, iFirstReactionIndex + iCnt);
3421  WM.PutColIndex(12 + iCnt, iFirstReactionIndex + iCnt);
3422  WM.PutRowIndex(12 + nConstraints + iCnt, iReactionPrimeIndex + iCnt);
3423  WM.PutColIndex(12 + nConstraints + iCnt, iReactionPrimeIndex + iCnt);
3424  }
3425 
3426  /* Recupera i dati che servono */
3427  Vec3 fn(pNode->GetRCurr()*tilde_fn);
3428 
3429  Vec3 Omega(pNode->GetWCurr());
3430 
3431  Vec3 bPrime(pNode->GetVCurr() + Omega.Cross(fn));
3432 
3433  /* F ed M sono gia' state aggiornate da InitialAssRes;
3434  * Recupero FPrime e MPrime*/
3435  Vec3 MPrime(::Zero3);
3436  Vec3 FPrime(::Zero3);
3437 
3438  for (unsigned iCnt = 0; iCnt < nPosConstraints; iCnt++) {
3439  FPrime(iPosIncid[iCnt]) = XCurr(iReactionPrimeIndex + 1 + iCnt);
3440  }
3441 
3442  for (unsigned iCnt = 0; iCnt < nRotConstraints; iCnt++) {
3443  MPrime(iRotIncid[iCnt]) = XCurr(iReactionPrimeIndex + 1 + nPosConstraints + iCnt);
3444  }
3445 
3446  Vec3 FTmp(Rch*F);
3447  Vec3 FPrimeTmp(Rch*FPrime);
3448 
3449  WM.Add(3 + 1, 3 + 1, Mat3x3(MatCrossCross, FTmp, fn));
3450  WM.Add(9 + 1, 3 + 1, Mat3x3(MatCrossCross, FPrimeTmp, fn));
3451 
3452  /* Constraints: Add only active rows/columns*/
3453 
3454  /* Positions contribution:*/
3455 
3456  Mat3x3 fnCross_Rch(fn.Cross(Rch)); // = [ fn x ] * Rc
3457 
3458  for (unsigned iCnt = 0 ; iCnt < nPosConstraints; iCnt++) {
3459  Vec3 vRch(Rch.GetVec(iPosIncid[iCnt]));
3460  Vec3 vfnCross_Rch(fnCross_Rch.GetVec(iPosIncid[iCnt]));
3461 
3462  /* Equilibrium, node */
3463  WM.Add(1, 12 + 1 + iCnt, vRch); // * Delta_F
3464  WM.Add(3 + 1, 12 + 1 + iCnt, vfnCross_Rch); // * Delta_F
3465 
3466  /* Constraint, node */
3467  WM.AddT(12 + 1 + iCnt, 1, vRch); // * Delta_x2
3468  WM.AddT(12 + 1 + iCnt, 3 + 1, vfnCross_Rch); // * Delta_g2
3469 
3470  /* d/dt(Equilibrium), node */
3471  WM.Add(6 + 1, 12 + 1 + nConstraints + iCnt, vRch); // * Delta_FP
3472  WM.Add(9 + 1, 12 + 1 + nConstraints + iCnt, vfnCross_Rch); // * Delta_FP
3473 
3474  /* d/dt(Constraint), node */
3475  WM.AddT(12 + 1 + nConstraints + iCnt, 6 + 1, vRch); // * Delta_xP2
3476  WM.AddT(12 + 1 + nConstraints + iCnt, 9 + 1, vfnCross_Rch); // * Delta_W2
3477  }
3478 
3479  for (unsigned iCnt = 0 ; iCnt < nRotConstraints; iCnt++) {
3480  Vec3 vRchr(Rchr.GetVec(iRotIncid[iCnt]));
3481 
3482  /* Equilibrium, node */
3483  WM.Add(3 + 1, 12 + 1 + nPosConstraints + iCnt, vRchr); // * Delta_M
3484 
3485  /* Constraint, node */
3486  WM.AddT(12 + 1 + nPosConstraints + iCnt, 3 + 1, vRchr); // * Delta_g2
3487 
3488  /* d/dt(Equilibrium), node */
3489  WM.Add(9 + 1, 12 + 1 + nConstraints + nPosConstraints + iCnt, vRchr); // * Delta_MP
3490 
3491  /* d/dt(Constraint), node */
3492  WM.AddT(12 + 1 + nConstraints + nPosConstraints + iCnt, 9 + 1, vRchr); // * Delta_W2
3493  }
3494 
3495  return WorkMat;
3496 }
TotalPinJoint::Rchr
Mat3x3 Rchr
Definition: totalj.h:265
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
TotalPinJoint::Rch
Mat3x3 Rch
Definition: totalj.h:264
Vec3
Definition: matvec3.h:98
TotalPinJoint::InitialWorkSpaceDim
virtual void InitialWorkSpaceDim(integer *piNumRows, integer *piNumCols) const
Definition: totalj.h:438
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
TotalPinJoint::nRotConstraints
unsigned int nRotConstraints
Definition: totalj.h:288
TotalPinJoint::tilde_fn
Vec3 tilde_fn
Definition: totalj.h:266
Mat3x3::GetVec
Vec3 GetVec(unsigned short int i) const
Definition: matvec3.h:893
FullSubMatrixHandler::AddT
void AddT(integer iRow, integer iCol, const Vec3 &v)
Definition: submat.cc:227
TotalPinJoint::iPosIncid
unsigned int iPosIncid[3]
Definition: totalj.h:292
TotalPinJoint::pNode
const StructNode * pNode
Definition: totalj.h:262
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
TotalPinJoint::iGetInitialNumDof
virtual unsigned int iGetInitialNumDof(void) const
Definition: totalj.h:434
FullSubMatrixHandler::ResizeReset
virtual void ResizeReset(integer, integer)
Definition: submat.cc:182
MatCrossCross
const MatCrossCross_Manip MatCrossCross
Definition: matvec3.cc:640
TotalPinJoint::nPosConstraints
unsigned int nPosConstraints
Definition: totalj.h:287
FullSubMatrixHandler::PutRowIndex
void PutRowIndex(integer iSubRow, integer iRow)
Definition: submat.h:311
StructDispNode::GetVCurr
virtual const Vec3 & GetVCurr(void) const
Definition: strnode.h:322
TotalPinJoint::iRotIncid
unsigned int iRotIncid[3]
Definition: totalj.h:293
DofOwnerOwner::iGetFirstIndex
virtual integer iGetFirstIndex(void) const
Definition: dofown.h:127
TotalPinJoint::nConstraints
unsigned int nConstraints
Definition: totalj.h:286
integer
long int integer
Definition: colamd.c:51

Here is the call graph for this function:

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

Implements SubjectToInitialAssembly.

Definition at line 3501 of file totalj.cc.

References TplDriveOwner< T >::bIsDifferentiable(), Vec3::Cross(), DEBUGCOUT, F, TplDriveOwner< T >::Get(), TplDriveOwner< T >::GetP(), StructNode::GetRCurr(), StructDispNode::GetVCurr(), StructNode::GetWCurr(), StructDispNode::GetXCurr(), DofOwnerOwner::iGetFirstIndex(), StructDispNode::iGetFirstPositionIndex(), iGetInitialNumDof(), InitialWorkSpaceDim(), iPosIncid, iRotIncid, M, Mat3x3::MulMT(), nConstraints, nPosConstraints, nRotConstraints, pNode, VectorHandler::PutCoef(), SubVectorHandler::PutRowIndex(), Rch, Rchr, RchrT, RchT, VectorHandler::ResizeReset(), RotManip::Rot(), VectorHandler::Sub(), ThetaDelta, ThetaDrv, tilde_fn, tilde_Rnhr, tilde_Xc, RotManip::VecRot(), XDrv, and Zero3.

3503 {
3504  if (iGetInitialNumDof() == 0) {
3505  WorkVec.ResizeReset(0);
3506  return WorkVec;
3507  }
3508 
3509  DEBUGCOUT("Entering TotalPinJoint::InitialAssRes()" << std::endl);
3510 
3511  /* Dimensiona e resetta la matrice di lavoro */
3512  integer iNumRows = 0;
3513  integer iNumCols = 0;
3514  InitialWorkSpaceDim(&iNumRows, &iNumCols);
3515  WorkVec.ResizeReset(iNumRows);
3516 
3517  /* Indici */
3518  integer iNodeFirstPosIndex = pNode->iGetFirstPositionIndex();
3519  integer iNodeFirstVelIndex = iNodeFirstPosIndex + 6;
3520  integer iFirstReactionIndex = iGetFirstIndex();
3521  integer iReactionPrimeIndex = iFirstReactionIndex + nConstraints;
3522 
3523  /* Setta gli indici */
3524  for (int iCnt = 1; iCnt <= 6; iCnt++) {
3525  WorkVec.PutRowIndex(iCnt, iNodeFirstPosIndex + iCnt);
3526  WorkVec.PutRowIndex(6 + iCnt, iNodeFirstVelIndex + iCnt);
3527  }
3528 
3529  for (unsigned int iCnt = 1; iCnt <= nConstraints; iCnt++) {
3530  WorkVec.PutRowIndex(12 + iCnt, iFirstReactionIndex + iCnt);
3531  WorkVec.PutRowIndex(12 + nConstraints + iCnt, iReactionPrimeIndex + iCnt);
3532  }
3533 
3534  /* Recupera i dati */
3535  /* Recupera i dati che servono */
3536  Vec3 fn(pNode->GetRCurr()*tilde_fn);
3537  Mat3x3 Rnhr = pNode->GetRCurr()*tilde_Rnhr;
3538  Vec3 Omega(pNode->GetWCurr());
3539 
3540  Vec3 FPrime(::Zero3);
3541  Vec3 MPrime(::Zero3);
3542 
3543  /* Aggiorna F ed M, che restano anche per InitialAssJac */
3544  for (unsigned iCnt = 0; iCnt < nPosConstraints; iCnt++) {
3545  F(iPosIncid[iCnt]) = XCurr(iFirstReactionIndex + 1 + iCnt);
3546  FPrime(iPosIncid[iCnt]) = XCurr(iReactionPrimeIndex + 1 + iCnt);
3547  }
3548 
3549  for (unsigned iCnt = 0; iCnt < nRotConstraints; iCnt++) {
3550  M(iRotIncid[iCnt]) = XCurr(iFirstReactionIndex + 1 + nPosConstraints + iCnt);
3551  MPrime(iRotIncid[iCnt]) = XCurr(iReactionPrimeIndex + 1 + nPosConstraints + iCnt);
3552  }
3553 
3554  Vec3 FTmp(Rch*F);
3555  Vec3 MTmp(Rchr*M);
3556  Vec3 FPrimeTmp(Rch*FPrime);
3557  Vec3 MPrimeTmp(Rchr*MPrime);
3558 
3559  /* Equilibrium, node 2 */
3560  WorkVec.Sub(1, FTmp);
3561  WorkVec.Sub(3 + 1, fn.Cross(FTmp) + MTmp);
3562 
3563  /* d/dt( Equilibrium ) , node 2 */
3564  WorkVec.Sub(6 + 1, FPrimeTmp);
3565  WorkVec.Sub(9 + 1, fn.Cross(FPrimeTmp) + MPrimeTmp);
3566 
3567  /* Constraint Equations */
3568  Vec3 XDelta = RchT*(pNode->GetXCurr() + fn) - tilde_Xc - XDrv.Get();
3569  Vec3 XDeltaPrime = RchT*(pNode->GetVCurr() + Omega.Cross(fn));
3570 
3571  if (XDrv.bIsDifferentiable()) {
3572  XDeltaPrime -= XDrv.GetP();
3573  }
3574 
3575  Mat3x3 R0 = RotManip::Rot(ThetaDrv.Get());
3576  Mat3x3 RDelta = RchrT*Rnhr.MulMT(R0);
3577  ThetaDelta = RotManip::VecRot(RDelta);
3578  Vec3 ThetaDeltaPrime = RchrT*Omega;
3579 
3580  if (ThetaDrv.bIsDifferentiable()) {
3581  ThetaDeltaPrime -= RDelta*ThetaDrv.GetP();
3582  }
3583 
3584  /* Position constraint: */
3585  for (unsigned iCnt = 0; iCnt < nPosConstraints; iCnt++) {
3586  WorkVec.PutCoef(12 + 1 + iCnt, -(XDelta(iPosIncid[iCnt])));
3587  WorkVec.PutCoef(12 + 1 + nConstraints + iCnt, -(XDeltaPrime(iPosIncid[iCnt])));
3588  }
3589 
3590  /* Rotation constraints: */
3591  for (unsigned iCnt = 0; iCnt < nRotConstraints; iCnt++) {
3592  WorkVec.PutCoef(12 + 1 + nPosConstraints + iCnt, -(ThetaDelta(iRotIncid[iCnt])));
3593  WorkVec.PutCoef(12 + 1 + nPosConstraints + nConstraints + iCnt, -(ThetaDeltaPrime(iRotIncid[iCnt])));
3594  }
3595 
3596  return WorkVec;
3597 }
TotalPinJoint::Rchr
Mat3x3 Rchr
Definition: totalj.h:265
Zero3
const Vec3 Zero3(0., 0., 0.)
Vec3::Cross
Vec3 Cross(const Vec3 &v) const
Definition: matvec3.h:218
TplDriveOwner::bIsDifferentiable
virtual bool bIsDifferentiable(void) const
Definition: tpldrive.h:118
TotalPinJoint::Rch
Mat3x3 Rch
Definition: totalj.h:264
Vec3
Definition: matvec3.h:98
TotalPinJoint::InitialWorkSpaceDim
virtual void InitialWorkSpaceDim(integer *piNumRows, integer *piNumCols) const
Definition: totalj.h:438
VectorHandler::ResizeReset
virtual void ResizeReset(integer)
Definition: vh.cc:55
StructNode::GetRCurr
virtual const Mat3x3 & GetRCurr(void) const
Definition: strnode.h:1012
TotalPinJoint::nRotConstraints
unsigned int nRotConstraints
Definition: totalj.h:288
VectorHandler::Sub
virtual void Sub(integer iRow, const Vec3 &v)
Definition: vh.cc:78
TotalPinJoint::tilde_fn
Vec3 tilde_fn
Definition: totalj.h:266
RotManip::VecRot
Vec3 VecRot(const Mat3x3 &Phi)
Definition: Rot.cc:136
TotalPinJoint::iPosIncid
unsigned int iPosIncid[3]
Definition: totalj.h:292
TplDriveOwner::GetP
virtual T GetP(void) const
Definition: tpldrive.h:121
SubVectorHandler::PutRowIndex
virtual void PutRowIndex(integer iSubRow, integer iRow)=0
TotalPinJoint::tilde_Rnhr
Mat3x3 tilde_Rnhr
Definition: totalj.h:268
TotalPinJoint::pNode
const StructNode * pNode
Definition: totalj.h:262
DEBUGCOUT
#define DEBUGCOUT(msg)
Definition: myassert.h:232
TplDriveOwner::Get
T Get(const doublereal &dVar) const
Definition: tpldrive.h:109
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
TotalPinJoint::iGetInitialNumDof
virtual unsigned int iGetInitialNumDof(void) const
Definition: totalj.h:434
VectorHandler::PutCoef
virtual void PutCoef(integer iRow, const doublereal &dCoef)=0
StructDispNode::GetXCurr
virtual const Vec3 & GetXCurr(void) const
Definition: strnode.h:310
TotalPinJoint::tilde_Xc
Vec3 tilde_Xc
Definition: totalj.h:275
TotalPinJoint::ThetaDelta
Vec3 ThetaDelta
Definition: totalj.h:311
TotalPinJoint::nPosConstraints
unsigned int nPosConstraints
Definition: totalj.h:287
TotalPinJoint::RchT
Mat3x3 RchT
Definition: totalj.h:274
StructDispNode::GetVCurr
virtual const Vec3 & GetVCurr(void) const
Definition: strnode.h:322
TotalPinJoint::iRotIncid
unsigned int iRotIncid[3]
Definition: totalj.h:293
Mat3x3::MulMT
Mat3x3 MulMT(const Mat3x3 &m) const
Definition: matvec3.cc:444
DofOwnerOwner::iGetFirstIndex
virtual integer iGetFirstIndex(void) const
Definition: dofown.h:127
TotalPinJoint::nConstraints
unsigned int nConstraints
Definition: totalj.h:286
TotalPinJoint::ThetaDrv
TplDriveOwner< Vec3 > ThetaDrv
Definition: totalj.h:282
integer
long int integer
Definition: colamd.c:51
TotalPinJoint::XDrv
TplDriveOwner< Vec3 > XDrv
Definition: totalj.h:278
TotalPinJoint::RchrT
Mat3x3 RchrT
Definition: totalj.h:276

Here is the call graph for this function:

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

Implements SubjectToInitialAssembly.

Definition at line 438 of file totalj.h.

References nConstraints.

Referenced by InitialAssJac(), and InitialAssRes().

438  {
439  *piNumCols = *piNumRows = 12 + 2*nConstraints;
440  };
TotalPinJoint::nConstraints
unsigned int nConstraints
Definition: totalj.h:286
void TotalPinJoint::Output ( OutputHandler &  OH) const
virtual

Reimplemented from ToBeOutput.

Definition at line 3319 of file totalj.cc.

References ToBeOutput::bToBeOutput(), F, OutputHandler::GetCurrentStep(), WithLabel::GetLabel(), StructNode::GetRCurr(), StructDispNode::GetVCurr(), StructNode::GetWCurr(), StructDispNode::GetXCurr(), OutputHandler::JOINTS, OutputHandler::Joints(), M, Joint::Output(), Vec3::pGetVec(), pNode, Rch, Rchr, RchrT, RchT, tilde_fn, tilde_Rnhr, tilde_Xc, OutputHandler::UseNetCDF(), OutputHandler::UseText(), and RotManip::VecRot().

3320 {
3321  if (bToBeOutput()) {
3322  const Vec3& Xn(pNode->GetXCurr());
3323  const Mat3x3& Rn(pNode->GetRCurr());
3324  const Vec3& Vn(pNode->GetVCurr());
3325  const Vec3& Omegan(pNode->GetWCurr());
3326 
3327  Vec3 fn(Rn*tilde_fn);
3328  Mat3x3 RnhrTmp(Rn*tilde_Rnhr);
3329  Mat3x3 RTmp(RchrT*RnhrTmp);
3330  Vec3 ThetaTmp(RotManip::VecRot(RTmp));
3331  Vec3 XTmp(RchT*(Xn + fn) - tilde_Xc);
3332  Vec3 VTmp(RchT*(Vn + Omegan.Cross(fn)));
3333  Vec3 OmegaTmp(RchrT*Omegan);
3334 
3335  Vec3 FTmp(Rch*F);
3336  Vec3 MTmp(Rchr*M);
3337 
3338 #ifdef USE_NETCDF
3339  if (OH.UseNetCDF(OutputHandler::JOINTS)) {
3340  Var_F_local->put_rec(F.pGetVec(), OH.GetCurrentStep());
3341  Var_M_local->put_rec(M.pGetVec(), OH.GetCurrentStep());
3342  Var_F_global->put_rec(FTmp.pGetVec(), OH.GetCurrentStep());
3343  Var_M_global->put_rec(MTmp.pGetVec(), OH.GetCurrentStep());
3344 
3345  Var_X->put_rec(XTmp.pGetVec(), OH.GetCurrentStep());
3346  Var_Phi->put_rec(ThetaTmp.pGetVec(), OH.GetCurrentStep());
3347  Var_V->put_rec(VTmp.pGetVec(), OH.GetCurrentStep());
3348  Var_Omega->put_rec(OmegaTmp.pGetVec(), OH.GetCurrentStep());
3349  }
3350 #endif // USE_NETCDF
3351 
3352  if (OH.UseText(OutputHandler::JOINTS)) {
3353  Joint::Output(OH.Joints(), "TotalPinJoint", GetLabel(),
3354  F, M, FTmp, MTmp)
3355  << " " << XTmp
3356  << " " << ThetaTmp
3357  << " " << VTmp
3358  << " " << OmegaTmp
3359  << std::endl;
3360  // accelerations?
3361  }
3362  }
3363 }
TotalPinJoint::Rchr
Mat3x3 Rchr
Definition: totalj.h:265
ToBeOutput::bToBeOutput
virtual bool bToBeOutput(void) const
Definition: output.cc:890
TotalPinJoint::Rch
Mat3x3 Rch
Definition: totalj.h:264
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
TotalPinJoint::tilde_fn
Vec3 tilde_fn
Definition: totalj.h:266
RotManip::VecRot
Vec3 VecRot(const Mat3x3 &Phi)
Definition: Rot.cc:136
TotalPinJoint::tilde_Rnhr
Mat3x3 tilde_Rnhr
Definition: totalj.h:268
TotalPinJoint::pNode
const StructNode * pNode
Definition: totalj.h:262
OutputHandler::GetCurrentStep
long GetCurrentStep(void) const
Definition: output.h:116
StructNode::GetWCurr
virtual const Vec3 & GetWCurr(void) const
Definition: strnode.h:1030
OutputHandler::Joints
std::ostream & Joints(void) const
Definition: output.h:443
StructDispNode::GetXCurr
virtual const Vec3 & GetXCurr(void) const
Definition: strnode.h:310
TotalPinJoint::tilde_Xc
Vec3 tilde_Xc
Definition: totalj.h:275
Vec3::pGetVec
const doublereal * pGetVec(void) const
Definition: matvec3.h:192
TotalPinJoint::RchT
Mat3x3 RchT
Definition: totalj.h:274
StructDispNode::GetVCurr
virtual const Vec3 & GetVCurr(void) const
Definition: strnode.h:322
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
OutputHandler::UseText
bool UseText(int out) const
Definition: output.cc:446
TotalPinJoint::RchrT
Mat3x3 RchrT
Definition: totalj.h:276

Here is the call graph for this function:

void TotalPinJoint::OutputPrepare ( OutputHandler &  OH)
virtual

Reimplemented from ToBeOutput.

Definition at line 3293 of file totalj.cc.

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

3294 {
3295  if (bToBeOutput()) {
3296 #ifdef USE_NETCDF
3297  if (OH.UseNetCDF(OutputHandler::JOINTS)) {
3298  std::string name;
3299  OutputPrepare_int("totalpin", OH, name);
3300 
3301  Var_X = OH.CreateVar<Vec3>(name + "X", "m",
3302  "local relative position (x, y, z)");
3303 
3304  // NOTE: by now, force ORIENTATION_VECTOR
3305  Var_Phi = OH.CreateRotationVar(name, "", ORIENTATION_VECTOR, "local relative");
3306 
3307  Var_V = OH.CreateVar<Vec3>(name + "V", "m/s",
3308  "local relative velocity (x, y, z)");
3309 
3310  Var_Omega = OH.CreateVar<Vec3>(name + "Omega", "radian/s",
3311  "local relative angular velocity (x, y, z)");
3312  }
3313 #endif // USE_NETCDF
3314  }
3315 }
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 * TotalPinJoint::ParseHint ( DataManager *  pDM,
const char *  s 
) const
virtual

Reimplemented from SimulationEntity.

Definition at line 2667 of file totalj.cc.

References STRLENOF.

2668 {
2669  if (strncasecmp(s, "offset{" /*}*/ , STRLENOF("offset{" /*}*/ )) == 0)
2670  {
2671  s += STRLENOF("offset{" /*}*/ );
2672 
2673  if (strcmp(&s[1], /*{*/ "}") != 0) {
2674  return 0;
2675  }
2676 
2677  switch (s[0]) {
2678  case '1':
2679  return new Joint::OffsetHint<1>;
2680 
2681  case '0':
2682  return new Joint::OffsetHint<0>;
2683  }
2684 
2685  } else if (strncasecmp(s, "position-hinge{" /*}*/, STRLENOF("position-hinge{" /*}*/)) == 0) {
2686  s += STRLENOF("position-hinge{" /*}*/);
2687 
2688  if (strcmp(&s[1], /*{*/ "}") != 0) {
2689  return 0;
2690  }
2691 
2692  switch (s[0]) {
2693  case '1':
2694  return new Joint::HingeHint<1>;
2695 
2696  case '0':
2697  return new Joint::HingeHint<0>;
2698  }
2699 
2700  } else if (strncasecmp(s, "position-drive3{" /*}*/, STRLENOF("position-drive3{" /*}*/)) == 0) {
2701  s += STRLENOF("position-");
2702 
2703  Hint *pH = ::ParseHint(pDM, s);
2704  if (pH) {
2705  TplDriveHint<Vec3> *pTDH = dynamic_cast<TplDriveHint<Vec3> *>(pH);
2706  if (pTDH) {
2707  return new PositionDriveHint<Vec3>(pTDH);
2708  }
2709  }
2710  return 0;
2711 
2712  } else if (strncasecmp(s, "orientation-hinge{" /*}*/, STRLENOF("orientation-hinge{" /*}*/)) == 0) {
2713  s += STRLENOF("orientation-hinge{" /*}*/);
2714 
2715  if (strcmp(&s[1], /*{*/ "}") != 0) {
2716  return 0;
2717  }
2718 
2719  switch (s[0]) {
2720  case '1':
2721  return new Joint::HingeHint<1>;
2722 
2723  case '0':
2724  return new Joint::HingeHint<0>;
2725  }
2726 
2727  } else if (strncasecmp(s, "orientation-drive3{" /*}*/, STRLENOF("orientation-drive3{" /*}*/)) == 0) {
2728  s += STRLENOF("orientation-");
2729 
2730  Hint *pH = ::ParseHint(pDM, s);
2731  if (pH) {
2732  TplDriveHint<Vec3> *pTDH = dynamic_cast<TplDriveHint<Vec3> *>(pH);
2733  if (pTDH) {
2734  return new OrientationDriveHint<Vec3>(pTDH);
2735  }
2736  }
2737  return 0;
2738  }
2739 
2740  return 0;
2741 }
Hint
Definition: hint.h:38
TotalPinJoint::ParseHint
virtual Hint * ParseHint(DataManager *pDM, const char *s) const
Definition: totalj.cc:2667
STRLENOF
#define STRLENOF(s)
Definition: mbdyn.h:166
std::ostream & TotalPinJoint::Restart ( std::ostream &  out) const
virtual

Implements Elem.

Definition at line 2774 of file totalj.cc.

References bAgvActive, bPosActive, bRotActive, bVelActive, WithLabel::GetLabel(), Mat3x3::GetVec(), OmegaDrv, OmegaPDrv, TplDriveOwner< T >::pGetDriveCaller(), pNode, Rch, Rchr, TplDriveCaller< T >::Restart(), Joint::Restart(), ThetaDrv, tilde_fn, tilde_Rnh, tilde_Rnhr, Vec3::Write(), Xc, XDrv, XPDrv, and XPPDrv.

2775 {
2776  Joint::Restart(out) << ", total pin joint, "
2777  << pNode->GetLabel() << ", "
2778  << "position, ";
2779  tilde_fn.Write(out, ", ") << ", "
2780  << "position orientation, "
2781  "1 , ";
2782  tilde_Rnh.GetVec(1).Write(out, ", ") << ", "
2783  "2 , ";
2784  tilde_Rnh.GetVec(2).Write(out, ", ") << ", "
2785  << "rotation orientation, "
2786  "1 , ";
2787  tilde_Rnhr.GetVec(1).Write(out, ", ") << ", "
2788  "2 , ";
2789  tilde_Rnhr.GetVec(2).Write(out, ", ") << ", "
2790  << "position, ";
2791  Xc.Write(out, ", ") << ", "
2792  << "position orientation, "
2793  "1 , ";
2794  Rch.GetVec(1).Write(out, ", ") << ", "
2795  "2 , ";
2796  Rch.GetVec(2).Write(out, ", ") << ", "
2797  << "rotation orientation, "
2798  "1 , ";
2799  Rchr.GetVec(1).Write(out, ", ") << ", "
2800  "2 , ";
2801  Rchr.GetVec(2).Write(out, ", ");
2802 
2803  if (bPosActive[0] || bPosActive[1] || bPosActive[2]
2804  || bVelActive[0] || bVelActive[1] || bVelActive[2])
2805  {
2806  out << ", position constraint";
2807  for (unsigned i = 0; i < 3; i++) {
2808  if (bPosActive[i]) {
2809  out << ", active";
2810  } else if (bVelActive[i]) {
2811  out << ", velocity";
2812  } else {
2813  out << ", inactive";
2814  }
2815  }
2816 
2817  out << ", ", XDrv.pGetDriveCaller()->Restart(out);
2818  if (XPDrv.pGetDriveCaller()) {
2819  out << ", ", XPDrv.pGetDriveCaller()->Restart(out)
2820  << ", ", XPPDrv.pGetDriveCaller()->Restart(out);
2821  }
2822  }
2823 
2824  if (bRotActive[0] || bRotActive[1] || bRotActive[2]
2825  || bAgvActive[0] || bAgvActive[1] || bAgvActive[2])
2826  {
2827  out << ", orientation constraint";
2828  for (unsigned i = 0; i < 3; i++) {
2829  if (bRotActive[i]) {
2830  out << ", active";
2831  } else if (bAgvActive[i]) {
2832  out << ", angular velocity";
2833  } else {
2834  out << ", inactive";
2835  }
2836  }
2837 
2838  out << ", ", ThetaDrv.pGetDriveCaller()->Restart(out);
2839  if (OmegaDrv.pGetDriveCaller()) {
2840  out << ", ", OmegaDrv.pGetDriveCaller()->Restart(out)
2841  << ", ", OmegaPDrv.pGetDriveCaller()->Restart(out);
2842  }
2843  }
2844 
2845  return out << ";" << std::endl;
2846 }
TotalPinJoint::Rchr
Mat3x3 Rchr
Definition: totalj.h:265
Vec3::Write
std::ostream & Write(std::ostream &out, const char *sFill=" ") const
Definition: matvec3.cc:738
TotalPinJoint::bPosActive
bool bPosActive[3]
Definition: totalj.h:269
TotalPinJoint::Rch
Mat3x3 Rch
Definition: totalj.h:264
TotalPinJoint::XPDrv
TplDriveOwner< Vec3 > XPDrv
Definition: totalj.h:279
TotalPinJoint::bRotActive
bool bRotActive[3]
Definition: totalj.h:270
TotalPinJoint::tilde_Rnh
Mat3x3 tilde_Rnh
Definition: totalj.h:267
TotalPinJoint::bVelActive
bool bVelActive[3]
Definition: totalj.h:271
TotalPinJoint::tilde_fn
Vec3 tilde_fn
Definition: totalj.h:266
Mat3x3::GetVec
Vec3 GetVec(unsigned short int i) const
Definition: matvec3.h:893
TotalPinJoint::OmegaPDrv
TplDriveOwner< Vec3 > OmegaPDrv
Definition: totalj.h:284
TotalPinJoint::tilde_Rnhr
Mat3x3 tilde_Rnhr
Definition: totalj.h:268
TplDriveOwner::pGetDriveCaller
TplDriveCaller< T > * pGetDriveCaller(void) const
Definition: tpldrive.h:105
TotalPinJoint::OmegaDrv
TplDriveOwner< Vec3 > OmegaDrv
Definition: totalj.h:283
TotalPinJoint::pNode
const StructNode * pNode
Definition: totalj.h:262
TotalPinJoint::XPPDrv
TplDriveOwner< Vec3 > XPPDrv
Definition: totalj.h:280
Joint::Restart
virtual std::ostream & Restart(std::ostream &out) const
Definition: joint.h:195
TplDriveCaller::Restart
virtual std::ostream & Restart(std::ostream &out) const =0
TotalPinJoint::bAgvActive
bool bAgvActive[3]
Definition: totalj.h:272
TotalPinJoint::ThetaDrv
TplDriveOwner< Vec3 > ThetaDrv
Definition: totalj.h:282
TotalPinJoint::XDrv
TplDriveOwner< Vec3 > XDrv
Definition: totalj.h:278
WithLabel::GetLabel
unsigned int GetLabel(void) const
Definition: withlab.cc:62

Here is the call graph for this function:

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

Reimplemented from Joint.

Definition at line 2581 of file totalj.cc.

References TplDriveOwner< T >::Get(), StructNode::GetRCurr(), StructDispNode::GetXCurr(), MBDYN_EXCEPT_ARGS, Mat3x3::MulTM(), OmegaDrv, OmegaPDrv, pNode, Joint::JointDriveHint< T >::pTDH, Rch, Rchr, RchrT, RchT, RotManip::Rot(), TplDriveOwner< T >::Set(), ThetaDrv, tilde_fn, tilde_Rnh, tilde_Rnhr, tilde_Xc, Mat3x3::Transpose(), WithLabel::uLabel, Xc, XDrv, XPDrv, and XPPDrv.

2584 {
2585  if (ph) {
2586  for (unsigned int i = 0; i < ph->size(); i++) {
2587  Joint::JointHint *pjh = dynamic_cast<Joint::JointHint *>((*ph)[i]);
2588 
2589  if (pjh) {
2590  if (dynamic_cast<Joint::OffsetHint<1> *>(pjh)) {
2591  Mat3x3 RnT(pNode->GetRCurr().Transpose());
2592 
2593  tilde_fn = RnT*(Xc + Rch*XDrv.Get() - pNode->GetXCurr());
2594 
2595  } else if (dynamic_cast<Joint::OffsetHint<0> *>(pjh)) {
2596  Xc = pNode->GetXCurr() + pNode->GetRCurr()*tilde_fn
2597  - Rch*XDrv.Get();
2598  tilde_Xc = RchT*Xc;
2599 
2600  } else if (dynamic_cast<Joint::HingeHint<1> *>(pjh)) {
2601  if (dynamic_cast<Joint::PositionHingeHint<1> *>(pjh)) {
2602  tilde_Rnh = pNode->GetRCurr().MulTM(Rch);
2603  /* NOTE: pointless */
2604 
2605  } else if (dynamic_cast<Joint::OrientationHingeHint<1> *>(pjh)) {
2606  tilde_Rnhr = pNode->GetRCurr().MulTM(Rchr*RotManip::Rot(ThetaDrv.Get()));
2607  }
2608 
2609  } else if (dynamic_cast<Joint::HingeHint<0> *>(pjh)) {
2610  if (dynamic_cast<Joint::PositionHingeHint<0> *>(pjh)) {
2611  Rch = pNode->GetRCurr()*tilde_Rnh;
2612  RchT = Rch.Transpose();
2613  tilde_Xc = RchT*Xc;
2614  /* NOTE: results in constraint violation if XDrv is not 0 */
2615 
2616  } else if (dynamic_cast<Joint::OrientationHingeHint<0> *>(pjh)) {
2617  Rchr = pNode->GetRCurr()*tilde_Rnhr*RotManip::Rot(-ThetaDrv.Get());
2618  RchrT = Rchr.Transpose();
2619  }
2620 
2621  } else if (dynamic_cast<Joint::JointDriveHint<Vec3> *>(pjh)) {
2622  Joint::JointDriveHint<Vec3> *pjdh
2623  = dynamic_cast<Joint::JointDriveHint<Vec3> *>(pjh);
2624  pedantic_cout("TotalPinJoint(" << uLabel << "): "
2625  "creating drive from hint[" << i << "]..." << std::endl);
2626 
2627  TplDriveCaller<Vec3> *pDC = pjdh->pTDH->pCreateDrive(pDM);
2628  if (pDC == 0) {
2629  silent_cerr("TotalPinJoint(" << uLabel << "): "
2630  "unable to create drive "
2631  "after hint #" << i << std::endl);
2632  throw ErrGeneric(MBDYN_EXCEPT_ARGS);
2633  }
2634 
2635  if (dynamic_cast<Joint::PositionDriveHint<Vec3> *>(pjdh)) {
2636  XDrv.Set(pDC);
2637 
2638  } else if (dynamic_cast<Joint::VelocityDriveHint<Vec3> *>(pjdh)) {
2639  XPDrv.Set(pDC);
2640 
2641  } else if (dynamic_cast<Joint::AccelerationDriveHint<Vec3> *>(pjdh)) {
2642  XPPDrv.Set(pDC);
2643 
2644  } else if (dynamic_cast<Joint::OrientationDriveHint<Vec3> *>(pjdh)) {
2645  ThetaDrv.Set(pDC);
2646 
2647  } else if (dynamic_cast<Joint::AngularVelocityDriveHint<Vec3> *>(pjdh)) {
2648  OmegaDrv.Set(pDC);
2649 
2650  } else if (dynamic_cast<Joint::AngularAccelerationDriveHint<Vec3> *>(pjdh)) {
2651  OmegaPDrv.Set(pDC);
2652 
2653  } else {
2654  delete pDC;
2655  }
2656 
2657  } else if (dynamic_cast<Joint::ReactionsHint *>(pjh)) {
2658  /* TODO */
2659  }
2660  continue;
2661  }
2662  }
2663  }
2664 }
TotalPinJoint::Rchr
Mat3x3 Rchr
Definition: totalj.h:265
TotalPinJoint::Rch
Mat3x3 Rch
Definition: totalj.h:264
MBDYN_EXCEPT_ARGS
#define MBDYN_EXCEPT_ARGS
Definition: except.h:63
TotalPinJoint::XPDrv
TplDriveOwner< Vec3 > XPDrv
Definition: totalj.h:279
StructNode::GetRCurr
virtual const Mat3x3 & GetRCurr(void) const
Definition: strnode.h:1012
Joint::JointDriveHint::pTDH
TplDriveHint< T > * pTDH
Definition: joint.h:137
TotalPinJoint::tilde_Rnh
Mat3x3 tilde_Rnh
Definition: totalj.h:267
TotalPinJoint::tilde_fn
Vec3 tilde_fn
Definition: totalj.h:266
TplDriveOwner::Set
void Set(const TplDriveCaller< T > *pDC)
Definition: tpldrive.h:97
TotalPinJoint::OmegaPDrv
TplDriveOwner< Vec3 > OmegaPDrv
Definition: totalj.h:284
TotalPinJoint::tilde_Rnhr
Mat3x3 tilde_Rnhr
Definition: totalj.h:268
TotalPinJoint::OmegaDrv
TplDriveOwner< Vec3 > OmegaDrv
Definition: totalj.h:283
TotalPinJoint::pNode
const StructNode * pNode
Definition: totalj.h:262
TplDriveOwner::Get
T Get(const doublereal &dVar) const
Definition: tpldrive.h:109
RotManip::Rot
Mat3x3 Rot(const Vec3 &phi)
Definition: Rot.cc:62
TotalPinJoint::XPPDrv
TplDriveOwner< Vec3 > XPPDrv
Definition: totalj.h:280
WithLabel::uLabel
unsigned int uLabel
Definition: withlab.h:44
Mat3x3::MulTM
Mat3x3 MulTM(const Mat3x3 &m) const
Definition: matvec3.cc:500
StructDispNode::GetXCurr
virtual const Vec3 & GetXCurr(void) const
Definition: strnode.h:310
TotalPinJoint::tilde_Xc
Vec3 tilde_Xc
Definition: totalj.h:275
Mat3x3::Transpose
Mat3x3 Transpose(void) const
Definition: matvec3.h:816
TotalPinJoint::RchT
Mat3x3 RchT
Definition: totalj.h:274
TotalPinJoint::ThetaDrv
TplDriveOwner< Vec3 > ThetaDrv
Definition: totalj.h:282
TotalPinJoint::XDrv
TplDriveOwner< Vec3 > XDrv
Definition: totalj.h:278
TotalPinJoint::RchrT
Mat3x3 RchrT
Definition: totalj.h:276

Here is the call graph for this function:

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

Reimplemented from Joint.

Definition at line 3267 of file totalj.cc.

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

3268 {
3269  ASSERT(iOrder == InverseDynamics::INVERSE_DYNAMICS);
3270 
3271  integer iFirstReactionIndex = iGetFirstIndex();
3272 
3273  /* Get constraint reactions */
3274  for (unsigned iCnt = 0; iCnt < nPosConstraints; iCnt++) {
3275  F(iPosIncid[iCnt]) = XCurr(iFirstReactionIndex + 1 + iPosEqIndex[iCnt]);
3276  }
3277 
3278  for (unsigned iCnt = 0; iCnt < nRotConstraints; iCnt++) {
3279  M(iRotIncid[iCnt]) = XCurr(iFirstReactionIndex + 1 + iRotEqIndex[iCnt]);
3280  }
3281 }
TotalPinJoint::nRotConstraints
unsigned int nRotConstraints
Definition: totalj.h:288
TotalPinJoint::iPosIncid
unsigned int iPosIncid[3]
Definition: totalj.h:292
TotalPinJoint::iPosEqIndex
unsigned int iPosEqIndex[3]
Definition: totalj.h:297
TotalPinJoint::iRotEqIndex
unsigned int iRotEqIndex[3]
Definition: totalj.h:298
ASSERT
#define ASSERT(expression)
Definition: colamd.c:977
TotalPinJoint::nPosConstraints
unsigned int nPosConstraints
Definition: totalj.h:287
TotalPinJoint::iRotIncid
unsigned int iRotIncid[3]
Definition: totalj.h:293
DofOwnerOwner::iGetFirstIndex
virtual integer iGetFirstIndex(void) const
Definition: dofown.h:127
integer
long int integer
Definition: colamd.c:51

Here is the call graph for this function:

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

Implements Elem.

Definition at line 389 of file totalj.h.

References nConstraints.

Referenced by AssJac(), and AssRes().

389  {
390  *piNumCols = *piNumRows = 6 + nConstraints ;
391  };
TotalPinJoint::nConstraints
unsigned int nConstraints
Definition: totalj.h:286

Member Data Documentation

bool TotalPinJoint::bAgvActive[3]
private

Definition at line 272 of file totalj.h.

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

bool TotalPinJoint::bPosActive[3]
private

Definition at line 269 of file totalj.h.

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

bool TotalPinJoint::bRotActive[3]
private

Definition at line 270 of file totalj.h.

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

bool TotalPinJoint::bVelActive[3]
private

Definition at line 271 of file totalj.h.

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

Vec3 TotalPinJoint::F
mutableprivate

Definition at line 310 of file totalj.h.

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

unsigned int TotalPinJoint::iAgvEqIndex[3]
private

Definition at line 300 of file totalj.h.

Referenced by AssRes(), and TotalPinJoint().

unsigned int TotalPinJoint::iAgvIncid[3]
private

Definition at line 295 of file totalj.h.

Referenced by AssRes(), and TotalPinJoint().

unsigned int TotalPinJoint::iPosEqIndex[3]
private

Definition at line 297 of file totalj.h.

Referenced by AssRes(), TotalPinJoint(), and Update().

unsigned int TotalPinJoint::iPosIncid[3]
private

Definition at line 292 of file totalj.h.

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

unsigned int TotalPinJoint::iRotEqIndex[3]
private

Definition at line 298 of file totalj.h.

Referenced by AssRes(), TotalPinJoint(), and Update().

unsigned int TotalPinJoint::iRotIncid[3]
private

Definition at line 293 of file totalj.h.

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

unsigned int TotalPinJoint::iVelEqIndex[3]
private

Definition at line 299 of file totalj.h.

Referenced by AssRes(), and TotalPinJoint().

unsigned int TotalPinJoint::iVelIncid[3]
private

Definition at line 294 of file totalj.h.

Referenced by AssRes(), and TotalPinJoint().

Vec3 TotalPinJoint::M
mutableprivate

Definition at line 309 of file totalj.h.

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

unsigned int TotalPinJoint::nAgvConstraints
private

Definition at line 290 of file totalj.h.

Referenced by AssRes(), DescribeDof(), DescribeEq(), and TotalPinJoint().

unsigned int TotalPinJoint::nConstraints
private

Definition at line 286 of file totalj.h.

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

unsigned int TotalPinJoint::nPosConstraints
private

Definition at line 287 of file totalj.h.

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

unsigned int TotalPinJoint::nRotConstraints
private

Definition at line 288 of file totalj.h.

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

unsigned int TotalPinJoint::nVelConstraints
private

Definition at line 289 of file totalj.h.

Referenced by AssRes(), DescribeDof(), DescribeEq(), and TotalPinJoint().

TplDriveOwner<Vec3> TotalPinJoint::OmegaDrv
private

Definition at line 283 of file totalj.h.

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

TplDriveOwner<Vec3> TotalPinJoint::OmegaPDrv
private

Definition at line 284 of file totalj.h.

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

const StructNode* TotalPinJoint::pNode
private

Definition at line 262 of file totalj.h.

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

Mat3x3 TotalPinJoint::Rch
private

Definition at line 264 of file totalj.h.

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

Mat3x3 TotalPinJoint::Rchr
private

Definition at line 265 of file totalj.h.

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

Mat3x3 TotalPinJoint::RchrT
private

Definition at line 276 of file totalj.h.

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

Mat3x3 TotalPinJoint::RchT
private

Definition at line 274 of file totalj.h.

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

Vec3 TotalPinJoint::ThetaDelta
mutableprivate

Definition at line 311 of file totalj.h.

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

Vec3 TotalPinJoint::ThetaDeltaPrev
mutableprivate

Definition at line 312 of file totalj.h.

Referenced by AfterConvergence().

Vec3 TotalPinJoint::ThetaDeltaRemnant
private

Definition at line 314 of file totalj.h.

Referenced by AfterConvergence(), and AssRes().

Vec3 TotalPinJoint::ThetaDeltaTrue
private

Definition at line 315 of file totalj.h.

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

TplDriveOwner<Vec3> TotalPinJoint::ThetaDrv
private

Definition at line 282 of file totalj.h.

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

Vec3 TotalPinJoint::tilde_fn
private

Definition at line 266 of file totalj.h.

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

Mat3x3 TotalPinJoint::tilde_Rnh
private

Definition at line 267 of file totalj.h.

Referenced by Restart(), and SetValue().

Mat3x3 TotalPinJoint::tilde_Rnhr
private

Definition at line 268 of file totalj.h.

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

Vec3 TotalPinJoint::tilde_Xc
private

Definition at line 275 of file totalj.h.

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

Vec3 TotalPinJoint::Xc
private

Definition at line 263 of file totalj.h.

Referenced by Restart(), and SetValue().

TplDriveOwner<Vec3> TotalPinJoint::XDrv
private

Definition at line 278 of file totalj.h.

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

TplDriveOwner<Vec3> TotalPinJoint::XPDrv
private

Definition at line 279 of file totalj.h.

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

TplDriveOwner<Vec3> TotalPinJoint::XPPDrv
private

Definition at line 280 of file totalj.h.

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

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