Backhoe issues (#67)

* backhoe issues * runDragStep edit * backhoe issues * runDragStep edit * Reduce large drag step progressively until convergence.
2024-06-24 22:24:38 -06:00
parent 79a1e4ed4c
commit 9058e0849d
20 changed files with 2224 additions and 1505 deletions
--- 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;
+}