Solve dragging (#47)

runPreDrag, runDragStep, runPostDrag, PosICDragNewtonRaphson
Ondsel-Development · Jan 11, 2024 · fe99ad2 · fe99ad2
1 parent 7371522
commit fe99ad2
Show file tree

Hide file tree

Showing 29 changed files with 4,197 additions and 100 deletions.
diff --git a/CMakeLists.txt b/CMakeLists.txt
@@ -278,6 +278,7 @@ set(ONDSELSOLVER_SRC
         OndselSolver/PointInLineJoint.cpp
         OndselSolver/PointInPlaneJoint.cpp
         OndselSolver/Polynomial.cpp
+        OndselSolver/PosICDragNewtonRaphson.cpp
         OndselSolver/PosICKineNewtonRaphson.cpp
         OndselSolver/PosICNewtonRaphson.cpp
         OndselSolver/PosKineNewtonRaphson.cpp
@@ -596,6 +597,7 @@ set(ONDSELSOLVER_HEADERS
         OndselSolver/PlanarJoint.h
         OndselSolver/PointInLineJoint.h
         OndselSolver/PointInPlaneJoint.h
+        OndselSolver/PosICDragNewtonRaphson.h
         OndselSolver/PosICKineNewtonRaphson.h
         OndselSolver/PosICNewtonRaphson.h
         OndselSolver/PosKineNewtonRaphson.h

diff --git a/OndselSolver/ASMTAssembly.cpp b/OndselSolver/ASMTAssembly.cpp
@@ -65,6 +65,12 @@ MbD::ASMTAssembly::ASMTAssembly() : ASMTSpatialContainer()
 	times = std::make_shared<FullRow<double>>();
 }
 
+std::shared_ptr<ASMTAssembly> MbD::ASMTAssembly::With()
+{
+	auto assembly = std::make_shared<ASMTAssembly>();
+	return assembly;
+}
+
 void MbD::ASMTAssembly::runSinglePendulumSuperSimplified()
 {
 	//In this version we skip declaration of variables that don't need as they use default values.
@@ -383,6 +389,28 @@ void MbD::ASMTAssembly::runSinglePendulum()
 	assembly->runKINEMATIC();
 }
 
+std::shared_ptr<ASMTAssembly> MbD::ASMTAssembly::assemblyFromFile(const char* fileName)
+{
+	std::ifstream stream(fileName);
+	if (stream.fail()) {
+		throw std::invalid_argument("File not found.");
+	}
+	std::string line;
+	std::vector<std::string> lines;
+	while (std::getline(stream, line)) {
+		lines.push_back(line);
+	}
+	auto assembly = ASMTAssembly::With();
+	auto str = assembly->popOffTop(lines);
+	bool bool1 = str == "freeCAD: 3D CAD with Motion Simulation  by  askoh.com";
+	bool bool2 = str == "OndselSolver";
+	assert(bool1 || bool2);
+	assert(assembly->readStringOffTop(lines) == "Assembly");
+	assembly->setFilename(fileName);
+	assembly->parseASMT(lines);
+	return assembly;
+}
+
 void MbD::ASMTAssembly::runFile(const char* fileName)
 {
 	std::ifstream stream(fileName);
@@ -408,6 +436,25 @@ void MbD::ASMTAssembly::runFile(const char* fileName)
 	}
 }
 
+void MbD::ASMTAssembly::runDraggingTest()
+{
+	auto assembly = ASMTAssembly::assemblyFromFile("../testapp/dragCrankSlider.asmt");
+	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);
@@ -1046,10 +1093,36 @@ void MbD::ASMTAssembly::solve()
 	runKINEMATIC();
 }
 
