Backhoe issues (#67)

* backhoe issues * runDragStep edit * backhoe issues * runDragStep edit * Reduce large drag step progressively until convergence.
Ondsel-Development · Jun 25, 2024 · 9058e08 · 9058e08
1 parent 79a1e4e
commit 9058e08
Show file tree

Hide file tree

Showing 20 changed files with 2,257 additions and 1,538 deletions.
diff --git a/OndselSolver/ASMTAssembly.cpp b/OndselSolver/ASMTAssembly.cpp
diff --git a/OndselSolver/ASMTAssembly.h b/OndselSolver/ASMTAssembly.h
@@ -79,6 +79,7 @@ namespace MbD {
 
 		void outputFor(AnalysisType type);
 		void preMbDrun(std::shared_ptr<System> mbdSys);
+		void preMbDrunDragStep(std::shared_ptr<System> mbdSys, std::shared_ptr<std::vector<std::shared_ptr<Part>>> dragParts);
 		void postMbDrun();
 		void calcCharacteristicDimensions();
 		double calcCharacteristicTime() const;
@@ -97,6 +98,7 @@ namespace MbD {
 		void runPreDrag();
 		void runDragStep(std::shared_ptr<std::vector<std::shared_ptr<ASMTPart>>> dragParts);
 		void runPostDrag();
+		void restorePosRot();
 		void runKINEMATIC();
 		void initprincipalMassMarker();
 		std::shared_ptr<ASMTSpatialContainer> spatialContainerAt(std::shared_ptr<ASMTAssembly> self, std::string& longname) const;

diff --git a/OndselSolver/ASMTPart.cpp b/OndselSolver/ASMTPart.cpp
@@ -126,6 +126,13 @@ void MbD::ASMTPart::createMbD(std::shared_ptr<System> mbdSys, std::shared_ptr<Un
 	if (isFixed) std::static_pointer_cast<Part>(mbdObject)->asFixed();
 }
 
+void MbD::ASMTPart::preMbDrunDragStep(std::shared_ptr<System> mbdSys, std::shared_ptr<Units> mbdUnits)
+{
+	auto mbdPart = std::static_pointer_cast<Part>(mbdObject);
+	mbdPart->qX(rOcmO()->times(1.0 / mbdUnits->length));
+	mbdPart->qE(qEp());
+}
+
 void MbD::ASMTPart::storeOnLevel(std::ofstream& os, size_t level)
 {
 	storeOnLevelString(os, level, "Part");

diff --git a/OndselSolver/ASMTPart.h b/OndselSolver/ASMTPart.h
@@ -26,6 +26,7 @@ namespace MbD {
         FColDsptr omeOpO() override;
         ASMTPart* part() override;
         void createMbD(std::shared_ptr<System> mbdSys, std::shared_ptr<Units> mbdUnits) override;
+        void preMbDrunDragStep(std::shared_ptr<System> mbdSys, std::shared_ptr<Units> mbdUnits);
         void storeOnLevel(std::ofstream& os, size_t level) override;
         void storeOnLevelMassMarker(std::ofstream& os, size_t level);
         void storeOnTimeSeries(std::ofstream& os) override;

diff --git a/OndselSolver/ASMTSpatialContainer.cpp b/OndselSolver/ASMTSpatialContainer.cpp
@@ -376,9 +376,11 @@ void MbD::ASMTSpatialContainer::updateFromMbD()
 	auto& aAPp = principalMassMarker->rotationMatrix;
 	auto aAOP = aAOp->timesTransposeFullMatrix(aAPp);
 	rotationMatrix = aAOP;
+	oldRotMat = rotationMatrix;
 	auto rPcmO = aAOP->timesFullColumn(rPcmP);
 	auto rOPO = rOcmO->minusFullColumn(rPcmO);
 	position3D = rOPO;
+	oldPos3D = position3D;
 	auto vOPO = vOcmO->minusFullColumn(omeOPO->cross(rPcmO));
 	velocity3D = vOPO;
 	auto aOPO = aOcmO->minusFullColumn(alpOPO->cross(rPcmO))->minusFullColumn(omeOPO->cross(omeOPO->cross(rPcmO)));

diff --git a/OndselSolver/ASMTSpatialItem.cpp b/OndselSolver/ASMTSpatialItem.cpp
@@ -169,3 +169,9 @@ FMatDsptr MbD::ASMTSpatialItem::getRotationMatrix(size_t i)
 	bryantAngles->calc();
 	return bryantAngles->aA;
 }
+
+void MbD::ASMTSpatialItem::restorePosRot()
+{
+	position3D = oldPos3D;
+	rotationMatrix = oldRotMat;
+}
diff --git a/OndselSolver/ASMTSpatialItem.h b/OndselSolver/ASMTSpatialItem.h
@@ -36,14 +36,17 @@ namespace MbD {
 		void storeOnLevelRotationMatrixRaw(std::ofstream& os, size_t level);
 		FColDsptr getPosition3D(size_t i);
 		FMatDsptr getRotationMatrix(size_t i);
+		void restorePosRot();
 
 		FColDsptr position3D = std::make_shared<FullColumn<double>>(3);
 		FMatDsptr rotationMatrix = std::make_shared<FullMatrix<double>>(ListListD{
 				{ 1, 0, 0 },
 				{ 0, 1, 0 },
 				{ 0, 0, 1 }
 			});
-		FRowDsptr xs, ys, zs, bryxs, bryys, bryzs;
+        FColDsptr oldPos3D = position3D;
+        FMatDsptr oldRotMat = rotationMatrix;
+        FRowDsptr xs, ys, zs, bryxs, bryys, bryzs;
 		FRowDsptr inxs, inys, inzs, inbryxs, inbryys, inbryzs;
 
 	};