PointInPlane constraint (#712)

* Allow Plane init with no specific x dir and add Plane.toPln

* PointInPlane constraint

Author: marcus7070

cadquery/assembly.py

@@ -4,8 +4,8 @@
 from uuid import uuid1 as uuid
 
 from .cq import Workplane
-from .occ_impl.shapes import Shape, Face, Edge
-from .occ_impl.geom import Location, Vector
+from .occ_impl.shapes import Shape, Face, Edge, Wire
+from .occ_impl.geom import Location, Vector, Plane
 from .occ_impl.assembly import Color
 from .occ_impl.solver import (
     ConstraintSolver,
@@ -18,7 +18,7 @@
 
 # type definitions
 AssemblyObjects = Union[Shape, Workplane, None]
-ConstraintKinds = Literal["Plane", "Point", "Axis"]
+ConstraintKinds = Literal["Plane", "Point", "Axis", "PointInPlane"]
 ExportLiterals = Literal["STEP", "XML"]
 
 PATH_DELIM = "/"
@@ -79,7 +79,7 @@ def __init__(
     ):
         """
         Construct a constraint.
-        
+
         :param objects: object names refernced in the constraint
         :param args: subshapes (e.g. faces or edges) of the objects
         :param sublocs: locations of the objects (only relevant if the objects are nested in a sub-assembly)
@@ -106,14 +106,25 @@ def _getAxis(self, arg: Shape) -> Vector:
 
         return rv
 
+    def _getPlane(self, arg: Shape) -> Plane:
+
+        if isinstance(arg, Face):
+            normal = arg.normalAt()
+        elif isinstance(arg, (Edge, Wire)):
+            normal = arg.normal()
+        else:
+            raise ValueError(f"Can not get normal from {arg}.")
+        origin = arg.Center()
+        return Plane(origin, normal=normal)
+
     def toPOD(self) -> ConstraintPOD:
         """
         Convert the constraint to a representation used by the solver.
         """
 
         rv: List[Tuple[ConstraintMarker, ...]] = []
 
-        for arg, loc in zip(self.args, self.sublocs):
+        for idx, (arg, loc) in enumerate(zip(self.args, self.sublocs)):
 
             arg = arg.located(loc * arg.location())
 
@@ -123,6 +134,11 @@ def toPOD(self) -> ConstraintPOD:
                 rv.append((arg.Center().toPnt(),))
             elif self.kind == "Plane":
                 rv.append((self._getAxis(arg).toDir(), arg.Center().toPnt()))
+            elif self.kind == "PointInPlane":
+                if idx == 0:
+                    rv.append((arg.Center().toPnt(),))
+                else:
+                    rv.append((self._getPlane(arg).toPln(),))
             else:
                 raise ValueError(f"Unknown constraint kind {self.kind}")
 
@@ -157,22 +173,22 @@ def __init__(
         """
         construct an assembly
 
-        :param obj: root object of the assembly (deafault: None)
-        :param loc: location of the root object (deafault: None, interpreted as identity transformation)
+        :param obj: root object of the assembly (default: None)
+        :param loc: location of the root object (default: None, interpreted as identity transformation)
         :param name: unique name of the root object (default: None, reasulting in an UUID being generated)
         :param color: color of the added object (default: None)
         :return: An Assembly object.
-        
-        
+
+
         To create an empty assembly use::
-            
+
             assy = Assembly(None)
-            
+
         To create one constraint a root object::
-            
+
             b = Workplane().box(1,1,1)
             assy = Assembly(b, Location(Vector(0,0,1)), name="root")
-            
+
         """
 
         self.obj = obj
@@ -211,12 +227,15 @@ def add(
         color: Optional[Color] = None,
     ) -> "Assembly":
         """
-        add a subassembly to the current assembly.
-        
+        Add a subassembly to the current assembly.
+
         :param obj: subassembly to be added
-        :param loc: location of the root object (deafault: None, resulting in the location stored in the subassembly being used)
-        :param name: unique name of the root object (default: None, resulting in the name stored in the subassembly being used)
-        :param color: color of the added object (default: None, resulting in the color stored in the subassembly being used)
+        :param loc: location of the root object (default: None, resulting in the location stored in
+          the subassembly being used)
+        :param name: unique name of the root object (default: None, resulting in the name stored in
+          the subassembly being used)
+        :param color: color of the added object (default: None, resulting in the color stored in the
+          subassembly being used)
         """
         ...
 
@@ -229,18 +248,20 @@ def add(
         color: Optional[Color] = None,
     ) -> "Assembly":
         """
-        add a subassembly to the current assembly with explicit location and name
-        
+        Add a subassembly to the current assembly with explicit location and name.
+
         :param obj: object to be added as a subassembly
-        :param loc: location of the root object (deafault: None, interpreted as identity transformation)
-        :param name: unique name of the root object (default: None, resulting in an UUID being generated)
+        :param loc: location of the root object (default: None, interpreted as identity
+          transformation)
+        :param name: unique name of the root object (default: None, resulting in an UUID being
+          generated)
         :param color: color of the added object (default: None)
         """
         ...
 
     def add(self, arg, **kwargs):
         """
-        add a subassembly to the current assembly.
+        Add a subassembly to the current assembly.
         """
 
         if isinstance(arg, Assembly):
@@ -270,18 +291,18 @@ def add(self, arg, **kwargs):
 
     def _query(self, q: str) -> Tuple[str, Optional[Shape]]:
         """
-        Execute a selector query on the assembly. 
+        Execute a selector query on the assembly.
         The query is expected to be in the following format:
-        
+
             name[?tag][@kind@args]
-            
+
         valid example include:
-        
+
             obj_name @ faces @ >Z
-            obj_name?tag1@faces@>Z          
+            obj_name?tag1@faces@>Z
             obj_name ? tag
             obj_name
-        
+
         """
 
         tmp: Workplane
@@ -311,7 +332,7 @@ def _query(self, q: str) -> Tuple[str, Optional[Shape]]:
     def _subloc(self, name: str) -> Tuple[Location, str]:
         """
         Calculate relative location of an object in a subassembly.
-        
+
         Returns the relative posiitons as well as the name of the top assembly.
         """
 
@@ -422,9 +443,9 @@ def save(
     ) -> "Assembly":
         """
         save as STEP or OCCT native XML file
-        
+
         :param path: filepath
-        :param exportType: export format (deafault: None, results in format being inferred form the path)
+        :param exportType: export format (default: None, results in format being inferred form the path)
         """
 
         if exportType is None:

cadquery/occ_impl/geom.py

@@ -2,7 +2,18 @@
 
 from typing import overload, Sequence, Union, Tuple, Type, Optional
 
-from OCP.gp import gp_Vec, gp_Ax1, gp_Ax3, gp_Pnt, gp_Dir, gp_Trsf, gp_GTrsf, gp, gp_XYZ
+from OCP.gp import (
+    gp_Vec,
+    gp_Ax1,
+    gp_Ax3,
+    gp_Pnt,
+    gp_Dir,
+    gp_Pln,
+    gp_Trsf,
+    gp_GTrsf,
+    gp_XYZ,
+    gp,
+)
 from OCP.Bnd import Bnd_Box
 from OCP.BRepBndLib import BRepBndLib
 from OCP.BRepMesh import BRepMesh_IncrementalMesh
@@ -500,30 +511,35 @@ def bottom(cls, origin=(0, 0, 0), xDir=Vector(1, 0, 0)):
         plane._setPlaneDir(xDir)
         return plane
 
-    def __init__(self, origin, xDir, normal):
-        """Create a Plane with an arbitrary orientation
-
-        TODO: project x and y vectors so they work even if not orthogonal
-        :param origin: the origin
-        :type origin: a three-tuple of the origin, in global coordinates
-        :param xDir: a vector representing the xDirection.
-        :type xDir: a three-tuple representing a vector, or a FreeCAD Vector
-        :param normal: the normal direction for the new plane
-        :type normal: a FreeCAD Vector
-        :raises: ValueError if the specified xDir is not orthogonal to the provided normal.
-        :return: a plane in the global space, with the xDirection of the plane in the specified direction.
+    def __init__(
+        self,
+        origin: Union[Tuple[float, float, float], Vector],
+        xDir: Optional[Union[Tuple[float, float, float], Vector]] = None,
+        normal: Union[Tuple[float, float, float], Vector] = (0, 0, 1),
+    ):
+        """
+        Create a Plane with an arbitrary orientation
+
+        :param origin: the origin in global coordinates
+        :param xDir: an optional vector representing the xDirection.
+        :param normal: the normal direction for the plane
+        :raises ValueError: if the specified xDir is not orthogonal to the provided normal
         """
         zDir = Vector(normal)
         if zDir.Length == 0.0:
             raise ValueError("normal should be non null")
 
-        xDir = Vector(xDir)
-        if xDir.Length == 0.0:
-            raise ValueError("xDir should be non null")
-
         self.zDir = zDir.normalized()
+
+        if xDir is None:
+            ax3 = gp_Ax3(Vector(origin).toPnt(), Vector(normal).toDir())
+            xDir = Vector(ax3.XDirection())
+        else:
+            xDir = Vector(xDir)
+            if xDir.Length == 0.0:
+                raise ValueError("xDir should be non null")
         self._setPlaneDir(xDir)
-        self.origin = origin
+        self.origin = Vector(origin)
 
     def _eq_iter(self, other):
         """Iterator to successively test equality"""
@@ -725,6 +741,10 @@ def location(self) -> "Location":
 
         return Location(self)
 
+    def toPln(self) -> gp_Pln:
+
+        return gp_Pln(gp_Ax3(self.origin.toPnt(), self.zDir.toDir(), self.xDir.toDir()))
+
 
 class BoundBox(object):
     """A BoundingBox for an object or set of objects. Wraps the OCP one"""

cadquery/occ_impl/solver.py

@@ -4,12 +4,12 @@
 from numpy import array, eye, zeros, pi
 from scipy.optimize import minimize
 
-from OCP.gp import gp_Vec, gp_Dir, gp_Pnt, gp_Trsf, gp_Quaternion
+from OCP.gp import gp_Vec, gp_Pln, gp_Lin, gp_Dir, gp_Pnt, gp_Trsf, gp_Quaternion
 
 from .geom import Location
 
 DOF6 = Tuple[float, float, float, float, float, float]
-ConstraintMarker = Union[gp_Dir, gp_Pnt]
+ConstraintMarker = Union[gp_Pln, gp_Dir, gp_Pnt]
 Constraint = Tuple[
     Tuple[ConstraintMarker, ...], Tuple[Optional[ConstraintMarker], ...], Optional[Any]
 ]
@@ -117,7 +117,25 @@ def dir_cost(
                 DIR_SCALING * (val - m1.Transformed(t1).Angle(m2.Transformed(t2))) ** 2
             )
 
+        def pnt_pln_cost(
+            m1: gp_Pnt,
+            m2: gp_Pln,
+            t1: gp_Trsf,
+            t2: gp_Trsf,
+            val: Optional[float] = None,
+        ) -> float:
+
+            val = 0 if val is None else val
+
+            m2_located = m2.Transformed(t2)
+            # offset in the plane's normal direction by val:
+            m2_located.Translate(gp_Vec(m2_located.Axis().Direction()).Multiplied(val))
+            return m2_located.SquareDistance(m1.Transformed(t1))
+
         def f(x):
+            """
+            Function to be minimized
+            """
 
             constraints = self.constraints
             ne = self.ne
@@ -133,10 +151,12 @@ def f(x):
                 t2 = transforms[k2] if k2 not in self.locked else gp_Trsf()
 
                 for m1, m2 in zip(ms1, ms2):
-                    if isinstance(m1, gp_Pnt):
+                    if isinstance(m1, gp_Pnt) and isinstance(m2, gp_Pnt):
                         rv += pt_cost(m1, m2, t1, t2, d)
                     elif isinstance(m1, gp_Dir):
                         rv += dir_cost(m1, m2, t1, t2, d)
+                    elif isinstance(m1, gp_Pnt) and isinstance(m2, gp_Pln):
+                        rv += pnt_pln_cost(m1, m2, t1, t2, d)
                     else:
                         raise NotImplementedError(f"{m1,m2}")
 
@@ -166,7 +186,7 @@ def jac(x):
                 t2 = transforms[k2] if k2 not in self.locked else gp_Trsf()
 
                 for m1, m2 in zip(ms1, ms2):
-                    if isinstance(m1, gp_Pnt):
+                    if isinstance(m1, gp_Pnt) and isinstance(m2, gp_Pnt):
                         tmp = pt_cost(m1, m2, t1, t2, d)
 
                         for j in range(NDOF):
@@ -197,6 +217,22 @@ def jac(x):
                             if k2 not in self.locked:
                                 tmp2 = dir_cost(m1, m2, t1, t2j, d)
                                 rv[k2 * NDOF + j] += (tmp2 - tmp) / DIFF_EPS
+
+                    elif isinstance(m1, gp_Pnt) and isinstance(m2, gp_Pln):
+                        tmp = pnt_pln_cost(m1, m2, t1, t2, d)
+
+                        for j in range(NDOF):
+
+                            t1j = transforms_delta[k1 * NDOF + j]
+                            t2j = transforms_delta[k2 * NDOF + j]
+
+                            if k1 not in self.locked:
+                                tmp1 = pnt_pln_cost(m1, m2, t1j, t2, d)
+                                rv[k1 * NDOF + j] += (tmp1 - tmp) / DIFF_EPS
+
+                            if k2 not in self.locked:
+                                tmp2 = pnt_pln_cost(m1, m2, t1, t2j, d)
+                                rv[k2 * NDOF + j] += (tmp2 - tmp) / DIFF_EPS
                     else:
                         raise NotImplementedError(f"{m1,m2}")