71 pNode1(pN1), pNode2(pN2),
72 tilde_f1(tilde_f1), tilde_f2(tilde_f2),
73 tilde_R1h(tilde_R1h), tilde_R2h(tilde_R2h),
108 tilde_f1.
Write(out,
", ") <<
", hinge, reference, node, 1, ",
112 tilde_f2.
Write(out,
", ") <<
", hinge, reference, node, 1, ",
115 return pGetConstLaw()->Restart(out) <<
';' << std::endl;
126 Var_tilde_d = OH.CreateVar<
Vec3>(name +
"d",
"m",
127 "relative position in local frame (x, y, z)");
128 Var_tilde_dPrime = OH.CreateVar<
Vec3>(name +
"dPrime",
"m/s",
129 "relative linear velocity in local frame (x, y, z)");
130 Var_d = OH.CreateVar<
Vec3>(name +
"D",
"m",
131 "relative position in global frame (x, y, z)");
132 Var_dPrime = OH.CreateVar<
Vec3>(name +
"DPrime",
"m/s",
133 "relative linear velocity in global frame (x, y, z)");
134 Var_Phi = OH.CreateRotationVar(name,
"",
od,
"global");
135 Var_Omega = OH.CreateVar<
Vec3>(name +
"Omega",
"radian/s",
136 "local relative angular velocity (x, y, z)");
241 for (
unsigned i = 0; i < ph->size(); i++) {
263 }
else if (dynamic_cast<Joint::ReactionsHint *>(pjh)) {
286 if (strncasecmp(s,
"offset{" ,
STRLENOF(
"offset{" )) == 0)
290 if (strcmp(&s[1],
"}") != 0) {
302 }
else if (strncasecmp(s,
"hinge{" ,
STRLENOF(
"hinge{" )) == 0) {
305 if (strcmp(&s[1],
"}") != 0) {
360 size_t l =
STRLENOF(
"constitutiveLaw.");
361 if (strncmp(s,
"constitutiveLaw.", l) == 0) {
421 return tilde_Theta(i - 3);
434 return tilde_dPrime(i - 6);
444 return tilde_Omega(i - 9);
453 return GetF()(i - 12);
470 WM.
Add(1, 4, FCross);
471 WM.
Sub(4, 1, FCross);
472 WM.
Add(4, 6 + 1, FCross);
473 WM.
Sub(6 + 1, 4, FCross);
477 WM.
Add(6 + 4, 6 + 4, MTmp);
478 WM.
Sub(4, 6 + 4, MTmp);
482 WM.
Sub(6 + 4, 4, MTmp);
500 WM.
Sub(1, 6 + 1, F_d);
501 WM.
Sub(6 + 1, 1, F_d);
502 WM.
Add(6 + 1, 6 + 1, F_d);
508 WM.
Add(6 + 1, 4, FTmp);
519 WM.
Add(1, 6 + 4, FTmp);
520 WM.
Sub(6 + 1, 6 + 4, FTmp);
532 WM.
Sub(4, 6 + 1, FTmp);
537 WM.
Sub(6 + 4, 1, FTmp);
538 WM.
Add(6 + 4, 6 + 1, FTmp);
556 WMB.
Add(1, 1, F_dPrime);
557 WMB.
Sub(1, 6 + 1, F_dPrime);
558 WMB.
Sub(6 + 1, 1, F_dPrime);
559 WMB.
Add(6 + 1, 6 + 1, F_dPrime);
565 WMB.
Add(6 + 1, 4, FTmp);
576 WMB.
Add(1, 6 + 4, FTmp);
577 WMB.
Sub(6 + 1, 6 + 4, FTmp);
586 FTmp =
d1.
Cross(F_dPrime) + M_dPrime;
589 WMB.
Sub(4, 6 + 1, FTmp);
592 FTmp =
d2.
Cross(F_dPrime) + M_dPrime;
594 WMB.
Sub(6 + 4, 1, FTmp);
595 WMB.
Add(6 + 4, 6 + 1, FTmp);
602 WMA.
Add(6 + 1, 1, MTmp);
603 WMA.
Add(1, 6 + 1, MTmp);
604 WMA.
Sub(6 + 1, 6 + 1, MTmp);
606 Mat3x3 A1dP(
d1.
Cross(F_dPrime) + M_dPrime);
607 Mat3x3 A2dP(
d2.
Cross(F_dPrime) + M_dPrime);
609 Mat3x3 A1tPw((
d1.
Cross(F_thetaPrime) + M_thetaPrime)*Mat3x3(
MatCross, Omega2*dCoef));
610 Mat3x3 A2tPw((
d2.
Cross(F_thetaPrime) + M_thetaPrime)*Mat3x3(
MatCross, Omega2*dCoef));
615 MTmp = A1dP*Mat3x3(
MatCross, Omega1*dCoef);
618 WMA.
Add(4, 6 + 1, MTmp);
620 MTmp = A2dP*Mat3x3(
MatCross, Omega1*dCoef);
622 WMA.
Add(6 + 4, 1, MTmp);
623 WMA.
Sub(6 + 4, 6 + 1, MTmp);
625 FTmp = F_thetaPrime*Mat3x3(
MatCross, Omega2*dCoef);
627 MTmp = F_dPrime*D1 + FTmp;
630 WMA.
Add(6 + 1, 4, MTmp);
632 MTmp = F_dPrime*D2 + FTmp;
634 WMA.
Add(1, 6 + 4, MTmp);
635 WMA.
Sub(6 + 1, 6 + 4, MTmp);
637 WMA.
Sub(4, 4, A1dP*D1 + A1tPw);
639 WMA.
Add(6 + 4, 4, A2dP*D1 + A2tPw);
641 WMA.
Add(4, 6 + 4, A1dP*D2 + A1tPw);
643 WMA.
Sub(6 + 4, 6 + 4, A2dP*D2 + A2tPw);
668 for (
int iCnt = 1; iCnt <= 6; iCnt++) {
671 WM.
PutRowIndex(6 + iCnt, iNode2FirstMomIndex + iCnt);
672 WM.
PutColIndex(6 + iCnt, iNode2FirstPosIndex + iCnt);
704 for (
int iCnt = 1; iCnt <= 6; iCnt++) {
707 WMA.
PutRowIndex(6 + iCnt, iNode2FirstMomIndex + iCnt);
708 WMA.
PutColIndex(6 + iCnt, iNode2FirstPosIndex + iCnt);
712 WMB.
PutRowIndex(6 + iCnt, iNode2FirstMomIndex + iCnt);
713 WMB.
PutColIndex(6 + iCnt, iNode2FirstPosIndex + iCnt);
737 for (
int iCnt = 1; iCnt <= 6; iCnt++) {
738 WorkVec.
PutRowIndex(iCnt, iNode1FirstMomIndex + iCnt);
739 WorkVec.
PutRowIndex(6 + iCnt, iNode2FirstMomIndex + iCnt);
771 for (
int iCnt = 1; iCnt <= 6; iCnt++) {
772 WorkVec.
PutRowIndex(iCnt, iNode1FirstMomIndex + iCnt);
773 WorkVec.
PutRowIndex(6 + iCnt, iNode2FirstMomIndex + iCnt);
797 for (
int iCnt = 1; iCnt <= 6; iCnt++) {
798 WorkVec.
PutRowIndex(iCnt, iNode1FirstPosIndex + iCnt);
799 WorkVec.
PutRowIndex(6 + iCnt, iNode2FirstPosIndex + iCnt);
817 const Vec3& tilde_f1,
818 const Vec3& tilde_f2,
824 DeformableJoint(uL, pDO, pCL, pN1, pN2, tilde_f1, tilde_f2, tilde_R1h, tilde_R2h, od, fOut),
869 for (
int iCnt = 1; iCnt <= 6; iCnt++) {
872 WMA.
PutRowIndex(6 + iCnt, iNode2FirstMomIndex + iCnt);
873 WMA.
PutColIndex(6 + iCnt, iNode2FirstPosIndex + iCnt);
897 AfterPredict(const_cast<VectorHandler&>(XCurr), const_cast<VectorHandler&>(XCurr));
997 for (
int iCnt = 1; iCnt <= 6; iCnt++) {
1000 WM.
PutRowIndex(6 + iCnt, iNode2FirstPosIndex + iCnt);
1001 WM.
PutColIndex(6 + iCnt, iNode2FirstPosIndex + iCnt);
1019 const Vec3& tilde_f1,
1020 const Vec3& tilde_f2,
1026 DeformableJoint(uL, pDO, pCL, pN1, pN2, tilde_f1, tilde_f2, tilde_R1h, tilde_R2h, od, fOut),
1071 for (
int iCnt = 1; iCnt <= 6; iCnt++) {
1072 WMA.
PutRowIndex(iCnt, iNode1FirstMomIndex + iCnt);
1073 WMA.
PutColIndex(iCnt, iNode1FirstPosIndex + iCnt);
1074 WMA.
PutRowIndex(6 + iCnt, iNode2FirstMomIndex + iCnt);
1075 WMA.
PutColIndex(6 + iCnt, iNode2FirstPosIndex + iCnt);
1179 for (
int iCnt = 1; iCnt <= 6; iCnt++) {
1182 WM.
PutRowIndex(6 + iCnt, iNode2FirstPosIndex + iCnt);
1183 WM.
PutColIndex(6 + iCnt, iNode2FirstPosIndex + iCnt);
1201 const Vec3& tilde_f1,
1202 const Vec3& tilde_f2,
1208 DeformableJoint(uL, pDO, pCL, pN1, pN2, tilde_f1, tilde_f2, tilde_R1h, tilde_R2h, od, fOut)
1313 for (
int iCnt = 1; iCnt <= 6; iCnt++) {
1316 WM.
PutRowIndex(6 + iCnt, iNode2FirstPosIndex + iCnt);
1317 WM.
PutColIndex(6 + iCnt, iNode2FirstPosIndex + iCnt);
1336 const Vec3& tilde_f1,
1337 const Vec3& tilde_f2,
1343 DeformableJoint(uL, pDO, pCL, pN1, pN2, tilde_f1, tilde_f2, tilde_R1h, tilde_R2h, od, fOut),
1471 for (
int iCnt = 1; iCnt <= 6; iCnt++) {
1474 WM.
PutRowIndex(6 + iCnt, iNode2FirstPosIndex + iCnt);
1475 WM.
PutColIndex(6 + iCnt, iNode2FirstPosIndex + iCnt);
VariableSubMatrixHandler & AssJac(VariableSubMatrixHandler &WorkMat, const VectorHandler &XCurr)
Mat3x3 MultRMRt(const Mat3x3 &m, const Mat3x3 &R)
void PutColIndex(integer iSubCol, integer iCol)
const Vec3 Zero3(0., 0., 0.)
Vec3 Cross(const Vec3 &v) const
std::ostream & Write(std::ostream &out, const char *sFill=" ") const
Vec3 MultRV(const Vec3 &v, const Mat3x3 &R)
virtual const Mat3x3 & GetRRef(void) const
virtual bool bToBeOutput(void) const
MatrixExpression< TransposedMatrix< MatrixDirectExpr< Matrix< T, N_rows, N_cols > > >, N_cols, N_rows > Transpose(const Matrix< T, N_rows, N_cols > &A)
ConstitutiveLaw< T, Tder > * pGetConstLaw(void) const
virtual void ResizeReset(integer)
const Vec3 & GetVec2(void) const
const MatCross_Manip MatCross
bool UseNetCDF(int out) const
void AssMats(FullSubMatrixHandler &WMA, FullSubMatrixHandler &WMB, doublereal dCoef)
FullSubMatrixHandler & SetFull(void)
virtual const Mat3x3 & GetRCurr(void) const
virtual Node::Type GetNodeType(void) const
bool bIsErgonomy(void) const
void Add(integer iRow, integer iCol, const Vec3 &v)
void AssVec(SubVectorHandler &WorkVec)
virtual unsigned int iGetPrivDataIdx(const char *s) const
virtual void InitialWorkSpaceDim(integer *piNumRows, integer *piNumCols) const
virtual Hint * ParseHint(DataManager *pDM, const char *s) const
std::ostream & Write(std::ostream &out, const FullMatrixHandler &m, const char *s, const char *s2)
virtual void AfterConvergence(const VectorHandler &X, const VectorHandler &XP)
virtual void Sub(integer iRow, const Vec3 &v)
const Tder & GetFDE(void) const
const Mat3x3 Eye3(1., 0., 0., 0., 1., 0., 0., 0., 1.)
virtual unsigned int iGetNumPrivData(void) const
virtual void InitialWorkSpaceDim(integer *piNumRows, integer *piNumCols) const =0
std::vector< Hint * > Hints
ElasticJointInv(unsigned int uL, const DofOwner *pDO, const ConstitutiveLaw6D *pCL, const StructNode *pN1, const StructNode *pN2, const Vec3 &tilde_f1, const Vec3 &tilde_f2, const Mat3x3 &tilde_R1h, const Mat3x3 &tilde_R2h, const OrientationDescription &od, flag fOut)
Vec3 GetVec(unsigned short int i) const
void PutMat22(const Mat3x3 &x)
Vec3 VecRot(const Mat3x3 &Phi)
virtual VariableSubMatrixHandler & InitialAssJac(VariableSubMatrixHandler &WorkMat, const VectorHandler &XCurr)
Vec3 MulTV(const Vec3 &v) const
virtual void AfterPredict(VectorHandler &X, VectorHandler &XP)
virtual void PutRowIndex(integer iSubRow, integer iRow)=0
void AssVec(SubVectorHandler &WorkVec)
void AssMats(FullSubMatrixHandler &WMA, FullSubMatrixHandler &WMB, doublereal dCoef)
const Vec3 & GetVec1(void) const
void PutMat11(const Mat3x3 &x)
Vec3 MatR2EulerAngles313(const Mat3x3 &R)
virtual void AfterConvergence(const VectorHandler &X, const VectorHandler &XP)
Matrix< T, 2, 2 > Inv(const Matrix< T, 2, 2 > &A)
ViscoElasticJoint(unsigned int uL, const DofOwner *pDO, const ConstitutiveLaw6D *pCL, const StructNode *pN1, const StructNode *pN2, const Vec3 &tilde_f1, const Vec3 &tilde_f2, const Mat3x3 &tilde_R1, const Mat3x3 &tilde_R2, const OrientationDescription &od, flag fOut)
const Vec6 Zero6(0., 0., 0., 0., 0., 0.)
virtual VariableSubMatrixHandler & InitialAssJac(VariableSubMatrixHandler &WorkMat, const VectorHandler &XCurr)
long GetCurrentStep(void) const
void Update(const VectorHandler &XCurr, InverseDynamics::Order iOrder=InverseDynamics::INVERSE_DYNAMICS)
ViscousJoint(unsigned int uL, const DofOwner *pDO, const ConstitutiveLaw6D *pCL, const StructNode *pN1, const StructNode *pN2, const Vec3 &tilde_f1, const Vec3 &tilde_f2, const Mat3x3 &tilde_R1, const Mat3x3 &tilde_R2, const OrientationDescription &od, flag fOut)
Vec3 MatR2EulerAngles123(const Mat3x3 &R)
void AssVec(SubVectorHandler &WorkVec)
Mat3x3 Rot(const Vec3 &phi)
virtual integer iGetFirstMomentumIndex(void) const =0
virtual integer iGetFirstPositionIndex(void) const
virtual const Vec3 & GetWCurr(void) const
virtual void AfterPredict(VectorHandler &X, VectorHandler &XP)
virtual std::ostream & Restart(std::ostream &out) const
std::ostream & Joints(void) const
void AfterConvergence(const T &Eps, const T &EpsPrime=mb_zero< T >())
void PutMat12(const Mat3x3 &x)
#define ASSERT(expression)
Mat3x3 MulTM(const Mat3x3 &m) const
VectorExpression< VectorCrossExpr< VectorLhsExpr, VectorRhsExpr >, 3 > Cross(const VectorExpression< VectorLhsExpr, 3 > &u, const VectorExpression< VectorRhsExpr, 3 > &v)
virtual void OutputPrepare_int(const std::string &type, OutputHandler &OH, std::string &name)
virtual const Vec3 & GetXCurr(void) const
virtual void Add(integer iRow, const Vec3 &v)
virtual void ResizeReset(integer, integer)
static void MultRMRtGammam1(Mat6x6 &M, const Mat3x3 &R, const Mat3x3 &Gammam1)
virtual doublereal dGetPrivData(unsigned int i) const
virtual void InitialWorkSpaceDim(integer *piNumRows, integer *piNumCols) const
Mat3x3 Transpose(void) const
const MatCrossCross_Manip MatCrossCross
virtual void InitialWorkSpaceDim(integer *piNumRows, integer *piNumCols) const
const doublereal * pGetMat(void) const
virtual VariableSubMatrixHandler & InitialAssJac(VariableSubMatrixHandler &WorkMat, const VectorHandler &XCurr)
void PutRowIndex(integer iSubRow, integer iRow)
const T & GetF(void) const
const doublereal * pGetVec(void) const
Vec3 MatR2EulerAngles321(const Mat3x3 &R)
virtual void WorkSpaceDim(integer *piNumRows, integer *piNumCols) const =0
virtual void AfterConvergence(const VectorHandler &X, const VectorHandler &XP)
void AssMats(FullSubMatrixHandler &WorkMatA, FullSubMatrixHandler &WorkMatB, doublereal dCoef)
virtual void AfterConvergence(const VectorHandler &X, const VectorHandler &XP)
virtual const Vec3 & GetVCurr(void) const
void Sub(integer iRow, integer iCol, const Vec3 &v)
virtual void InitialWorkSpaceDim(integer *piNumRows, integer *piNumCols) const
Mat3x3 MulMT(const Mat3x3 &m) const
virtual void SetValue(DataManager *pDM, VectorHandler &X, VectorHandler &XP, SimulationEntity::Hints *h=0)
const Tder & GetFDEPrime(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
void Update(const T &Eps, const T &EpsPrime=mb_zero< T >())
virtual void AfterPredict(VectorHandler &X, VectorHandler &XP)
void AssVec(SubVectorHandler &WorkVec)
unsigned int GetLabel(void) const
void AssMats(FullSubMatrixHandler &WMA, FullSubMatrixHandler &WMB, doublereal dCoef)
virtual VariableSubMatrixHandler & InitialAssJac(VariableSubMatrixHandler &WorkMat, const VectorHandler &XCurr)
ElasticJoint(unsigned int uL, const DofOwner *pDO, const ConstitutiveLaw6D *pCL, const StructNode *pN1, const StructNode *pN2, const Vec3 &tilde_f1, const Vec3 &tilde_f2, const Mat3x3 &tilde_R1h, const Mat3x3 &tilde_R2h, const OrientationDescription &od, flag fOut)
Mat3x3 DRot_I(const Vec3 &phi)
bool UseText(int out) const
virtual void AfterPredict(VectorHandler &X, VectorHandler &XP)
void PutMat21(const Mat3x3 &x)