diff --git a/utils/inscribed_circle.py b/utils/inscribed_circle.py index 5c8975b989..2610581210 100644 --- a/utils/inscribed_circle.py +++ b/utils/inscribed_circle.py @@ -64,57 +64,70 @@ def calc_inscribed_circle(verts, on_concave=ERROR): """ n = len(verts) verts = np.array(verts) - + e1 = verts[1] - verts[0] e2 = verts[2] - verts[1] n1 = np.cross(e1, e2) - + approx = linear_approximation(verts) plane = approx.most_similar_plane() plane_matrix = plane.get_matrix() + + # Flip plane matrix, in case approximation gave us a plane + # with normal pointing in direction opposite to polygon normal. plane_normal = np.array(plane.normal) if np.dot(n1, plane_normal) < 0: plane_matrix = Matrix.Diagonal(Vector([-1,-1,-1])) @ plane_matrix verts = verts[::-1] plane_normal = - plane_normal - m = plane_matrix - m = np.array(m.to_3x3()) - inv = np.linalg.inv(m) + + # Project all vertices to 2D space + inv = np.linalg.inv(plane_matrix.to_3x3()) pt0 = np.array(approx.center) verts2d = np_multiply_matrices_vectors(inv, verts - pt0) + if on_concave != ASIS: if not is_convex_2d(verts2d): if on_concave == ERROR: raise Exception("Polygon is not convex") else: return None + + # we will need only 2 coordinates verts2d = verts2d[:,0:2] + # linprog method automatically assumes that all variables are non-negative. + # So move all vertices to first quadrant. origin = verts2d.min(axis=0) verts2d -= origin[:2] + edges2d = np.roll(verts2d, -1, axis=0) - verts2d # (n, 2) edges2d_normalized = edges2d / np.linalg.norm(edges2d, axis=1, keepdims=True) - + + # edges line equations matrix edges_eq = np.zeros((n+1, 3)) edges_eq[:n,0] = edges2d_normalized[:,1] edges_eq[:n,1] = -edges2d_normalized[:,0] edges_eq[:n,2] = 1 - edges_eq[n,:] = np.array([0, 0, -1]) - + edges_eq[n,:] = np.array([0, 0, -1]) # -Z <= 0 + + # right-hand side of edges line equations D = np_dot(-edges_eq[:n,:2], verts2d) D = np.append(D, 0) - + + # function to be minimized: it's -Z (we need to maximize Z). c = np.array([0.0, 0.0, -1.0]) - + res = linprog(c, A_ub = edges_eq, b_ub = -D, method='highs') center2d = res.x if center2d is None: return None center2d[2] = 0 - + distances = np_dot(-edges_eq[:n,:2], center2d[:2]) - D[:n] - rho = abs(distances).min() + rho = abs(distances).min() o = np.zeros((3,)) o[:2] = origin + # Return to 3D space center = plane_matrix @ (Vector(center2d) + Vector(o)) + Vector(pt0) return CircleEquation3D.from_center_radius_normal(center, rho, plane_normal)