55 pNode1(pN1), pNode2(pN2), R1h(R1), R2h(R2),
112 for (
unsigned i = 0; i < ph->size(); i++) {
122 }
else if (dynamic_cast<Joint::ReactionsHint *>(pjh)) {
131 pedantic_cout(
"DriveHingeJoint(" <<
uLabel <<
"): "
132 "creating drive from hint[" << i <<
"]..." << std::endl);
136 silent_cerr(
"DriveHingeJoint(" <<
uLabel <<
"): "
137 "unable to create drive "
138 "after hint #" << i << std::endl);
152 if (strncasecmp(s,
"hinge{" ,
STRLENOF(
"hinge{" )) == 0)
156 if (strcmp(&s[1],
"}") != 0) {
175 const char *prefix,
bool bInitial)
const
180 << prefix << iIndex + 1 <<
"->" << iIndex + 3 <<
": "
181 "reaction couples [mx,my,mz]" << std::endl;
186 << prefix << iIndex + 1 <<
"->" << iIndex + 3 <<
": "
187 "reaction couple derivatives [mPx,mPy,mPz]" << std::endl;
193 static const char xyz[] =
"xyz";
194 static const char *
dof[] = {
"reaction couple m",
"reaction couple derivative mP" };
195 static const char *
eq[] = {
"orientation constraint g",
"orientation constraint derivative w" };
199 bool bInitial,
int i)
const
212 std::ostringstream os;
213 os <<
"DriveHingeJoint(" <<
GetLabel() <<
")";
216 std::string name = os.str();
217 for (i = 0; i < iend; i++) {
219 os.seekp(0, std::ios_base::end);
220 os <<
": " <<
dof[i/3] <<
xyz[i%3];
226 os <<
": " <<
dof[i/3] <<
xyz[i%3];
233 const char *prefix,
bool bInitial)
const
238 << prefix << iIndex + 1 <<
"->" << iIndex + 3 <<
": "
239 "orientation constraints [gx1=gx2,gy1=gy2,gz1=gz2]" << std::endl;
244 << prefix << iIndex + 1 <<
"->" << iIndex + 3 <<
": "
245 "angular velocity constraints [wx1=wx2,wy1=wy2,wz1=wz2]" << std::endl;
254 bool bInitial,
int i)
const
267 std::ostringstream os;
268 os <<
"DriveHingeJoint(" <<
GetLabel() <<
")";
271 std::string name = os.str();
272 for (i = 0; i < iend; i++) {
274 os.seekp(0, std::ios_base::end);
275 os <<
": " <<
eq[i/3] <<
xyz[i%3];
281 os <<
": " <<
eq[i/3] <<
xyz[i%3];
299 unsigned int idx = 0;
312 if (s[1] ==
'\0' || s[2] !=
'\0') {
357 DEBUGCOUT(
"Entering DriveHingeJoint::AssJac()" << std::endl);
375 for (
int iCnt = 1; iCnt <= 3; iCnt++) {
378 WM.
PutRowIndex(3 + iCnt, iNode2FirstMomIndex + iCnt);
379 WM.
PutColIndex(3 + iCnt, iNode2FirstPosIndex + iCnt);
380 WM.
PutRowIndex(6 + iCnt, iFirstReactionIndex + iCnt);
381 WM.
PutColIndex(6 + iCnt, iFirstReactionIndex + iCnt);
406 RRef = GammaRefm1*R1T;
418 WM.
Add(1, 1, MCross);
419 WM.
Sub(4, 1, MCross);
436 DEBUGCOUT(
"Entering DriveHingeJoint::AssRes()" << std::endl);
450 for (
int iCnt = 1; iCnt <= 3; iCnt++) {
451 WorkVec.
PutRowIndex(iCnt, iNode1FirstMomIndex + iCnt);
452 WorkVec.
PutRowIndex(3 + iCnt, iNode2FirstMomIndex + iCnt);
453 WorkVec.
PutRowIndex(6 + iCnt, iFirstReactionIndex + iCnt);
456 M =
Vec3(XCurr, iFirstReactionIndex+1);
480 WorkVec.
Add(1, MTmp);
481 WorkVec.
Sub(3 + 1, MTmp);
493 DEBUGCOUT(
"Entering DriveHingeJoint::InitialAssJac()" << std::endl);
507 integer iNode1FirstVelIndex = iNode1FirstPosIndex + 6;
508 integer iNode2FirstVelIndex = iNode2FirstPosIndex + 6;
509 integer iReactionPrimeIndex = iFirstReactionIndex + 3;
512 for (
int iCnt = 1; iCnt <= 3; iCnt++) {
515 WM.
PutRowIndex(3 + iCnt, iNode1FirstVelIndex + iCnt);
516 WM.
PutColIndex(3 + iCnt, iNode1FirstVelIndex + iCnt);
517 WM.
PutRowIndex(6 + iCnt, iNode2FirstPosIndex + iCnt);
518 WM.
PutColIndex(6 + iCnt, iNode2FirstPosIndex + iCnt);
519 WM.
PutRowIndex(9 + iCnt, iNode2FirstVelIndex + iCnt);
520 WM.
PutColIndex(9 + iCnt, iNode2FirstVelIndex + iCnt);
521 WM.
PutRowIndex(12 + iCnt, iFirstReactionIndex + iCnt);
522 WM.
PutColIndex(12 + iCnt, iFirstReactionIndex + iCnt);
523 WM.
PutRowIndex(15 + iCnt, iReactionPrimeIndex + iCnt);
524 WM.
PutColIndex(15 + iCnt, iReactionPrimeIndex + iCnt);
528 Mat3x3 RaT(Ra.Transpose());
536 WM.
Add(3 + 1, 3 + 1, MTmp);
537 WM.
Sub(6 + 1, 1, MTmp);
538 WM.
Sub(9 + 1, 3 + 1, MTmp);
540 MTmp = Wa.Cross(MTmp) + MPrimeTmp;
541 WM.
Add(3 + 1, 1, MTmp);
542 WM.
Sub(9 + 1, 1, MTmp);
544 WM.
Add(6 + 1, 12 + 1, Ra);
545 WM.
Add(9 + 1, 15 + 1, Ra);
546 WM.
Sub(1, 12 + 1, Ra);
547 WM.
Sub(3 + 1, 15 + 1, Ra);
550 WM.
Add(9 + 1, 12 + 1, MTmp);
551 WM.
Sub(3 + 1, 12 + 1, MTmp);
553 WM.
Add(12 + 1, 6 + 1, RaT);
554 WM.
Sub(12 + 1, 1, RaT);
555 WM.
Sub(15 + 1, 3 + 1, RaT);
556 WM.
Add(15 + 1, 9 + 1, RaT);
568 DEBUGCOUT(
"Entering DriveHingeJoint::InitialAssRes()" << std::endl);
580 integer iNode1FirstVelIndex = iNode1FirstPosIndex + 6;
581 integer iNode2FirstVelIndex = iNode2FirstPosIndex + 6;
582 integer iReactionPrimeIndex = iFirstReactionIndex + 3;
585 for (
int iCnt = 1; iCnt <= 3; iCnt++) {
586 WorkVec.
PutRowIndex(iCnt, iNode1FirstPosIndex + iCnt);
587 WorkVec.
PutRowIndex(3 + iCnt, iNode1FirstVelIndex + iCnt);
588 WorkVec.
PutRowIndex(6 + iCnt, iNode2FirstPosIndex + iCnt);
589 WorkVec.
PutRowIndex(9 + iCnt, iNode2FirstVelIndex + iCnt);
590 WorkVec.
PutRowIndex(12 + iCnt, iFirstReactionIndex + iCnt);
591 WorkVec.
PutRowIndex(15 + iCnt, iReactionPrimeIndex + iCnt);
595 Mat3x3 R1T(R1.Transpose());
599 M =
Vec3(XCurr, iFirstReactionIndex+1);
600 Vec3 MPrime =
Vec3(XCurr, iReactionPrimeIndex+1);
603 Vec3 MPrimeTmp(Wa.Cross(MTmp) + R1*MPrime);
608 Vec3 ThetaPrime = R1T*(Wb-Wa);
610 WorkVec.
Add(1, MTmp);
611 WorkVec.
Add(3 + 1, MPrimeTmp);
612 WorkVec.
Sub(6 + 1, MTmp);
613 WorkVec.
Sub(9 + 1, MPrimeTmp);
614 WorkVec.
Add(12 + 1,
Get() - ThetaCurr);
616 ThetaPrime -=
GetP();
618 WorkVec.
Sub(15 + 1, ThetaPrime);
virtual ~DriveHingeJoint(void)
virtual std::ostream & Restart(std::ostream &out) const
void PutColIndex(integer iSubCol, integer iCol)
const Vec3 Zero3(0., 0., 0.)
virtual const Mat3x3 & GetRRef(void) const
virtual bool bToBeOutput(void) const
DriveHingeJoint(unsigned int uL, const DofOwner *pDO, const TplDriveCaller< Vec3 > *pDC, const StructNode *pN1, const StructNode *pN2, const Mat3x3 &R1, const Mat3x3 &R2, flag fOut)
virtual bool bIsDifferentiable(void) const
#define MBDYN_EXCEPT_ARGS
virtual void ResizeReset(integer)
const MatCross_Manip MatCross
virtual void AfterPredict(VectorHandler &X, VectorHandler &XP)
FullSubMatrixHandler & SetFull(void)
virtual const Mat3x3 & GetRCurr(void) const
virtual Node::Type GetNodeType(void) const
void Add(integer iRow, integer iCol, const Vec3 &v)
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 Sub(integer iRow, const Vec3 &v)
const Mat3x3 Eye3(1., 0., 0., 0., 1., 0., 0., 0., 1.)
virtual Hint * ParseHint(DataManager *pDM, const char *s) const
virtual void Output(OutputHandler &OH) const
virtual doublereal dGetPrivData(unsigned int i=0) const
virtual SubVectorHandler & AssRes(SubVectorHandler &WorkVec, doublereal dCoef, const VectorHandler &XCurr, const VectorHandler &XPrimeCurr)
virtual const Vec3 & GetWRef(void) const
void SetValue(DataManager *pDM, VectorHandler &X, VectorHandler &XP, SimulationEntity::Hints *ph=0)
std::vector< Hint * > Hints
Vec3 GetVec(unsigned short int i) const
void Set(const TplDriveCaller< T > *pDC)
Vec3 VecRot(const Mat3x3 &Phi)
virtual void WorkSpaceDim(integer *piNumRows, integer *piNumCols) const
virtual Vec3 GetP(void) const
static const char * dof[]
virtual void PutRowIndex(integer iSubRow, integer iRow)=0
virtual unsigned int iGetNumPrivData(void) const
virtual std::ostream & DescribeDof(std::ostream &out, const char *prefix="", bool bInitial=false) const
TplDriveCaller< Vec3 > * pGetDriveCaller(void) const
unsigned int iGetPrivDataIdx(const char *s) const
const doublereal & dGet(unsigned short int iRow) const
virtual void InitialWorkSpaceDim(integer *piNumRows, integer *piNumCols) const
const StructNode * pNode2
virtual integer iGetFirstMomentumIndex(void) const =0
virtual integer iGetFirstPositionIndex(void) const
virtual VariableSubMatrixHandler & AssJac(VariableSubMatrixHandler &WorkMat, doublereal dCoef, const VectorHandler &XCurr, const VectorHandler &XPrimeCurr)
virtual const Vec3 & GetWCurr(void) const
virtual std::ostream & Restart(std::ostream &out) const
std::ostream & Joints(void) const
virtual VariableSubMatrixHandler & InitialAssJac(VariableSubMatrixHandler &WorkMat, const VectorHandler &XCurr)
virtual std::ostream & Restart(std::ostream &out) const =0
TplDriveCaller< T > * pCreateDrive(DataManager *pDM) const
#define ASSERT(expression)
Vec3 MatR2EulerAngles(const Mat3x3 &R)
virtual void Add(integer iRow, const Vec3 &v)
virtual void ResizeReset(integer, integer)
const StructNode * pNode1
Mat3x3 Transpose(void) const
virtual SubVectorHandler & InitialAssRes(SubVectorHandler &WorkVec, const VectorHandler &XCurr)
void AssMat(FullSubMatrixHandler &WM, doublereal dCoef)
void AssVec(SubVectorHandler &WorkVec, doublereal dCoef)
void PutRowIndex(integer iSubRow, integer iRow)
virtual std::ostream & DescribeEq(std::ostream &out, const char *prefix="", bool bInitial=false) const
void Sub(integer iRow, integer iCol, const Vec3 &v)
virtual integer iGetFirstIndex(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
unsigned int GetLabel(void) const
Mat3x3 DRot_I(const Vec3 &phi)