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, :]
