setmotionJoint and TranslationLimit

OndselSolver/ASMTAssembly.cpp

@@ -480,6 +480,43 @@ void MbD::ASMTAssembly::runDraggingTest()
 	assembly->runPostDrag();	//Do this after last drag
 }
 
+void MbD::ASMTAssembly::runDraggingTest2()
+{
+	//auto assembly = ASMTAssembly::assemblyFromFile("../testapp/pistonWithLimits.asmt");
+	auto assembly = ASMTAssembly::assemblyFromFile("../testapp/dragCrankSlider.asmt");
+
+	auto limit1 = ASMTRotationLimit::With();
+	limit1->setName("Limit1");
+	limit1->setmotionJoint("/Assembly1/Joint1");
+	limit1->settype("=>");
+	limit1->setlimit("0.0*pi/180.0");
+	limit1->settol("1.0e-9");
+	assembly->addLimit(limit1);
+
+	auto limit2 = ASMTTranslationLimit::With();
+	limit2->setName("Limit2");
+	limit2->setmotionJoint("/Assembly1/Joint4");
+	limit2->settype("=<");
+	limit2->setlimit("0.0");
+	limit2->settol("1.0e-9");
+	assembly->addLimit(limit2);
+
+	auto& dragPart = assembly->parts->at(0);
+	auto dragParts = std::make_shared<std::vector<std::shared_ptr<ASMTPart>>>();
+	dragParts->push_back(dragPart);
+	assembly->runPreDrag();	//Do this before first drag
+	FColDsptr pos3D, delta;
+	pos3D = dragPart->position3D;
+	delta = std::make_shared<FullColumn<double>>(ListD{ 0.1, 0.2, 0.3 });
+	dragPart->updateMbDFromPosition3D(pos3D->plusFullColumn(delta));
+	assembly->runDragStep(dragParts);
+	pos3D = dragPart->position3D;
+	delta = std::make_shared<FullColumn<double>>(ListD{ 0.3, 0.2, 0.1 });
+	dragPart->updateMbDFromPosition3D(pos3D->plusFullColumn(delta));
+	assembly->runDragStep(dragParts);
+	assembly->runPostDrag();	//Do this after last drag
+}
+
 void MbD::ASMTAssembly::readWriteFile(const char* fileName)
 {
 	std::ifstream stream(fileName);

OndselSolver/ASMTAssembly.h

@@ -42,6 +42,7 @@ namespace MbD {
 		static std::shared_ptr<ASMTAssembly> assemblyFromFile(const char* chars);
 		static void runFile(const char* chars);
 		static void runDraggingTest();
+		static void runDraggingTest2();
 		static void readWriteFile(const char* chars);
 		void initialize() override;
 		ASMTAssembly* root() override;

OndselSolver/ASMTLimit.cpp

@@ -87,6 +87,11 @@ void MbD::ASMTLimit::createMbD(std::shared_ptr<System> mbdSys, std::shared_ptr<U
 	limitIJ->tol = geotol->getValue();
 }
 
+void MbD::ASMTLimit::setmotionJoint(std::string _motionJoint)
+{
+	motionJoint = _motionJoint;
+}
+
 void MbD::ASMTLimit::settype(std::string _type)
 {
 	type = _type;

OndselSolver/ASMTLimit.h

@@ -24,6 +24,7 @@ namespace MbD {
         void readTol(std::vector<std::string>& lines);
         void parseASMT(std::vector<std::string>& lines) override;
         void createMbD(std::shared_ptr<System> mbdSys, std::shared_ptr<Units> mbdUnits) override;
+        void setmotionJoint(std::string _motionJoint);
         void settype(std::string _type);
         void setlimit(std::string _limit);
         void settol(std::string _tol);

OndselSolver/AngleZIecJec.cpp

@@ -10,6 +10,7 @@
 
 #include "AngleZIecJec.h"
 #include "Numeric.h"
+#include <iostream>
 
 using namespace MbD;
 
@@ -30,6 +31,8 @@ void MbD::AngleZIecJec::calcPostDynCorrectorIteration()
 	auto sumOfSquaresSquared = sumOfSquares * sumOfSquares;
 	auto thez0to2pi = Numeric::arcTan0to2piYoverX(sthez, cthez);
 	thez = std::round((thez - thez0to2pi) / (2.0 * OS_M_PI)) * (2.0 * OS_M_PI) + thez0to2pi;
+	//std::cout << "AngleZIecJec thez = " << thez << std::endl;
+
 	cosOverSSq = cthez / sumOfSquares;
 	sinOverSSq = sthez / sumOfSquares;
 	twoCosSinOverSSqSq = 2.0 * cthez * sthez / sumOfSquaresSquared;

OndselSolver/DispCompIecJecKeqc.cpp

@@ -38,6 +38,7 @@ void DispCompIecJecKeqc::calcPostDynCorrectorIteration()
 	aAjOKe = efrmKqc->aAjOe(axisK);
 	rIeJeO = frmJqc->rOeO->minusFullColumn(frmIqc->rOeO);
 	riIeJeKe = aAjOKe->dot(rIeJeO);
+	//std::cout << "DispCompIecJecKeqc riIeJeKe = " << riIeJeKe << std::endl;
 	pAjOKepEKT = efrmKqc->pAjOepET(axisK);
 	ppAjOKepEKpEK = efrmKqc->ppAjOepEpE(axisK);
 	for (size_t i = 0; i < 4; i++)

OndselSolver/TranslationLimitIJ.cpp

@@ -14,7 +14,7 @@ std::shared_ptr<TranslationLimitIJ> MbD::TranslationLimitIJ::With()
 void MbD::TranslationLimitIJ::initializeGlobally()
 {
 	if (constraints->empty()) {
-		auto transConIJ = TranslationConstraintIJ::With(frmJ, frmI, 2);
+		auto transConIJ = TranslationConstraintIJ::With(frmI, frmJ, 2);
 		transConIJ->setConstant(limit);
 		addConstraint(transConIJ);
 		this->root()->hasChanged = true;

testapp/OndselSolver.cpp

@@ -26,6 +26,7 @@ void sharedptrTest();
 
 int main()
 {
+	ASMTAssembly::runDraggingTest2();
 	ASMTAssembly::runDraggingTest();
 	//ASMTAssembly::runFile("../testapp/pistonWithLimits.asmt");
 	ASMTAssembly::runFile("../testapp/pistonAllowZRotation.asmt");