def calculate_point_height(self, x, y, func):
point = (x, y, self.outer.minz)
if not self.outer.is_point_inside(point):
return None
for poly in self.inner:
if poly.is_point_inside(point):
return None
point = (x, y, self.outer.minz)
line_distances = []
for line in self.lines:
cross_product = pcross(line.dir, psub(point, line.p1))
if cross_product[2] > 0:
close_points = []
close_point = line.closest_point(point)
if not line.is_point_inside(close_point):
close_points.append(line.p1)
close_points.append(line.p2)
else:
close_points.append(close_point)
for p in close_points:
direction = psub(point, p)
dist = pnorm(direction)
line_distances.append(dist)
elif cross_product[2] == 0:
# the point is on the line
line_distances.append(0.0)
# no other line can get closer than this
break
else:
# the point is in the left of this line
pass
line_distances.sort()
return self.z_level + func(line_distances[0])
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 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 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 is_point_inside(self, p):
if (p == self.p1) or (p == self.p2):
# these conditions are not covered by the code below
return True
dir1 = pnormalized(psub(p, self.p1))
dir2 = pnormalized(psub(self.p2, p))
# True if the two parts of the line have the same direction or if the
# point is self.p1 or self.p2.
return (dir1 == dir2 == self.dir) or (dir1 is None) or (dir2 is 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_plane(self, direction, triangle, start=None):
if start is None:
start = self.location
ccp, cp, l = intersect_circle_plane(start, self.distance_majorradius,
direction, triangle)
# offset intersection
if ccp:
cl = psub(cp, psub(ccp, start))
return (cl, ccp, cp, l)
return (None, None, None, INFINITE)
def intersect_circle_plane(self, direction, triangle, start=None):
if start is None:
start = self.location
(ccp, cp, d) = intersect_circle_plane(
padd(psub(start, self.location), self.center),
self.distance_radius, direction, triangle)
if ccp and cp:
cl = padd(cp, psub(start, ccp))
return (cl, ccp, cp, d)
return (None, None, None, INFINITE)
def intersect_circle_line(self, direction, edge, start=None):
if start is None:
start = self.location
(ccp, cp, l) = intersect_circle_line(
padd(psub(start, self.location), self.center), self.axis,
self.distance_radius, self.distance_radiussq, direction, edge)
if ccp:
cl = padd(cp, psub(start, ccp))
return (cl, ccp, cp, l)
return (None, None, None, INFINITE)
def intersect_cylinder_point(self, direction, point, start=None):
if start is None:
start = self.location
(ccp, cp, l) = intersect_cylinder_point(
padd(psub(start, self.location), self.center), self.axis,
self.distance_radius, self.distance_radiussq, direction, point)
# offset intersection
if ccp:
cl = padd(start, psub(cp, ccp))
return (cl, ccp, cp, l)
return (None, None, None, INFINITE)
def intersect_sphere_line(self, direction, edge, start=None):
if start is None:
start = self.location
(ccp, cp, l) = intersect_sphere_line(padd(psub(start, self.location), self.center),
self.distance_radius, self.distance_radiussq,
direction, edge)
# offset intersection
if ccp:
cl = psub(cp, psub(ccp, start))
return (cl, ccp, cp, l)
return (None, None, None, INFINITE)
def intersect_torus_plane(self, direction, triangle, start=None):
if start is None:
start = self.location
(ccp, cp, l) = intersect_torus_plane(
padd(psub(start, self.location),
self.center), self.axis, self.distance_majorradius,
self.distance_minorradius, direction, triangle)
if cp:
cl = padd(cp, psub(start, ccp))
return (cl, ccp, cp, l)
return (None, None, None, INFINITE)
def intersect_circle_line(self, direction, edge, start=None):
if start is None:
start = self.location
(ccp, cp, l_len) = intersect_circle_line(start, self.axis,
self.distance_majorradius,
self.distance_majorradiussq,
direction, edge)
if ccp:
cl = psub(cp, psub(ccp, start))
return (cl, ccp, cp, l_len)
return (None, None, None, INFINITE)
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_torus_point(self, direction, point, start=None):
if start is None:
start = self.location
(ccp, cp, l) = intersect_torus_point(
padd(psub(start, self.location),
self.center), self.axis, self.distance_majorradius,
self.distance_minorradius, self.distance_majorradiussq,
self.distance_minorradiussq, direction, point)
if ccp:
cl = padd(point, psub(start, ccp))
return (cl, ccp, point, l)
return (None, None, None, INFINITE)
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 filter_toolpath(self, toolpath):
feedrate = min_feedrate = 1
new_path = []
last_pos = None
limit = self.settings["timelimit"]
duration = 0
for move_type, args in toolpath:
if move_type in (MOVE_STRAIGHT, MOVE_STRAIGHT_RAPID):
if last_pos:
new_distance = pdist(args, last_pos)
new_duration = new_distance / max(feedrate, min_feedrate)
if (new_duration > 0) and (duration + new_duration >
limit):
partial = (limit - duration) / new_duration
destination = padd(last_pos,
pmul(psub(args, last_pos), partial))
duration = limit
else:
destination = args
duration += new_duration
else:
destination = args
new_path.append((move_type, destination))
last_pos = args
if (move_type == MACHINE_SETTING) and (args[0] == "feedrate"):
feedrate = args[1]
if duration >= limit:
break
return new_path
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 filter_toolpath(self, toolpath):
feedrate = min_feedrate = 1
new_path = []
last_pos = None
limit = self.settings["timelimit"]
duration = 0
for step in toolpath:
if step.action in MOVES_LIST:
if last_pos:
new_distance = pdist(step.position, last_pos)
new_duration = new_distance / max(feedrate, min_feedrate)
if (new_duration > 0) and (duration + new_duration > limit):
partial = (limit - duration) / new_duration
destination = padd(last_pos, pmul(psub(step.position, last_pos), partial))
duration = limit
else:
destination = step.position
duration += new_duration
else:
destination = step.position
new_path.append(ToolpathSteps.get_step_class_by_action(step.action)(destination))
last_pos = step.position
if (step.action == MACHINE_SETTING) and (step.key == "feedrate"):
feedrate = step.value
if duration >= limit:
break
return new_path
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 get_x3d_cone(start: tuple, end: tuple, position: float, length: float, radius: float,
color: tuple):
# by default the X3D cone points along the y axis
original_vector = (0, 1, 0)
line_vector = psub(end, start)
line_normalized = pnormalized(line_vector)
# the cone position is at its bottom
bottom_position = padd(start, pmul(line_vector, position))
# handling of corner cases
if pnormsq(pcross(original_vector, line_normalized)) == 0:
# Both vectors are aligned - but maybe point into different directions.
rotation_axis = (1, 0, 0)
rotation_angle = math.pi if original_vector != line_normalized else 0
else:
# Both vectors are not aligned. Use a normal rotation.
# Rotate around the vector in the vector in the middle by 180 degrees.
rotation_axis = padd(original_vector, line_normalized)
rotation_angle = math.pi
yield ('<Transform translation="{:f} {:f} {:f}" rotation="{:f} {:f} {:f} {:f}">'
.format(*bottom_position, *rotation_axis, rotation_angle))
yield "<Shape>"
yield "<Appearance>"
yield ('<Material diffuseColor="{:f} {:f} {:f}" transparency="{:f}" />'
.format(color["red"], color["green"], color["blue"], 1 - color["alpha"]))
yield "</Appearance>"
yield '<Cone bottomRadius="{:f}" topRadius="0" height="{:f}" />'.format(radius, length)
yield "</Shape>"
yield "</Transform>"
def filter_toolpath(self, toolpath):
feedrate = min_feedrate = 1
new_path = []
last_pos = None
limit = self.settings["timelimit"]
duration = 0
for move_type, args in toolpath:
if move_type in (MOVE_STRAIGHT, MOVE_STRAIGHT_RAPID):
if last_pos:
new_distance = pdist(args, last_pos)
new_duration = new_distance / max(feedrate, min_feedrate)
if (new_duration > 0) and (duration + new_duration > limit):
partial = (limit - duration) / new_duration
destination = padd(last_pos, pmul(psub(args, last_pos), partial))
duration = limit
else:
destination = args
duration += new_duration
else:
destination = args
new_path.append((move_type, destination))
last_pos = args
if (move_type == MACHINE_SETTING) and (args[0] == "feedrate"):
feedrate = args[1]
if duration >= limit:
break
return new_path
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_circle_point(center, axis, radius, radiussq, direction, point):
# take a plane through the base
plane = Plane(center, axis)
# intersect with line gives ccp
(ccp, l) = plane.intersect_point(direction, point)
# check if inside circle
if ccp and (pnormsq(psub(center, ccp)) < radiussq - epsilon):
return (ccp, point, -l)
return (None, None, INFINITE)
def intersect_cylinder_vertex(self, direction, point, start=None):
if start is None:
start = self.location
(cl, ccp, cp, l) = self.intersect_cylinder_point(direction,
point,
start=start)
if ccp and ccp[2] < padd(psub(start, self.location), self.center)[2]:
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 _check_deviance_of_adjacent_points(p1, p2, p3, min_distance):
straight = psub(p3, p1)
added = pdist(p2, p1) + pdist(p3, p2)
# compare only the x/y distance of p1 and p3 with min_distance
if straight[0]**2 + straight[1]**2 < min_distance**2:
# the points are too close together
return True
else:
# allow 0.1% deviance - this is an angle of around 2 degrees
return (added / pnorm(straight)) < 1.001
def get_tool_x3d(self):
yield '<Cylinder radius="{:f}" height="{:f}" />'.format(
self.radius, self.height)
yield '<Torus innerRadius="{:f}" outerRadius="{:f}" />'.format(
self.minorradius, self.majorradius)
yield '<Transform center="{:f} {:f} {:f}">'.format(
psub(self.location, self.center))
yield '<Disk2D innerRadius="{:f}" outerRadius="{:f}" />'.format(
0, self.majorradius)
yield "</Transform>"
def intersect_sphere_point(self, direction, point, start=None):
if start is None:
start = self.location
(ccp, cp, l) = intersect_sphere_point(padd(psub(start, self.location), self.center),
self.distance_radius, self.distance_radiussq,
direction, point)
# offset intersection
cl = None
if cp:
cl = padd(start, pmul(direction, l))
return (cl, 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_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
