From 7ac9e4fd771fa6abc65753c21735c1592dbc9aa1 Mon Sep 17 00:00:00 2001 From: Luke Parry Date: Sun, 10 Dec 2023 11:53:51 +0000 Subject: [PATCH] Added method for sweeping a polygon along a path pyslm.support.utils.sweepPolygon --- pyslm/support/geometry.py | 124 ++++++++++++++++++++++++++++++++++++++ 1 file changed, 124 insertions(+) diff --git a/pyslm/support/geometry.py b/pyslm/support/geometry.py index 63ea88f..ec77909 100644 --- a/pyslm/support/geometry.py +++ b/pyslm/support/geometry.py @@ -81,6 +81,130 @@ def checkStrutCylinderIntersection(pntA: np.array, pntB: np.array, radius: float return hasIntersection +def sweepPolygon(polygon, path, angles=None, scaleFactors=None, **kwargs) -> trimesh.Trimesh: + """ + Sweeps a polygon with a variable size across the length of the path. The function is based on that internally + used in trimesh. + + :param polygon: Polygon to sweep + :param path: The path segment to sweep across + :param angles: Variable angles used to rotate the polygon across the path + :param scaleFactors: Variable scale-factors used to rotate the polygon across the path + :return: The swept mesh + """ + + from trimesh import util + from trimesh import transformations as tf + from trimesh.creation import triangulate_polygon + path = np.asanyarray(path, dtype=np.float64) + + if not util.is_shape(path, (-1, 3)): + raise ValueError('Path must be (n, 3)!') + + # Extract 2D vertices and triangulation + verts_2d = np.array(polygon.exterior.xy).T + base_verts_2d, faces_2d = triangulate_polygon( + polygon, **kwargs) + n = len(verts_2d) + + # Create basis for first planar polygon cap + x, y, z = util.generate_basis(path[0] - path[1]) + tf_mat = np.ones((4, 4)) + tf_mat[:3, :3] = np.c_[x, y, z] + tf_mat[:3, 3] = path[0] + + prevScalFactor = scaleFactors[0] + tf_mat1 = tf.scale_matrix(scaleFactors[0, 0], origin=path[0]) + + # Compute 3D locations of those vertices + verts_3d = np.c_[verts_2d, np.zeros(n)] + verts_3d = tf.transform_points(verts_3d, tf_mat) + verts_3d = tf.transform_points(verts_3d, tf_mat1) + + base_verts_3d = np.c_[base_verts_2d, + np.zeros(len(base_verts_2d))] + base_verts_3d = tf.transform_points(base_verts_3d, + tf_mat) + + base_verts_3d = tf.transform_points(base_verts_3d, + tf_mat1) + + # keep matching sequence of vertices and 0- indexed faces + vertices = [base_verts_3d] + faces = [faces_2d] + + # Compute plane normals for each turn -- + # each turn induces a plane halfway between the two vectors + v1s = util.unitize(path[1:-1] - path[:-2]) + v2s = util.unitize(path[1:-1] - path[2:]) + norms = np.cross(np.cross(v1s, v2s), v1s + v2s) + norms[(norms == 0.0).all(1)] = v1s[(norms == 0.0).all(1)] + norms = util.unitize(norms) + final_v1 = util.unitize(path[-1] - path[-2]) + norms = np.vstack((norms, final_v1)) + v1s = np.vstack((v1s, final_v1)) + + # Create all side walls by projecting the 3d vertices into each plane in succession + + prevScalFactor = scaleFactors[0] + + for i in range(len(norms)): + verts_3d_prev = verts_3d + + # Rotate if needed + if angles is not None: + tf_mat = tf.rotation_matrix(angles[i], + norms[i], + path[i]) + verts_3d_prev = tf.transform_points(verts_3d_prev, + tf_mat) + """ + Apply the scale factors across the paths if specified + The scale factors are applied incrementally in order, therefore as a result the previous scale factor + must be inverted to ensure the correct scaling is applied + """ + if scaleFactors is not None: + tf_mat1 = tf.scale_matrix(1 / scaleFactors[i, 0], path[i]) + verts_3d_prev = tf.transform_points(verts_3d_prev, tf_mat1) + + tf_mat1 = tf.scale_matrix(scaleFactors[i + 1, 0], path[i + 1]) + verts_3d_prev = tf.transform_points(verts_3d_prev, tf_mat1) + + # Project vertices onto plane in 3D + ds = np.einsum('ij,j->i', (path[i + 1] - verts_3d_prev), norms[i]) + ds = ds / np.dot(v1s[i], norms[i]) + + verts_3d_new = np.einsum('i,j->ij', ds, v1s[i]) + verts_3d_prev + + # Add to face and vertex lists + new_faces = [[i + n, (i + 1) % n, i] for i in range(n)] + new_faces.extend([[(i - 1) % n + n, i + n, i] for i in range(n)]) + + # save faces and vertices into a sequence + faces.append(np.array(new_faces)) + vertices.append(np.vstack((verts_3d, verts_3d_new))) + + verts_3d = verts_3d_new + + # do the main stack operation from a sequence to (n,3) arrays + # doing one vstack provides a substantial speedup by + # avoiding a bunch of temporary allocations + vertices, faces = util.append_faces(vertices, faces) + + # Create final cap of the sweep path + x, y, z = util.generate_basis(path[-1] - path[-2]) + vecs = verts_3d - path[-1] + coords = np.c_[np.einsum('ij,j->i', vecs, x), + np.einsum('ij,j->i', vecs, y)] + base_verts_2d, faces_2d = triangulate_polygon(Polygon(coords), **kwargs) + base_verts_3d = (np.einsum('i,j->ij', base_verts_2d[:, 0], x) + + np.einsum('i,j->ij', base_verts_2d[:, 1], y)) + path[-1] + faces = np.vstack((faces, faces_2d + len(vertices))) + vertices = np.vstack((vertices, base_verts_3d)) + + mesh = trimesh.Trimesh(vertices, faces) + + return mesh def extrudeFace(extrudeMesh: trimesh.Trimesh, height: Optional[float] = None,