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 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 get_lines_layer(lines,
z,
last_z=None,
step_width=None,
milling_style=MillingStyle.CONVENTIONAL):
get_proj_point = lambda proj_point: (proj_point[0], proj_point[1], z)
projected_lines = []
for line in lines:
if (last_z is not None) and (last_z < line.minz):
# the line was processed before
continue
elif line.minz < z < line.maxz:
# Split the line at the point at z level and do the calculation
# for both point pairs.
factor = (z - line.p1[2]) / (line.p2[2] - line.p1[2])
plane_point = padd(line.p1, pmul(line.vector, factor))
if line.p1[2] < z:
p1 = get_proj_point(line.p1)
p2 = line.p2
else:
p1 = line.p1
p2 = get_proj_point(line.p2)
projected_lines.append(Line(p1, plane_point))
yield Line(plane_point, p2)
elif line.minz < last_z < line.maxz:
plane = Plane((0, 0, last_z), (0, 0, 1, 'v'))
cp = plane.intersect_point(line.dir, line.p1)[0]
# we can be sure that there is an intersection
if line.p1[2] > last_z:
p1, p2 = cp, line.p2
else:
p1, p2 = line.p1, cp
projected_lines.append(Line(p1, p2))
else:
if line.maxz <= z:
# the line is completely below z
projected_lines.append(
Line(get_proj_point(line.p1), get_proj_point(line.p2)))
elif line.minz >= z:
projected_lines.append(line)
else:
_log.warn(
"Unexpected condition 'get_lines_layer': %s / %s / %s / %s",
line.p1, line.p2, z, last_z)
# process all projected lines
for line in projected_lines:
points = []
if step_width is None:
points.append(line.p1)
points.append(line.p2)
else:
if isiterable(step_width):
steps = step_width
else:
steps = floatrange(0.0, line.len, inc=step_width)
for step in steps:
next_point = padd(line.p1, pmul(line.dir, step))
points.append(next_point)
yield points
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_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 GenerateToolPathLinePush(self, pa, line, z, previous_z,
draw_callback=None):
if previous_z <= line.minz:
# the line is completely above the previous level
pass
elif line.minz < z < line.maxz:
# Split the line at the point at z level and do the calculation
# for both point pairs.
factor = (z - line.p1.z) / (line.p2.z - line.p1.z)
plane_point = line.p1.add(line.vector.mul(factor))
self.GenerateToolPathLinePush(pa, Line(line.p1, plane_point), z,
previous_z, draw_callback=draw_callback)
self.GenerateToolPathLinePush(pa, Line(plane_point, line.p2), z,
previous_z, draw_callback=draw_callback)
elif line.minz < previous_z < line.maxz:
plane = Plane(Point(0, 0, previous_z), Vector(0, 0, 1))
cp = plane.intersect_point(line.dir, line.p1)[0]
# we can be sure that there is an intersection
if line.p1.z > previous_z:
p1, p2 = cp, line.p2
else:
p1, p2 = line.p1, cp
self.GenerateToolPathLinePush(pa, Line(p1, p2), z, previous_z,
draw_callback=draw_callback)
else:
if line.maxz <= z:
# the line is completely below z
p1 = Point(line.p1.x, line.p1.y, z)
p2 = Point(line.p2.x, line.p2.y, z)
elif line.minz >= z:
p1 = line.p1
p2 = line.p2
else:
log.warn("Unexpected condition EC_GTPLP: %s / %s / %s / %s" % \
(line.p1, line.p2, z, previous_z))
return
# no model -> no possible obstacles
# model is completely below z (e.g. support bridges) -> no obstacles
relevant_models = [m for m in self.models if m.maxz >= z]
if not relevant_models:
points = [p1, p2]
elif self.physics:
points = get_free_paths_ode(self.physics, p1, p2)
else:
points = get_free_paths_triangles(relevant_models, self.cutter,
p1, p2)
if points:
for point in points:
pa.append(point)
if draw_callback:
draw_callback(tool_position=points[-1], toolpath=pa.paths)
def GenerateToolPathLinePush(self, pa, line, z, previous_z,
draw_callback=None):
if previous_z <= line.minz:
# the line is completely above the previous level
pass
elif line.minz < z < line.maxz:
# Split the line at the point at z level and do the calculation
# for both point pairs.
factor = (z - line.p1.z) / (line.p2.z - line.p1.z)
plane_point = line.p1.add(line.vector.mul(factor))
self.GenerateToolPathLinePush(pa, Line(line.p1, plane_point), z,
previous_z, draw_callback=draw_callback)
self.GenerateToolPathLinePush(pa, Line(plane_point, line.p2), z,
previous_z, draw_callback=draw_callback)
elif line.minz < previous_z < line.maxz:
plane = Plane(Point(0, 0, previous_z), Vector(0, 0, 1))
cp = plane.intersect_point(line.dir, line.p1)[0]
# we can be sure that there is an intersection
if line.p1.z > previous_z:
p1, p2 = cp, line.p2
else:
p1, p2 = line.p1, cp
self.GenerateToolPathLinePush(pa, Line(p1, p2), z, previous_z,
draw_callback=draw_callback)
else:
if line.maxz <= z:
# the line is completely below z
p1 = Point(line.p1.x, line.p1.y, z)
p2 = Point(line.p2.x, line.p2.y, z)
elif line.minz >= z:
p1 = line.p1
p2 = line.p2
else:
log.warn("Unexpected condition EC_GTPLP: %s / %s / %s / %s" % \
(line.p1, line.p2, z, previous_z))
return
# no model -> no possible obstacles
# model is completely below z (e.g. support bridges) -> no obstacles
relevant_models = [m for m in self.models if m.maxz >= z]
if not relevant_models:
points = [p1, p2]
elif self.physics:
points = get_free_paths_ode(self.physics, p1, p2)
else:
points = get_free_paths_triangles(relevant_models, self.cutter,
p1, p2)
if points:
for point in points:
pa.append(point)
if draw_callback:
draw_callback(tool_position=points[-1], toolpath=pa.paths)
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 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 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 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 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)
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)
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 d.dot(axis) == 0:
if direction.dot(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 = pc.sub(center).normsq
if d_sq >= radiussq:
return (None, None, INFINITE)
a = sqrt(radiussq - d_sq)
d1 = p1.sub(pc).dot(d)
d2 = p2.sub(pc).dot(d)
ccp = None
cp = None
if abs(d1) < a - epsilon:
ccp = p1
cp = p1.sub(direction.mul(l))
elif abs(d2) < a - epsilon:
ccp = p2
cp = p2.sub(direction.mul(l))
elif ((d1 < -a + epsilon) and (d2 > a - epsilon)) \
or ((d2 < -a + epsilon) and (d1 > a - epsilon)):
ccp = pc
cp = pc.sub(direction.mul(l))
return (ccp, cp, -l)
n = d.cross(direction)
if n.norm == 0:
# no contact point, but should check here if circle *always* intersects
# line...
return (None, None, INFINITE)
n = n.normalized()
# 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 = axis.cross(n)
if v.norm == 0:
return (None, None, INFINITE)
v = v.normalized()
# take plane through intersection line and parallel to axis
n2 = v.cross(axis)
if n2.norm == 0:
return (None, None, INFINITE)
n2 = n2.normalized()
# distance from center to this plane
dist = n2.dot(center) - n2.dot(lp)
distsq = dist * dist
if distsq > radiussq - epsilon:
return (None, None, INFINITE)
# must be on circle
dist2 = sqrt(radiussq - distsq)
if d.dot(axis) < 0:
dist2 = -dist2
ccp = center.sub(n2.mul(dist)).sub(v.mul(dist2))
plane = Plane(edge.p1, d.cross(direction).cross(d))
(cp, l) = plane.intersect_point(direction, ccp)
return (ccp, cp, l)
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 d.dot(axis) == 0:
if direction.dot(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 = pc.sub(center).normsq
if d_sq >= radiussq:
return (None, None, INFINITE)
a = sqrt(radiussq - d_sq)
d1 = p1.sub(pc).dot(d)
d2 = p2.sub(pc).dot(d)
ccp = None
cp = None
if abs(d1) < a - epsilon:
ccp = p1
cp = p1.sub(direction.mul(l))
elif abs(d2) < a - epsilon:
ccp = p2
cp = p2.sub(direction.mul(l))
elif ((d1 < -a + epsilon) and (d2 > a - epsilon)) \
or ((d2 < -a + epsilon) and (d1 > a - epsilon)):
ccp = pc
cp = pc.sub(direction.mul(l))
return (ccp, cp, -l)
n = d.cross(direction)
if n.norm == 0:
# no contact point, but should check here if circle *always* intersects
# line...
return (None, None, INFINITE)
n = n.normalized()
# 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 = axis.cross(n)
if v.norm == 0:
return (None, None, INFINITE)
v = v.normalized()
# take plane through intersection line and parallel to axis
n2 = v.cross(axis)
if n2.norm == 0:
return (None, None, INFINITE)
n2 = n2.normalized()
# distance from center to this plane
dist = n2.dot(center) - n2.dot(lp)
distsq = dist * dist
if distsq > radiussq - epsilon:
return (None, None, INFINITE)
# must be on circle
dist2 = sqrt(radiussq - distsq)
if d.dot(axis) < 0:
dist2 = -dist2
ccp = center.sub(n2.mul(dist)).sub(v.mul(dist2))
plane = Plane(edge.p1, d.cross(direction).cross(d))
(cp, l) = plane.intersect_point(direction, ccp)
return (ccp, cp, l)
