def closest_point(self, p):
v = self.dir
if v is None:
# for zero-length lines
return self.p1
dist = pdot(self.p1, v) - pdot(p, v)
return psub(self.p1, pmul(v, dist))
def intersect_sphere_line(center, radius, radiussq, direction, edge):
# make a plane by sliding the line along the direction (1)
d = edge.dir
n = pcross(d, direction)
if pnorm(n) == 0:
# no contact point, but should check here if sphere *always* intersects
# line...
return (None, None, INFINITE)
n = pnormalized(n)
# calculate the distance from the sphere center to the plane
dist = -pdot(center, n) + pdot(edge.p1, n)
if abs(dist) > radius - epsilon:
return (None, None, INFINITE)
# this gives us the intersection circle on the sphere
# now take a plane through the edge and perpendicular to the direction (2)
# find the center on the circle closest to this plane
# which means the other component is perpendicular to this plane (2)
n2 = pnormalized(pcross(n, d))
# the contact point is on a big circle through the sphere...
dist2 = sqrt(radiussq - dist * dist)
# ... and it's on the plane (1)
ccp = padd(center, padd(pmul(n, dist), pmul(n2, dist2)))
# now intersect a line through this point with the plane (2)
plane = Plane(edge.p1, n2)
(cp, l) = plane.intersect_point(direction, ccp)
return (ccp, cp, l)
def reset_cache(self):
self.minx = min(self.p1[0], self.p2[0], self.p3[0])
self.miny = min(self.p1[1], self.p2[1], self.p3[1])
self.minz = min(self.p1[2], self.p2[2], self.p3[2])
self.maxx = max(self.p1[0], self.p2[0], self.p3[0])
self.maxy = max(self.p1[1], self.p2[1], self.p3[1])
self.maxz = max(self.p1[2], self.p2[2], self.p3[2])
self.e1 = Line(self.p1, self.p2)
self.e2 = Line(self.p2, self.p3)
self.e3 = Line(self.p3, self.p1)
# calculate normal, if p1-p2-pe are in clockwise order
if self.normal is None:
self.normal = pnormalized(
pcross(psub(self.p3, self.p1), psub(self.p2, self.p1)))
if not len(self.normal) > 3:
self.normal = (self.normal[0], self.normal[1], self.normal[2], 'v')
self.center = pdiv(padd(padd(self.p1, self.p2), self.p3), 3)
self.plane = Plane(self.center, self.normal)
# calculate circumcircle (resulting in radius and middle)
denom = pnorm(pcross(psub(self.p2, self.p1), psub(self.p3, self.p2)))
self.radius = (pdist(self.p2, self.p1) * pdist(self.p3, self.p2) *
pdist(self.p3, self.p1)) / (2 * denom)
self.radiussq = self.radius**2
denom2 = 2 * denom * denom
alpha = pdist_sq(self.p3, self.p2) * pdot(psub(
self.p1, self.p2), psub(self.p1, self.p3)) / denom2
beta = pdist_sq(self.p1, self.p3) * pdot(psub(
self.p2, self.p1), psub(self.p2, self.p3)) / denom2
gamma = pdist_sq(self.p1, self.p2) * pdot(psub(
self.p3, self.p1), psub(self.p3, self.p2)) / denom2
self.middle = (self.p1[0] * alpha + self.p2[0] * beta +
self.p3[0] * gamma, self.p1[1] * alpha +
self.p2[1] * beta + self.p3[1] * gamma,
self.p1[2] * alpha + self.p2[2] * beta +
self.p3[2] * gamma)
def intersect_point(self, direction, point):
if (direction is not None) and (pnorm(direction) != 1):
# calculations will go wrong, if the direction is not a unit vector
direction = pnormalized(direction)
if direction is None:
return (None, INFINITE)
denom = pdot(self.n, direction)
if denom == 0:
return (None, INFINITE)
l = -(pdot(self.n, point) - pdot(self.n, self.p)) / denom
cp = padd(point, pmul(direction, l))
return (cp, l)
def intersect_sphere_plane(center, radius, direction, triangle):
# let n be the normal to the plane
n = triangle.normal
if pdot(n, direction) == 0:
return (None, None, INFINITE)
# the cutter contact point is on the sphere, where the surface normal is n
if pdot(n, direction) < 0:
ccp = psub(center, pmul(n, radius))
else:
ccp = padd(center, pmul(n, radius))
# intersect the plane with a line through the contact point
(cp, d) = triangle.plane.intersect_point(direction, ccp)
return (ccp, cp, d)
def intersect_torus_point(center, axis, majorradius, minorradius,
majorradiussq, minorradiussq, direction, point):
dist = 0
if (direction[0] == 0) and (direction[1] == 0):
# drop
minlsq = (majorradius - minorradius)**2
maxlsq = (majorradius + minorradius)**2
l_sq = (point[0] - center[0])**2 + (point[1] - center[1])**2
if (l_sq < minlsq + epsilon) or (l_sq > maxlsq - epsilon):
return (None, None, INFINITE)
l_len = sqrt(l_sq)
z_sq = minorradiussq - (majorradius - l_len)**2
if z_sq < 0:
return (None, None, INFINITE)
z = sqrt(z_sq)
ccp = (point[0], point[1], center[2] - z)
dist = ccp[2] - point[2]
elif direction[2] == 0:
# push
z = point[2] - center[2]
if abs(z) > minorradius - epsilon:
return (None, None, INFINITE)
l_len = majorradius + sqrt(minorradiussq - z * z)
n = pcross(axis, direction)
d = pdot(n, point) - pdot(n, center)
if abs(d) > l_len - epsilon:
return (None, None, INFINITE)
a = sqrt(l_len * l_len - d * d)
ccp = padd(padd(center, pmul(n, d)), pmul(direction, a))
ccp = (ccp[0], ccp[1], point[2])
dist = pdot(psub(point, ccp), direction)
else:
# general case
x = psub(point, center)
v = pmul(direction, -1)
x_x = pdot(x, x)
x_v = pdot(x, v)
x1 = (x[0], x[1], 0)
v1 = (v[0], v[1], 0)
x1_x1 = pdot(x1, x1)
x1_v1 = pdot(x1, v1)
v1_v1 = pdot(v1, v1)
r2_major = majorradiussq
r2_minor = minorradiussq
a = 1.0
b = 4 * x_v
c = 2 * (x_x + 2 * x_v**2 +
(r2_major - r2_minor) - 2 * r2_major * v1_v1)
d = 4 * (x_x * x_v + x_v *
(r2_major - r2_minor) - 2 * r2_major * x1_v1)
e = ((x_x)**2 + 2 * x_x * (r2_major - r2_minor) +
(r2_major - r2_minor)**2 - 4 * r2_major * x1_x1)
r = poly4_roots(a, b, c, d, e)
if not r:
return (None, None, INFINITE)
else:
l_len = min(r)
ccp = padd(point, pmul(direction, -l_len))
dist = l_len
return (ccp, point, dist)
def intersect_cylinder_point(center, axis, radius, radiussq, direction, point):
# take a plane along direction and axis
n = pnormalized(pcross(direction, axis))
# distance of the point to this plane
d = pdot(n, point) - pdot(n, center)
if abs(d) > radius - epsilon:
return (None, None, INFINITE)
# ccl is on cylinder
d2 = sqrt(radiussq - d * d)
ccl = padd(padd(center, pmul(n, d)), pmul(direction, d2))
# take plane through ccl and axis
plane = Plane(ccl, direction)
# intersect point with plane
(ccp, l) = plane.intersect_point(direction, point)
return (ccp, point, -l)
def get_angle_pi(p1, p2, p3, up_vector, pi_factor=False):
""" calculate the angle between three points
Visualization:
p3
/
/
/\
/ \
p2--------p1
The result is in a range between 0 and 2*PI.
"""
d1 = pnormalized(psub(p2, p1))
d2 = pnormalized(psub(p2, p3))
if (d1 is None) or (d2 is None):
return 2 * math.pi
angle = math.acos(pdot(d1, d2))
# check the direction of the points (clockwise/anti)
# The code is taken from Polygon.get_area
value = [0, 0, 0]
for (pa, pb) in ((p1, p2), (p2, p3), (p3, p1)):
value[0] += pa[1] * pb[2] - pa[2] * pb[1]
value[1] += pa[2] * pb[0] - pa[0] * pb[2]
value[2] += pa[0] * pb[1] - pa[1] * pb[0]
area = up_vector[0] * value[0] + up_vector[1] * value[1] + up_vector[
2] * value[2]
if area > 0:
# The points are in anti-clockwise order. Thus the angle is greater
# than 180 degree.
angle = 2 * math.pi - angle
if pi_factor:
# the result is in the range of 0..2
return angle / math.pi
else:
return angle
def get_plane_projection(self, plane):
if plane == self.plane:
return self
elif pdot(plane.n, self.plane.n) == 0:
log.warn("Polygon projection onto plane: orthogonal projection is not possible")
return None
else:
result = Polygon(plane)
for line in self.get_lines():
p1 = plane.get_point_projection(line.p1)
p2 = plane.get_point_projection(line.p2)
result.append(Line(p1, p2))
# check if the projection would revert the direction of the polygon
if pdot(plane.n, self.plane.n) < 0:
result.reverse_direction()
return result
def calc_normal(main, normals):
suitable = (0, 0, 0, 'v')
for normal, weight in normals:
dot = pdot(main, normal)
if dot > 0:
suitable = padd(suitable, pmul(normal, weight * dot))
return pnormalized(suitable)
def append(self, point):
# Sort the points in positive x/y direction - otherwise the
# PolygonExtractor breaks.
if self.points and (pdot(psub(point, self.points[0]), self.__forward) <
0):
self.points.insert(0, point)
else:
self.points.append(point)
def intersect_triangle(self, triangle, counter_clockwise=False):
""" Returns the line of intersection of a triangle with a plane.
"None" is returned, if:
- the triangle does not intersect with the plane
- all vertices of the triangle are on the plane
The line always runs clockwise through the triangle.
"""
# don't import Line in the header -> circular import
from pycam.Geometry.Line import Line
collisions = []
for edge, point in ((triangle.e1, triangle.p1),
(triangle.e2, triangle.p2), (triangle.e3,
triangle.p3)):
cp, l = self.intersect_point(edge.dir, point)
# filter all real collisions
# We don't want to count vertices double -> thus we only accept
# a distance that is lower than the length of the edge.
if (cp is not None) and (-epsilon < l < edge.len - epsilon):
collisions.append(cp)
elif (cp is None) and (pdot(self.n, edge.dir) == 0):
cp, dist = self.intersect_point(self.n, point)
if abs(dist) < epsilon:
# the edge is on the plane
collisions.append(point)
if len(collisions) == 3:
# All points of the triangle are on the plane.
# We don't return a waterline, as there should be another non-flat
# triangle with the same waterline.
return None
if len(collisions) == 2:
collision_line = Line(collisions[0], collisions[1])
# no further calculation, if the line is zero-sized
if collision_line.len == 0:
return collision_line
cross = pcross(self.n, collision_line.dir)
if (pdot(cross, triangle.normal) <
0) == bool(not counter_clockwise):
# anti-clockwise direction -> revert the direction of the line
collision_line = Line(collision_line.p2, collision_line.p1)
return collision_line
elif len(collisions) == 1:
# only one point is on the plane
# This waterline (with zero length) should be of no use.
return None
else:
return None
def intersect_sphere_edge(self, direction, edge, start=None):
(cl, ccp, cp, l) = self.intersect_sphere_line(direction, edge, start=start)
if cp:
# check if the contact point is between the endpoints
d = psub(edge.p2, edge.p1)
m = pdot(psub(cp, edge.p1), d)
if (m < -epsilon) or (m > pnormsq(d) + epsilon):
return (None, INFINITE, None)
return (cl, l, cp)
def intersect_circle_edge(self, direction, edge, start=None):
(cl, ccp, cp, l) = self.intersect_circle_line(direction,
edge,
start=start)
if cp:
# check if the contact point is between the endpoints
m = pdot(psub(cp, edge.p1), edge.dir)
if (m < -epsilon) or (m > edge.len + epsilon):
return (None, INFINITE, cp)
return (cl, l, cp)
def get_shifted_vertex(self, index, offset):
p1 = self._points[index]
p2 = self._points[(index + 1) % len(self._points)]
cross_offset = pnormalized(pcross(psub(p2, p1), self.plane.n))
bisector_normalized = self.get_bisector(index)
factor = pdot(cross_offset, bisector_normalized)
if factor != 0:
bisector_sized = pmul(bisector_normalized, offset / factor)
return padd(p1, bisector_sized)
else:
return p2
def intersect_torus_plane(center, axis, majorradius, minorradius, direction,
triangle):
# take normal to the plane
n = triangle.normal
if pdot(n, direction) == 0:
return (None, None, INFINITE)
if pdot(n, axis) == 1:
return (None, None, INFINITE)
# find place on torus where surface normal is n
b = pmul(n, -1)
z = axis
a = psub(b, pmul(z, pdot(z, b)))
a_sq = pnormsq(a)
if a_sq <= 0:
return (None, None, INFINITE)
a = pdiv(a, sqrt(a_sq))
ccp = padd(padd(center, pmul(a, majorradius)), pmul(b, minorradius))
# find intersection with plane
(cp, l) = triangle.plane.intersect_point(direction, ccp)
return (ccp, cp, l)
def intersect_cylinder_edge(self, direction, edge, start=None):
(cl, ccp, cp, l) = self.intersect_cylinder_line(direction,
edge,
start=start)
if ccp and ccp[2] < self.center[2]:
return (None, INFINITE, None)
if ccp:
m = pdot(psub(cp, edge.p1), edge.dir)
if (m < -epsilon) or (m > edge.len + epsilon):
return (None, INFINITE, None)
return (cl, l, cp)
def get_intersection(self, line, infinite_lines=False):
""" Get the point of intersection between two lines. Intersections
outside the length of these lines are ignored.
Returns (None, None) if no valid intersection was found.
Otherwise the result is (CollisionPoint, distance). Distance is between
0 and 1.
"""
x1, x2, x3, x4 = self.p1, self.p2, line.p1, line.p2
a = psub(x2, x1)
b = psub(x4, x3)
c = psub(x3, x1)
# see http://mathworld.wolfram.com/Line-LineIntersection.html (24)
try:
factor = pdot(pcross(c, b), pcross(a, b)) / pnormsq(pcross(a, b))
except ZeroDivisionError:
# lines are parallel
# check if they are _one_ line
if pnorm(pcross(a, c)) != 0:
# the lines are parallel with a distance
return None, None
# the lines are on one straight
candidates = []
if self.is_point_inside(x3):
candidates.append((x3, pnorm(c) / pnorm(a)))
elif self.is_point_inside(x4):
candidates.append((x4, pdist(line.p2, self.p1) / pnorm(a)))
elif line.is_point_inside(x1):
candidates.append((x1, 0))
elif line.is_point_inside(x2):
candidates.append((x2, 1))
else:
return None, None
# return the collision candidate with the lowest distance
candidates.sort(key=lambda collision: collision[1])
return candidates[0]
if infinite_lines or (-epsilon <= factor <= 1 + epsilon):
intersec = padd(x1, pmul(a, factor))
# check if the intersection is between x3 and x4
if infinite_lines:
return intersec, factor
elif ((min(x3[0], x4[0]) - epsilon <= intersec[0] <=
max(x3[0], x4[0]) + epsilon)
and (min(x3[1], x4[1]) - epsilon <= intersec[1] <=
max(x3[1], x4[1]) + epsilon)
and (min(x3[2], x4[2]) - epsilon <= intersec[2] <=
max(x3[2], x4[2]) + epsilon)):
return intersec, factor
else:
# intersection outside of the length of line(x3, x4)
return None, None
else:
# intersection outside of the length of line(x1, x2)
return None, None
def intersect_cylinder_edge(self, direction, edge, start=None):
if start is None:
start = self.location
(cl, ccp, cp, l) = self.intersect_cylinder_line(direction,
edge,
start=start)
if not ccp:
return (None, INFINITE, None)
m = pdot(psub(cp, edge.p1), edge.dir)
if (m < -epsilon) or (m > edge.len + epsilon):
return (None, INFINITE, None)
if ccp[2] < padd(psub(start, self.location), self.center)[2]:
return (None, INFINITE, None)
return (cl, l, cp)
def is_point_inside(self, p):
# http://www.blackpawn.com/texts/pointinpoly/default.html
# Compute vectors
v0 = psub(self.p3, self.p1)
v1 = psub(self.p2, self.p1)
v2 = psub(p, self.p1)
# Compute dot products
dot00 = pdot(v0, v0)
dot01 = pdot(v0, v1)
dot02 = pdot(v0, v2)
dot11 = pdot(v1, v1)
dot12 = pdot(v1, v2)
# Compute barycentric coordinates
denom = dot00 * dot11 - dot01 * dot01
if denom == 0:
return False
inv_denom = 1.0 / denom
# Originally, "u" and "v" are multiplied with "1/denom".
# We don't do this to avoid division by zero (for triangles that are
# "almost" invalid).
u = (dot11 * dot02 - dot01 * dot12) * inv_denom
v = (dot00 * dot12 - dot01 * dot02) * inv_denom
# Check if point is in triangle
return (u > 0) and (v > 0) and (u + v < 1)
def __init__(self, barycenter, location, p1, p2, p3):
self.location = location
self.position = p2
self.direction = pnormalized(get_bisector(p1, p2, p3, self.up_vector))
preferred_direction = pnormalized(psub(p2, barycenter))
# direction_factor: 0..1 (bigger -> better)
direction_factor = (pdot(preferred_direction, self.direction) + 1) / 2
angle = get_angle_pi(p1, p2, p3, self.up_vector, pi_factor=True)
# angle_factor: 0..1 (bigger -> better)
if angle > 0.5:
# use only angles > 90 degree
angle_factor = angle / 2.0
else:
angle_factor = 0
# priority: 0..1 (bigger -> better)
self.priority = angle_factor * direction_factor
def get_bisector(p1, p2, p3, up_vector):
""" Calculate the bisector between p1, p2 and p3, whereas p2 is the origin
of the angle.
"""
d1 = pnormalized(psub(p2, p1))
d2 = pnormalized(psub(p2, p3))
bisector_dir = pnormalized(padd(d1, d2))
if bisector_dir is None:
# the two vectors pointed to opposite directions
bisector_dir = pnormalized(pcross(d1, up_vector))
else:
skel_up_vector = pcross(bisector_dir, psub(p2, p1))
if pdot(up_vector, skel_up_vector) < 0:
# reverse the skeleton vector to point outwards
bisector_dir = pmul(bisector_dir, -1)
return bisector_dir
def intersect_circle_plane(center, radius, direction, triangle):
# let n be the normal to the plane
n = triangle.normal
if pdot(n, direction) == 0:
return (None, None, INFINITE)
# project onto z=0
n2 = (n[0], n[1], 0)
if pnorm(n2) == 0:
(cp, d) = triangle.plane.intersect_point(direction, center)
ccp = psub(cp, pmul(direction, d))
return (ccp, cp, d)
n2 = pnormalized(n2)
# the cutter contact point is on the circle, where the surface normal is n
ccp = padd(center, pmul(n2, -radius))
# intersect the plane with a line through the contact point
(cp, d) = triangle.plane.intersect_point(direction, ccp)
return (ccp, cp, d)
def intersect_sphere_point(center, radius, radiussq, direction, point):
# line equation
# (1) x = p_0 + \lambda * d
# sphere equation
# (2) (x-x_0)^2 = R^2
# (1) in (2) gives a quadratic in \lambda
p0_x0 = psub(center, point)
a = pnormsq(direction)
b = 2 * pdot(p0_x0, direction)
c = pnormsq(p0_x0) - radiussq
d = b * b - 4 * a * c
if d < 0:
return (None, None, INFINITE)
if a < 0:
l = (-b + sqrt(d)) / (2 * a)
else:
l = (-b - sqrt(d)) / (2 * a)
# cutter contact point
ccp = padd(point, pmul(direction, -l))
return (ccp, point, l)
def intersect_cylinder_line(center, axis, radius, radiussq, direction, edge):
d = edge.dir
# take a plane throught the line and along the cylinder axis (1)
n = pcross(d, axis)
if pnorm(n) == 0:
# no contact point, but should check here if cylinder *always*
# intersects line...
return (None, None, INFINITE)
n = pnormalized(n)
# the contact line between the cylinder and this plane (1)
# is where the surface normal is perpendicular to the plane
# so line := ccl + \lambda * axis
if pdot(n, direction) < 0:
ccl = psub(center, pmul(n, radius))
else:
ccl = padd(center, pmul(n, radius))
# now extrude the contact line along the direction, this is a plane (2)
n2 = pcross(direction, axis)
if pnorm(n2) == 0:
# no contact point, but should check here if cylinder *always*
# intersects line...
return (None, None, INFINITE)
n2 = pnormalized(n2)
plane1 = Plane(ccl, n2)
# intersect this plane with the line, this gives us the contact point
(cp, l) = plane1.intersect_point(d, edge.p1)
if not cp:
return (None, None, INFINITE)
# now take a plane through the contact line and perpendicular to the
# direction (3)
plane2 = Plane(ccl, direction)
# the intersection of this plane (3) with the line through the contact point
# gives us the cutter contact point
(ccp, l) = plane2.intersect_point(direction, cp)
cp = padd(ccp, pmul(direction, -l))
return (ccp, cp, -l)
def ImportModel(filename, use_kdtree=True, callback=None, **kwargs):
global vertices, edges, kdtree
vertices = 0
edges = 0
kdtree = None
normal_conflict_warning_seen = False
if hasattr(filename, "read"):
# make sure that the input stream can seek and has ".len"
f = StringIO(filename.read())
# useful for later error messages
filename = "input stream"
else:
try:
url_file = pycam.Utils.URIHandler(filename).open()
# urllib.urlopen objects do not support "seek" - so we need to read
# the whole file at once. This is ugly - anyone with a better idea?
f = StringIO(url_file.read())
# TODO: the above ".read" may be incomplete - this is ugly
# see http://patrakov.blogspot.com/2011/03/case-of-non-raised-exception.html
# and http://stackoverflow.com/questions/1824069/
url_file.close()
except IOError as err_msg:
log.error("STLImporter: Failed to read file (%s): %s", filename,
err_msg)
return None
# Read the first two lines of (potentially non-binary) input - they should
# contain "solid" and "facet".
header_lines = []
while len(header_lines) < 2:
line = f.readline(200)
if len(line) == 0:
# empty line (not even a line-feed) -> EOF
log.error("STLImporter: No valid lines found in '%s'", filename)
return None
# ignore comment lines
# note: partial comments (starting within a line) are not handled
if not line.startswith(";"):
header_lines.append(line)
header = "".join(header_lines)
# read byte 80 to 83 - they contain the "numfacets" value in binary format
f.seek(80)
numfacets = unpack("<I", f.read(4))[0]
binary = False
log.debug("STL import info: %s / %s / %s / %s", f.len, numfacets,
header.find("solid"), header.find("facet"))
if f.len == (84 + 50 * numfacets):
binary = True
elif header.find("solid") >= 0 and header.find("facet") >= 0:
binary = False
f.seek(0)
else:
log.error("STLImporter: STL binary/ascii detection failed")
return None
if use_kdtree:
kdtree = PointKdtree([], 3, 1, epsilon)
model = Model(use_kdtree)
t = None
p1 = None
p2 = None
p3 = None
if binary:
for i in range(1, numfacets + 1):
if callback and callback():
log.warn("STLImporter: load model operation cancelled")
return None
a1 = unpack("<f", f.read(4))[0]
a2 = unpack("<f", f.read(4))[0]
a3 = unpack("<f", f.read(4))[0]
n = (float(a1), float(a2), float(a3), 'v')
v11 = unpack("<f", f.read(4))[0]
v12 = unpack("<f", f.read(4))[0]
v13 = unpack("<f", f.read(4))[0]
p1 = UniqueVertex(float(v11), float(v12), float(v13))
v21 = unpack("<f", f.read(4))[0]
v22 = unpack("<f", f.read(4))[0]
v23 = unpack("<f", f.read(4))[0]
p2 = UniqueVertex(float(v21), float(v22), float(v23))
v31 = unpack("<f", f.read(4))[0]
v32 = unpack("<f", f.read(4))[0]
v33 = unpack("<f", f.read(4))[0]
p3 = UniqueVertex(float(v31), float(v32), float(v33))
# not used (additional attributes)
f.read(2)
dotcross = pdot(n, pcross(psub(p2, p1), psub(p3, p1)))
if a1 == a2 == a3 == 0:
dotcross = pcross(psub(p2, p1), psub(p3, p1))[2]
n = None
if dotcross > 0:
# Triangle expects the vertices in clockwise order
t = Triangle(p1, p3, p2)
elif dotcross < 0:
if not normal_conflict_warning_seen:
log.warn(
"Inconsistent normal/vertices found in facet definition %d of '%s'. "
"Please validate the STL file!", i, filename)
normal_conflict_warning_seen = True
t = Triangle(p1, p2, p3)
else:
# the three points are in a line - or two points are identical
# usually this is caused by points, that are too close together
# check the tolerance value in pycam/Geometry/PointKdtree.py
log.warn(
"Skipping invalid triangle: %s / %s / %s (maybe the resolution of the "
"model is too high?)", p1, p2, p3)
continue
if n:
t.normal = n
model.append(t)
else:
solid = re.compile(r"\s*solid\s+(\w+)\s+.*")
endsolid = re.compile(r"\s*endsolid\s*")
facet = re.compile(r"\s*facet\s*")
normal = re.compile(
r"\s*facet\s+normal" +
r"\s+(?P<x>[-+]?(\d+(\.\d*)?|\.\d+)([eE][-+]?\d+)?)" +
r"\s+(?P<y>[-+]?(\d+(\.\d*)?|\.\d+)([eE][-+]?\d+)?)" +
r"\s+(?P<z>[-+]?(\d+(\.\d*)?|\.\d+)([eE][-+]?\d+)?)\s+")
endfacet = re.compile(r"\s*endfacet\s+")
loop = re.compile(r"\s*outer\s+loop\s+")
endloop = re.compile(r"\s*endloop\s+")
vertex = re.compile(
r"\s*vertex" +
r"\s+(?P<x>[-+]?(\d+(\.\d*)?|\.\d+)([eE][-+]?\d+)?)" +
r"\s+(?P<y>[-+]?(\d+(\.\d*)?|\.\d+)([eE][-+]?\d+)?)" +
r"\s+(?P<z>[-+]?(\d+(\.\d*)?|\.\d+)([eE][-+]?\d+)?)\s+")
current_line = 0
for line in f:
if callback and callback():
log.warn("STLImporter: load model operation cancelled")
return None
current_line += 1
m = solid.match(line)
if m:
model.name = m.group(1)
continue
m = facet.match(line)
if m:
m = normal.match(line)
if m:
n = (float(m.group('x')), float(m.group('y')),
float(m.group('z')), 'v')
else:
n = None
continue
m = loop.match(line)
if m:
continue
m = vertex.match(line)
if m:
p = UniqueVertex(float(m.group('x')), float(m.group('y')),
float(m.group('z')))
if p1 is None:
p1 = p
elif p2 is None:
p2 = p
elif p3 is None:
p3 = p
else:
log.error(
"STLImporter: more then 3 points in facet (line %d)",
current_line)
continue
m = endloop.match(line)
if m:
continue
m = endfacet.match(line)
if m:
if None in (p1, p2, p3):
log.warn(
"Invalid facet definition in line %d of '%s'. Please validate the "
"STL file!", current_line, filename)
n, p1, p2, p3 = None, None, None, None
continue
if not n:
n = pnormalized(pcross(psub(p2, p1), psub(p3, p1)))
# validate the normal
# The three vertices of a triangle in an STL file are supposed
# to be in counter-clockwise order. This should match the
# direction of the normal.
if n is None:
# invalid triangle (zero-length vector)
dotcross = 0
else:
# make sure the points are in ClockWise order
dotcross = pdot(n, pcross(psub(p2, p1), psub(p3, p1)))
if dotcross > 0:
# Triangle expects the vertices in clockwise order
t = Triangle(p1, p3, p2, n)
elif dotcross < 0:
if not normal_conflict_warning_seen:
log.warn(
"Inconsistent normal/vertices found in line %d of '%s'. Please "
"validate the STL file!", current_line, filename)
normal_conflict_warning_seen = True
t = Triangle(p1, p2, p3, n)
else:
# The three points are in a line - or two points are
# identical. Usually this is caused by points, that are too
# close together. Check the tolerance value in
# pycam/Geometry/PointKdtree.py.
log.warn(
"Skipping invalid triangle: %s / %s / %s (maybe the resolution of "
"the model is too high?)", p1, p2, p3)
n, p1, p2, p3 = (None, None, None, None)
continue
n, p1, p2, p3 = (None, None, None, None)
model.append(t)
continue
m = endsolid.match(line)
if m:
continue
log.info("Imported STL model: %d vertices, %d edges, %d triangles",
vertices, edges, len(model.triangles()))
vertices = 0
edges = 0
kdtree = None
if not model:
# no valid items added to the model
return None
else:
return model
def import_model(filename, use_kdtree=True, callback=None, **kwargs):
global vertices, edges, kdtree
vertices = 0
edges = 0
kdtree = None
normal_conflict_warning_seen = False
if hasattr(filename, "read"):
# make sure that the input stream can seek and has ".len"
f = BufferedReader(filename)
# useful for later error messages
filename = "input stream"
else:
try:
url_file = pycam.Utils.URIHandler(filename).open()
# urllib.urlopen objects do not support "seek" - so we need a buffered reader
# Is there a better approach than consuming the whole file at once?
f = BufferedReader(BytesIO(url_file.read()))
url_file.close()
except IOError as exc:
raise LoadFileError(
"STLImporter: Failed to read file ({}): {}".format(
filename, exc))
# the facet count is only available for the binary format
facet_count = get_facet_count_if_binary_format(f)
is_binary = (facet_count is not None)
if use_kdtree:
kdtree = PointKdtree([], 3, 1, epsilon)
model = Model(use_kdtree)
t = None
p1 = None
p2 = None
p3 = None
if is_binary:
# Skip the header and count fields of binary stl file
f.seek(HEADER_SIZE + COUNT_SIZE)
for i in range(1, facet_count + 1):
if callback and callback():
raise AbortOperationException(
"STLImporter: load model operation cancelled")
a1 = unpack("<f", f.read(4))[0]
a2 = unpack("<f", f.read(4))[0]
a3 = unpack("<f", f.read(4))[0]
n = (float(a1), float(a2), float(a3), 'v')
v11 = unpack("<f", f.read(4))[0]
v12 = unpack("<f", f.read(4))[0]
v13 = unpack("<f", f.read(4))[0]
p1 = get_unique_vertex(float(v11), float(v12), float(v13))
v21 = unpack("<f", f.read(4))[0]
v22 = unpack("<f", f.read(4))[0]
v23 = unpack("<f", f.read(4))[0]
p2 = get_unique_vertex(float(v21), float(v22), float(v23))
v31 = unpack("<f", f.read(4))[0]
v32 = unpack("<f", f.read(4))[0]
v33 = unpack("<f", f.read(4))[0]
p3 = get_unique_vertex(float(v31), float(v32), float(v33))
# not used (additional attributes)
f.read(2)
dotcross = pdot(n, pcross(psub(p2, p1), psub(p3, p1)))
if a1 == a2 == a3 == 0:
dotcross = pcross(psub(p2, p1), psub(p3, p1))[2]
n = None
if dotcross > 0:
# Triangle expects the vertices in clockwise order
t = Triangle(p1, p3, p2)
elif dotcross < 0:
if not normal_conflict_warning_seen:
log.warn(
"Inconsistent normal/vertices found in facet definition %d of '%s'. "
"Please validate the STL file!", i, filename)
normal_conflict_warning_seen = True
t = Triangle(p1, p2, p3)
else:
# the three points are in a line - or two points are identical
# usually this is caused by points, that are too close together
# check the tolerance value in pycam/Geometry/PointKdtree.py
log.warn(
"Skipping invalid triangle: %s / %s / %s (maybe the resolution of the "
"model is too high?)", p1, p2, p3)
continue
if n:
t.normal = n
model.append(t)
else:
# from here on we want to use a text based input stream (not bytes)
f = TextIOWrapper(f, encoding="utf-8")
solid = re.compile(r"\s*solid\s+(\w+)\s+.*")
endsolid = re.compile(r"\s*endsolid\s*")
facet = re.compile(r"\s*facet\s*")
normal = re.compile(
r"\s*facet\s+normal" +
r"\s+(?P<x>[-+]?(\d+(\.\d*)?|\.\d+)([eE][-+]?\d+)?)" +
r"\s+(?P<y>[-+]?(\d+(\.\d*)?|\.\d+)([eE][-+]?\d+)?)" +
r"\s+(?P<z>[-+]?(\d+(\.\d*)?|\.\d+)([eE][-+]?\d+)?)\s+")
endfacet = re.compile(r"\s*endfacet\s+")
loop = re.compile(r"\s*outer\s+loop\s+")
endloop = re.compile(r"\s*endloop\s+")
vertex = re.compile(
r"\s*vertex" +
r"\s+(?P<x>[-+]?(\d+(\.\d*)?|\.\d+)([eE][-+]?\d+)?)" +
r"\s+(?P<y>[-+]?(\d+(\.\d*)?|\.\d+)([eE][-+]?\d+)?)" +
r"\s+(?P<z>[-+]?(\d+(\.\d*)?|\.\d+)([eE][-+]?\d+)?)\s+")
current_line = 0
for line in f:
if callback and callback():
raise AbortOperationException(
"STLImporter: load model operation cancelled")
current_line += 1
m = solid.match(line)
if m:
model.name = m.group(1)
continue
m = facet.match(line)
if m:
m = normal.match(line)
if m:
n = (float(m.group('x')), float(m.group('y')),
float(m.group('z')), 'v')
else:
n = None
continue
m = loop.match(line)
if m:
continue
m = vertex.match(line)
if m:
p = get_unique_vertex(float(m.group('x')), float(m.group('y')),
float(m.group('z')))
if p1 is None:
p1 = p
elif p2 is None:
p2 = p
elif p3 is None:
p3 = p
else:
log.error(
"STLImporter: more then 3 points in facet (line %d)",
current_line)
continue
m = endloop.match(line)
if m:
continue
m = endfacet.match(line)
if m:
if None in (p1, p2, p3):
log.warn(
"Invalid facet definition in line %d of '%s'. Please validate the "
"STL file!", current_line, filename)
n, p1, p2, p3 = None, None, None, None
continue
if not n:
n = pnormalized(pcross(psub(p2, p1), psub(p3, p1)))
# validate the normal
# The three vertices of a triangle in an STL file are supposed
# to be in counter-clockwise order. This should match the
# direction of the normal.
if n is None:
# invalid triangle (zero-length vector)
dotcross = 0
else:
# make sure the points are in ClockWise order
dotcross = pdot(n, pcross(psub(p2, p1), psub(p3, p1)))
if dotcross > 0:
# Triangle expects the vertices in clockwise order
t = Triangle(p1, p3, p2, n)
elif dotcross < 0:
if not normal_conflict_warning_seen:
log.warn(
"Inconsistent normal/vertices found in line %d of '%s'. Please "
"validate the STL file!", current_line, filename)
normal_conflict_warning_seen = True
t = Triangle(p1, p2, p3, n)
else:
# The three points are in a line - or two points are
# identical. Usually this is caused by points, that are too
# close together. Check the tolerance value in
# pycam/Geometry/PointKdtree.py.
log.warn(
"Skipping invalid triangle: %s / %s / %s (maybe the resolution of "
"the model is too high?)", p1, p2, p3)
n, p1, p2, p3 = (None, None, None, None)
continue
n, p1, p2, p3 = (None, None, None, None)
model.append(t)
continue
m = endsolid.match(line)
if m:
continue
# TODO display unique vertices and edges count - currently not counted
log.info("Imported STL model: %d triangles", len(model.triangles()))
vertices = 0
edges = 0
kdtree = None
if not model:
# no valid items added to the model
raise LoadFileError(
"Failed to load model from STL file: no elements found")
else:
return model
def get_collision_waterline_of_triangle(model, cutter, up_vector, triangle, z):
# TODO: there are problems with "material allowance > 0"
plane = Plane((0, 0, z), up_vector)
if triangle.minz >= z:
# no point of the triangle is below z
# try all edges
# Case (4)
proj_points = []
for p in triangle.get_points():
proj_p = plane.get_point_projection(p)
if proj_p not in proj_points:
proj_points.append(proj_p)
if len(proj_points) == 3:
edges = []
for index in range(3):
edge = Line(proj_points[index - 1], proj_points[index])
# the edge should be clockwise around the model
if pdot(pcross(edge.dir, triangle.normal), up_vector) < 0:
edge = Line(edge.p2, edge.p1)
edges.append((edge, proj_points[index - 2]))
outer_edges = []
for edge, other_point in edges:
# pick only edges, where the other point is on the right side
if pdot(pcross(psub(other_point, edge.p1), edge.dir),
up_vector) > 0:
outer_edges.append(edge)
if len(outer_edges) == 0:
# the points seem to be an one line
# pick the longest edge
long_edge = edges[0][0]
for edge, other_point in edges[1:]:
if edge.len > long_edge.len:
long_edge = edge
outer_edges = [long_edge]
else:
edge = Line(proj_points[0], proj_points[1])
if pdot(pcross(edge.dir, triangle.normal), up_vector) < 0:
edge = Line(edge.p2, edge.p1)
outer_edges = [edge]
else:
# some parts of the triangle are above and some below the cutter level
# Cases (2a), (2b), (3a) and (3b)
points_above = [
plane.get_point_projection(p) for p in triangle.get_points()
if p[2] > z
]
waterline = plane.intersect_triangle(triangle)
if waterline is None:
if len(points_above) == 0:
# the highest point of the triangle is at z
outer_edges = []
else:
if abs(triangle.minz - z) < epsilon:
# This is just an accuracy issue (see the
# "triangle.minz >= z" statement above).
outer_edges = []
elif not [
p for p in triangle.get_points() if p[2] > z + epsilon
]:
# same as above: fix for inaccurate floating calculations
outer_edges = []
else:
# this should not happen
raise ValueError((
"Could not find a waterline, but there are points above z "
"level (%f): %s / %s") % (z, triangle, points_above))
else:
# remove points that are not part of the waterline
points_above = [
p for p in points_above
if (p != waterline.p1) and (p != waterline.p2)
]
if len(points_above) == 0:
# part of case (2a)
outer_edges = [waterline]
elif len(points_above) == 1:
other_point = points_above[0]
dot = pdot(
pcross(psub(other_point, waterline.p1), waterline.dir),
up_vector)
if dot > 0:
# Case (2b)
outer_edges = [waterline]
elif dot < 0:
# Case (3b)
edges = []
edges.append(Line(waterline.p1, other_point))
edges.append(Line(waterline.p2, other_point))
outer_edges = []
for edge in edges:
if pdot(pcross(edge.dir, triangle.normal),
up_vector) < 0:
outer_edges.append(Line(edge.p2, edge.p1))
else:
outer_edges.append(edge)
else:
# the three points are on one line
# part of case (2a)
edges = []
edges.append(waterline)
edges.append(Line(waterline.p1, other_point))
edges.append(Line(waterline.p2, other_point))
edges.sort(key=lambda x: x.len)
edge = edges[-1]
if pdot(pcross(edge.dir, triangle.normal), up_vector) < 0:
outer_edges = [Line(edge.p2, edge.p1)]
else:
outer_edges = [edge]
else:
# two points above
other_point = points_above[0]
dot = pdot(
pcross(psub(other_point, waterline.p1), waterline.dir),
up_vector)
if dot > 0:
# Case (2b)
# the other two points are on the right side
outer_edges = [waterline]
elif dot < 0:
# Case (3a)
edge = Line(points_above[0], points_above[1])
if pdot(pcross(edge.dir, triangle.normal), up_vector) < 0:
outer_edges = [Line(edge.p2, edge.p1)]
else:
outer_edges = [edge]
else:
edges = []
# pick the longest combination of two of these points
# part of case (2a)
# TODO: maybe we should use the waterline instead?
# (otherweise the line could be too long and thus
# connections to the adjacent waterlines are not discovered?
# Test this with an appropriate test model.)
points = [waterline.p1, waterline.p2] + points_above
for p1 in points:
for p2 in points:
if p1 is not p2:
edges.append(Line(p1, p2))
edges.sort(key=lambda x: x.len)
edge = edges[-1]
if pdot(pcross(edge.dir, triangle.normal), up_vector) < 0:
outer_edges = [Line(edge.p2, edge.p1)]
else:
outer_edges = [edge]
# calculate the maximum diagonal length within the model
x_dim = abs(model.maxx - model.minx)
y_dim = abs(model.maxy - model.miny)
z_dim = abs(model.maxz - model.minz)
max_length = sqrt(x_dim**2 + y_dim**2 + z_dim**2)
result = []
for edge in outer_edges:
direction = pnormalized(pcross(up_vector, edge.dir))
if direction is None:
continue
direction = pmul(direction, max_length)
edge_dir = psub(edge.p2, edge.p1)
# TODO: Adapt the number of potential starting positions to the length
# of the line. Don't use 0.0 and 1.0 - this could result in ambiguous
# collisions with triangles sharing these vertices.
for factor in (0.5, epsilon, 1.0 - epsilon, 0.25, 0.75):
start = padd(edge.p1, pmul(edge_dir, factor))
# We need to use the triangle collision algorithm here - because we
# need the point of collision in the triangle.
collisions = get_free_paths_triangles([model],
cutter,
start,
padd(start, direction),
return_triangles=True)
for index, coll in enumerate(collisions):
if ((index % 2 == 0) and (coll[1] is not None)
and (coll[2] is not None)
and (pdot(psub(coll[0], start), direction) > 0)):
cl, hit_t, cp = coll
break
else:
log.debug("Failed to detect any collision: %s / %s -> %s",
edge, start, direction)
continue
proj_cp = plane.get_point_projection(cp)
# e.g. the Spherical Cutter often does not collide exactly above
# the potential collision line.
# TODO: maybe an "is cp inside of the triangle" check would be good?
if (triangle is hit_t) or (edge.is_point_inside(proj_cp)):
result.append((cl, edge))
# continue with the next outer_edge
break
# Don't check triangles again that are completely above the z level and
# did not return any collisions.
if not result and (triangle.minz > z):
# None indicates that the triangle needs no further evaluation
return None
return result
def intersect_circle_line(center, axis, radius, radiussq, direction, edge):
# make a plane by sliding the line along the direction (1)
d = edge.dir
if pdot(d, axis) == 0:
if pdot(direction, axis) == 0:
return (None, None, INFINITE)
plane = Plane(center, axis)
(p1, l) = plane.intersect_point(direction, edge.p1)
(p2, l) = plane.intersect_point(direction, edge.p2)
pc = Line(p1, p2).closest_point(center)
d_sq = pnormsq(psub(pc, center))
if d_sq >= radiussq:
return (None, None, INFINITE)
a = sqrt(radiussq - d_sq)
d1 = pdot(psub(p1, pc), d)
d2 = pdot(psub(p2, pc), d)
ccp = None
cp = None
if abs(d1) < a - epsilon:
ccp = p1
cp = psub(p1, pmul(direction, l))
elif abs(d2) < a - epsilon:
ccp = p2
cp = psub(p2, pmul(direction, l))
elif ((d1 < -a + epsilon) and (d2 > a - epsilon)) \
or ((d2 < -a + epsilon) and (d1 > a - epsilon)):
ccp = pc
cp = psub(pc, pmul(direction, l))
return (ccp, cp, -l)
n = pcross(d, direction)
if pnorm(n) == 0:
# no contact point, but should check here if circle *always* intersects
# line...
return (None, None, INFINITE)
n = pnormalized(n)
# take a plane through the base
plane = Plane(center, axis)
# intersect base with line
(lp, l) = plane.intersect_point(d, edge.p1)
if not lp:
return (None, None, INFINITE)
# intersection of 2 planes: lp + \lambda v
v = pcross(axis, n)
if pnorm(v) == 0:
return (None, None, INFINITE)
v = pnormalized(v)
# take plane through intersection line and parallel to axis
n2 = pcross(v, axis)
if pnorm(n2) == 0:
return (None, None, INFINITE)
n2 = pnormalized(n2)
# distance from center to this plane
dist = pdot(n2, center) - pdot(n2, lp)
distsq = dist * dist
if distsq > radiussq - epsilon:
return (None, None, INFINITE)
# must be on circle
dist2 = sqrt(radiussq - distsq)
if pdot(d, axis) < 0:
dist2 = -dist2
ccp = psub(center, psub(pmul(n2, dist), pmul(v, dist2)))
plane = Plane(edge.p1, pcross(pcross(d, direction), d))
(cp, l) = plane.intersect_point(direction, ccp)
return (ccp, cp, l)