+void MbD::ASMTAssembly::runPreDrag()
+{
+	mbdSystem = std::make_shared<System>();
+	mbdSystem->externalSystem->asmtAssembly = this;
+	try {
+		mbdSystem->runPreDrag(mbdSystem);
+	}
+	catch (SimulationStoppingError ex) {
+
+	}
+}
+
+void MbD::ASMTAssembly::runDragStep(std::shared_ptr<std::vector<std::shared_ptr<ASMTPart>>> dragParts)
+{
+	auto dragMbDParts = std::make_shared<std::vector<std::shared_ptr<Part>>>();
+	for (auto& dragPart : *dragParts) {
+		auto dragMbDPart = std::static_pointer_cast<Part>(dragPart->mbdObject);
+		dragMbDParts->push_back(dragMbDPart);
+	}
+	mbdSystem->runDragStep(dragMbDParts);
+}
+
+void MbD::ASMTAssembly::runPostDrag()
+{
+	runPreDrag();
+}
+
 void MbD::ASMTAssembly::runKINEMATIC()
 {
-	auto mbdSystem = std::make_shared<System>();
-	mbdObject = mbdSystem;
+	mbdSystem = std::make_shared<System>();
 	mbdSystem->externalSystem->asmtAssembly = this;
 	try {
 		mbdSystem->runKINEMATIC(mbdSystem);

diff --git a/OndselSolver/ASMTAssembly.h b/OndselSolver/ASMTAssembly.h
@@ -33,11 +33,14 @@ namespace MbD {
 		//
 	public:
 		ASMTAssembly();
+		static std::shared_ptr<ASMTAssembly> With();
 		static void runSinglePendulumSuperSimplified();
 		static void runSinglePendulumSuperSimplified2();
 		static void runSinglePendulumSimplified();
 		static void runSinglePendulum();
+		static std::shared_ptr<ASMTAssembly> assemblyFromFile(const char* chars);
 		static void runFile(const char* chars);
+		static void runDraggingTest();
 		static void readWriteFile(const char* chars);
 		void initialize() override;
 		ASMTAssembly* root() override;
@@ -83,6 +86,9 @@ namespace MbD {
 		/* This function performs a one shot solve of the assembly.*/
 		void solve();
 
+		void runPreDrag();
+		void runDragStep(std::shared_ptr<std::vector<std::shared_ptr<ASMTPart>>> dragParts);
+		void runPostDrag();
 		void runKINEMATIC();
 		void initprincipalMassMarker();
 		std::shared_ptr<ASMTSpatialContainer> spatialContainerAt(std::shared_ptr<ASMTAssembly> self, std::string& longname);
@@ -128,6 +134,7 @@ namespace MbD {
 		std::shared_ptr<std::vector<double>> times = std::make_shared<std::vector<double>>();
 		std::shared_ptr<ASMTTime> asmtTime = std::make_shared<ASMTTime>();
 		std::shared_ptr<Units> mbdUnits = std::make_shared<Units>();
+		std::shared_ptr<System> mbdSystem;
 		MBDynSystem* mbdynItem = nullptr;
 	};
 }

diff --git a/OndselSolver/ASMTItem.cpp b/OndselSolver/ASMTItem.cpp
@@ -56,6 +56,22 @@ void MbD::ASMTItem::parseASMT(std::vector<std::string>&)
 	assert(false);
 }
 
+std::string MbD::ASMTItem::popOffTop(std::vector<std::string>& args)
+{
+	auto str = args.at(0);	//Must copy string
+	args.erase(args.begin());
+	return str;
+}
+
+std::string MbD::ASMTItem::readStringOffTop(std::vector<std::string>& args)
+{
+	auto iss = std::istringstream(args.at(0));
+	args.erase(args.begin());
+	std::string str;
+	iss >> str;
+	return str;
+}
+
 FRowDsptr MbD::ASMTItem::readRowOfDoubles(std::string& line)
 {
 	std::istringstream iss(line);

diff --git a/OndselSolver/ASMTItem.h b/OndselSolver/ASMTItem.h
@@ -19,6 +19,7 @@ namespace MbD {
 	{
 		//
 	public:
+		ASMTItem() {}
 		virtual ~ASMTItem() {}
 		virtual ASMTAssembly* root();
 		virtual ASMTSpatialContainer* partOrAssembly();
@@ -29,6 +30,8 @@ namespace MbD {
 		virtual std::string classname();
 		void setName(std::string str);
 		virtual void parseASMT(std::vector<std::string>& lines);
+		std::string popOffTop(std::vector<std::string>& args);
+		std::string readStringOffTop(std::vector<std::string>& args);
 		FRowDsptr readRowOfDoubles(std::string& line);
 		FColDsptr readColumnOfDoubles(std::string& line);
 		double readDouble(std::string& line);

diff --git a/OndselSolver/ASMTMarker.cpp b/OndselSolver/ASMTMarker.cpp
@@ -15,57 +15,57 @@
 #include "MarkerFrame.h"
 #include "ASMTPrincipalMassMarker.h"
 
-namespace MbD {
-    void ASMTMarker::parseASMT(std::vector<std::string>& lines)
-    {
-        readName(lines);
-        readPosition3D(lines);
-        readRotationMatrix(lines);
-    }
+using namespace MbD;
 
-    FColDsptr ASMTMarker::rpmp()
-    {
-        //p is cm
-        auto refItem = static_cast<ASMTRefItem*>(owner);
-        auto& rPrefP = refItem->position3D;
-        auto& aAPref = refItem->rotationMatrix;
-        auto& rrefmref = position3D;
-        auto rPmP = rPrefP->plusFullColumn(aAPref->timesFullColumn(rrefmref));
-        auto& principalMassMarker = static_cast<ASMTPart*>(refItem->owner)->principalMassMarker;
-        auto& rPcmP = principalMassMarker->position3D;
-        auto& aAPcm = principalMassMarker->rotationMatrix;
-        auto rpmp = aAPcm->transposeTimesFullColumn(rPmP->minusFullColumn(rPcmP));
-        return rpmp;
-    }
+void ASMTMarker::parseASMT(std::vector<std::string>& lines)
+{
+	readName(lines);
+	readPosition3D(lines);
+	readRotationMatrix(lines);
+}
 
-    FMatDsptr ASMTMarker::aApm()
-    {
-        //p is cm
-        auto refItem = static_cast<ASMTRefItem*>(owner);
-        auto& aAPref = refItem->rotationMatrix;
-        auto& aArefm = rotationMatrix;
-        auto& principalMassMarker = static_cast<ASMTPart*>(refItem->owner)->principalMassMarker;
-        auto& aAPcm = principalMassMarker->rotationMatrix;
-        auto aApm = aAPcm->transposeTimesFullMatrix(aAPref->timesFullMatrix(aArefm));
-        return aApm;
-    }
+FColDsptr ASMTMarker::rpmp()
+{
+	//p is cm
+	auto refItem = static_cast<ASMTRefItem*>(owner);
+	auto& rPrefP = refItem->position3D;
+	auto& aAPref = refItem->rotationMatrix;
+	auto& rrefmref = position3D;
+	auto rPmP = rPrefP->plusFullColumn(aAPref->timesFullColumn(rrefmref));
+	auto& principalMassMarker = static_cast<ASMTPart*>(refItem->owner)->principalMassMarker;
+	auto& rPcmP = principalMassMarker->position3D;
+	auto& aAPcm = principalMassMarker->rotationMatrix;
+	auto rpmp = aAPcm->transposeTimesFullColumn(rPmP->minusFullColumn(rPcmP));
+	return rpmp;
+}
 
-    void ASMTMarker::createMbD(std::shared_ptr<System>, std::shared_ptr<Units> mbdUnits)
-    {
-        auto mkr = CREATE<MarkerFrame>::With(name.c_str());
-        auto prt = std::static_pointer_cast<Part>(partOrAssembly()->mbdObject);
-        prt->partFrame->addMarkerFrame(mkr);
+FMatDsptr ASMTMarker::aApm()
+{
+	//p is cm
+	auto refItem = static_cast<ASMTRefItem*>(owner);
+	auto& aAPref = refItem->rotationMatrix;
+	auto& aArefm = rotationMatrix;
+	auto& principalMassMarker = static_cast<ASMTPart*>(refItem->owner)->principalMassMarker;
+	auto& aAPcm = principalMassMarker->rotationMatrix;
+	auto aApm = aAPcm->transposeTimesFullMatrix(aAPref->timesFullMatrix(aArefm));
+	return aApm;
+}
 
-        mkr->rpmp = rpmp()->times(1.0 / mbdUnits->length);
-        mkr->aApm = aApm();
-        mbdObject = mkr->endFrames->at(0);
-    }
+void ASMTMarker::createMbD(std::shared_ptr<System>, std::shared_ptr<Units> mbdUnits)
+{
+	auto mkr = CREATE<MarkerFrame>::With(name.c_str());
+	auto prt = std::static_pointer_cast<Part>(partOrAssembly()->mbdObject);
+	prt->partFrame->addMarkerFrame(mkr);
+
+	mkr->rpmp = rpmp()->times(1.0 / mbdUnits->length);
+	mkr->aApm = aApm();
+	mbdObject = mkr->endFrames->at(0);
+}
 
-    void ASMTMarker::storeOnLevel(std::ofstream& os, int level)
-    {
-        storeOnLevelString(os, level, "Marker");
-        storeOnLevelString(os, level + 1, "Name");
-        storeOnLevelString(os, level + 2, name);
-        ASMTSpatialItem::storeOnLevel(os, level);
-    }
+void ASMTMarker::storeOnLevel(std::ofstream& os, int level)
+{
+	storeOnLevelString(os, level, "Marker");
+	storeOnLevelString(os, level + 1, "Name");
+	storeOnLevelString(os, level + 2, name);
+	ASMTSpatialItem::storeOnLevel(os, level);
 }
diff --git a/OndselSolver/ASMTMotion.cpp b/OndselSolver/ASMTMotion.cpp
@@ -9,36 +9,36 @@
 
 #include "ASMTMotion.h"
 
-namespace MbD {
-    void ASMTMotion::readMotionSeries(std::vector<std::string>& lines)
-    {
-        std::string str = lines[0];
-        std::string substr = "MotionSeries";
-        auto pos = str.find(substr);
-        assert(pos != std::string::npos);
-        str.erase(0, pos + substr.length());
-        auto seriesName = readString(str);
-        assert(fullName("") == seriesName);
-        lines.erase(lines.begin());
-        readFXonIs(lines);
-        readFYonIs(lines);
-        readFZonIs(lines);
-        readTXonIs(lines);
-        readTYonIs(lines);
-        readTZonIs(lines);
-    }
+using namespace MbD;
 
-    void ASMTMotion::initMarkers()
-    {
-    }
+void ASMTMotion::readMotionSeries(std::vector<std::string>& lines)
+{
+	std::string str = lines[0];
+	std::string substr = "MotionSeries";
+	auto pos = str.find(substr);
+	assert(pos != std::string::npos);
+	str.erase(0, pos + substr.length());
+	auto seriesName = readString(str);
+	assert(fullName("") == seriesName);
+	lines.erase(lines.begin());
+	readFXonIs(lines);
+	readFYonIs(lines);
+	readFZonIs(lines);
+	readTXonIs(lines);
+	readTYonIs(lines);
+	readTZonIs(lines);
+}
+
+void ASMTMotion::initMarkers()
+{
+}
 
-    void ASMTMotion::storeOnLevel(std::ofstream&, int)
-    {
-        assert(false);
-    }
+void ASMTMotion::storeOnLevel(std::ofstream&, int)
+{
+	assert(false);
+}
 
-    void ASMTMotion::storeOnTimeSeries(std::ofstream&)
-    {
-        assert(false);
-    }
+void ASMTMotion::storeOnTimeSeries(std::ofstream&)
+{
+	assert(false);
 }
diff --git a/OndselSolver/ASMTRotationalMotion.cpp b/OndselSolver/ASMTRotationalMotion.cpp
@@ -73,7 +73,7 @@ void MbD::ASMTRotationalMotion::createMbD(std::shared_ptr<System> mbdSys, std::s
 	geoPhi->createMbD(mbdSys, mbdUnits);
 	//std::cout << *geoPhi << std::endl;
 	auto simple = geoPhi->simplified(geoPhi);
-	//std::cout << *simple << std::endl;
+	std::cout << *simple << std::endl;
 	std::static_pointer_cast<ZRotation>(mbdObject)->phiBlk = simple;
 }