# SPDX-License-Identifier: LGPL-2.1-or-later # *************************************************************************** # * Copyright (c) 2009, 2010 Yorik van Havre * # * Copyright (c) 2009, 2010 Ken Cline * # * * # * This file is part of the FreeCAD CAx development system. * # * * # * This program is free software; you can redistribute it and/or modify * # * it under the terms of the GNU Lesser General Public License (LGPL) * # * as published by the Free Software Foundation; either version 2 of * # * the License, or (at your option) any later version. * # * for detail see the LICENCE text file. * # * * # * FreeCAD is distributed in the hope that it will be useful, * # * but WITHOUT ANY WARRANTY; without even the implied warranty of * # * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * # * GNU Library General Public License for more details. * # * * # * You should have received a copy of the GNU Library General Public * # * License along with FreeCAD; if not, write to the Free Software * # * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 * # * USA * # * * # *************************************************************************** """Provides various functions for general geometrical calculations.""" ## @package geometry # \ingroup draftgeoutils # \brief Provides various functions for general geometrical calculations. import math import lazy_loader.lazy_loader as lz import FreeCAD as App import DraftVecUtils import draftutils.gui_utils as gui_utils from draftgeoutils.general import geomType, vec # Delay import of module until first use because it is heavy Part = lz.LazyLoader("Part", globals(), "Part") ## \addtogroup draftgeoutils # @{ def findPerpendicular(point, edgeslist, force=None): """Find the perpendicular distance between a point and a list of edges. If force is specified, only the edge[force] will be considered, and it will be considered infinite. Returns ------- [vector_from_point_to_closest_edge, edge_index] The vector and the index in the list. None If no perpendicular vector could be found. """ if not isinstance(edgeslist, list): try: edgeslist = edgeslist.Edges except AttributeError: print("Doesn't have 'Edges'") return None if force is None: valid = None for edge in edgeslist: dist = findDistance(point, edge, strict=True) if dist: if not valid: valid = [dist, edgeslist.index(edge)] else: if dist.Length < valid[0].Length: valid = [dist, edgeslist.index(edge)] return valid else: edge = edgeslist[force] dist = findDistance(point, edge) if dist: return [dist, force] else: return None return None def findDistance(point, edge, strict=False): """Return a vector from the point to its closest point on the edge. If `strict` is `True`, the vector will be returned only if its endpoint lies on the `edge`. Edge can also be a list of 2 points. """ if isinstance(point, App.Vector): if isinstance(edge, list): segment = edge[1].sub(edge[0]) chord = edge[0].sub(point) norm = segment.cross(chord) perp = segment.cross(norm) dist = DraftVecUtils.project(chord, perp) if not dist: return None newpoint = point.add(dist) if dist.Length == 0: return None if strict: s1 = newpoint.sub(edge[0]) s2 = newpoint.sub(edge[1]) if s1.Length <= segment.Length and s2.Length <= segment.Length: return dist else: return None else: return dist elif geomType(edge) == "Line": segment = vec(edge) chord = edge.Vertexes[0].Point.sub(point) norm = segment.cross(chord) perp = segment.cross(norm) dist = DraftVecUtils.project(chord, perp) if not dist: return None newpoint = point.add(dist) if dist.Length == 0: return None if strict: s1 = newpoint.sub(edge.Vertexes[0].Point) s2 = newpoint.sub(edge.Vertexes[-1].Point) if s1.Length <= segment.Length and s2.Length <= segment.Length: return dist else: return None else: return dist elif geomType(edge) == "Circle": ve1 = edge.Vertexes[0].Point if len(edge.Vertexes) > 1: ve2 = edge.Vertexes[-1].Point else: ve2 = None center = edge.Curve.Center segment = center.sub(point) if segment.Length == 0: return None ratio = (segment.Length - edge.Curve.Radius) / segment.Length dist = segment.multiply(ratio) newpoint = App.Vector.add(point, dist) if dist.Length == 0: return None if strict and ve2: # Note 1: DraftVecUtils.angle(App.Vector(1, 1, 0)) => -0.7854 # Note 2: Angles are in the +pi to -pi range. ang1 = DraftVecUtils.angle(ve1.sub(center)) ang2 = DraftVecUtils.angle(ve2.sub(center)) angpt = DraftVecUtils.angle(newpoint.sub(center)) if ang1 >= ang2: # Arc does not cross the 9 o'clock point. if ang1 >= angpt and angpt >= ang2: return dist else: return None elif ang1 >= angpt or angpt >= ang2: return dist else: return None else: return dist elif geomType(edge) == "BSplineCurve" or geomType(edge) == "BezierCurve": try: pr = edge.Curve.parameter(point) np = edge.Curve.value(pr) dist = np.sub(point) except Part.OCCError: print("DraftGeomUtils: Unable to get curve parameter " "for point ", point) return None else: return dist else: print("DraftGeomUtils: Couldn't project point") return None else: print("DraftGeomUtils: Couldn't project point") return None def get_spline_normal(edge, tol=-1): """Find the normal of a BSpline edge.""" if edge.isNull(): return None if is_straight_line(shape, tol): return None plane = edge.findPlane(tol) if plane: normal = plane.Axis return normal else: return None def get_shape_normal(shape): """Find the normal of a shape or list of points or colinear edges, if possible.""" # New function based on get_normal() drafted by @Roy_043 # in discussion https://forum.freecad.org/viewtopic.php?p=717862#p717862 # # The normal would not be affected by the 3D view direction and flipped as # get_normal would be. Placement of the Shape is taken into account in this # new function instead : # - Normal of a planar shape is usually bi-directional, this function return # the one 'in the direction' of the shape's placement 'normal' (z-direction). # As reference, shape.findPlane() favour positive axes, get_normal() # favour the 'view direction' (obtained by getViewDirection() ). # - Even when the Shape is an edge or colinear edges, its Placement is taken # into account, and this function return one 'in the direction' of the # shape's placement 'normal' (z-direction). # https://forum.freecad.org/viewtopic.php?p=715850#p715850 # The normal direction of a Draft wire (co-planar) or arc used to depends on # the way the shape is constructed i.e. by vertexes in clockwise or # anti-clockwise. This cryptic behaviour no longer matters. if shape.isNull(): return None # Roy_043's remarks on the behavior of Shape.findPlane(): # - https://forum.freecad.org/viewtopic.php?p=713993#p713993 # Check if the shape is planar # If True: check if the outer wire is closed: # if True : Return a (plane with) normal based on the direction of the outer wire (CW or CCW). # if False : ? Return a (plane with) normal that points towards positive global axes with a certain priority # (probably? Z, Y and then X). # If False: return None. # # Further remarks : # - Tested Sketch with 2 colinear edges return a Plane with Axis # - Tested Sketch with 1 edge return no Plane with no Axis shape_rot = shape.Placement.Rotation plane = shape.findPlane() if plane is None: if not is_straight_line(shape): return None start_edge = shape.Edges[0] x_vec = start_edge.tangentAt( start_edge.FirstParameter ) # Return vector is in global coordinate local_x_vec = shape_rot.inverted().multVec(x_vec) # local_rot = App.Rotation(local_x_vec, App.Vector(0, 1, 0), App.Vector(0, 0, 1), "XZY") # see https://blog.freecad.org/2023/01/16/the-rotation-api-in-freecad/ for App.Rotation(vecx, vecy, vecz, string) # discussion - https://forum.freecad.org/viewtopic.php?p=717738#p717738 return shape_rot.multiply(local_rot).multVec(App.Vector(0, 0, 1)) normal = plane.Axis shape_normal = shape_rot.multVec(App.Vector(0, 0, 1)) # Now, check the normal direction of the plane (plane.Axis). # The final deduced normal should be 'in the direction' of the z-direction of the shape's placement, # if plane.Axis is in the 'opposite' direction of the Z direction of the shape's placement, reverse it. if normal.getAngle(shape_normal) > math.pi / 2: normal = normal.negative() return normal def get_normal(shape, tol=-1): """Find the normal of a shape or list of points, if possible.""" # for points if isinstance(shape, (list, tuple)): if len(shape) <= 2: return None else: poly = Part.makePolygon(shape) if is_straight_line(poly, tol): return None plane = poly.findPlane(tol) if plane: normal = plane.Axis return normal else: return None # for shapes if shape.isNull(): return None if is_straight_line(shape, tol): return None else: plane = find_plane(shape, tol) if plane: normal = plane.Axis else: return None # Check the 3D view to flip the normal if the GUI is available if App.GuiUp: view = gui_utils.get_3d_view() if view is not None: v_dir = view.getViewDirection() if normal.getAngle(v_dir) < 0.78: normal = normal.negative() return normal def getRotation(v1, v2=App.Vector(0, 0, 1)): """Get the rotation Quaternion between 2 vectors.""" if (v1.dot(v2) > 0.999999) or (v1.dot(v2) < -0.999999): # vectors are opposite return None axis = v1.cross(v2) axis.normalize() # angle = math.degrees(math.sqrt(v1.Length^2 * v2.Length^2) + v1.dot(v2)) angle = math.degrees(DraftVecUtils.angle(v1, v2, axis)) return App.Rotation(axis, angle) def is_planar(shape, tol=-1): """Return True if the given shape or list of points is planar.""" # for points if isinstance(shape, list): if len(shape) <= 3: return True else: poly = Part.makePolygon(shape) if is_straight_line(poly, tol): return True plane = poly.findPlane(tol) if plane: return True else: return False # for shapes if shape.isNull(): return False # because Part.Shape.findPlane return None for Vertex and straight edges if shape.ShapeType == "Vertex": return True if is_straight_line(shape, tol): return True plane = find_plane(shape, tol) if plane: return True else: return False def is_straight_line(shape, tol=-1): """Return True if shape is a straight line. function used in other methods because Part.Shape.findPlane assign a plane and normal to straight wires creating privileged directions and to deal with straight wires with overlapped edges.""" if shape.isNull(): return False if len(shape.Faces) != 0: return False if len(shape.Edges) == 0: return False if len(shape.Edges) >= 1: start_edge = shape.Edges[0] dir_start_edge = start_edge.tangentAt(start_edge.FirstParameter) point_start_edge = start_edge.firstVertex().Point # set tolerance if tol <= 0: err = shape.globalTolerance(tol) else: err = tol for edge in shape.Edges: first_point = edge.firstVertex().Point last_point = edge.lastVertex().Point dir_edge = edge.tangentAt(edge.FirstParameter) # check if edge is curve or no parallel to start_edge # because sin(x) = x + O(x**3), for small angular deflection it's # enough use the cross product of directions (or dot with a normal) if ( abs(edge.Length - first_point.distanceToPoint(last_point)) > err # https://forum.freecad.org/viewtopic.php?p=726101#p726101 # Shape with parallel edges but not colinear used to return True # by this function, below line added fixes this bug. # Further remark on the below fix : # - get_normal() use this function, may had created problems # previously but not reported; on the other hand, this fix # seems have no 'regression' created as get_normal use # plane.Axis (with 3DView check). See get_shape_normal() # which take into account shape's placement # - other functions had been using this also : Keep In View to # see if there is 'regression' : # # $ grep -rni "is_straight" ./ # ./draftfunctions/upgrade.py # ./draftgeoutils/geometry.py # ./draftmake/make_wire.py or first_point.distanceToLine(point_start_edge, dir_start_edge) > err # or dir_start_edge.cross(dir_edge).Length > err ): return False return True def are_coplanar(shape_a, shape_b, tol=-1): """Return True if exist a plane containing both shapes.""" if shape_a.isNull() or shape_b.isNull(): return False if not is_planar(shape_a, tol) or not is_planar(shape_b, tol): return False if shape_a.isEqual(shape_b): return True plane_a = find_plane(shape_a, tol) plane_b = find_plane(shape_b, tol) # set tolerance if tol <= 0: err = 1e-7 else: err = tol if plane_a and plane_b: normal_a = plane_a.Axis normal_b = plane_b.Axis proj = plane_a.projectPoint(plane_b.Position) if normal_a.cross(normal_b).Length > err or plane_b.Position.sub(proj).Length > err: return False else: return True elif plane_a and not plane_b: normal_a = plane_a.Axis for vertex in shape_b.Vertexes: dir_ver_b = vertex.Point.sub(plane_a.Position).normalize() if abs(normal_a.dot(dir_ver_b)) > err: proj = plane_a.projectPoint(vertex.Point) if vertex.Point.sub(proj).Length > err: return False return True elif plane_b and not plane_a: normal_b = plane_b.Axis for vertex in shape_a.Vertexes: dir_ver_a = vertex.Point.sub(plane_b.Position).normalize() if abs(normal_b.dot(dir_ver_a)) > err: proj = plane_b.projectPoint(vertex.Point) if vertex.Point.sub(proj).Length > err: return False return True # not normal_a and not normal_b: else: points_a = [vertex.Point for vertex in shape_a.Vertexes] points_b = [vertex.Point for vertex in shape_b.Vertexes] poly = Part.makePolygon(points_a + points_b) if is_planar(poly, tol): return True else: return False def get_spline_surface_normal(shape, tol=-1): """Check if shape formed by BSpline surfaces is planar and get normal. If shape is not planar return None.""" if shape.isNull(): return None if len(shape.Faces) == 0: return None # set tolerance if tol <= 0: err = shape.globalTolerance(tol) else: err = tol first_surf = shape.Faces[0].Surface if not first_surf.isPlanar(tol): return None # find bounds of first_surf u0, u1, v0, v1 = first_surf.bounds() u = (u0 + u1) / 2 v = (v0 + v1) / 2 first_normal = first_surf.normal(u, v) # check if all faces are planar and parallel for face in shape.Faces: surf = face.Surface if not surf.isPlanar(tol): return None u0, u1, v0, v1 = surf.bounds() u = (u0 + u1) / 2 v = (v0 + v1) / 2 surf_normal = surf.normal(u, v) if first_normal.cross(surf_normal).Length > err: return None normal = first_normal return normal def find_plane(shape, tol=-1): """Find the plane containing the shape if possible. Use this function as a workaround due Part.Shape.findPlane fail to find plane on BSpline surfaces.""" if shape.isNull(): return None if shape.ShapeType == "Vertex": return None if is_straight_line(shape, tol): return None plane = shape.findPlane(tol) if plane: return plane elif len(shape.Faces) >= 1: # in case shape have BSpline surfaces normal = get_spline_surface_normal(shape, tol) if normal: position = shape.CenterOfMass return Part.Plane(position, normal) else: return None else: return None def calculatePlacement(shape): """Return a placement located in the center of gravity of the shape. If the given shape is planar, return a placement located at the center of gravity of the shape, and oriented towards the shape's normal. Otherwise, it returns a null placement. """ if not is_planar(shape): return App.Placement() pos = shape.BoundBox.Center norm = get_normal(shape) # for backward compatibility with previous getNormal implementation if norm is None: norm = App.Vector(0, 0, 1) pla = App.Placement() pla.Base = pos r = getRotation(norm) if r: pla.Rotation = r return pla def mirror(point, edge): """Find mirror point relative to an edge.""" normPoint = point.add(findDistance(point, edge, False)) if normPoint: normPoint_point = App.Vector.sub(point, normPoint) normPoint_refl = normPoint_point.negative() refl = App.Vector.add(normPoint, normPoint_refl) return refl else: return None def mirror_matrix(mtx, pos, nor): """Return a mirrored copy of a matrix. Parameters ---------- mtx: Base::Matrix Matrix. pos: Base::Vector3 Point on mirror plane. nor: Base::Vector3 Normal of mirror plane. Returns ------- Base::Matrix """ # Code by Jolbas: # https://forum.freecad.org/viewtopic.php?p=702793#p702793 mtx_copy = App.Matrix(mtx) mtx_copy.move(-pos) mtx_copy.scale(-1) mtx_copy = App.Rotation(nor, 180) * mtx_copy mtx_copy.move(pos) return mtx_copy def uv_vectors_from_face(face, vec_z=App.Vector(0, 0, 1), tol=-1): """Return the u and v vectors of a planar face. It is up to the calling function to ensure the face is planar. If the u vector matches +/-vec_z, or the v vector matches -vec_z, the vectors are rotated to ensure the v vector matches +vec_z. Parameters ---------- face: Part.Face Face. vec_z: Base::Vector3, optional Defaults to Vector(0, 0, 1). Z axis vector used for reference. Is replaced by Vector(0, 0, 1) if it matches the +/-normal of the face. tol: float, optional Defaults to -1. Internal tolerance. 1e-7 is used if tol <=0. Returns ------- tuple U and v vector (Base::Vector3). """ err = 1e-7 if tol <= 0 else tol if not vec_z.isEqual(App.Vector(0, 0, 1), err): nor = face.normalAt(0, 0) if vec_z.isEqual(nor, err) or vec_z.isEqual(nor.negative(), err): vec_z = App.Vector(0, 0, 1) vec_u, vec_v = face.tangentAt(0, 0) if face.Orientation == "Reversed": vec_u, vec_v = vec_v, vec_u if vec_v.isEqual(vec_z.negative(), err): vec_u, vec_v = vec_u.negative(), vec_v.negative() elif vec_u.isEqual(vec_z, err): vec_u, vec_v = vec_v.negative(), vec_u elif vec_u.isEqual(vec_z.negative(), err): vec_u, vec_v = vec_v, vec_u.negative() return vec_u, vec_v def placement_from_face(face, vec_z=App.Vector(0, 0, 1), rotated=False, tol=-1): """Return a placement from the center of gravity, and the u and v vectors of a planar face. It is up to the calling function to ensure the face is planar. Parameters ---------- face: Part.Face Face. vec_z: Base::Vector3, optional Defaults to Vector(0, 0, 1). Z axis vector used for reference. Is replaced by Vector(0, 0, 1) if it matches the +/-normal of the face. rotated: bool, optional Defaults to `False`. If `False` the v vector of the face defines the Y axis of the placement. If `True` the -v vector of the face defines the Z axis of the placement (used by Arch_Window). The u vector defines the X axis in both cases. tol: float, optional Defaults to -1. Internal tolerance. 1e-7 is used if tol <=0. Returns ------- Base::Placement See also -------- DraftGeomUtils.uv_vectors_from_face """ pt_pos = face.CenterOfGravity vec_u, vec_v = uv_vectors_from_face(face, vec_z, tol) if rotated: return App.Placement(pt_pos, App.Rotation(vec_u, App.Vector(), vec_v.negative(), "XZY")) else: return App.Placement(pt_pos, App.Rotation(vec_u, vec_v, App.Vector(), "XYZ")) def placement_from_points(pt_pos, pt_x, pt_y, as_vectors=False, tol=-1): """Return a placement from 3 points defining an origin, an X axis and a Y axis. If the vectors calculated from the arguments are too short or parallel, the returned placement will have a default rotation. Parameters ---------- pt_pos: Base::Vector3 Origin (Base of Placement). pt_x: Base::Vector3 Point on positive X axis. Or X axis vector if as_vectors is `True`. pt_y: Base::Vector3 Point on positive Y axis. Or Y axis vector if as_vectors is `True`. as_vectors: bool, optional Defaults to `False`. If `True` treat pt_x and pt_y as axis vectors. tol: float, optional Defaults to -1. Internal tolerance. 1e-7 is used if tol <=0. Returns ------- Base::Placement See also -------- DraftGeomUtils.getRotation DraftVecUtils.getRotation """ err = 1e-7 if tol <= 0 else tol if as_vectors is False: vec_u = pt_x - pt_pos vec_v = pt_y - pt_pos else: vec_u = App.Vector(pt_x) vec_v = App.Vector(pt_y) if vec_u.Length < err or vec_v.Length < err: rot = App.Rotation() else: vec_u.normalize() vec_v.normalize() if vec_u.isEqual(vec_v, err) or vec_u.isEqual(vec_v.negative(), err): rot = App.Rotation() else: rot = App.Rotation(vec_u, vec_v, App.Vector(), "XYZ") return App.Placement(pt_pos, rot) # Code separated from WorkingPlane.py (offsetToPoint function). # Note that the return value of this function has the opposite sign. def distance_to_plane(point, base, normal): """Return the signed distance from a plane to a point. The distance is positive if the point lies on the +normal side of the plane. Parameters ---------- point: Base::Vector3 Point to project. base: Base::Vector3 Point on plane. normal: Base::Vector3 Normal of plane. Returns ------- float """ return (point - base).dot(normal) # Code separated from WorkingPlane.py (projectPoint function). # See: https://github.com/FreeCAD/FreeCAD/pull/5307 def project_point_on_plane(point, base, normal, direction=None, force_projection=False, tol=-1): """Project a point onto a plane. Parameters ---------- point: Base::Vector3 Point to project. base: Base::Vector3 Point on plane. normal: Base::Vector3 Normal of plane. direction: Base::Vector3, optional Defaults to `None` in which case the normal is used. Direction of projection. force_projection: Bool, optional Defaults to `False`. If `True` forces the projection if the deviation between the direction and the normal is less than tol from the orthogonality. The direction of projection is then modified to a tol deviation between the direction and the orthogonal. tol: float, optional Defaults to -1. Internal tolerance. 1e-7 is used if tol <=0. Returns ------- Base::Vector3 or `None` """ err = 1e-7 if tol <= 0 else tol normal = App.Vector(normal).normalize() if direction is None: direction = normal else: direction = App.Vector(direction).normalize() cos = direction.dot(normal) delta_ax_proj = (point - base).dot(normal) # check the only conflicting case: direction orthogonal to normal if abs(cos) < err: if force_projection: cos = math.copysign(err, delta_ax_proj) direction = normal.cross(direction).cross(normal) - cos * normal else: return None return point - delta_ax_proj / cos * direction # compatibility layer getSplineNormal = get_spline_normal getNormal = get_normal isPlanar = is_planar ## @}