43 pNode1(pN1), pNode2(pN2), Rv(RvTmp), p(pTmp), F(
Zero3)
59 "INDEX ERROR in InLineJoint::GetEqType");
67 Joint::Restart(out) <<
", in line, not implemented yet" << std::endl;
91 Vec3 FTmp(RvTmp*(
F*dCoef));
94 Vec3 Tmp1_1(RvTmp.GetVec(1).Cross(x2mx1));
95 Vec3 Tmp1_2(RvTmp.GetVec(2).Cross(x2mx1));
96 for(
int iCnt = 1; iCnt <= 3; iCnt++) {
98 WM.
PutItem(iCnt, iNode1FirstMomIndex+iCnt,
99 iFirstReactionIndex+1, -d);
100 WM.
PutItem(3+iCnt, iNode2FirstMomIndex+iCnt,
101 iFirstReactionIndex+1, d);
103 WM.
PutItem(6+iCnt, iFirstReactionIndex+1,
104 iNode1FirstPosIndex+iCnt, -d);
105 WM.
PutItem(9+iCnt, iFirstReactionIndex+1,
106 iNode2FirstPosIndex+iCnt, d);
108 d = RvTmp.dGet(iCnt, 2);
109 WM.
PutItem(12+iCnt, iNode1FirstMomIndex+iCnt,
110 iFirstReactionIndex+2, -d);
111 WM.
PutItem(15+iCnt, iNode2FirstMomIndex+iCnt,
112 iFirstReactionIndex+2, d);
114 WM.
PutItem(18+iCnt, iFirstReactionIndex+2,
115 iNode1FirstPosIndex+iCnt, -d);
116 WM.
PutItem(21+iCnt, iFirstReactionIndex+2,
117 iNode2FirstPosIndex+iCnt, d);
119 d = Tmp1_1.dGet(iCnt);
120 WM.
PutItem(24+iCnt, iNode1FirstMomIndex+3+iCnt,
121 iFirstReactionIndex+1, d);
123 WM.
PutItem(27+iCnt, iFirstReactionIndex+1,
124 iNode1FirstPosIndex+3+iCnt, d);
126 d = Tmp1_2.dGet(iCnt);
127 WM.
PutItem(30+iCnt, iNode1FirstMomIndex+3+iCnt,
128 iFirstReactionIndex+2, d);
130 WM.
PutItem(33+iCnt, iFirstReactionIndex+2,
131 iNode1FirstPosIndex+3+iCnt, d);
134 WM.
PutCross(36+1, iNode1FirstMomIndex,
135 iNode1FirstPosIndex+3, FTmp);
136 WM.
PutCross(42+1, iNode1FirstMomIndex+3,
137 iNode1FirstPosIndex, -FTmp);
138 WM.
PutMat3x3(48+1, iNode1FirstMomIndex+3,
140 WM.
PutCross(57+1, iNode1FirstMomIndex+3,
141 iNode2FirstPosIndex, FTmp);
142 WM.
PutCross(63+1, iNode2FirstMomIndex,
143 iNode2FirstPosIndex+3, -FTmp);
163 for (
int iCnt = 1; iCnt <= 6; iCnt++) {
164 WorkVec.
PutRowIndex(iCnt, iNode1FirstMomIndex+iCnt);
165 WorkVec.
PutRowIndex(6+iCnt, iNode2FirstMomIndex+iCnt);
176 F.
Put(1, XCurr(iFirstReactionIndex+1));
177 F.
Put(2, XCurr(iFirstReactionIndex+2));
180 WorkVec.
Add(1, FTmp);
181 WorkVec.
Add(4, x2mx1.Cross(FTmp));
182 WorkVec.
Sub(7, FTmp);
241 integer iReactionPrimeIndex = iFirstReactionIndex+2;
244 for(
int iCnt = 1; iCnt <= 12; iCnt++) {
252 for(
int iCnt = 1; iCnt <= 4; iCnt++) {
263 Vec3 FPrime(XCurr(iReactionPrimeIndex+1),
264 XCurr(iReactionPrimeIndex+2),
268 Vec3 FPrimeTmp(RvTmp*FPrime);
270 Vec3 v1(RvTmp.GetVec(1));
271 Vec3 Tmp1_1(v1.Cross(x2mx1));
272 Vec3 Tmp2_1(Omega.Cross(v1));
273 Vec3 Tmp3_1((Omega.Cross(x2mx1)-xp2mxp1).
Cross(v1));
274 Vec3 Tmp4_1(-(xp2mxp1.Cross(v1)+x2mx1.Cross(Tmp2_1)));
275 Vec3 v2(RvTmp.GetVec(2));
276 Vec3 Tmp1_2(v2.Cross(x2mx1));
277 Vec3 Tmp2_2(Omega.Cross(v2));
278 Vec3 Tmp3_2((Omega.Cross(x2mx1)-xp2mxp1).
Cross(v2));
279 Vec3 Tmp4_2(-(xp2mxp1.Cross(v2)+x2mx1.Cross(Tmp2_2)));
280 for(
int iCnt = 1; iCnt <= 3; iCnt++) {
307 d = Tmp1_1.dGet(iCnt);
314 d = Tmp1_2.dGet(iCnt);
321 d = Tmp2_1.dGet(iCnt);
328 d = Tmp2_2.dGet(iCnt);
335 d = Tmp3_1.dGet(iCnt);
338 d = Tmp3_2.dGet(iCnt);
341 d = Tmp4_1.dGet(iCnt);
344 d = Tmp4_2.dGet(iCnt);
353 WM.
Add(10, 19, MTmp);
358 WM.
Sub(19, 10, MTmp);
364 WM.
Add(10, 10, MTmp);
372 WM.
Add(10, 13, MTmp);
394 integer iReactionPrimeIndex = iFirstReactionIndex+2;
397 for(
int iCnt = 1; iCnt <= 12; iCnt++) {
398 WorkVec.
PutRowIndex(iCnt, iNode1FirstPosIndex+iCnt);
399 WorkVec.
PutRowIndex(12+iCnt, iNode2FirstPosIndex+iCnt);
415 F.
Put(1, XCurr(iFirstReactionIndex+1));
416 F.
Put(2, XCurr(iFirstReactionIndex+2));
417 Vec3 FPrime(XCurr(iReactionPrimeIndex+1),
418 XCurr(iReactionPrimeIndex+2),
421 Vec3 FPrimeTmp(RvTmp*FPrime);
422 Vec3 Tmp(Omega.Cross(FTmp)+FPrimeTmp);
424 WorkVec.
Add(1, FTmp);
425 WorkVec.
Add(4, x2mx1.Cross(FTmp));
427 WorkVec.
Add(10, xp2mxp1.Cross(FTmp)+x2mx1.Cross(Tmp));
428 WorkVec.
Sub(13, FTmp);
429 WorkVec.
Sub(19, Tmp);
433 WorkVec.
PutCoef(27, x2mx1.Dot(RvTmp.GetVec(1).Cross(Omega))-RvTmp.GetVec(1).Dot(xp2mxp1));
434 WorkVec.
PutCoef(28, x2mx1.Dot(RvTmp.GetVec(2).Cross(Omega))-RvTmp.GetVec(2).Dot(xp2mxp1));
454 pNode1(pN1), pNode2(pN2), Rv(RvTmp), p(pTmp), q(qTmp), F(
Zero3)
469 Joint::Restart(out) <<
", in line, not implemented yet" << std::endl;
494 Vec3 FTmp(RvTmp*(
F*dCoef));
497 Vec3 Tmp1_1(RvTmp.GetVec(1).Cross(x2qmx1));
498 Vec3 Tmp1_2(RvTmp.GetVec(2).Cross(x2qmx1));
499 Vec3 Tmp2_1(qTmp.Cross(RvTmp.GetVec(1)));
500 Vec3 Tmp2_2(qTmp.Cross(RvTmp.GetVec(2)));
501 for(
int iCnt = 1; iCnt <= 3; iCnt++) {
503 WM.
PutItem(iCnt, iNode1FirstMomIndex+iCnt,
504 iFirstReactionIndex+1, -d);
505 WM.
PutItem(3+iCnt, iNode2FirstMomIndex+iCnt,
506 iFirstReactionIndex+1, d);
508 WM.
PutItem(6+iCnt, iFirstReactionIndex+1,
509 iNode1FirstPosIndex+iCnt, -d);
510 WM.
PutItem(9+iCnt, iFirstReactionIndex+1,
511 iNode2FirstPosIndex+iCnt, d);
513 d = RvTmp.dGet(iCnt, 2);
514 WM.
PutItem(12+iCnt, iNode1FirstMomIndex+iCnt,
515 iFirstReactionIndex+2, -d);
516 WM.
PutItem(15+iCnt, iNode2FirstMomIndex+iCnt,
517 iFirstReactionIndex+2, d);
519 WM.
PutItem(18+iCnt, iFirstReactionIndex+2,
520 iNode1FirstPosIndex+iCnt, -d);
521 WM.
PutItem(21+iCnt, iFirstReactionIndex+2,
522 iNode2FirstPosIndex+iCnt, d);
524 d = Tmp1_1.dGet(iCnt);
525 WM.
PutItem(24+iCnt, iNode1FirstMomIndex+3+iCnt,
526 iFirstReactionIndex+1, d);
528 WM.
PutItem(27+iCnt, iFirstReactionIndex+1,
529 iNode1FirstPosIndex+3+iCnt, d);
531 d = Tmp1_2.dGet(iCnt);
532 WM.
PutItem(30+iCnt, iNode1FirstMomIndex+3+iCnt,
533 iFirstReactionIndex+2, d);
535 WM.
PutItem(33+iCnt, iFirstReactionIndex+2,
536 iNode1FirstPosIndex+3+iCnt, d);
538 d = Tmp2_1.dGet(iCnt);
539 WM.
PutItem(36+iCnt, iNode2FirstMomIndex+3+iCnt,
540 iFirstReactionIndex+1, d);
542 WM.
PutItem(39+iCnt, iFirstReactionIndex+1,
543 iNode2FirstPosIndex+3+iCnt, d);
545 d = Tmp2_2.
dGet(iCnt);
546 WM.
PutItem(42+iCnt, iNode2FirstMomIndex+3+iCnt,
547 iFirstReactionIndex+2, d);
549 WM.
PutItem(45+iCnt, iFirstReactionIndex+2,
550 iNode2FirstPosIndex+3+iCnt, d);
553 WM.
PutCross(48+1, iNode1FirstMomIndex,
554 iNode1FirstPosIndex+3, FTmp);
555 WM.
PutCross(54+1, iNode1FirstMomIndex+3,
556 iNode1FirstPosIndex, -FTmp);
557 WM.
PutMat3x3(60+1, iNode1FirstMomIndex+3,
559 WM.
PutCross(69+1, iNode1FirstMomIndex+3,
560 iNode2FirstPosIndex, FTmp);
561 WM.
PutCross(75+1, iNode2FirstMomIndex,
562 iNode2FirstPosIndex+3, -FTmp);
565 WM.
PutMat3x3(81+1, iNode1FirstMomIndex+3,
566 iNode2FirstPosIndex+3, -MTmp);
567 WM.
PutMat3x3(90+1, iNode2FirstMomIndex+3,
568 iNode2FirstPosIndex+3, -MTmp);
570 WM.
PutMat3x3(99+1, iNode2FirstMomIndex+3,
591 for (
int iCnt = 1; iCnt <= 6; iCnt++) {
592 WorkVec.
PutRowIndex(iCnt, iNode1FirstMomIndex+iCnt);
593 WorkVec.
PutRowIndex(6+iCnt, iNode2FirstMomIndex+iCnt);
605 F.
Put(1, XCurr(iFirstReactionIndex+1));
606 F.
Put(2, XCurr(iFirstReactionIndex+2));
609 WorkVec.
Add(1, FTmp);
610 WorkVec.
Add(4, x2qmx1.Cross(FTmp));
611 WorkVec.
Sub(7, FTmp);
612 WorkVec.
Sub(10, qTmp.Cross(FTmp));
625 "INDEX ERROR in InLineWithOffsetJoint::GetEqType");
626 if ((i==13) or (i==14)) {
655 integer iReactionPrimeIndex = iFirstReactionIndex+2;
658 for(
int iCnt = 1; iCnt <= 12; iCnt++) {
666 for(
int iCnt = 1; iCnt <= 4; iCnt++) {
680 Vec3 FPrime(XCurr(iReactionPrimeIndex+1),
681 XCurr(iReactionPrimeIndex+2),
684 Vec3 FPrimeTmp(RvTmp*FPrime);
686 Vec3 v1(RvTmp.GetVec(1));
687 Vec3 Tmp1_1(v1.Cross(x2qmx1));
688 Vec3 Tmp2_1(Omega1.Cross(v1));
689 Vec3 Tmp3_1((Omega1.Cross(x2qmx1)-xp2qmxp1).
Cross(v1));
690 Vec3 Tmp4_1(-(xp2qmxp1.Cross(v1)+x2qmx1.Cross(Tmp2_1)));
692 Vec3 Tmp5_1(qTmp.Cross(v1));
693 Vec3 Tmp6_1(qTmp.Cross(v1.Cross(Omega2-Omega1)));
694 Vec3 Tmp7_1(qTmp.Cross(Omega1.Cross(v1))-v1.Cross(Omega2.Cross(qTmp)));
696 Vec3 v2(RvTmp.GetVec(2));
697 Vec3 Tmp1_2(v2.Cross(x2qmx1));
698 Vec3 Tmp2_2(Omega1.Cross(v2));
699 Vec3 Tmp3_2((Omega1.Cross(x2qmx1)-xp2qmxp1).
Cross(v2));
700 Vec3 Tmp4_2(-(xp2qmxp1.Cross(v2)+x2qmx1.Cross(Tmp2_2)));
702 Vec3 Tmp5_2(qTmp.Cross(v2));
703 Vec3 Tmp6_2(qTmp.Cross(v2.Cross(Omega2-Omega1)));
704 Vec3 Tmp7_2(qTmp.Cross(Omega1.Cross(v2))-v2.Cross(Omega2.Cross(qTmp)));
706 for(
int iCnt = 1; iCnt <= 3; iCnt++) {
733 d = Tmp1_1.dGet(iCnt);
740 d = Tmp1_2.dGet(iCnt);
747 d = Tmp2_1.dGet(iCnt);
754 d = Tmp2_2.dGet(iCnt);
761 d = Tmp3_1.dGet(iCnt);
764 d = Tmp3_2.dGet(iCnt);
767 d = Tmp4_1.dGet(iCnt);
770 d = Tmp4_2.dGet(iCnt);
773 d = Tmp5_1.dGet(iCnt);
780 d = Tmp5_2.dGet(iCnt);
787 d = Tmp6_1.dGet(iCnt);
790 d = Tmp6_2.dGet(iCnt);
793 d = Tmp7_1.
dGet(iCnt);
796 d = Tmp7_2.
dGet(iCnt);
805 WM.
Add(10, 19, MTmp);
810 WM.
Sub(19, 10, MTmp);
816 WM.
Add(10, 10, MTmp);
824 WM.
Add(10, 13, MTmp);
831 WM.
Add(16, 16, MTmp);
832 WM.
Add(22, 22, MTmp);
835 WM.
Sub(10, 22, MTmp);
839 WM.
Add(22, 10, MTmp);
842 WM.
Add(22, 16, MTmp);
843 WM.
Sub(10, 16, MTmp);
865 integer iReactionPrimeIndex = iFirstReactionIndex+2;
868 for(
int iCnt = 1; iCnt <= 12; iCnt++) {
869 WorkVec.
PutRowIndex(iCnt, iNode1FirstPosIndex+iCnt);
870 WorkVec.
PutRowIndex(12+iCnt, iNode2FirstPosIndex+iCnt);
888 F.
Put(1, XCurr(iFirstReactionIndex+1));
889 F.
Put(2, XCurr(iFirstReactionIndex+2));
890 Vec3 FPrime(XCurr(iReactionPrimeIndex+1),
891 XCurr(iReactionPrimeIndex+2),
894 Vec3 FPrimeTmp(RvTmp*FPrime);
895 Vec3 Tmp(Omega1.Cross(FTmp)+FPrimeTmp);
897 WorkVec.
Add(1, FTmp);
898 WorkVec.
Add(4, x2qmx1.Cross(FTmp));
900 WorkVec.
Add(10, xp2qmxp1.Cross(FTmp)+x2qmx1.Cross(Tmp));
901 WorkVec.
Sub(13, FTmp);
902 WorkVec.
Sub(16, qTmp.Cross(FTmp));
903 WorkVec.
Sub(19, Tmp);
904 WorkVec.
Sub(22, qTmp.Cross(Tmp)+(Omega2.Cross(qTmp)).
Cross(FTmp));
908 WorkVec.
PutCoef(27, x2qmx1.Dot(RvTmp.GetVec(1).Cross(Omega1))-RvTmp.GetVec(1).Dot(xp2qmxp1));
909 WorkVec.
PutCoef(28, x2qmx1.Dot(RvTmp.GetVec(2).Cross(Omega1))-RvTmp.GetVec(2).Dot(xp2qmxp1));
void PutColIndex(integer iSubCol, integer iCol)
void PutMat3x3(integer iSubIt, integer iFirstRow, integer iFirstCol, const Mat3x3 &m)
const Vec3 Zero3(0., 0., 0.)
Vec3 Cross(const Vec3 &v) const
#define ASSERTMSGBREAK(expr, msg)
virtual const Mat3x3 & GetRRef(void) const
virtual bool bToBeOutput(void) const
#define DEBUGCOUTFNAME(fname)
virtual void ResizeReset(integer)
const MatCross_Manip MatCross
bool UseNetCDF(int out) const
FullSubMatrixHandler & SetFull(void)
virtual const Mat3x3 & GetRCurr(void) const
doublereal Dot(const Vec3 &v) const
void Add(integer iRow, integer iCol, const Vec3 &v)
virtual std::ostream & Restart(std::ostream &out) const
SubVectorHandler & InitialAssRes(SubVectorHandler &WorkVec, const VectorHandler &XCurr)
virtual void Sub(integer iRow, const Vec3 &v)
DofOrder::Order GetEqType(unsigned int i) const
void PutCoef(integer iRow, integer iCol, const doublereal &dCoef)
void ResizeReset(integer iNewRow, integer iNewCol)
virtual const Vec3 & GetWRef(void) const
void PutCross(integer iSubIt, integer iFirstRow, integer iFirstCol, const Vec3 &v)
Vec3 GetVec(unsigned short int i) const
const StructNode * pNode2
virtual void PutRowIndex(integer iSubRow, integer iRow)=0
virtual std::ostream & Restart(std::ostream &out) const
void PutItem(integer iSubIt, integer iRow, integer iCol, const doublereal &dCoef)
VariableSubMatrixHandler & InitialAssJac(VariableSubMatrixHandler &WorkMat, const VectorHandler &XCurr)
const StructNode * pNode1
VariableSubMatrixHandler & InitialAssJac(VariableSubMatrixHandler &WorkMat, const VectorHandler &XCurr)
long GetCurrentStep(void) const
const doublereal & dGet(unsigned short int iRow) const
virtual void Output(OutputHandler &OH) const
virtual integer iGetFirstMomentumIndex(void) const =0
virtual integer iGetFirstPositionIndex(void) const
virtual const Vec3 & GetWCurr(void) const
virtual std::ostream & Restart(std::ostream &out) const
DofOrder::Order GetEqType(unsigned int i) const
std::ostream & Joints(void) const
const StructNode * pNode1
SubVectorHandler & AssRes(SubVectorHandler &WorkVec, doublereal dCoef, const VectorHandler &XCurr, const VectorHandler &XPrimeCurr)
#define ASSERT(expression)
InLineWithOffsetJoint(unsigned int uL, const DofOwner *pDO, const StructNode *pN1, const StructNode *pN2, const Mat3x3 &RvTmp, const Vec3 &pTmp, const Vec3 &qTmp, flag fOut)
SubVectorHandler & InitialAssRes(SubVectorHandler &WorkVec, const VectorHandler &XCurr)
virtual void PutCoef(integer iRow, const doublereal &dCoef)=0
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)
Mat3x3 Transpose(void) const
const MatCrossCross_Manip MatCrossCross
void PutRowIndex(integer iSubRow, integer iRow)
VariableSubMatrixHandler & AssJac(VariableSubMatrixHandler &WorkMat, doublereal dCoef, const VectorHandler &XCurr, const VectorHandler &XPrimeCurr)
const doublereal * pGetVec(void) 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 const Vec3 & GetVCurr(void) const
void Sub(integer iRow, integer iCol, const Vec3 &v)
~InLineWithOffsetJoint(void)
SparseSubMatrixHandler & SetSparse(void)
virtual integer iGetFirstIndex(void) const
void OutputPrepare(OutputHandler &OH)
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
InLineJoint(unsigned int uL, const DofOwner *pDO, const StructNode *pN1, const StructNode *pN2, const Mat3x3 &RvTmp, const Vec3 &pTmp, flag fOut)
unsigned int GetLabel(void) const
virtual void Output(OutputHandler &OH) const
void Put(unsigned short int iRow, const doublereal &dCoef)
const StructNode * pNode2
bool UseText(int out) const
virtual unsigned int iGetNumDof(void) const