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 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_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)