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 get_barycenter(self): area = self.get_area() if not area: return None # see: http://stackoverflow.com/questions/2355931/foo/2360507 # first: calculate cx and y cxy, cxz, cyx, cyz, czx, czy = (0, 0, 0, 0, 0, 0) for index in range(len(self._points)): p1 = self._points[index] p2 = self._points[(index + 1) % len(self._points)] cxy += (p1[0] + p2[0]) * (p1[0] * p2[1] - p1[1] * p2[0]) cxz += (p1[0] + p2[0]) * (p1[0] * p2[2] - p1[2] * p2[0]) cyx += (p1[1] + p2[1]) * (p1[0] * p2[1] - p1[1] * p2[0]) cyz += (p1[1] + p2[1]) * (p1[1] * p2[2] - p1[2] * p2[1]) czx += (p1[2] + p2[2]) * (p1[2] * p2[0] - p1[0] * p2[2]) czy += (p1[2] + p2[2]) * (p1[1] * p2[2] - p1[2] * p2[1]) if abs(self.maxz - self.minz) < epsilon: return (cxy / (6 * area), cyx / (6 * area), self.minz) elif abs(self.maxy - self.miny) < epsilon: return (cxz / (6 * area), self.miny, czx / (6 * area)) elif abs(self.maxx - self.minx) < epsilon: return (self.minx, cyz / (6 * area), czy / (6 * area)) else: # calculate area of xy projection poly_xy = self.get_plane_projection(Plane((0, 0, 0), (0, 0, 1))) poly_xz = self.get_plane_projection(Plane((0, 0, 0), (0, 1, 0))) poly_yz = self.get_plane_projection(Plane((0, 0, 0), (1, 0, 0))) if (poly_xy is None) or (poly_xz is None) or (poly_yz is None): log.warn("Invalid polygon projection for barycenter: %s", str(self)) return None area_xy = poly_xy.get_area() area_xz = poly_xz.get_area() area_yz = poly_yz.get_area() if 0 in (area_xy, area_xz, area_yz): log.info( "Failed assumtion: zero-sized projected area - %s / %s / %s", area_xy, area_xz, area_yz) return None if abs(cxy / area_xy - cxz / area_xz) > epsilon: log.info("Failed assumption: barycenter xy/xz - %s / %s", cxy / area_xy, cxz / area_xz) if abs(cyx / area_xy - cyz / area_yz) > epsilon: log.info("Failed assumption: barycenter yx/yz - %s / %s", cyx / area_xy, cyz / area_yz) if abs(czx / area_xz - czy / area_yz) > epsilon: log.info("Failed assumption: barycenter zx/zy - %s / %s", czx / area_xz, cyz / area_yz) return (cxy / (6 * area_xy), cyx / (6 * area_xy), czx / (6 * area_xz))
def GenerateToolPath(self, callback=None): triangles = self.model.triangles() equations = {} # patching Point Point.__hash__ = lambda self: hash(' '.join( [str(self.x), str(self.y), str(self.z)])) Point.__eq__ = lambda self, other : _is_near(self.x, other.x) \ and _is_near(self.y, other.y) \ and _is_near(self.z, other.z) #patching Line def equation2D(x1, y1, x2, y2): if abs(x1 - x2) < epsilon: # care of floating-point imprecision return "x=%s" % x1 else: a = (y2 - y1) / (x2 - x1) return "y=%sx+%s" % (a, y2 - a * x2) Line.__hash__ = lambda self : hash(' '.join([ equation2D(self.p1.x, self.p1.y, self.p2.x, self.p2.y), \ equation2D(self.p1.x, self.p1.z, self.p2.x, self.p2.z), \ equation2D(self.p1.y, self.p1.z, self.p2.y, self.p2.z)])) Line.__eq__ = lambda self, other: hash(self) == hash(other) for height in self.grid.iterate_on_layers(): equations.clear() planeInf = Plane(Point(0, 0, height), self.grid.up_vector) planeSup = Plane(Point(0, 0, INFINITE), self.grid.up_vector) lines = [ligne for triangle in triangles for ligne in \ self.triangleBetweenTwoPlanes(triangle, planeInf, planeSup)] for line in lines: if line not in equations: equations[line] = [line] else: equations[line].append(line) to_project = [] for equation in iter(equations): lines = equations[equation] uniques = self.mergeRiddanceLines(lines) to_project.extend(uniques) for line in to_project: projection = planeInf.get_line_projection(line) self.grid.discretise_line(projection) #self.grid.display_complete_layer(height) #self.grid.draw_contour_PBM() self.pa.initialise(self.grid) self.pa.do_path() self.grid.free() del self.grid return self.pa.paths
def GenerateToolPath(self, callback=None) : triangles = self.model.triangles() equations = {} # patching Point Point.__hash__ = lambda self : hash(' '.join( [str(self.x), str(self.y), str(self.z)])) Point.__eq__ = lambda self, other : _is_near(self.x, other.x) \ and _is_near(self.y, other.y) \ and _is_near(self.z, other.z) #patching Line def equation2D(x1, y1, x2, y2): if abs(x1 - x2) < epsilon : # care of floating-point imprecision return "x=%s" % x1 else: a = (y2-y1)/(x2-x1) return "y=%sx+%s" % (a, y2-a*x2) Line.__hash__ = lambda self : hash(' '.join([ equation2D(self.p1.x, self.p1.y, self.p2.x, self.p2.y), \ equation2D(self.p1.x, self.p1.z, self.p2.x, self.p2.z), \ equation2D(self.p1.y, self.p1.z, self.p2.y, self.p2.z)])) Line.__eq__ = lambda self, other : hash(self) == hash(other) for height in self.grid.iterate_on_layers() : equations.clear() planeInf = Plane(Point(0, 0, height), self.grid.up_vector) planeSup = Plane(Point(0, 0, INFINITE), self.grid.up_vector) lines = [ligne for triangle in triangles for ligne in \ self.triangleBetweenTwoPlanes(triangle, planeInf, planeSup)] for line in lines : if line not in equations : equations[line] = [line] else : equations[line].append(line) to_project = [] for equation in iter(equations) : lines = equations[equation] uniques = self.mergeRiddanceLines(lines) to_project.extend(uniques) for line in to_project : projection = planeInf.get_line_projection(line) self.grid.discretise_line(projection) #self.grid.display_complete_layer(height) #self.grid.draw_contour_PBM() self.pa.initialise(self.grid) self.pa.do_path() self.grid.free() del self.grid return self.pa.paths
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 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 _projection(self, widget=None): models = self._get_projectable_models() if not models: return progress = self.core.get("progress") progress.update(text="Calculating 2D projection") progress.set_multiple(len(models), "Model") for model_dict in models: model = model_dict.model for objname, z_level in ( ("ProjectionModelTop", model.maxz), ("ProjectionModelMiddle", (model.minz + model.maxz) / 2.0), ("ProjectionModelBottom", model.minz), ("ProjectionModelCustom", self.gui.get_object("ProjectionZLevel").get_value())): if self.gui.get_object(objname).get_active(): plane = Plane((0, 0, z_level), (0, 0, 1, 'v')) self.log.info("Projecting 3D model at level z=%g" % plane.p[2]) new_model = model.get_waterline_contour( plane, callback=progress.update) if new_model: self.core.get("models").add_model( new_model, name_template="Projected model #%d") else: self.log.warn("The 2D projection at z=%g is empty. Aborted." % \ plane.p[2]) break progress.update_multiple() progress.finish()
def get_shifted_waterline(up_vector, waterline, cutter_location): # Project the waterline and the cutter location down to the slice plane. # This is necessary for calculating the horizontal distance between the # cutter and the triangle waterline. plane = Plane(cutter_location, up_vector) wl_proj = plane.get_line_projection(waterline) if wl_proj.len < epsilon: return None offset = wl_proj.dist_to_point(cutter_location) if offset < epsilon: return wl_proj # shift both ends of the waterline towards the cutter location shift = cutter_location.sub(wl_proj.closest_point(cutter_location)) # increase the shift width slightly to avoid "touch" collisions shift = shift.mul(1.0 + epsilon) shifted_waterline = Line(wl_proj.p1.add(shift), wl_proj.p2.add(shift)) return shifted_waterline
def get_support_distributed(model, z_plane, average_distance, min_bridges_per_polygon, thickness, height, length, bounds=None, start_at_corners=False): if (average_distance == 0) or (length == 0) or (thickness == 0) or (height == 0): return result = Model() if not hasattr(model, "get_polygons"): model = model.get_waterline_contour( Plane((0, 0, max(model.minz, z_plane)), (0, 0, 1, 'v'))) if model: model = model.get_flat_projection( Plane((0, 0, z_plane), (0, 0, 1, 'v'))) if model and bounds: model = model.get_cropped_model_by_bounds(bounds) if model: polygons = model.get_polygons() else: return None # minimum required distance between two bridge start points avoid_distance = 1.5 * (abs(length) + thickness) if start_at_corners: bridge_calculator = _get_corner_bridges else: bridge_calculator = _get_edge_bridges for polygon in polygons: # no grid for _small_ inner polygons # TODO: calculate a reasonable factor (see below) if polygon.is_closed and (not polygon.is_outer()) \ and (abs(polygon.get_area()) < 25000 * thickness ** 2): continue bridges = bridge_calculator(polygon, z_plane, min_bridges_per_polygon, average_distance, avoid_distance) for pos, direction in bridges: _add_cuboid_to_model(result, pos, pmul(direction, length), height, thickness) return result
def __init__(self, plane=None): import pycam.Exporters.SVGExporter super(ContourModel, self).__init__() self.name = "contourmodel%d" % self.id if plane is None: # the default plane points upwards along the z axis plane = Plane((0, 0, 0), (0, 0, 1, 'v')) self._plane = plane self._line_groups = [] self._item_groups.append(self._line_groups) # there is always just one plane self._plane_groups = [self._plane] self._item_groups.append(self._plane_groups) self._export_function = pycam.Exporters.SVGExporter.SVGExporterContourModel
def __init__(self, plane=None): super(ContourModel, self).__init__() self.name = "contourmodel%d" % self.id if plane is None: # the default plane points upwards along the z axis plane = Plane(Point(0, 0, 0), Vector(0, 0, 1)) self._plane = plane self._line_groups = [] self._item_groups.append(self._line_groups) # there is always just one plane self._plane_groups = [self._plane] self._item_groups.append(self._plane_groups) self._cached_offset_models = {} self._export_function = \ pycam.Exporters.SVGExporter.SVGExporterContourModel
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 __init__(self, plane=None): super().__init__() if plane is None: # the default plane points upwards along the z axis plane = Plane((0, 0, 0), (0, 0, 1, 'v')) self.plane = plane self._points = [] self.is_closed = False self.maxx = None self.minx = None self.maxy = None self.miny = None self.maxz = None self.minz = None self._lines_cache = None self._area_cache = None self._cached_offset_polygons = {}
def get_cropped_line(self, minx, maxx, miny, maxy, minz, maxz): if self.is_completely_inside(minx, maxx, miny, maxy, minz, maxz): return self elif self.is_completely_outside(minx, maxx, miny, maxy, minz, maxz): return None else: # the line needs to be cropped # generate the six planes of the cube for possible intersections minp = (minx, miny, minz) maxp = (maxx, maxy, maxz) planes = [ Plane(minp, (1, 0, 0)), Plane(minp, (0, 1, 0)), Plane(minp, (0, 0, 1)), Plane(maxp, (1, 0, 0)), Plane(maxp, (0, 1, 0)), Plane(maxp, (0, 0, 1)) ] # calculate all intersections intersections = [ plane.intersect_point(self.dir, self.p1) for plane in planes ] # remove all intersections outside the box and outside the line valid_intersections = [ (cp, dist) for cp, dist in intersections if cp and (-epsilon <= dist <= self.len + epsilon) and cp.is_inside(minx, maxx, miny, maxy, minz, maxz) ] # sort the intersections according to their distance to self.p1 valid_intersections.sort(key=lambda collision: collision[1]) # Check if p1 is within the box - otherwise use the closest # intersection. The check for "valid_intersections" is necessary # to prevent an IndexError due to floating point inaccuracies. if self.p1.is_inside(minx, maxx, miny, maxy, minz, maxz) \ or not valid_intersections: new_p1 = self.p1 else: new_p1 = valid_intersections[0][0] # Check if p2 is within the box - otherwise use the intersection # most distant from p1. if self.p2.is_inside(minx, maxx, miny, maxy, minz, maxz) or not valid_intersections: new_p2 = self.p2 else: new_p2 = valid_intersections[-1][0] if new_p1 == new_p2: # no real line return None else: return Line(new_p1, new_p2)
def _get_waterlines(self): models = [m.model for m in self.models_widget.get_value()] polygons = [] # get all waterlines and polygons for model in models: if hasattr(model, "get_polygons"): for poly in model.get_polygons(): polygons.append(poly.copy()) elif hasattr(model, "get_waterline_contour"): z_slice = self.gui.get_object("ToolpathCropZSlice").get_value() plane = Plane((0, 0, z_slice)) for poly in model.get_waterline_contour(plane).get_polygons(): polygons.append(poly.copy()) # add an offset if requested margin = self.gui.get_object("ToolpathCropMargin").get_value() if margin != 0: shifted = [] for poly in polygons: shifted.extend(poly.get_offset_polygons(margin)) polygons = shifted return polygons
def reset_cache(self): self.minx = min(self.p1.x, self.p2.x, self.p3.x) self.miny = min(self.p1.y, self.p2.y, self.p3.y) self.minz = min(self.p1.z, self.p2.z, self.p3.z) self.maxx = max(self.p1.x, self.p2.x, self.p3.x) self.maxy = max(self.p1.y, self.p2.y, self.p3.y) self.maxz = max(self.p1.z, self.p2.z, self.p3.z) 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 = self.p3.sub(self.p1).cross(self.p2.sub( \ self.p1)).normalized() if not isinstance(self.normal, Vector): self.normal = self.normal.get_vector() # make sure that the normal has always a unit length self.normal = self.normal.normalized() self.center = self.p1.add(self.p2).add(self.p3).div(3) self.plane = Plane(self.center, self.normal) # calculate circumcircle (resulting in radius and middle) denom = self.p2.sub(self.p1).cross(self.p3.sub(self.p2)).norm self.radius = (self.p2.sub(self.p1).norm \ * self.p3.sub(self.p2).norm * self.p3.sub(self.p1).norm) \ / (2 * denom) self.radiussq = self.radius**2 denom2 = 2 * denom * denom alpha = self.p3.sub(self.p2).normsq \ * self.p1.sub(self.p2).dot(self.p1.sub(self.p3)) / denom2 beta = self.p1.sub(self.p3).normsq \ * self.p2.sub(self.p1).dot(self.p2.sub(self.p3)) / denom2 gamma = self.p1.sub(self.p2).normsq \ * self.p3.sub(self.p1).dot(self.p3.sub(self.p2)) / denom2 self.middle = Point( self.p1.x * alpha + self.p2.x * beta + self.p3.x * gamma, self.p1.y * alpha + self.p2.y * beta + self.p3.y * gamma, self.p1.z * alpha + self.p2.z * beta + self.p3.z * gamma)
def get_collision_waterline_of_triangle(model, cutter, up_vector, triangle, z): # TODO: there are problems with "material allowance > 0" plane = Plane(Point(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 not proj_p 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 edge.dir.cross(triangle.normal).dot(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 other_point.sub(edge.p1).cross(edge.dir).dot(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 edge.dir.cross(triangle.normal).dot(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.z > 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.z > 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 = other_point.sub(waterline.p1).cross(waterline.dir).dot( 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 edge.dir.cross(triangle.normal).dot(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 edge.dir.cross(triangle.normal).dot(up_vector) < 0: outer_edges = [Line(edge.p2, edge.p1)] else: outer_edges = [edge] else: # two points above other_point = points_above[0] dot = other_point.sub(waterline.p1).cross(waterline.dir).dot( 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 edge.dir.cross(triangle.normal).dot(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 not p1 is p2: edges.append(Line(p1, p2)) edges.sort(key=lambda x: x.len) edge = edges[-1] if edge.dir.cross(triangle.normal).dot(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 = up_vector.cross(edge.dir).normalized() if direction is None: continue direction = direction.mul(max_length) edge_dir = edge.p2.sub(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 = edge.p1.add(edge_dir.mul(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, start.add(direction), return_triangles=True) for index, coll in enumerate(collisions): if (index % 2 == 0) and (not coll[1] is None) \ and (not coll[2] is None) \ and (coll[0].sub(start).dot(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 (len(result) == 0) 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 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 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)