44 #ifndef MBDYN_X_DISTANCE_JOINT
55 pNode1(pN1), pNode2(pN2), v(
Zero3), dAlpha(0.)
80 if (strcmp(s,
"d") == 0) {
116 DEBUGCOUT(
"Entering DistanceJoint::AssJac()" << std::endl);
156 for (
int iCnt = 1; iCnt <= 3; iCnt++) {
158 WM.
PutItem(iCnt, iFirstReactionIndex+iCnt,
159 iNode1FirstPosIndex+iCnt, -dCoef);
162 WM.
PutItem(3+iCnt, iFirstReactionIndex+iCnt,
163 iNode2FirstPosIndex+iCnt, dCoef);
166 WM.
PutItem(6+iCnt, iNode1FirstMomIndex+iCnt,
167 iFirstReactionIndex+iCnt, -
dAlpha);
170 WM.
PutItem(9+iCnt, iNode2FirstMomIndex+iCnt,
171 iFirstReactionIndex+iCnt,
dAlpha);
174 WM.
PutItem(12+iCnt, iFirstReactionIndex+iCnt,
175 iFirstReactionIndex+iCnt, -dDistance);
178 WM.
PutItem(15+iCnt, iNode1FirstMomIndex+iCnt,
179 iFirstReactionIndex+4, -
v.
dGet(iCnt));
182 WM.
PutItem(18+iCnt, iNode2FirstMomIndex+iCnt,
183 iFirstReactionIndex+4,
v.
dGet(iCnt));
187 ASSERT(d > std::numeric_limits<doublereal>::epsilon());
188 if (d > std::numeric_limits<doublereal>::epsilon()) {
191 for (
int iCnt = 3; iCnt > 0; iCnt--) {
192 WM.
PutItem(21+iCnt, iFirstReactionIndex+4,
193 iFirstReactionIndex+iCnt,
v.
dGet(iCnt)/d);
207 DEBUGCOUT(
"Entering DistanceJoint::AssRes()" << std::endl);
219 for (
int iCnt = 1; iCnt <= 3; iCnt++) {
221 WorkVec.
PutRowIndex(iCnt, iNode1FirstMomIndex+iCnt);
224 WorkVec.
PutRowIndex(3+iCnt, iNode2FirstMomIndex+iCnt);
228 for (
int iCnt = 1; iCnt <= 4; iCnt++) {
229 WorkVec.
PutRowIndex(6+iCnt, iFirstReactionIndex+iCnt);
236 v =
Vec3(XCurr, iFirstReactionIndex+1);
237 dAlpha = XCurr(iFirstReactionIndex+4);
242 if (
fabs(dDistance) <= std::numeric_limits<doublereal>::epsilon()) {
243 silent_cerr(
"DistanceJoint(" <<
GetLabel() <<
"): "
244 "near-zero distance" << std::endl);
249 WorkVec.
Add(1, TmpVec);
250 WorkVec.
Sub(4, TmpVec);
252 WorkVec.
Add(7, x1-x2+v*dDistance);
255 ASSERT(d > std::numeric_limits<doublereal>::epsilon());
256 if (d > std::numeric_limits<doublereal>::epsilon()) {
273 if (
fabs(d) > std::numeric_limits<doublereal>::epsilon()) {
280 <<
" " <<
v <<
" " << d << std::endl;
293 DEBUGCOUT(
"Entering DistanceJoint::InitialAssJac()" << std::endl);
301 integer iNode1FirstVelIndex = iNode1FirstPosIndex+6;
303 integer iNode2FirstVelIndex = iNode2FirstPosIndex+6;
305 integer iReactionPrimeIndex = iFirstReactionIndex+4;
341 dAlpha = XCurr(iFirstReactionIndex+4);
342 doublereal dBeta = XCurr(iReactionPrimeIndex+4);
343 v =
Vec3(XCurr, iFirstReactionIndex+1);
344 Vec3 w(XCurr, iReactionPrimeIndex+1);
347 for (
int iCnt = 1; iCnt <= 3; iCnt++) {
352 WM.
PutItem(6+iCnt, iNode1FirstPosIndex+iCnt,
353 iFirstReactionIndex+iCnt, -d);
356 WM.
PutItem(9+iCnt, iNode2FirstPosIndex+iCnt,
357 iFirstReactionIndex+iCnt, d);
361 WM.
PutItem(12+iCnt, iNode1FirstPosIndex+iCnt,
362 iFirstReactionIndex+4, -d);
365 WM.
PutItem(15+iCnt, iNode2FirstPosIndex+iCnt,
366 iFirstReactionIndex+4, d);
370 WM.
PutItem(18+iCnt, iNode1FirstVelIndex+iCnt,
371 iReactionPrimeIndex+iCnt, -d);
374 WM.
PutItem(21+iCnt, iNode2FirstVelIndex+iCnt,
375 iReactionPrimeIndex+iCnt, d);
379 WM.
PutItem(24+iCnt, iNode1FirstVelIndex+iCnt,
380 iReactionPrimeIndex+4, -d);
383 WM.
PutItem(27+iCnt, iNode2FirstVelIndex+iCnt,
384 iReactionPrimeIndex+4, d);
388 WM.
PutItem(30+iCnt, iFirstReactionIndex+iCnt,
389 iFirstReactionIndex+iCnt, -d);
392 WM.
PutItem(33+iCnt, iReactionPrimeIndex+iCnt,
393 iReactionPrimeIndex+iCnt, 1.);
396 WM.
PutItem(36+iCnt, iReactionPrimeIndex+iCnt,
397 iFirstReactionIndex+iCnt, -1.);
401 WM.
PutItem(39+iCnt, iReactionPrimeIndex+4,
402 iNode1FirstVelIndex+iCnt, -d);
405 WM.
PutItem(42+iCnt, iReactionPrimeIndex+4,
406 iNode2FirstVelIndex+iCnt, d);
408 d = deltaXP.dGet(iCnt);
410 WM.
PutItem(45+iCnt, iReactionPrimeIndex+4,
411 iReactionPrimeIndex+iCnt, d);
417 ASSERT(d > std::numeric_limits<doublereal>::epsilon());
418 if (d > std::numeric_limits<doublereal>::epsilon()) {
420 for (
int iCnt = 1; iCnt <= 3; iCnt++) {
421 WM.
PutItem(48+iCnt, iFirstReactionIndex+4,
422 iFirstReactionIndex+iCnt,
v.
dGet(iCnt)/d);
425 for (
int iCnt = 1; iCnt <= 3; iCnt++) {
426 WM.
PutItem(48+iCnt, iFirstReactionIndex+4,
427 iFirstReactionIndex+iCnt, 0.);
431 for (
int iCnt = 1; iCnt <= 3; iCnt++) {
433 WM.
PutItem(iCnt, iFirstReactionIndex+iCnt,
434 iNode1FirstPosIndex+iCnt, -1.);
437 WM.
PutItem(iCnt+3, iFirstReactionIndex+iCnt,
438 iNode2FirstPosIndex+iCnt, 1.);
450 DEBUGCOUT(
"Entering DistanceJoint::InitialAssRes()" << std::endl);
456 integer iNode1FirstVelIndex = iNode1FirstPosIndex+6;
458 integer iNode2FirstVelIndex = iNode2FirstPosIndex+6;
460 integer iReactionPrimeIndex = iFirstReactionIndex+4;
462 for (
int iCnt = 1; iCnt <= 3; iCnt++) {
464 WorkVec.
PutRowIndex(iCnt, iNode1FirstPosIndex+iCnt);
467 WorkVec.
PutRowIndex(3+iCnt, iNode1FirstVelIndex+iCnt);
470 WorkVec.
PutRowIndex(6+iCnt, iNode2FirstPosIndex+iCnt);
473 WorkVec.
PutRowIndex(9+iCnt, iNode2FirstVelIndex+iCnt);
477 for (
int iCnt = 1; iCnt <= 8; iCnt++) {
478 WorkVec.
PutRowIndex(12+iCnt, iFirstReactionIndex+iCnt);
485 v =
Vec3(XCurr, iFirstReactionIndex+1);
486 Vec3 w(XCurr, iReactionPrimeIndex+1);
487 dAlpha = XCurr(iFirstReactionIndex+4);
488 doublereal dBeta = XCurr(iReactionPrimeIndex+4);
493 if (
fabs(dDistance) <= std::numeric_limits<doublereal>::epsilon()) {
494 silent_cerr(
"DistanceJoint(" <<
GetLabel() <<
"): "
495 "near-zero distance" << std::endl);
499 WorkVec.
Add(3+1, w*dBeta);
500 WorkVec.
Add(6+1, -v*dAlpha);
501 WorkVec.
Add(9+1, -w*dBeta);
502 WorkVec.
Add(12+1, x1-x2+v*dDistance);
505 ASSERT(d > std::numeric_limits<doublereal>::epsilon());
506 if(d > std::numeric_limits<doublereal>::epsilon()) {
514 WorkVec.
Add(16+1, v-w);
526 if (
fabs(dDistance) <= std::numeric_limits<doublereal>::epsilon()) {
527 silent_cerr(
"DistanceJoint(" <<
GetLabel() <<
"): "
528 "near-zero distance" << std::endl);
534 if (d < std::numeric_limits<doublereal>::epsilon()) {
535 silent_cerr(
"DistanceJoint(" <<
GetLabel() <<
"): "
536 "initial length is null" << std::endl);
541 X.
Put(iFirstIndex+1,
v);
542 X.
Put(iFirstIndex+4+1,
v);
552 for (
unsigned i = 0; i < ph->size(); i++) {
556 pedantic_cout(
"DistanceJoint(" <<
uLabel <<
"): "
557 "creating drive from hint[" << i <<
"]..." << std::endl);
561 silent_cerr(
"DistanceJoint(" <<
uLabel <<
"): "
562 "unable to create drive after hint "
563 "#" << i << std::endl);
577 if (
fabs(dDistance) <= std::numeric_limits<doublereal>::epsilon()) {
578 silent_cerr(
"DistanceJoint(" <<
uLabel <<
"): "
579 "near-zero distance" << std::endl);
587 if (d <= std::numeric_limits<doublereal>::epsilon()) {
588 silent_cerr(
"DistanceJoint(" <<
uLabel <<
") "
591 "nodes are coincident;" << std::endl
592 <<
"initial joint assembly is recommended"
617 Joint(uL, pDO, fOut),
619 pNode1(pN1), pNode2(pN2), f1(f1Tmp), f2(f2Tmp), v(
Zero3), dAlpha(0.)
648 if (strcmp(s,
"d") == 0) {
673 <<
", reference, node, ",
676 <<
", reference, node, ",
689 DEBUGCOUT(
"Entering DistanceJointWithOffset::AssJac()" << std::endl);
708 for (
int iCnt = 1; iCnt <= 3; iCnt++) {
711 WM.
PutItem(iCnt, iFirstReactionIndex+iCnt,
712 iNode1FirstPosIndex+iCnt, -dCoef);
715 WM.
PutItem(3+iCnt, iFirstReactionIndex+iCnt,
716 iNode2FirstPosIndex+iCnt, dCoef);
719 WM.
PutItem(6+iCnt, iNode1FirstMomIndex+iCnt,
720 iFirstReactionIndex+iCnt, -
dAlpha);
723 WM.
PutItem(9+iCnt, iNode2FirstMomIndex+iCnt,
724 iFirstReactionIndex+iCnt,
dAlpha);
727 WM.
PutItem(12+iCnt, iFirstReactionIndex+iCnt,
728 iFirstReactionIndex+iCnt, -dDistance);
733 WM.
PutItem(15+iCnt, iNode1FirstMomIndex+iCnt,
734 iFirstReactionIndex+4, -d);
737 WM.
PutItem(18+iCnt, iNode2FirstMomIndex+iCnt,
738 iFirstReactionIndex+4, d);
743 WM.
PutCross(22, iFirstReactionIndex,
744 iNode1FirstPosIndex+3, f1Tmp*dCoef);
746 WM.
PutCross(28, iFirstReactionIndex,
747 iNode2FirstPosIndex+3, f2Tmp*(-dCoef));
758 WM.
PutCross(52, iNode1FirstMomIndex+3,
759 iFirstReactionIndex, f1Tmp*(-
dAlpha));
760 WM.
PutCross(58, iNode2FirstMomIndex+3,
761 iFirstReactionIndex, f2Tmp*
dAlpha);
764 for (
int iCnt = 1; iCnt <= 3; iCnt++) {
765 WM.
PutItem(63+iCnt, iNode1FirstMomIndex+3+iCnt,
766 iFirstReactionIndex+4, Tmp.
dGet(iCnt));
770 for (
int iCnt = 1; iCnt <= 3; iCnt++) {
771 WM.
PutItem(66+iCnt, iNode2FirstMomIndex+3+iCnt,
772 iFirstReactionIndex+4, Tmp.
dGet(iCnt));
777 ASSERT(d > std::numeric_limits<doublereal>::epsilon());
778 if (d > std::numeric_limits<doublereal>::epsilon()) {
785 for (
int iCnt = 1; iCnt <= 3; iCnt++) {
786 WM.
PutItem(69+iCnt, iFirstReactionIndex+4,
787 iFirstReactionIndex+iCnt,
v.
dGet(iCnt)/d);
801 DEBUGCOUT(
"Entering DistanceJointWithOffset::AssRes()" << std::endl);
813 for (
int iCnt = 1; iCnt <= 6; iCnt++) {
815 WorkVec.
PutRowIndex(iCnt, iNode1FirstMomIndex+iCnt);
816 WorkVec.
PutRowIndex(6+iCnt, iNode2FirstMomIndex+iCnt);
820 for(
int iCnt = 1; iCnt <= 4; iCnt++) {
821 WorkVec.
PutRowIndex(12+iCnt, iFirstReactionIndex+iCnt);
830 v =
Vec3(XCurr, iFirstReactionIndex+1);
831 dAlpha = XCurr(iFirstReactionIndex+4);
836 if (
fabs(dDistance) <= std::numeric_limits<doublereal>::epsilon()) {
837 silent_cerr(
"DistanceJoint(" <<
GetLabel() <<
"): "
838 "near-zero distance" << std::endl);
842 WorkVec.
Add(1, TmpVec);
843 WorkVec.
Add(4, f1Tmp.Cross(TmpVec));
844 WorkVec.
Sub(7, TmpVec);
845 WorkVec.
Sub(10, f2Tmp.Cross(TmpVec));
847 WorkVec.
Add(13, x1+f1Tmp-x2-f2Tmp+v*dDistance);
864 if (
fabs(d) > std::numeric_limits<doublereal>::epsilon()) {
871 <<
" " <<
v <<
" " << d << std::endl;
881 DEBUGCOUT(
"Entering DistanceJointWithOffset::InitialAssJac()" << std::endl);
890 integer iReactionPrimeIndex = iFirstReactionIndex+4;
892 for (
int iCnt = 1; iCnt <= 12; iCnt++) {
899 for (
int iCnt = 1; iCnt <= 8; iCnt++) {
904 doublereal dAlphaP = XCurr(iReactionPrimeIndex+4);
905 Vec3 vP(XCurr, iReactionPrimeIndex+1);
915 for (
int iCnt = 1; iCnt <= 3; iCnt++) {
922 WM.
PutCoef(6+iCnt, 24+iCnt, -dAlphaP);
923 WM.
PutCoef(18+iCnt, 24+iCnt, dAlphaP);
939 for (
int iCnt = 1; iCnt <= 3; iCnt++) {
945 Tmp = f2Tmp.Cross(
v);
946 for (
int iCnt = 1; iCnt <= 3; iCnt++) {
952 Tmp =
v.
Cross(Omega1.Cross(f1Tmp))+vP.
Cross(f1Tmp);
953 for (
int iCnt = 1; iCnt <= 3; iCnt++) {
958 Tmp =
v.
Cross(f2Tmp.Cross(Omega2))-vP.
Cross(f2Tmp);
959 for (
int iCnt = 1; iCnt <= 3; iCnt++) {
969 WM.
Add(10, 10, MTmp);
973 WM.
Add(10, 29, MTmp);
977 WM.
Add(16, 16, MTmp);
978 WM.
Add(22, 22, MTmp);
981 WM.
Add(16, 25, MTmp);
982 WM.
Add(22, 29, MTmp);
986 MTmp =
Mat3x3(
MatCross, f1Tmp.Cross(Omega1*dAlpha) - f1Tmp*dAlphaP);
987 WM.
Add(10, 25, MTmp);
990 WM.
Add(22, 16, MTmp);
991 MTmp =
Mat3x3(
MatCross, f2Tmp.Cross(Omega2*(-dAlpha)) + f2Tmp*dAlphaP);
992 WM.
Add(22, 25, MTmp);
995 for (
int iCnt = 1; iCnt <= 3; iCnt++) {
996 WM.
PutCoef(24+iCnt, iCnt, -1.);
997 WM.
PutCoef(24+iCnt, 12+iCnt, 1.);
999 WM.
PutCoef(24+iCnt, 24+iCnt, -dDistance);
1001 WM.
PutCoef(28+iCnt, 6+iCnt, -1.);
1002 WM.
PutCoef(28+iCnt, 18+iCnt, 1.);
1004 WM.
PutCoef(28+iCnt, 28+iCnt, -dDistance);
1008 WM.
Add(25, 4, MTmp);
1009 WM.
Add(29, 10, MTmp);
1012 WM.
Sub(25, 16, MTmp);
1013 WM.
Sub(29, 22, MTmp);
1016 WM.
Add(29, 4, MTmp);
1019 WM.
Sub(29, 16, MTmp);
1022 ASSERT(d > std::numeric_limits<doublereal>::epsilon());
1023 if (d > std::numeric_limits<doublereal>::epsilon()) {
1027 for (
int iCnt = 1; iCnt <= 3; iCnt++) {
1033 Tmp = vP/d-
v*((vP.
Dot(
v))/
pow(d, 3));
1034 for (
int iCnt = 1; iCnt <= 3; iCnt++) {
1049 DEBUGCOUT(
"Entering DistanceJointWithOffset::InitialAssRes()" << std::endl);
1057 integer iReactionPrimeIndex = iFirstReactionIndex+4;
1059 for (
int iCnt = 1; iCnt <= 12; iCnt++) {
1060 WorkVec.
PutRowIndex(iCnt, iNode1FirstPosIndex+iCnt);
1061 WorkVec.
PutRowIndex(12+iCnt, iNode2FirstPosIndex+iCnt);
1064 for (
int iCnt = 1; iCnt <= 8; iCnt++) {
1065 WorkVec.
PutRowIndex(24+iCnt, iFirstReactionIndex+iCnt);
1068 v =
Vec3(XCurr, iFirstReactionIndex+1);
1069 Vec3 vP(XCurr, iReactionPrimeIndex+1);
1070 dAlpha = XCurr(iFirstReactionIndex+4);
1071 doublereal dAlphaP = XCurr(iReactionPrimeIndex+4);
1084 if (
fabs(dDistance) <= std::numeric_limits<doublereal>::epsilon()) {
1085 silent_cerr(
"DistanceJoint(" <<
GetLabel() <<
"): "
1086 "near-zero distance" << std::endl);
1090 WorkVec.
Put(1, Tmp);
1091 WorkVec.
Put(4, f1Tmp.Cross(Tmp));
1092 WorkVec.
Put(13, -Tmp);
1095 Tmp = vP*dAlpha+
v*dAlphaP;
1096 WorkVec.
Put(7, Tmp);
1097 WorkVec.
Put(10, (Omega1.Cross(f1Tmp)).
Cross(
v*dAlpha)+f1Tmp.Cross(Tmp));
1098 WorkVec.
Put(19, -Tmp);
1099 WorkVec.
Put(22, (f2Tmp.Cross(Omega2)).
Cross(
v*dAlpha)+Tmp.
Cross(f2Tmp));
1101 WorkVec.
Put(25,
v*dDistance-x2-f2Tmp+x1+f1Tmp);
1102 WorkVec.
Put(29, vP*dDistance
1103 -v2-Omega2.
Cross(f2Tmp)+v1+Omega1.
Cross(f1Tmp));
1107 ASSERT(d > std::numeric_limits<doublereal>::epsilon());
1121 if (
fabs(dDistance) <= std::numeric_limits<doublereal>::epsilon()) {
1122 silent_cerr(
"DistanceJoint(" <<
GetLabel() <<
"): "
1123 "near-zero distance" << std::endl);
1136 v = x2+f2Tmp-x1-f1Tmp;
1138 if (d < std::numeric_limits<doublereal>::epsilon()) {
1139 silent_cerr(
"DistanceJoint(" <<
GetLabel() <<
"): "
1140 "initial length is null" << std::endl);
1145 X.
Put(iFirstIndex+1, v);
1146 X.
Put(iFirstIndex+5, (v2+Omega2.Cross(f2Tmp)-v1-Omega1.Cross(f1Tmp))/d);
1156 for (
unsigned i = 0; i < ph->size(); i++) {
1157 pedantic_cout(
"DistanceJointWithOffset(" <<
uLabel <<
"): "
1158 "creating drive from hint..." << std::endl);
1165 silent_cerr(
"DistanceJointWithOffset(" <<
uLabel <<
"): "
1166 "unable to create drive after hint "
1167 "#" << i << std::endl);
1181 if (
fabs(dDistance) <= std::numeric_limits<doublereal>::epsilon()) {
1182 silent_cerr(
"DistanceJoint(" <<
GetLabel() <<
"):"
1183 "near-zero distance" << std::endl);
1189 if (d <= std::numeric_limits<doublereal>::epsilon()) {
1190 silent_cerr(
"DistanceJoint(" <<
GetLabel() <<
") "
1193 "nodes are coincident;" << std::endl
1194 <<
"this is no longer supported" << std::endl);
1217 Joint(uL, pD, fOut),
1218 pNode(pN), XClamp(X0), RClamp(R0), F(
Zero3), M(
Zero3)
1236 << prefix << iIndex + 1 <<
"->" << iIndex + 3 <<
": "
1237 "reaction forces [Fx,Fy,Fz]" << std::endl
1238 << prefix << iIndex + 4 <<
"->" << iIndex + 6 <<
": "
1239 "reaction couples [mx,my,mz]" << std::endl;
1244 << prefix << iIndex + 1 <<
"->" << iIndex + 3 <<
": "
1245 "reaction force derivatives [FPx,FPy,FPz]" << std::endl
1246 << prefix << iIndex + 4 <<
"->" << iIndex + 6 <<
": "
1247 "reaction couple derivatives [mPx,mPy,mPz]" << std::endl;
1253 static const char xyz[] =
"xyz";
1256 "reaction couple m",
1257 "reaction force derivative fP",
1258 "reaction couple derivative mP"
1260 static const char *
eq[] = {
1261 "position constraint P",
1262 "orientation constraint theta",
1263 "position constraint derivative v",
1264 "orientation constraint derivative w"
1269 bool bInitial,
int i)
const
1282 std::ostringstream os;
1283 os <<
"ClampJoint(" <<
GetLabel() <<
")";
1286 std::string name = os.str();
1287 for (i = 0; i < iend; i++) {
1289 os.seekp(0, std::ios_base::end);
1290 os <<
": " <<
dof[i/3] <<
xyz[i%3];
1296 os <<
": " <<
dof[i/3] <<
xyz[i%3];
1307 << prefix << iIndex + 1 <<
"->" << iIndex + 3 <<
": "
1308 "position constraints [Px=Px0,Py=Py0,Pz=Pz0]" << std::endl
1309 << prefix << iIndex + 4 <<
"->" << iIndex + 6 <<
": "
1310 "orientation constraints [thetax=thetax0,thetay=thetay0,thetaz=thetaz0]" << std::endl;
1315 << prefix << iIndex + 1 <<
"->" << iIndex + 3 <<
": "
1316 "velocity constraints [vx=0,vy=0,vz=0]" << std::endl
1317 << prefix << iIndex + 4 <<
"->" << iIndex + 6 <<
": "
1318 "angular velocity constraints [wx=0,wy=0,wz=0]" << std::endl;
1326 bool bInitial,
int i)
const
1339 std::ostringstream os;
1340 os <<
"ClampJoint(" <<
GetLabel() <<
")";
1343 std::string name = os.str();
1344 for (i = 0; i < iend; i++) {
1346 os.seekp(0, std::ios_base::end);
1347 os <<
": " <<
eq[i/3] <<
xyz[i%3];
1353 os <<
": " <<
eq[i/3] <<
xyz[i%3];
1372 <<
"initial state, ",
F.
Write(out,
", ")
1373 <<
", ",
M.
Write(out,
", ") <<
';' << std::endl;
1383 DEBUGCOUT(
"Entering ClampJoint::AssJac()" << std::endl);
1395 for (
integer iCnt = 1; iCnt <= 6; iCnt++) {
1396 WM.
PutItem(iCnt, iFirstReactionIndex + iCnt,
1397 iFirstPositionIndex + iCnt, 1.);
1398 WM.
PutItem(6 + iCnt, iFirstMomentumIndex + iCnt,
1399 iFirstReactionIndex + iCnt, 1.);
1413 DEBUGCOUT(
"Entering ClampJoint::AssJac()" << std::endl);
1423 for (
integer iCnt = 1; iCnt <= 6; iCnt++) {
1424 WM.
PutItem(iCnt, iFirstReactionIndex + iCnt,
1425 iFirstPositionIndex + iCnt, 1.);
1438 DEBUGCOUT(
"Entering ClampJoint::AssMats(); will result in call to AssJac()"
1442 AssJac(WorkMatB, 1., XCurr, XPrimeCurr);
1452 DEBUGCOUT(
"Entering ClampJoint::AssRes()" << std::endl);
1459 for (
integer iCnt = 1; iCnt <= 6; iCnt++) {
1460 WorkVec.
PutRowIndex(iCnt, iFirstMomentumIndex + iCnt);
1461 WorkVec.
PutRowIndex(6 + iCnt, iFirstReactionIndex + iCnt);
1465 F =
Vec3(XCurr, iFirstReactionIndex + 1);
1466 M =
Vec3(XCurr, iFirstReactionIndex + 3 + 1);
1476 WorkVec.
Sub(3 + 1,
M);
1482 WorkVec.
Sub(9 + 1, theta_c/dCoef);
1502 DEBUGCOUT(
"Entering ClampJoint::AssRes()" << std::endl);
1510 for (
integer iCnt = 1; iCnt <= 6; iCnt++) {
1511 WorkVec.
PutRowIndex(iCnt, iFirstReactionIndex+iCnt);
1522 WorkVec.
Sub(3 + 1, theta_c);
1540 F =
Vec3(XCurr, iFirstReactionIndex + 1);
1541 M =
Vec3(XCurr, iFirstReactionIndex + 4);
1560 DEBUGCOUT(
"Entering ClampJoint::InitialAssJac()" << std::endl);
1566 integer iFirstVelocityIndex = iFirstPositionIndex+6;
1568 integer iReactionPrimeIndex = iFirstReactionIndex+6;
1584 for (
int iCnt = 1; iCnt <= 6; iCnt++) {
1585 WM.
PutItem(iCnt, iFirstReactionIndex+iCnt,
1586 iFirstPositionIndex+iCnt, 1.);
1588 WM.
PutItem(6+iCnt, iReactionPrimeIndex+iCnt,
1589 iFirstVelocityIndex+iCnt, 1.);
1591 WM.
PutItem(12+iCnt, iFirstPositionIndex+iCnt,
1592 iFirstReactionIndex+iCnt, 1.);
1594 WM.
PutItem(18+iCnt, iFirstVelocityIndex+iCnt,
1595 iReactionPrimeIndex+iCnt, 1.);
1607 DEBUGCOUT(
"Entering ClampJoint::InitialAssRes()" << std::endl);
1617 integer iReactionPrimeIndex = iFirstReactionIndex+6;
1619 for (
int iCnt = 1; iCnt <= 12; iCnt++) {
1620 WorkVec.
PutRowIndex(iCnt, iFirstPositionIndex+iCnt);
1621 WorkVec.
PutRowIndex(12+iCnt, iFirstReactionIndex+iCnt);
1626 WorkVec.
Put(1, -
Vec3(XCurr, iFirstReactionIndex+1));
1629 WorkVec.
Put(4, -
Vec3(XCurr, iFirstReactionIndex+4));
1632 WorkVec.
Put(7, -
Vec3(XCurr, iReactionPrimeIndex+1));
1635 WorkVec.
Put(10, -
Vec3(XCurr, iReactionPrimeIndex+4));
1659 X.
Put(iFirstReactionIndex+1,
F);
1660 X.
Put(iFirstReactionIndex+4,
M);
1677 unsigned int idx = 0;
1718 if (i >= 1 && i <= 3) {
1722 if (i >= 4 && i <= 6) {
VariableSubMatrixHandler & AssJac(VariableSubMatrixHandler &WorkMat, doublereal dCoef, const VectorHandler &XCurr, const VectorHandler &XPrimeCurr)
void AssMats(VariableSubMatrixHandler &WorkMatA, VariableSubMatrixHandler &WorkMatB, const VectorHandler &XCurr, const VectorHandler &XPrimeCurr)
void PutColIndex(integer iSubCol, integer iCol)
virtual void Output(OutputHandler &OH) const
SubVectorHandler & InitialAssRes(SubVectorHandler &WorkVec, const VectorHandler &XCurr)
SubVectorHandler & InitialAssRes(SubVectorHandler &WorkVec, const VectorHandler &XCurr)
void PutMat3x3(integer iSubIt, integer iFirstRow, integer iFirstCol, const Mat3x3 &m)
const Vec3 Zero3(0., 0., 0.)
Vec3 Cross(const Vec3 &v) const
std::ostream & Write(std::ostream &out, const char *sFill=" ") const
SubVectorHandler & AssRes(SubVectorHandler &WorkVec, doublereal dCoef, const VectorHandler &XCurr, const VectorHandler &XPrimeCurr)
SubVectorHandler & AssRes(SubVectorHandler &WorkVec, doublereal dCoef, const VectorHandler &XCurr, const VectorHandler &XPrimeCurr)
VariableSubMatrixHandler & InitialAssJac(VariableSubMatrixHandler &WorkMat, const VectorHandler &XCurr)
virtual const Mat3x3 & GetRRef(void) const
virtual bool bToBeOutput(void) const
virtual std::ostream & Restart(std::ostream &out) const
GradientExpression< BinaryExpr< FuncPow, LhsExpr, RhsExpr > > pow(const GradientExpression< LhsExpr > &u, const GradientExpression< RhsExpr > &v)
#define MBDYN_EXCEPT_ARGS
virtual doublereal dGetPrivData(unsigned int i) const
virtual void ResizeReset(integer)
virtual unsigned int iGetPrivDataIdx(const char *s) const
const MatCross_Manip MatCross
FullSubMatrixHandler & SetFull(void)
virtual const Mat3x3 & GetRCurr(void) const
virtual Node::Type GetNodeType(void) const
virtual std::ostream & Restart(std::ostream &out) const
doublereal Dot(const Vec3 &v) const
void Add(integer iRow, integer iCol, const Vec3 &v)
virtual void SetValue(DataManager *pDM, VectorHandler &X, VectorHandler &XP, SimulationEntity::Hints *ph=0)
virtual void WorkSpaceDim(integer *piNumRows, integer *piNumCols) const
virtual void Sub(integer iRow, const Vec3 &v)
doublereal dGet(void) const
void PutCoef(integer iRow, integer iCol, const doublereal &dCoef)
void ResizeReset(integer iNewRow, integer iNewCol)
virtual std::ostream & DescribeDof(std::ostream &out, const char *prefix="", bool bInitial=false) const
virtual const Vec3 & GetWRef(void) const
void PutCross(integer iSubIt, integer iFirstRow, integer iFirstCol, const Vec3 &v)
std::vector< Hint * > Hints
virtual unsigned int iGetPrivDataIdx(const char *s) const
GradientExpression< UnaryExpr< FuncFabs, Expr > > fabs(const GradientExpression< Expr > &u)
virtual std::ostream & Restart(std::ostream &out) const =0
Vec3 VecRot(const Mat3x3 &Phi)
virtual void SetInitialValue(VectorHandler &X)
virtual unsigned int iGetPrivDataIdx(const char *s) const
Vec3 MulTV(const Vec3 &v) const
VariableSubMatrixHandler & AssJac(VariableSubMatrixHandler &WorkMat, doublereal dCoef, const VectorHandler &XCurr, const VectorHandler &XPrimeCurr)
const StructNode * pNode1
static const char * dof[]
virtual void PutRowIndex(integer iSubRow, integer iRow)=0
void Update(const VectorHandler &XCurr, InverseDynamics::Order iOrder=InverseDynamics::INVERSE_DYNAMICS)
void PutItem(integer iSubIt, integer iRow, integer iCol, const doublereal &dCoef)
DistanceJointWithOffset(unsigned int uL, const DofOwner *pDO, const StructNode *pN1, const StructNode *pN2, const Vec3 &f1Tmp, const Vec3 &f2Tmp, const DriveCaller *pDC, flag fOut)
const StructDispNode * pNode1
const StructNode * pNode2
const doublereal & dGet(unsigned short int iRow) const
SubVectorHandler & InitialAssRes(SubVectorHandler &WorkVec, const VectorHandler &XCurr)
virtual bool bInverseDynamics(void) const
ClampJoint(unsigned int uL, const DofOwner *pD, const StructNode *pN, const Vec3 &X0, const Mat3x3 &R0, flag fOut)
virtual ~ClampJoint(void)
virtual void SetValue(DataManager *pDM, VectorHandler &X, VectorHandler &XP, SimulationEntity::Hints *ph=0)
virtual integer iGetFirstMomentumIndex(void) const =0
virtual integer iGetFirstPositionIndex(void) const
virtual const Vec3 & GetWCurr(void) const
VariableSubMatrixHandler & InitialAssJac(VariableSubMatrixHandler &WorkMat, const VectorHandler &XCurr)
virtual std::ostream & Restart(std::ostream &out) const
virtual unsigned int iGetNumPrivData(void) const
SubVectorHandler & AssRes(SubVectorHandler &WorkVec, doublereal dCoef, const VectorHandler &XCurr, const VectorHandler &XPrimeCurr)
std::ostream & Joints(void) const
DriveCaller * pCreateDrive(DataManager *pDM) const
DotTraits< VectorExprLhs, VectorExprRhs, N_rows, N_rows >::ExpressionType Dot(const VectorExpression< VectorExprLhs, N_rows > &u, const VectorExpression< VectorExprRhs, N_rows > &v)
~DistanceJointWithOffset(void)
#define ASSERT(expression)
DistanceJoint(unsigned int uL, const DofOwner *pDO, const StructDispNode *pN1, const StructDispNode *pN2, const DriveCaller *pDC, flag fOut)
virtual unsigned int iGetNumPrivData(void) const
GradientExpression< UnaryExpr< FuncSqrt, Expr > > sqrt(const GradientExpression< Expr > &u)
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 InitialWorkSpaceDim(integer *piNumRows, integer *piNumCols) const
DriveCaller * pGetDriveCaller(void) const
virtual const Vec3 & GetXCurr(void) const
virtual void Add(integer iRow, const Vec3 &v)
virtual void ResizeReset(integer, integer)
virtual doublereal dGet(const doublereal &dVar) const =0
const MatCrossCross_Manip MatCrossCross
virtual void Put(integer iRow, const Vec3 &v)
virtual std::ostream & DescribeEq(std::ostream &out, const char *prefix="", bool bInitial=false) const
const StructDispNode * pNode2
VariableSubMatrixHandler & InitialAssJac(VariableSubMatrixHandler &WorkMat, const VectorHandler &XCurr)
virtual void WorkSpaceDim(integer *piNumRows, integer *piNumCols) const
virtual void Output(OutputHandler &OH) const
void PutRowIndex(integer iSubRow, integer iRow)
void Set(const DriveCaller *pDC)
virtual const Vec3 & GetVCurr(void) const
void Sub(integer iRow, integer iCol, const Vec3 &v)
virtual void SetValue(DataManager *pDM, VectorHandler &X, VectorHandler &XP, SimulationEntity::Hints *ph=0)
Mat3x3 MulMT(const Mat3x3 &m) const
void ReadInitialState(MBDynParser &HP)
SparseSubMatrixHandler & SetSparse(void)
virtual integer iGetFirstIndex(void) const
virtual void Output(OutputHandler &OH) const
virtual std::ostream & Restart(std::ostream &out) 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
VariableSubMatrixHandler & AssJac(VariableSubMatrixHandler &WorkMat, doublereal dCoef, const VectorHandler &XCurr, const VectorHandler &XPrimeCurr)
virtual Vec3 GetVec3(void)
virtual void SetInitialValue(VectorHandler &X)
virtual doublereal dGetPrivData(unsigned int i) const
virtual void Resize(integer iNewSize)=0
virtual unsigned int iGetNumPrivData(void) const
virtual doublereal dGetPrivData(unsigned int i) const