def recenter(self, pts, recenter=False): """Calculates the center of mass and moves CoM to origin if recenter is True Args: pts (list): list of tuples that represent the points that make up the Face in 2D space. recenter (bool): whether to move the center of mass to origin or not. """ self.pts_2D = [(p[0], p[1]) for p in pts] # Put centroid of polygon at origin xs = [p[0] for p in pts] + [pts[0][0]] ys = [p[1] for p in pts] + [pts[0][1]] a, cx, cy = 0, 0, 0 for i in range(len(pts)): a += (xs[i] * ys[i + 1] - xs[i + 1] * ys[i]) / 2 cx += (xs[i] + xs[i + 1]) * (xs[i] * ys[i + 1] - xs[i + 1] * ys[i]) / 6 cy += (ys[i] + ys[i + 1]) * (xs[i] * ys[i + 1] - xs[i + 1] * ys[i]) / 6 self.area = a if a == 0: self.pts_2D = [(p[0], p[1]) for p in pts] self.com_2D = (0, 0) else: if recenter: self.pts_2D = [(p[0] - cx / a, p[1] - cy / a) for p in pts] self.com_2D = (0, 0) else: self.pts_2D = [(p[0], p[1]) for p in pts] self.com_2D = (cx / a, cy / a) self.pts_4D = np.transpose(np.array([list(x) + [0, 1] for x in self.pts_2D])) self.com_4D = np.array(list(self.com_2D) + [0, 1])
def inflate(face, thickness=.1, edges=False): """ Inflates a face by creating two faces that are thickness apart. Args: face (Face): the Face object to inflate. thickness (float): the thickness to create the inflated face with. edges (bool): whether to return faces as a list of edges or as Face objects. Returns: List of two new faces that represent the inflated face. """ dt = np.array([[0], [0], [thickness / 2.], [0]]) nf = face - dt pf = face + dt faces = [] if edges: faces.append(np.transpose(np.array((pf[:, 0], nf[:, 0], pf[:, 1])))) faces.append(np.transpose(np.array((nf[:, 0], nf[:, 1], pf[:, 1])))) else: faces.append(pf) # top face faces.append(nf[:, ::-1]) # bottom face return faces
def transform(self, scale=1, angle=0, origin=(0, 0)): r = np.array([[np.cos(angle), -np.sin(angle)], [np.sin(angle), np.cos(angle)]]) * scale o = np.array([origin] * len(self.pts_2D)) pts = np.transpose(np.dot(r, np.transpose(np.array(self.pts_2D)))) + o self.pts_2D = [tuple(x) for x in np.rows(pts)] for (i, d) in enumerate(self.decorations): o = np.array([origin] * len(d[0])) pts = np.transpose(np.dot(r, np.transpose(np.array(d[0])))) + o self.decorations[i] = ([tuple(x) for x in np.rows(pts)], d[1])
def DCM_2_quat(dcm): den = np.array([ 1.0 + dcm[0, 0] + dcm[1, 1] + dcm[2, 2], 1.0 + dcm[0, 0] - dcm[1, 1] - dcm[2, 2], 1.0 - dcm[0, 0] + dcm[1, 1] - dcm[2, 2], 1.0 - dcm[0, 0] - dcm[1, 1] + dcm[2, 2] ]) #max_index = [x[0] for x in enumerate(list(den)) if x[1] == max(den)][0] max_index = 0 # XXX Can't find symbolically? q = [0] * 4 q[max_index] = 0.5 * np.sqrt(den[max_index]) denom = 4.0 * q[max_index] if (max_index == 0): q[1] = -(dcm[1, 2] - dcm[2, 1]) / denom q[2] = -(dcm[2, 0] - dcm[0, 2]) / denom q[3] = -(dcm[0, 1] - dcm[1, 0]) / denom if (max_index == 1): q[0] = -(dcm[1, 2] - dcm[2, 1]) / denom q[2] = (dcm[0, 1] + dcm[1, 0]) / denom q[3] = (dcm[0, 2] + dcm[2, 0]) / denom if (max_index == 2): q[0] = -(dcm[2, 0] - dcm[0, 2]) / denom q[1] = (dcm[0, 1] + dcm[1, 0]) / denom q[3] = (dcm[1, 2] + dcm[2, 1]) / denom if (max_index == 3): q[0] = -(dcm[0, 1] - dcm[1, 0]) / denom q[1] = (dcm[0, 2] + dcm[2, 0]) / denom q[2] = (dcm[1, 2] + dcm[2, 1]) / denom return q
def get_triangle_dict(self, separateHoles=True): # print self.pts2d # if len(self.pts2d > 0): # print type(self.pts2d[0]) vertices = self.pts_2D segments = [(i, (i + 1) % len(vertices)) for i in range(len(vertices))] holes = [] hole_vertices = [] hole_segments = [] for d in (x[0] for x in self.decorations if x[1] == "hole"): ld = len(d) if separateHoles: lv = len(hole_vertices) hole_vertices.append(d) hole_segments.extend([(lv + ((i + 1) % ld), lv + i) for i in range(ld)]) else: lv = len(vertices) vertices.extend(d) segments.extend([(lv + ((i + 1) % ld), lv + i) for i in range(ld)]) holes.append(tuple(np.sum([np.array(x) for x in d]) / len(d))) if hole_vertices: return dict(vertices=(vertices), segments=(segments),hole_vertices=(hole_vertices),hole_segments=(hole_segments), holes=(holes)) if holes: return dict(vertices=(vertices), segments=(segments), holes=(holes)) else: return dict(vertices=(vertices), segments=(segments))
def rotate_x_to(pt, pt2=(0, 0)): dx = pt[0] - pt2[0] dy = pt[1] - pt2[1] l = np.sqrt(dx * dx + dy * dy) dx = dx / l dy = dy / l r = np.array([[dx, -dy, 0, 0], [dy, dx, 0, 0], [0, 0, 1, 0], [0, 0, 0, 1]]) return r
def multiply_quat(quaternion1, quaternion0): w0, x0, y0, z0 = quaternion0 w1, x1, y1, z1 = quaternion1 return np.array([ -x1 * x0 - y1 * y0 - z1 * z0 + w1 * w0, x1 * w0 + y1 * z0 - z1 * y0 + w1 * x0, -x1 * z0 + y1 * w0 + z1 * x0 + w1 * y0, x1 * y0 - y1 * x0 + z1 * w0 + w1 * z0 ])
def get_2D_decorations(self): if self.transform_2D is not None: edges = [] for i, e in enumerate(self.decorations): if e[1] == "hole": for j in range(len(e[0])): name = self.name + ".d%d.e%d" % (i, j) pt1 = np.dot(self.transform_2D, np.array(list(e[0][j - 1]) + [0, 1]))[0:2] pt2 = np.dot(self.transform_2D, np.array(list(e[0][j]) + [0, 1]))[0:2] # XXX use EdgeType appropriately edges.append([name, pt1, pt2, 1]) else: name = self.name + ".d%d" % i pt1 = np.dot(self.transform_2D, np.array(list(e[0][0]) + [0, 1]))[0:2] pt2 = np.dot(self.transform_2D, np.array(list(e[0][1]) + [0, 1]))[0:2] edges.append([name, pt1, pt2, e[1]]) return edges return []
def reflect_across_2D_pts(edgepts): x1 = edgepts[0][0] y1 = edgepts[0][1] x2 = edgepts[1][0] y2 = edgepts[1][1] dx = (x1 - x2) if dx == 0: #Edge is a vertical line shift = np.array([[1, 0, 0, -x1], [0, 1, 0, 0], [0, 0, 1, 0], [0, 0, 0, 1]]) shift2 = np.array([[1, 0, 0, x1], [0, 1, 0, 0], [0, 0, 1, 0], [0, 0, 0, 1]]) reflect = np.array([[-1, 0, 0, 0], [0, 1, 0, 0], [0, 0, 1, 0], [0, 0, 0, 1]]) r = np.dot(shift2, np.dot(reflect, shift)) return r m = (y1 - y2) / dx n = 1 + m * m b = y1 - (m * x1) a = m * -1 d = 1 + (m * m) #Product of two opposite shifts of b in the y direction and a reflection across y = mx shift = np.array([[1, 0, 0, 0], [0, 1, 0, -b], [0, 0, 1, 0], [0, 0, 0, 1]]) shift2 = np.array([[1, 0, 0, 0], [0, 1, 0, b], [0, 0, 1, 0], [0, 0, 0, 1]]) reflec = np.array([[1 - 2 * (a * a) / n, -2 * a / n, 0, 0], [-2 * a / n, 1 - 2 / n, 0, 0], [0, 0, 1, 0], [0, 0, 0, 1]]) return np.dot(shift2, np.dot(reflec, shift))
def quat_2_DCM(quat): (a, b, c, d) = quat r = np.array([[ a**2 + b**2 - c**2 - d**2, 2 * b * c - 2 * a * d, 2 * b * d + 2 * a * c, 0 ], [ 2 * b * c + 2 * a * d, a**2 - b**2 + c**2 - d**2, 2 * c * d - 2 * a * b, 0 ], [ 2 * b * d - 2 * a * c, 2 * c * d + 2 * a * b, a**2 - b**2 - c**2 + d**2, 0 ], [0, 0, 0, 1]]) return r
def place_faces(self, face, edge_from, transform_2D, placed=None, allow_overlap=False): """Recursively adds faces to the 2D drawing. Args: face: the current face being placed edge_from: the edge by which to attach the current face transform_2D: A tranformation matrix to move the face into its position in 2D space placed: dictionary containing metadata about previously placed entities allow_overlap: Whether or not to allow two face to be placed on top of each other """ if placed is not None and face in placed['faces']: #This face has already been placed, do not place again return if placed is None: #No faces have been placed yet, initialize data structures placed = {'faces': [], 'edges': {}, 'overlapping': []} check_for_overlap = not allow_overlap else: #Overlap checking is handled only in top level call check_for_overlap = False """ Placing faces involves the notion of "pretransformed" and "transformed" values. Pretransformation moves the edge a face is being connected by to the x axis, and can be thought of as a face's relative position to its neighbor. Transformation moves a face to its absolute position in 2D space. """ if edge_from is not None: #Align connected edges pretransform_matrix = face.pre_transform(edge_from) else: #Place edge as is pretransform_matrix = np.eye(4) transform_matrix = np.dot(transform_2D, pretransform_matrix) #4D pts are the homogenous coordinates of the face i.e. [x,y,z,1] pretransformed_pts_4D = np.dot(pretransform_matrix, face.pts_4D) transfromed_pts_4D = np.dot(transform_matrix, face.pts_4D) pretransformed_pts_2D = pretransformed_pts_4D[0:2,:] #Numerical values for the coordinates are required for placement transfromed_pts_2D = eval_equation(transfromed_pts_4D[0:2, :]) if not self.add_face(transfromed_pts_2D): #If face cannot be placed without collisions, attempt to reflect it reflection_matrix = np.array([[1, 0, 0, 0], [0, -1, 0, 0], [0, 0, 1, 0], [0, 0, 0, 1]]) #Recompute pretransform and transform with rotation pretransform_matrix = np.dot(reflection_matrix, pretransform_matrix) transform_matrix = np.dot(transform_2D, pretransform_matrix) pretransformed_pts_4D = np.dot(pretransform_matrix, face.pts_4D) transfromed_pts_4D = np.dot(transform_matrix, face.pts_4D) pretransformed_pts_2D = pretransformed_pts_4D[0:2,:] transfromed_pts_2D = eval_equation(transfromed_pts_4D[0:2, :]) if not self.add_face(transfromed_pts_2D) and not allow_overlap: #Face was not able to be placed connected to this edge #Keep track of face and hope it gets placed elsewhere #TODO: Try connecting along other edges or undoing previous placements? placed['overlapping'].append(face) return #Face is being placed placed['faces'].append(face) if face in placed['overlapping']: placed['overlapping'].remove(face) face.transform_2D = transform_matrix #Will this break if a face has to be flipped? ## find out how to transform the decoration. ## for e in face.get_2D_decorations(): self.edges[e[0]] = DrawingEdge(e[0], [eval_equation(x) for x in e[1]], [eval_equation(x) for x in e[2]], EdgeType(e[3])) ## edges here is a dictionary, the key is decoration edge name. e is nested list and its first element is the edge name. ## #Place each edge for (i, edge) in enumerate(face.edges): #HACK: Do not place temporary edges if edge is None or edge.name[:4] == "temp": continue #Get the endpoints of the edge edge_pts_2D = (transfromed_pts_2D[:,i - 1],transfromed_pts_2D[:,i]) if edge.name in placed['edges'].keys(): edge_alias = placed['edges'][edge.name] if diff_edge(edge_alias, edge_pts_2D,2): #If the edge has already been placed in a different place, a cut must be made #Create a new edge self.edges['temp' + edge.name] = DrawingEdge('temp' + edge.name, edge_pts_2D[0], edge_pts_2D[1], Cut()) # Make old edge into a cut self.edges[edge.name] = DrawingEdge(edge.name, edge_alias[0], edge_alias[1], Cut()) else: #Add edge normally if len(edge.faces) == 1: edge_type = Cut() else: edge_type = Fold() self.edges[edge.name] = DrawingEdge(edge.name, edge_pts_2D[0], edge_pts_2D[1], edge_type) placed['edges'][edge.name] = edge_pts_2D if len(edge.faces) <= 1: # No other faces to be found, move on to next edge. continue if edge.is_tab(): # Don't follow faces off of a Tab continue #Compute new transform matrix for next face rotation_matrix = rotate_x_to(pretransformed_pts_2D[:,i],pretransformed_pts_2D[:,i-1]) origin_matrix = move_origin_to(pretransformed_pts_2D[:,i-1]) next_transfrom_2D = np.dot(transform_2D,np.dot(origin_matrix,np.dot(rotation_matrix,np.eye(4)))) #Place faces connected to edge for(f,a) in edge.faces.iteritems(): self.place_faces(f, edge, next_transfrom_2D, placed) if check_for_overlap and len(placed['overlapping']): #Placement has finished, but some edges are still unplaced raise Exception('One or more faces could not be placed without overlap!')
def stl_write(faces, filename, thickness=0): """ Writes a graph object represented by faces to an STL file. Args: faces (list): list of faces that compose the object. filename (str): filename to write STL to. thickness (int or float): thickness of each face to draw. """ import triangle shape = None shells = [] triangles = [] for f in faces: r = f[0] # print ("the tramsform function is ",r) # if r is None: # continue # if r is None: #No transform # r = np.eye(4) A = f[1] facets = [] B = triangle.triangulate(A, opts='p') if not 'triangles' in B: print "No triangles in " + f[2] continue if thickness: for t in [ np.transpose( np.array([ list(B['vertices'][x]) + [0, 1] for x in (face[0], face[1], face[2]) ])) for face in B['triangles'] ]: facets.extend( [np.dot(r, x) for x in inflate(t, thickness=thickness)]) for t in [ np.transpose( np.array([ list(A['vertices'][x]) + [0, 1] for x in (edge[0], edge[1]) ])) for edge in A['segments'] ]: facets.extend([ np.dot(r, x) for x in inflate(t, thickness=thickness, edges=True) ]) else: for t in [ np.transpose( np.array([ list(B['vertices'][x]) + [0, 1] for x in (face[0], face[1], face[2]) ])) for face in B['triangles'] ]: #print "Here",r,t facets.append(np.dot(r, t)) triangles.extend(facets) if thickness: FREECADPATH = '/usr/lib64/freecad/lib' import sys sys.path.append(FREECADPATH) import Part meshes = [] for f in (np.transpose(t[0:3, :]) for t in facets): try: meshes.append( Part.Face( Part.Wire([ Part.makeLine(tuple(f[x]), tuple(f[x - 1])) for x in range(3) ]))) except RuntimeError: print "Skipping face: " + repr(f) shell = Part.makeShell(meshes) shells.append(shell) if shape is None: shape = shell else: shape = shape.fuse(shell) if shape: with open("freecad" + filename, 'wb') as fp: shape.exportStl("freecad" + filename) from stlwriter import Binary_STL_Writer faces = triangles with open(filename, 'wb') as fp: writer = Binary_STL_Writer(fp) writer.add_faces(faces) writer.close()
def rotate_z(angle): r = np.array([[np.cos(angle), -np.sin(angle), 0, 0], [np.sin(angle), np.cos(angle), 0, 0], [0, 0, 1, 0], [0, 0, 0, 1]]) return r
def translate(origin): r = np.array([[1, 0, 0, origin[0]], [0, 1, 0, origin[1]], [0, 0, 1, origin[2]], [0, 0, 0, 1]]) return r
def get_3D_normal(self): if self.transform_3D is not None: o = np.dot(self.transform_3D, np.array([0, 0, 0, 1])) z = np.dot(self.transform_3D, np.array([0, 0, 1, 1])) return (z - o)[0:3, :]