Handle constraints from infinite faces (#797)

* Face.toPln method added

* solver: set xtol_abs

* Constraint: handle infinite faces

Infinite faces have their center at 1e99, which was causing overflows in
the solver and also was not what the user intended when creating the
Shape. They are now converted to the expected values.

cadquery/assembly.py

@@ -15,6 +15,9 @@
 from .occ_impl.exporters.assembly import exportAssembly, exportCAF
 
 from .selectors import _expression_grammar as _selector_grammar
+from OCP.BRepTools import BRepTools
+from OCP.gp import gp_Pln, gp_Pnt
+from OCP.Precision import Precision
 
 # type definitions
 AssemblyObjects = Union[Shape, Workplane, None]
@@ -106,16 +109,33 @@ def _getAxis(self, arg: Shape) -> Vector:
 
         return rv
 
-    def _getPlane(self, arg: Shape) -> Plane:
+    def _getPln(self, arg: Shape) -> gp_Pln:
 
         if isinstance(arg, Face):
-            normal = arg.normalAt()
+            rv = gp_Pln(self._getPnt(arg), arg.normalAt().toDir())
         elif isinstance(arg, (Edge, Wire)):
             normal = arg.normal()
+            origin = arg.Center()
+            plane = Plane(origin, normal=normal)
+            rv = plane.toPln()
         else:
-            raise ValueError(f"Can not get normal from {arg}.")
-        origin = arg.Center()
-        return Plane(origin, normal=normal)
+            raise ValueError(f"Can not construct a plane for {arg}.")
+
+        return rv
+
+    def _getPnt(self, arg: Shape) -> gp_Pnt:
+
+        # check for infinite face
+        if isinstance(arg, Face) and any(
+            Precision.IsInfinite_s(x) for x in BRepTools.UVBounds_s(arg.wrapped)
+        ):
+            # fall back to gp_Pln center
+            pln = arg.toPln()
+            center = Vector(pln.Location())
+        else:
+            center = arg.Center()
+
+        return center.toPnt()
 
     def toPOD(self) -> ConstraintPOD:
         """
@@ -131,14 +151,14 @@ def toPOD(self) -> ConstraintPOD:
             if self.kind == "Axis":
                 rv.append((self._getAxis(arg).toDir(),))
             elif self.kind == "Point":
-                rv.append((arg.Center().toPnt(),))
+                rv.append((self._getPnt(arg),))
             elif self.kind == "Plane":
-                rv.append((self._getAxis(arg).toDir(), arg.Center().toPnt()))
+                rv.append((self._getAxis(arg).toDir(), self._getPnt(arg)))
             elif self.kind == "PointInPlane":
                 if idx == 0:
-                    rv.append((arg.Center().toPnt(),))
+                    rv.append((self._getPnt(arg),))
                 else:
-                    rv.append((self._getPlane(arg).toPln(),))
+                    rv.append((self._getPln(arg),))
             else:
                 raise ValueError(f"Unknown constraint kind {self.kind}")
 

cadquery/occ_impl/shapes.py

@@ -2264,6 +2264,17 @@ def chamfer2D(self, d: float, vertices: Iterable[Vertex]) -> "Face":
 
         return self.__class__(chamfer_builder.Shape()).fix()
 
+    def toPln(self) -> gp_Pln:
+        """
+        Convert this face to a gp_Pln.
+
+        Note the Location of the resulting plane may not equal the center of this face,
+        however the resulting plane will still contain the center of this face.
+        """
+
+        adaptor = BRepAdaptor_Surface(self.wrapped)
+        return adaptor.Plane()
+
 
 class Shell(Shape):
     """

cadquery/occ_impl/solver.py

@@ -256,7 +256,7 @@ def func(x, grad):
         opt.set_ftol_abs(0)
         opt.set_ftol_rel(0)
         opt.set_xtol_rel(TOL)
-        opt.set_xtol_abs(0)
+        opt.set_xtol_abs(TOL * 1e-3)
         opt.set_maxeval(MAXITER)
 
         x = opt.optimize(x0)

tests/test_assembly.py

@@ -71,6 +71,15 @@ def box_and_vertex():
     return assy
 
 
+def solve_result_check(solve_result: dict) -> bool:
+    checks = [
+        solve_result["status"] == nlopt.XTOL_REACHED,
+        solve_result["cost"] < 1e-9,
+        solve_result["iters"] > 0,
+    ]
+    return all(checks)
+
+
 def test_color():
 
     c1 = cq.Color("red")
@@ -228,9 +237,7 @@ def test_constrain(simple_assy, nested_assy):
 
     simple_assy.solve()
 
-    assert simple_assy._solve_result["status"] == nlopt.XTOL_REACHED
-    assert simple_assy._solve_result["cost"] < 1e-9
-    assert simple_assy._solve_result["iters"] > 0
+    assert solve_result_check(simple_assy._solve_result)
 
     assert (
         simple_assy.loc.wrapped.Transformation()
@@ -247,9 +254,7 @@ def test_constrain(simple_assy, nested_assy):
 
     nested_assy.solve()
 
-    assert nested_assy._solve_result["status"] == nlopt.XTOL_REACHED
-    assert nested_assy._solve_result["cost"] < 1e-9
-    assert nested_assy._solve_result["iters"] > 0
+    assert solve_result_check(nested_assy._solve_result)
 
     assert (
         nested_assy.children[0]
@@ -302,6 +307,7 @@ def test_PointInPlane_constraint(box_and_vertex):
         param=0,
     )
     box_and_vertex.solve()
+    solve_result_check(box_and_vertex._solve_result)
 
     x_pos = (
         box_and_vertex.children[0].loc.wrapped.Transformation().TranslationPart().X()
@@ -311,6 +317,7 @@ def test_PointInPlane_constraint(box_and_vertex):
     # add a second PointInPlane constraint
     box_and_vertex.constrain("vertex", "box@faces@>Y", "PointInPlane", param=0)
     box_and_vertex.solve()
+    solve_result_check(box_and_vertex._solve_result)
 
     vertex_translation_part = (
         box_and_vertex.children[0].loc.wrapped.Transformation().TranslationPart()
@@ -323,6 +330,7 @@ def test_PointInPlane_constraint(box_and_vertex):
     # add a third PointInPlane constraint
     box_and_vertex.constrain("vertex", "box@faces@>Z", "PointInPlane", param=0)
     box_and_vertex.solve()
+    solve_result_check(box_and_vertex._solve_result)
 
     # should now be on the >X and >Y and >Z corner
     assert (
@@ -342,6 +350,7 @@ def test_PointInPlane_3_parts(box_and_vertex):
     box_and_vertex.constrain("vertex", "cylinder@faces@>Z", "PointInPlane")
     box_and_vertex.constrain("vertex", "box@faces@>X", "PointInPlane")
     box_and_vertex.solve()
+    solve_result_check(box_and_vertex._solve_result)
     vertex_translation_part = (
         box_and_vertex.children[0].loc.wrapped.Transformation().TranslationPart()
     )
@@ -356,6 +365,7 @@ def test_PointInPlane_param(box_and_vertex, param0, param1):
     box_and_vertex.constrain("vertex", "box@faces@>Z", "PointInPlane", param=param0)
     box_and_vertex.constrain("vertex", "box@faces@>X", "PointInPlane", param=param1)
     box_and_vertex.solve()
+    solve_result_check(box_and_vertex._solve_result)
 
     vertex_translation_part = (
         box_and_vertex.children[0].loc.wrapped.Transformation().TranslationPart()
@@ -364,9 +374,9 @@ def test_PointInPlane_param(box_and_vertex, param0, param1):
     assert vertex_translation_part.X() - 0.5 == pytest.approx(param1, abs=1e-6)
 
 
-def test_constraint_getPlane():
+def test_constraint_getPln():
     """
-    Test that _getPlane does the right thing with different arguments
+    Test that _getPln does the right thing with different arguments
     """
     ids = (0, 1)
     sublocs = (cq.Location(), cq.Location())
@@ -377,11 +387,19 @@ def make_constraint(shape0):
     def fail_this(shape0):
         c0 = make_constraint(shape0)
         with pytest.raises(ValueError):
-            c0._getPlane(c0.args[0])
+            c0._getPln(c0.args[0])
 
-    def resulting_plane(shape0):
+    def resulting_pln(shape0):
         c0 = make_constraint(shape0)
-        return c0._getPlane(c0.args[0])
+        return c0._getPln(c0.args[0])
+
+    def resulting_plane(shape0):
+        p0 = resulting_pln(shape0)
+        return cq.Plane(
+            cq.Vector(p0.Location()),
+            cq.Vector(p0.XAxis().Direction()),
+            cq.Vector(p0.Axis().Direction()),
+        )
 
     # point should fail
     fail_this(cq.Vertex.makeVertex(0, 0, 0))
@@ -423,7 +441,7 @@ def resulting_plane(shape0):
     wonky_shape = cq.Wire.makePolygon(points3)
     fail_this(wonky_shape)
 
-    # all faces should succeed
+    # all makePlane faces should succeed
     for length, width in product([None, 10], [None, 11]):
         f0 = cq.Face.makePlane(
             length=length, width=width, basePnt=(1, 2, 3), dir=(1, 0, 0)
@@ -482,3 +500,36 @@ def test_toCompound(simple_assy, nested_assy):
     assert cq.Vector(0, 0, 18) in [x.Center() for x in c3.Faces()]
     # also check with bounding box
     assert c3.BoundingBox().zlen == pytest.approx(18)
+
+
+@pytest.mark.parametrize("origin", [(0, 0, 0), (10, -10, 10)])
+@pytest.mark.parametrize("normal", [(0, 0, 1), (-1, -1, 1)])
+def test_infinite_face_constraint_PointInPlane(origin, normal):
+    """
+    An OCCT infinite face has a center at (1e99, 1e99), but when a user uses it
+    in a constraint, the center should be basePnt.
+    """
+
+    f0 = cq.Face.makePlane(length=None, width=None, basePnt=origin, dir=normal)
+
+    c0 = cq.assembly.Constraint(
+        ("point", "plane"),
+        (cq.Vertex.makeVertex(10, 10, 10), f0),
+        sublocs=(cq.Location(), cq.Location()),
+        kind="PointInPlane",
+    )
+    p0 = c0._getPln(c0.args[1])  # a gp_Pln
+    derived_origin = cq.Vector(p0.Location())
+    assert derived_origin == cq.Vector(origin)
+
+
+@pytest.mark.parametrize("kind", ["Plane", "PointInPlane", "Point"])
+def test_infinite_face_constraint_Plane(kind):
+
+    assy = cq.Assembly(cq.Workplane().sphere(1), name="part0")
+    assy.add(cq.Workplane().sphere(1), name="part1")
+    assy.constrain(
+        "part0", cq.Face.makePlane(), "part1", cq.Face.makePlane(), kind,
+    )
+    assy.solve()
+    assert solve_result_check(assy._solve_result)

tests/test_cadquery.py

@@ -4595,3 +4595,26 @@ def testBrepImportExport(self):
 
         self.assertTrue(si.isValid())
         self.assertAlmostEqual(si.Volume(), 1)
+
+    def testFaceToPln(self):
+
+        origin = (1, 2, 3)
+        normal = (1, 1, 1)
+        f0 = Face.makePlane(length=None, width=None, basePnt=origin, dir=normal)
+        p0 = f0.toPln()
+
+        self.assertTrue(Vector(p0.Location()) == Vector(origin))
+        self.assertTrue(Vector(p0.Axis().Direction()) == Vector(normal).normalized())
+
+        origin1 = (0, 0, -3)
+        normal1 = (-1, 1, -1)
+        f1 = Face.makePlane(length=0.1, width=100, basePnt=origin1, dir=normal1)
+        p1 = f1.toPln()
+
+        self.assertTrue(Vector(p1.Location()) == Vector(origin1))
+        self.assertTrue(Vector(p1.Axis().Direction()) == Vector(normal1).normalized())
+
+        f2 = Workplane().box(1, 1, 10, centered=False).faces(">Z").val()
+        p2 = f2.toPln()
+        self.assertTrue(p2.Contains(f2.Center().toPnt(), 0.1))
+        self.assertTrue(Vector(p2.Axis().Direction()) == f2.normalAt())