Assembly: Adds a pre-solve when creating joint, preventing wrong orthogonal solutions from solver.

src/Mod/Assembly/UtilsAssembly.py

@@ -1,5 +1,5 @@
 # SPDX-License-Identifier: LGPL-2.1-or-later
-# /****************************************************************************
+# /**************************************************************************
 #                                                                           *
 #    Copyright (c) 2023 Ondsel <development@ondsel.com>                     *
 #                                                                           *
@@ -19,7 +19,7 @@
 #    License along with FreeCAD. If not, see                                *
 #    <https://www.gnu.org/licenses/>.                                       *
 #                                                                           *
-# ***************************************************************************/
+# **************************************************************************/
 
 import FreeCAD as App
 import Part
@@ -237,6 +237,33 @@ def getObjectInPart(objName, part):
     return None
 
 
+# get the placement of Obj relative to its containing Part
+# Example : assembly.part1.part2.partn.body1 : placement of Obj relative to part1
+def getObjPlcRelativeToPart(objName, part):
+    obj = getObjectInPart(objName, part)
+
+    # we need plc to be relative to the containing part
+    obj_global_plc = getGlobalPlacement(obj, part)
+    part_global_plc = getGlobalPlacement(part)
+
+    return part_global_plc.inverse() * obj_global_plc
+
+
+# Example : assembly.part1.part2.partn.body1 : jcsPlc is relative to body1
+# This function returns jcsPlc relative to part1
+def getJcsPlcRelativeToPart(jcsPlc, objName, part):
+    obj_relative_plc = getObjPlcRelativeToPart(objName, part)
+    return obj_relative_plc * jcsPlc
+
+
+# Return the jcs global placement
+def getJcsGlobalPlc(jcsPlc, objName, part):
+    obj = getObjectInPart(objName, part)
+
+    obj_global_plc = getGlobalPlacement(obj, part)
+    return obj_global_plc * jcsPlc
+
+
 # The container is used to support cases where the same object appears at several places
 # which happens when you have a link to a part.
 def getGlobalPlacement(targetObj, container=None):
@@ -249,6 +276,13 @@ def getGlobalPlacement(targetObj, container=None):
     return App.Placement()
 
 
+def isThereOneRootAssembly():
+    for part in App.activeDocument().RootObjects:
+        if part.TypeId == "Assembly::AssemblyObject":
+            return True
+    return False
+
+
 def getTargetPlacementRelativeTo(
     targetObj, part, container, inContainerBranch, ignorePlacement=False
 ):