From 47c22bec65efbead15c801fad09c54a67f604a74 Mon Sep 17 00:00:00 2001
From: PaddleStroke <pierrelouis.boyer@gmail.com>
Date: Fri, 7 Jun 2024 11:00:20 +0200
Subject: [PATCH] Assembly: Add validation step during dragging to ignore steps
 where grounded objects moved.

---
 src/Mod/Assembly/App/AssemblyObject.cpp | 73 ++++++++++++++++---------
 src/Mod/Assembly/App/AssemblyObject.h   |  2 +
 2 files changed, 48 insertions(+), 27 deletions(-)

diff --git a/src/Mod/Assembly/App/AssemblyObject.cpp b/src/Mod/Assembly/App/AssemblyObject.cpp
index e3c8e1089e..4eea5f8617 100644
--- a/src/Mod/Assembly/App/AssemblyObject.cpp
+++ b/src/Mod/Assembly/App/AssemblyObject.cpp
@@ -221,14 +221,57 @@ void AssemblyObject::doDragStep()
 
         auto dragPartsVec = std::make_shared<std::vector<std::shared_ptr<ASMTPart>>>(dragMbdParts);
         mbdAssembly->runDragStep(dragPartsVec);
-        setNewPlacements();
-        redrawJointPlacements(getJoints());
+        if (validateNewPlacements()) {
+            setNewPlacements();
+            redrawJointPlacements(getJoints());
+        }
     }
     catch (...) {
         // We do nothing if a solve step fails.
     }
 }
 
+Base::Placement AssemblyObject::getMbdPlacement(std::shared_ptr<ASMTPart> mbdPart)
+{
+    double x, y, z;
+    mbdPart->getPosition3D(x, y, z);
+    Base::Vector3d pos = Base::Vector3d(x, y, z);
+
+    double q0, q1, q2, q3;
+    mbdPart->getQuarternions(q3, q0, q1, q2);
+    Base::Rotation rot = Base::Rotation(q0, q1, q2, q3);
+
+    return Base::Placement(pos, rot);
+}
+
+bool AssemblyObject::validateNewPlacements()
+{
+    // First we check if a grounded object has moved. It can happen that they flip.
+    for (auto* obj : getGroundedParts()) {
+        auto* propPlacement =
+            dynamic_cast<App::PropertyPlacement*>(obj->getPropertyByName("Placement"));
+        if (propPlacement) {
+            Base::Placement oldPlc = propPlacement->getValue();
+
+            auto it = objectPartMap.find(obj);
+            if (it != objectPartMap.end()) {
+                std::shared_ptr<MbD::ASMTPart> mbdPart = it->second;
+                Base::Placement newPlacement = getMbdPlacement(mbdPart);
+                if (!oldPlc.isSame(newPlacement)) {
+                    Base::Console().Warning(
+                        "Assembly : Ignoring bad solve, a grounded object moved.\n");
+                    return false;
+                }
+            }
+        }
+    }
+
+    // TODO: We could do further tests
+    // For example check if the joints connectors are correctly aligned.
+
+    return true;
+}
+
 void AssemblyObject::postDrag()
 {
     mbdAssembly->runPostDrag();  // Do this after last drag
@@ -320,31 +363,7 @@ void AssemblyObject::setNewPlacements()
             continue;
         }
 
-        double x, y, z;
-        mbdPart->getPosition3D(x, y, z);
-        // Base::Console().Warning("in set placement : (%f, %f, %f)\n", x, y, z);
-        Base::Vector3d pos = Base::Vector3d(x, y, z);
-
-        // TODO : replace with quaternion to simplify
-        auto& r0 = mbdPart->rotationMatrix->at(0);
-        auto& r1 = mbdPart->rotationMatrix->at(1);
-        auto& r2 = mbdPart->rotationMatrix->at(2);
-        Base::Vector3d row0 = Base::Vector3d(r0->at(0), r0->at(1), r0->at(2));
-        Base::Vector3d row1 = Base::Vector3d(r1->at(0), r1->at(1), r1->at(2));
-        Base::Vector3d row2 = Base::Vector3d(r2->at(0), r2->at(1), r2->at(2));
-        Base::Matrix4D mat;
-        mat.setRow(0, row0);
-        mat.setRow(1, row1);
-        mat.setRow(2, row2);
-        Base::Rotation rot = Base::Rotation(mat);
-
-        /*double q0, q1, q2, q3;
-        mbdPart->getQuarternions(q0, q1, q2, q3);
-        Base::Rotation rot = Base::Rotation(q0, q1, q2, q3);*/
-
-        Base::Placement newPlacement = Base::Placement(pos, rot);
-
-        propPlacement->setValue(newPlacement);
+        propPlacement->setValue(getMbdPlacement(mbdPart));
         obj->purgeTouched();
     }
 }
diff --git a/src/Mod/Assembly/App/AssemblyObject.h b/src/Mod/Assembly/App/AssemblyObject.h
index 9cbc449a80..ff1760bbd1 100644
--- a/src/Mod/Assembly/App/AssemblyObject.h
+++ b/src/Mod/Assembly/App/AssemblyObject.h
@@ -155,6 +155,8 @@ public:
 
     void exportAsASMT(std::string fileName);
 
+    Base::Placement getMbdPlacement(std::shared_ptr<MbD::ASMTPart> mbdPart);
+    bool validateNewPlacements();
     void setNewPlacements();
     void recomputeJointPlacements(std::vector<App::DocumentObject*> joints);
     void redrawJointPlacements(std::vector<App::DocumentObject*> joints);