Beispiel #1
0
def get_max_height_triangles(model, cutter, x, y, minz, maxz):
    if model is None:
        return Point(x, y, minz)
    p = Point(x, y, maxz)
    height_max = None
    box_x_min = cutter.get_minx(p)
    box_x_max = cutter.get_maxx(p)
    box_y_min = cutter.get_miny(p)
    box_y_max = cutter.get_maxy(p)
    box_z_min = minz
    box_z_max = maxz
    triangles = model.triangles(box_x_min, box_y_min, box_z_min, box_x_max,
                                box_y_max, box_z_max)
    for t in triangles:
        cut = cutter.drop(t, start=p)
        if cut and ((height_max is None) or (cut.z > height_max)):
            height_max = cut.z
    # don't do a complete boundary check for the height
    # this avoids zero-cuts for models that exceed the bounding box height
    if (height_max is None) or (height_max < minz + epsilon):
        height_max = minz
    if height_max > maxz + epsilon:
        return None
    else:
        return Point(x, y, height_max)
Beispiel #2
0
 def get_line(i1, i2):
     a = list(coords[i1 % 4])
     b = list(coords[i2 % 4])
     # the contour points of the model will always be at level zero
     a[2] = self.z_level
     b[2] = self.z_level
     return Line(Point(*a), Point(*b))
Beispiel #3
0
 def calculate_point_height(self, x, y, func):
     point = 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 = Point(x, y, self.outer.minz)
     line_distances = []
     for line in self.lines:
         cross_product = line.dir.cross(point.sub(line.p1))
         if cross_product.z > 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 = point.sub(p)
                 dist = direction.norm
                 line_distances.append(dist)
         elif cross_product.z == 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])
Beispiel #4
0
 def render(self,
            text,
            origin=None,
            skew=0,
            line_spacing=1.0,
            pitch=1.0,
            align=None):
     result = ContourModel()
     if origin is None:
         origin = Point(0, 0, 0)
     if align is None:
         align = TEXT_ALIGN_LEFT
     base = origin
     letter_spacing = self.letterspacing * pitch
     word_spacing = self.wordspacing * pitch
     line_factor = self.default_linespacing * self.linespacingfactor \
             * line_spacing
     for line in text.splitlines():
         current_line = ContourModel()
         line_height = self.default_height
         for character in line:
             if character == " ":
                 base = base.add(Point(word_spacing, 0, 0))
             elif character in self.letters.keys():
                 charset_letter = self.letters[character]
                 new_model = ContourModel()
                 for line in charset_letter.get_positioned_lines(base,
                                                                 skew=skew):
                     new_model.append(line, allow_reverse=True)
                 for polygon in new_model.get_polygons():
                     # add polygons instead of lines -> more efficient
                     current_line.append(polygon)
                 # update line height
                 line_height = max(line_height, charset_letter.maxy())
                 # shift the base position
                 base = base.add(
                     Point(charset_letter.maxx() + letter_spacing, 0, 0))
             else:
                 # unknown character - add a small whitespace
                 base = base.add(Point(letter_spacing, 0, 0))
         # go to the next line
         base = Point(origin.x, base.y - line_height * line_factor,
                      origin.z)
         if not current_line.maxx is None:
             if align == TEXT_ALIGN_CENTER:
                 current_line.shift(-current_line.maxx / 2, 0, 0)
             elif align == TEXT_ALIGN_RIGHT:
                 current_line.shift(-current_line.maxx, 0, 0)
             else:
                 # left align
                 if current_line.minx != 0:
                     current_line.shift(-current_line.minx, 0, 0)
         for polygon in current_line.get_polygons():
             result.append(polygon)
     # the text should be just above the x axis
     if result.miny:
         # don't shift, if result.miny is None (e.g.: no content) or zero
         result.shift(0, -result.miny, 0)
     return result
Beispiel #5
0
def get_lines_layer(lines, z, last_z=None, step_width=None,
        milling_style=MILLING_STYLE_CONVENTIONAL):
    get_proj_point = lambda proj_point: Point(proj_point.x, proj_point.y, z)
    projected_lines = []
    for line in lines:
        if (not last_z is 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.z) / (line.p2.z - line.p1.z)
            plane_point = line.p1.add(line.vector.mul(factor))
            if line.p1.z < 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(Point(0, 0, last_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 > 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 = line.p1.add(line.dir.mul(step))
                points.append(next_point)
        yield points
Beispiel #6
0
def intersect_torus_point(center, axis, majorradius, minorradius,
                          majorradiussq, minorradiussq, direction, point):
    dist = 0
    if (direction.x == 0) and (direction.y == 0):
        # drop
        minlsq = (majorradius - minorradius)**2
        maxlsq = (majorradius + minorradius)**2
        l_sq = (point.x - center.x)**2 + (point.y - center.y)**2
        if (l_sq < minlsq + epsilon) or (l_sq > maxlsq - epsilon):
            return (None, None, INFINITE)
        l = sqrt(l_sq)
        z_sq = minorradiussq - (majorradius - l)**2
        if z_sq < 0:
            return (None, None, INFINITE)
        z = sqrt(z_sq)
        ccp = Point(point.x, point.y, center.z - z)
        dist = ccp.z - point.z
    elif direction.z == 0:
        # push
        z = point.z - center.z
        if abs(z) > minorradius - epsilon:
            return (None, None, INFINITE)
        l = majorradius + sqrt(minorradiussq - z * z)
        n = axis.cross(direction)
        d = n.dot(point) - n.dot(center)
        if abs(d) > l - epsilon:
            return (None, None, INFINITE)
        a = sqrt(l * l - d * d)
        ccp = center.add(n.mul(d).add(direction.mul(a)))
        ccp.z = point.z
        dist = point.sub(ccp).dot(direction)
    else:
        # general case
        x = point.sub(center)
        v = direction.mul(-1)
        x_x = x.dot(x)
        x_v = x.dot(v)
        x1 = Point(x.x, x.y, 0)
        v1 = Point(v.x, v.y, 0)
        x1_x1 = x1.dot(x1)
        x1_v1 = x1.dot(v1)
        v1_v1 = v1.dot(v1)
        R2 = majorradiussq
        r2 = minorradiussq
        a = 1.0
        b = 4 * x_v
        c = 2 * (x_x + 2 * x_v**2 + (R2 - r2) - 2 * R2 * v1_v1)
        d = 4 * (x_x * x_v + x_v * (R2 - r2) - 2 * R2 * x1_v1)
        e = (x_x)**2 + 2 * x_x * (R2 - r2) + (R2 - r2)**2 - 4 * R2 * x1_x1
        r = poly4_roots(a, b, c, d, e)
        if not r:
            return (None, None, INFINITE)
        else:
            l = min(r)
        ccp = point.add(direction.mul(-l))
        dist = l
    return (ccp, point, dist)
Beispiel #7
0
 def _get_axes_vectors(self):
     """calculate the model vectors along the screen's x and y axes"""
     # The "up" vector defines, in what proportion each axis of the model is
     # in line with the screen's y axis.
     v_up = self.view["up"]
     factors_y = (number(v_up[0]), number(v_up[1]), number(v_up[2]))
     # Calculate the proportion of each model axis according to the x axis of
     # the screen.
     distv = self.view["distance"]
     distv = Point(distv[0], distv[1], distv[2]).normalized()
     factors_x = distv.cross(Point(v_up[0], v_up[1], v_up[2])).normalized()
     factors_x = (factors_x.x, factors_x.y, factors_x.z)
     return (factors_x, factors_y)
Beispiel #8
0
 def _get_axes_vectors(self):
     """calculate the model vectors along the screen's x and y axes"""
     # The "up" vector defines, in what proportion each axis of the model is
     # in line with the screen's y axis.
     v_up = self.view["up"]
     factors_y = (number(v_up[0]), number(v_up[1]), number(v_up[2]))
     # Calculate the proportion of each model axis according to the x axis of
     # the screen.
     distv = self.view["distance"]
     distv = Point(distv[0], distv[1], distv[2]).normalized()
     factors_x = distv.cross(Point(v_up[0], v_up[1], v_up[2])).normalized()
     factors_x = (factors_x.x, factors_x.y, factors_x.z)
     return (factors_x, factors_y)
Beispiel #9
0
 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)
Beispiel #10
0
def _add_aligned_cuboid_to_model(minx, maxx, miny, maxy, minz, maxz):
    points = (
            Point(minx, miny, minz),
            Point(maxx, miny, minz),
            Point(maxx, maxy, minz),
            Point(minx, maxy, minz),
            Point(minx, miny, maxz),
            Point(maxx, miny, maxz),
            Point(maxx, maxy, maxz),
            Point(minx, maxy, maxz))
    triangles = []
    # lower face
    triangles.extend(_get_triangles_for_face(
            (points[0], points[1], points[2], points[3])))
    # upper face
    triangles.extend(_get_triangles_for_face(
            (points[7], points[6], points[5], points[4])))
    # front face
    triangles.extend(_get_triangles_for_face(
            (points[0], points[4], points[5], points[1])))
    # back face
    triangles.extend(_get_triangles_for_face(
            (points[2], points[6], points[7], points[3])))
    # right face
    triangles.extend(_get_triangles_for_face(
            (points[1], points[5], points[6], points[2])))
    # left face
    triangles.extend(_get_triangles_for_face(
            (points[3], points[7], points[4], points[0])))
    # add all triangles to the model
    model = Model()
    for t in triangles:
        model.append(t)
    return model
Beispiel #11
0
def get_test_model():
    points = []
    points.append(Point(-2, 1, 4))
    points.append(Point(2, 1, 4))
    points.append(Point(0, -2, 4))
    points.append(Point(-5, 2, 2))
    points.append(Point(-1, 3, 2))
    points.append(Point(5, 2, 2))
    points.append(Point(4, -1, 2))
    points.append(Point(2, -4, 2))
    points.append(Point(-2, -4, 2))
    points.append(Point(-3, -2, 2))

    lines = []
    lines.append(Line(points[0], points[1]))
    lines.append(Line(points[1], points[2]))
    lines.append(Line(points[2], points[0]))
    lines.append(Line(points[0], points[3]))
    lines.append(Line(points[3], points[4]))
    lines.append(Line(points[4], points[0]))
    lines.append(Line(points[4], points[1]))
    lines.append(Line(points[4], points[5]))
    lines.append(Line(points[5], points[1]))
    lines.append(Line(points[5], points[6]))
    lines.append(Line(points[6], points[1]))
    lines.append(Line(points[6], points[2]))
    lines.append(Line(points[6], points[7]))
    lines.append(Line(points[7], points[2]))
    lines.append(Line(points[7], points[8]))
    lines.append(Line(points[8], points[2]))
    lines.append(Line(points[8], points[9]))
    lines.append(Line(points[9], points[2]))
    lines.append(Line(points[9], points[0]))
    lines.append(Line(points[9], points[3]))

    model = Model()
    for p1, p2, p3, l1, l2, l3 in (
            (0, 1, 2, 0, 1, 2),
            (0, 3, 4, 3, 4, 5),
            (0, 4, 1, 5, 6, 0),
            (1, 4, 5, 6, 7, 8),
            (1, 5, 6, 8, 9, 10),
            (1, 6, 2, 10, 11, 1),
            (2, 6, 7, 11, 12, 13),
            (2, 7, 8, 13, 14, 15),
            (2, 8, 9, 15, 16, 17),
            (2, 9, 0, 17, 18, 2),
            (0, 9, 3, 18, 19, 3)):
        model.append(Triangle(points[p1], points[p2], points[p3]))
    return model
Beispiel #12
0
    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
Beispiel #13
0
 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), Point(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
Beispiel #14
0
def get_fixed_grid_line(start, end, line_pos, z, step_width=None,
        grid_direction=GRID_DIRECTION_X):
    if step_width is None:
        # useful for PushCutter operations
        steps = (start, end)
    elif isiterable(step_width):
        steps = step_width
    else:
        steps = floatrange(start, end, inc=step_width)
    if grid_direction == GRID_DIRECTION_X:
        get_point = lambda pos: Point(pos, line_pos, z)
    else:
        get_point = lambda pos: Point(line_pos, pos, z)
    for pos in steps:
        yield get_point(pos)
Beispiel #15
0
 def auto_adjust_distance(self):
     s = self.settings
     v = self.view
     # adjust the distance to get a view of the whole object
     dimx = s.get("maxx") - s.get("minx")
     dimy = s.get("maxy") - s.get("miny")
     dimz = s.get("maxz") - s.get("minz")
     max_dim = max(max(dimx, dimy), dimz)
     distv = Point(v["distance"][0], v["distance"][1], v["distance"][2]).normalized()
     # The multiplier "1.25" is based on experiments. 1.414 (sqrt(2)) should
     # be roughly sufficient for showing the diagonal of any model.
     distv = distv.mul((max_dim * 1.25) / number(math.sin(v["fovy"] / 2)))
     self.view["distance"] = (distv.x, distv.y, distv.z)
     # Adjust the "far" distance for the camera to make sure, that huge
     # models (e.g. x=1000) are still visible.
     self.view["zfar"] = 100 * max_dim
Beispiel #16
0
 def parse_vertex(self):
     start_line = self.line_number
     point = [None, None, 0]
     color = None
     key, value = self._read_key_value()
     while (not key is None) and (key != self.KEYS["MARKER"]):
         if key == self.KEYS["P1_X"]:
             point[0] = value
         elif key == self.KEYS["P1_Y"]:
             point[1] = value
         elif key == self.KEYS["P1_Z"]:
             point[2] = value
         elif key == self.KEYS["COLOR"]:
             color = value
         else:
             pass
         key, value = self._read_key_value()
     end_line = self.line_number
     if not key is None:
         self._push_on_stack(key, value)
     if self._color_as_height and (not color is None):
         # use the color code as the z coordinate
         point[2] = float(color) / 255
     if None in point:
         log.warn("DXFImporter: Missing attribute of VERTEX item" + \
                 "between line %d and %d" % (start_line, end_line))
     else:
         self._open_sequence_items.append(
             Point(point[0], point[1], point[2]))
Beispiel #17
0
 def shrink_box_avoiding_collision(self, box_index, collision_func):
     box = self.boxes[box_index]
     aabb = box.getAABB()
     end_height, start_height = aabb[-2:]
     height_half = (start_height - end_height) / 2.0
     x_pos = box.position.x
     y_pos = box.position.y
     new_z = end_height
     box.setPosition((x_pos, y_pos, end_height - height_half))
     loops_left = 12
     upper_limit = start_height
     lower_limit = end_height
     if collision_func(box):
         # the cutter goes down to zero (end_height) - we can skip the rest
         loops_left = 0
     while loops_left > 0:
         new_z = (upper_limit + lower_limit) / 2.0
         box.setPosition((x_pos, y_pos, new_z - height_half))
         if collision_func(box):
             upper_limit = new_z
         else:
             lower_limit = new_z
         loops_left -= 1
     del self.boxes[box_index]
     # The height should never be zero - otherwise ODE will throw a
     # "bNormalizationResult" assertion.
     new_height = max(new_z - end_height, 0.1)
     z_pos = new_z - new_height / 2.0
     new_box = ode.GeomBox(self.space, (aabb[1] - aabb[0], aabb[3] - aabb[2],
             new_height))
     new_box.position = Point(x_pos, y_pos, z_pos)
     new_box.setBody(box.getBody())
     new_box.setPosition((x_pos, y_pos, z_pos))
     self.boxes.insert(box_index, new_box)
Beispiel #18
0
def get_max_height_ode(physics, x, y, minz, maxz):
    # Take a small float inaccuracy for the upper limit into account.
    # Otherwise an upper bound at maxz of the model will not return
    # a valid surface. That's why we add 'epsilon'.
    low, high = minz, maxz + epsilon
    trip_start = 20
    safe_z = None
    # check if the full step-down would be ok
    physics.set_drill_position((x, y, minz))
    if physics.check_collision():
        # there is an object between z1 and z0 - we need more=None loops
        trips = trip_start
    else:
        # No need for further collision detection - we can go down the whole
        # range z1..z0.
        trips = 0
        safe_z = minz
    while trips > 0:
        current_z = (low + high) / 2
        physics.set_drill_position((x, y, current_z))
        if physics.check_collision():
            low = current_z
        else:
            high = current_z
            safe_z = current_z
        trips -= 1
    if safe_z is None:
        # skip this point (by going up to safety height)
        return None
    else:
        return Point(x, y, safe_z)
Beispiel #19
0
 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(Point(0, 0, z_level), Vector(0, 0, 1))
                 self.log.info("Projecting 3D model at level z=%g" %
                               plane.p.z)
                 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.z)
                 break
         progress.update_multiple()
     progress.finish()
Beispiel #20
0
 def extend_shape(diff_x, diff_y, diff_z):
     reset_shape()
     # see http://mathworld.wolfram.com/RotationMatrix.html
     hypotenuse = sqrt(diff_x * diff_x + diff_y * diff_y)
     # Some paths contain two identical points (e.g. a "touch" of the
     # PushCutter). We don't need any extension for these.
     if hypotenuse == 0:
         return
     cosinus = diff_x / hypotenuse
     sinus = diff_y / hypotenuse
     # create the cyclinder at the other end
     geom_end_transform = ode.GeomTransform(geom.space)
     geom_end_transform.setBody(geom.getBody())
     geom_end = ode.GeomCapsule(None, radius, self.height)
     geom_end.setPosition((diff_x, diff_y, diff_z + center_height))
     geom_end_transform.setGeom(geom_end)
     # create the block that connects the two cylinders at the end
     rot_matrix_box = (cosinus, sinus, 0.0, -sinus, cosinus, 0.0,
                       0.0, 0.0, 1.0)
     geom_connect_transform = ode.GeomTransform(geom.space)
     geom_connect_transform.setBody(geom.getBody())
     geom_connect = ode_physics.get_parallelepiped_geom(
         (Point(-hypotenuse / 2, radius, -diff_z / 2),
          Point(hypotenuse / 2, radius, diff_z / 2),
          Point(hypotenuse / 2, -radius, diff_z / 2),
          Point(-hypotenuse / 2, -radius, -diff_z / 2)),
         (Point(-hypotenuse / 2, radius, self.height - diff_z / 2),
          Point(hypotenuse / 2, radius, self.height + diff_z / 2),
          Point(hypotenuse / 2, -radius, self.height + diff_z / 2),
          Point(-hypotenuse / 2, -radius,
                self.height - diff_z / 2)))
     geom_connect.setRotation(rot_matrix_box)
     geom_connect.setPosition((hypotenuse / 2, 0, radius))
     geom_connect_transform.setGeom(geom_connect)
     # Create a cylinder, that connects the two half spheres at the
     # lower end of both drills.
     geom_cyl_transform = ode.GeomTransform(geom.space)
     geom_cyl_transform.setBody(geom.getBody())
     hypotenuse_3d = Matrix.get_length((diff_x, diff_y, diff_z))
     geom_cyl = ode.GeomCylinder(None, radius, hypotenuse_3d)
     # rotate cylinder vector
     cyl_original_vector = (0, 0, hypotenuse_3d)
     cyl_destination_vector = (diff_x, diff_y, diff_z)
     matrix = Matrix.get_rotation_matrix_from_to(
         cyl_original_vector, cyl_destination_vector)
     flat_matrix = matrix[0] + matrix[1] + matrix[2]
     geom_cyl.setRotation(flat_matrix)
     # The rotation is around the center - thus we ignore negative
     # diff values.
     geom_cyl.setPosition((abs(diff_x / 2), abs(diff_y / 2),
                           radius - additional_distance))
     geom_cyl_transform.setGeom(geom_cyl)
     # sort the geoms in order of collision probability
     geom.children.extend([
         geom_connect_transform, geom_cyl_transform,
         geom_end_transform
     ])
Beispiel #21
0
    def add_cutter(self, c):
        cx = c.location.x
        cy = c.location.y
        rsq = c.radiussq
        minx = int((c.minx - self.minx) / (self.maxx - self.minx) * self.xres) \
                - 1
        maxx = int((c.maxx - self.minx) / (self.maxx - self.minx) * self.xres) \
                + 1
        miny = int((c.miny - self.miny) / (self.maxy - self.miny) * self.yres) \
                - 1
        maxy = int((c.maxy - self.miny) / (self.maxy - self.miny) * self.yres) \
                + 1
        if minx < 0:
            minx = 0
        if maxx < 0:
            maxx = 0
        if minx > self.xres - 1:
            minx = self.xres - 1
        if maxx > self.xres - 1:
            maxx = self.xres - 1
        if miny < 0:
            miny = 0
        if maxy < 0:
            maxy = 0
        if maxy > self.yres - 1:
            maxy = self.yres - 1
        if miny > self.yres - 1:
            miny = self.yres - 1
        p = Point(0, 0, 0)
        zaxis = Point(0, 0, -1)

        for y in range(miny, maxy):
            p.y = py = self.y[y]
            for x in range(minx, maxx):
                p.x = px = self.x[x]
                if (px - cx) * (px - cx) + (py - cy) * (py - cy) \
                        <= rsq + EPSILON:
                    (cl, ccp, cp, l) = c.intersect_point(zaxis, p)
                    if ccp:
                        pz = l
                        if pz < self.buf[y][x].z:
                            self.buf[y][x].z = pz
                            self.buf[y + 0][x + 0].changed = True
                            self.buf[y + 0][x + 1].changed = True
                            self.buf[y + 1][x + 0].changed = True
                            self.buf[y + 1][x + 1].changed = True
                            self.changed = True
Beispiel #22
0
 def discretise_line(
     self, line
 ):  # TODO: OPTIMISER !!!!! (pas de boucle, un seul cas avec 2 passages, calcul de l'intervale sans list.index)
     # on discrétise les deux extrémités du segment
     c1, c2 = self.discretiser_point(ligne.p1), self.discretiser_point(
         ligne.p2)
     c1.lignes.append(ligne)
     c2.lignes.append(ligne)
     if ligne.p1.x == ligne.p2.x and ligne.p1.x in self.rangex:  # la ligne appartient au pavage vertical
         # alors on va ajouter toutes les paires de cases qu'elle rencontre
         x = self.rangex.index(ligne.p1.x)
         if c1.y > c2.y: c1, c2 = c2, c1
         for y in range(c1.y, c2.y + 1):
             self.get_case(c1.z, x, y).lignes.append(ligne)
     elif ligne.p1.y == ligne.p2.y and ligne.p1.y in self.rangey:  # la ligne appartient au pavage horizontal
         y = self.rangey.index(ligne.p1.y)
         if c1.x > c2.x: c1, c2 = c2, c1
         for x in range(c1.x, c2.x + 1):
             self.get_case(c1.z, x, y).lignes.append(ligne)
     else:  # sinon la ligne est simplement verticale, horizontale ou oblique
         # alors on va discrétiser toutes les cases traversées
         # d'abord les intersections verticales
         ordx = 1
         if c1.x > c2.x: c1, c2 = c2, c1  # pour itérer dans le bon sens
         for x in self.rangex[c1.x + 1:c2.x]:
             # pour cela on calcule l'intersection de la ligne avec la grille des X
             sec, d = ligne.get_intersection(Line(Point(x, self.miny, ligne.p1.z), \
                                             Point(x, self.maxy, ligne.p1.z)))
             if sec is None:  # il n'y a pas intersection : les deux lignes sont parallèles et distinctes
                 break  # la ligne sera traitée par l'itération verticale
             # puis on trouve à quelle colonne l'intersection appartient
             dsec = self.Case.get_emplacement(0, sec.y, 0)
             # et on discrétise alors la case correspondante
             self.get_case(c1.z, c1.x + ordx, dsec[1]).lignes.append(ligne)
             # le [1] correspond à la composante Y de la coordonnée
             ordx += 1
         # puis celles horizontales de la même manière
         ordy = 1
         if c1.y > c2.y: c1, c2 = c2, c1
         for y in self.rangey[c1.y + 1:c2.y]:
             sec, d = ligne.get_intersection(Line(Point(self.minx, y, ligne.p1.z), \
                                             Point(self.maxx, y, ligne.p1.z)))
             if sec is None:
                 break  # la ligne est horizontale, et donc déjà traitée
             dsec = self.Case.get_emplacement(sec.x, 0, 0)
             self.get_case(c1.z, dsec[0], c1.y + ordy).lignes.append(ligne)
             ordy += 1
Beispiel #23
0
def intersect_circle_plane(center, radius, direction, triangle):
    # let n be the normal to the plane
    n = triangle.normal
    if n.dot(direction) == 0:
        return (None, None, INFINITE)
    # project onto z=0
    n2 = Point(n.x, n.y, 0)
    if n2.norm == 0:
        (cp, d) = triangle.plane.intersect_point(direction, center)
        ccp = cp.sub(direction.mul(d))
        return (ccp, cp, d)
    n2 = n2.normalized()
    # the cutter contact point is on the circle, where the surface normal is n
    ccp = center.add(n2.mul(-radius))
    # intersect the plane with a line through the contact point
    (cp, d) = triangle.plane.intersect_point(direction, ccp)
    return (ccp, cp, d)
Beispiel #24
0
 def __init__(self, reverse=False):
     super(ContourCutter, self).__init__()
     self.curr_path = None
     self.scanline = None
     self.polygon_extractor = None
     self.points = []
     self.reverse = reverse
     self.__forward = Point(1, 1, 0)
Beispiel #25
0
 def auto_adjust_distance(self):
     s = self.core
     v = self.view
     # adjust the distance to get a view of the whole object
     low_high = zip(*self._get_low_high_dims())
     if (None, None) in low_high:
         return
     max_dim = max([high - low for low, high in low_high])
     distv = Point(v["distance"][0], v["distance"][1],
             v["distance"][2]).normalized()
     # The multiplier "1.25" is based on experiments. 1.414 (sqrt(2)) should
     # be roughly sufficient for showing the diagonal of any model.
     distv = distv.mul((max_dim * 1.25) / number(math.sin(v["fovy"] / 2)))
     self.view["distance"] = (distv.x, distv.y, distv.z)
     # Adjust the "far" distance for the camera to make sure, that huge
     # models (e.g. x=1000) are still visible.
     self.view["zfar"] = 100 * max_dim
Beispiel #26
0
def intersect_circle_plane(center, radius, direction, triangle):
    # let n be the normal to the plane
    n = triangle.normal
    if n.dot(direction) == 0:
        return (None, None, INFINITE)
    # project onto z=0
    n2 = Point(n.x, n.y, 0)
    if n2.norm == 0:
        (cp, d) = triangle.plane.intersect_point(direction, center)
        ccp = cp.sub(direction.mul(d))
        return (ccp, cp, d)
    n2 = n2.normalized()
    # the cutter contact point is on the circle, where the surface normal is n
    ccp = center.add(n2.mul(-radius))
    # intersect the plane with a line through the contact point
    (cp, d) = triangle.plane.intersect_point(direction, ccp)
    return (ccp, cp, d)
Beispiel #27
0
 def auto_adjust_distance(self):
     s = self.core
     v = self.view
     # adjust the distance to get a view of the whole object
     low_high = zip(*self._get_low_high_dims())
     if (None, None) in low_high:
         return
     max_dim = max([high - low for low, high in low_high])
     distv = Point(v["distance"][0], v["distance"][1],
                   v["distance"][2]).normalized()
     # The multiplier "1.25" is based on experiments. 1.414 (sqrt(2)) should
     # be roughly sufficient for showing the diagonal of any model.
     distv = distv.mul((max_dim * 1.25) / number(math.sin(v["fovy"] / 2)))
     self.view["distance"] = (distv.x, distv.y, distv.z)
     # Adjust the "far" distance for the camera to make sure, that huge
     # models (e.g. x=1000) are still visible.
     self.view["zfar"] = 100 * max_dim
Beispiel #28
0
 def auto_adjust_distance(self):
     s = self.settings
     v = self.view
     # adjust the distance to get a view of the whole object
     dimx = s.get("maxx") - s.get("minx")
     dimy = s.get("maxy") - s.get("miny")
     dimz = s.get("maxz") - s.get("minz")
     max_dim = max(max(dimx, dimy), dimz)
     distv = Point(v["distance"][0], v["distance"][1],
             v["distance"][2]).normalized()
     # The multiplier "1.25" is based on experiments. 1.414 (sqrt(2)) should
     # be roughly sufficient for showing the diagonal of any model.
     distv = distv.mul((max_dim * 1.25) / number(math.sin(v["fovy"] / 2)))
     self.view["distance"] = (distv.x, distv.y, distv.z)
     # Adjust the "far" distance for the camera to make sure, that huge
     # models (e.g. x=1000) are still visible.
     self.view["zfar"] = 100 * max_dim
Beispiel #29
0
def get_free_paths_ode(physics, p1, p2, depth=8):
    """ Recursive function for splitting a line (usually along x or y) into
    small pieces to gather connected paths for the PushCutter.
    Strategy: check if the whole line is free (without collisions). Do a
    recursive call (for the first and second half), if there was a
    collision.

    Usually either minx/maxx or miny/maxy should be equal, unless you want
    to do a diagonal cut.
    @param minx: lower limit of x
    @type minx: float
    @param maxx: upper limit of x; should equal minx for a cut along the x axis
    @type maxx: float
    @param miny: lower limit of y
    @type miny: float
    @param maxy: upper limit of y; should equal miny for a cut along the y axis
    @type maxy: float
    @param z: the fixed z level
    @type z: float
    @param depth: number of splits to be calculated via recursive calls; the
        accuracy can be calculated as (maxx-minx)/(2^depth)
    @type depth: int
    @returns: a list of points that describe the tool path of the PushCutter;
        each pair of points defines a collision-free path
    @rtype: list(pycam.Geometry.Point.Point)
    """
    points = []
    # "resize" the drill along the while x/y range and check for a collision
    physics.extend_drill(p2.x - p1.x, p2.y - p1.y, p2.z - p1.z)
    physics.set_drill_position((p1.x, p1.y, p1.z))
    if physics.check_collision():
        # collision detected
        if depth > 0:
            middle_x = (p1.x + p2.x) / 2
            middle_y = (p1.y + p2.y) / 2
            middle_z = (p1.z + p2.z) / 2
            p_middle = Point(middle_x, middle_y, middle_z)
            group1 = get_free_paths_ode(physics, p1, p_middle, depth - 1)
            group2 = get_free_paths_ode(physics, p_middle, p2, depth - 1)
            if group1 and group2 and (group1[-1] == group2[0]):
                # The last pair of the first group ends where the first pair of
                # the second group starts.
                # We will combine them into a single pair.
                points.extend(group1[:-1])
                points.extend(group2[1:])
            else:
                # the two groups are not connected - just add both
                points.extend(group1)
                points.extend(group2)
        else:
            # no points to be added
            pass
    else:
        # no collision - the line is free
        points.append(p1)
        points.append(p2)
    physics.reset_drill()
    return points
Beispiel #30
0
def get_rotation_matrix_from_to(v_orig, v_dest):
    """ calculate the rotation matrix used to transform one vector into another
    
    The result is useful for modifying the rotation matrix of a 3d object.
    See the "extend_shape" code in each of the cutter classes (for ODE).
    The simplest example is the following with the original vector pointing
    along the x axis, while the destination vectors goes along the y axis:
        get_rotation_matrix((1, 0, 0), (0, 1, 0))
    Basically this describes a rotation around the z axis by 90 degrees.
    The resulting 3x3 matrix (tuple of tuple of floats) can be multiplied with
    any other vector to rotate it in the same way around the z axis.
    @type v_orig: tuple(float) | list(float) | pycam.Geometry.Point
    @value v_orig: the original 3d vector
    @type v_dest: tuple(float) | list(float) | pycam.Geometry.Point
    @value v_dest: the destination 3d vector
    @rtype: tuple(tuple(float))
    @return: the roation matrix (3x3)
    """
    if isinstance(v_orig, Point):
        v_orig = (v_orig.x, v_orig.y, v_orig.z)
    if isinstance(v_dest, Point):
        v_dest = (v_dest.x, v_dest.y, v_dest.z)
    v_orig_length = get_length(v_orig)
    v_dest_length = get_length(v_dest)
    cross_product = get_length(get_cross_product(v_orig, v_dest))
    try:
        arcsin = cross_product / (v_orig_length * v_dest_length)
    except ZeroDivisionError:
        return None
    # prevent float inaccuracies to crash the calculation (within limits)
    if 1 < arcsin < 1 + epsilon:
        arcsin = 1.0
    elif -1 - epsilon < arcsin < -1:
        arcsin = -1.0
    rot_angle = math.asin(arcsin)
    # calculate the rotation axis
    # The rotation axis is equal to the cross product of the original and
    # destination vectors.
    rot_axis = Point(v_orig[1] * v_dest[2] - v_orig[2] * v_dest[1],
                     v_orig[2] * v_dest[0] - v_orig[0] * v_dest[2],
                     v_orig[0] * v_dest[1] -
                     v_orig[1] * v_dest[0]).normalized()
    if not rot_axis:
        return None
    # get the rotation matrix
    # see http://www.fastgraph.com/makegames/3drotation/
    c = math.cos(rot_angle)
    s = math.sin(rot_angle)
    t = 1 - c
    return ((t * rot_axis.x * rot_axis.x + c,
             t * rot_axis.x * rot_axis.y - s * rot_axis.z,
             t * rot_axis.x * rot_axis.z + s * rot_axis.y),
            (t * rot_axis.x * rot_axis.y + s * rot_axis.z,
             t * rot_axis.y * rot_axis.y + c,
             t * rot_axis.y * rot_axis.z - s * rot_axis.x),
            (t * rot_axis.x * rot_axis.z - s * rot_axis.y,
             t * rot_axis.y * rot_axis.z + s * rot_axis.x,
             t * rot_axis.z * rot_axis.z + c))
Beispiel #31
0
def _get_edge_bridges(polygon, z_plane, min_bridges, average_distance,
        avoid_distance):
    def is_near_list(point_list, point, distance):
        for p in point_list:
            if p.sub(point).norm <= distance:
                return True
        return False
    lines = polygon.get_lines()
    poly_lengths = polygon.get_lengths()
    num_of_bridges = max(min_bridges,
            int(round(sum(poly_lengths) / average_distance)))
    real_average_distance = sum(poly_lengths) / num_of_bridges
    max_line_index = poly_lengths.index(max(poly_lengths))
    positions = []
    current_line_index = max_line_index
    distance_processed = poly_lengths[current_line_index] / 2
    positions.append(current_line_index)
    while len(positions) < num_of_bridges:
        current_line_index += 1
        current_line_index %= len(poly_lengths)
        # skip lines that are not at least twice as long as the grid width
        while (distance_processed + poly_lengths[current_line_index] \
                < real_average_distance):
            distance_processed += poly_lengths[current_line_index]
            current_line_index += 1
            current_line_index %= len(poly_lengths)
        positions.append(current_line_index)
        distance_processed += poly_lengths[current_line_index]
        distance_processed %= real_average_distance
    result = []
    bridge_positions = []
    for line_index in positions:
        position = polygon.get_middle_of_line(line_index)
        # skip bridges that are close to another existing bridge
        if is_near_list(bridge_positions, position, avoid_distance):
            line = polygon.get_lines()[line_index]
            # calculate two alternative points on the same line
            position1 = position.add(line.p1).div(2)
            position2 = position.add(line.p2).div(2)
            if is_near_list(bridge_positions, position1, avoid_distance):
                if is_near_list(bridge_positions, position2,
                        avoid_distance):
                    # no valid alternative - we skip this bridge
                    continue
                else:
                    # position2 is OK
                    position = position2
            else:
                # position1 is OK
                position = position1
        # append the original position (ignoring z_plane)
        bridge_positions.append(position)
        # move the point to z_plane
        position = Point(position.x, position.y, z_plane)
        bridge_dir = lines[line_index].dir.cross(
                polygon.plane.n).normalized()
        result.append((position, bridge_dir))
    return result
Beispiel #32
0
 def parse_line(self):
     start_line = self.line_number
     # the z-level defaults to zero (for 2D models)
     p1 = [None, None, 0]
     p2 = [None, None, 0]
     color = None
     key, value = self._read_key_value()
     while (not key is None) and (key != self.KEYS["MARKER"]):
         if key == self.KEYS["P1_X"]:
             p1[0] = value
         elif key == self.KEYS["P1_Y"]:
             p1[1] = value
         elif key == self.KEYS["P1_Z"]:
             p1[2] = value
         elif key == self.KEYS["P2_X"]:
             p2[0] = value
         elif key == self.KEYS["P2_Y"]:
             p2[1] = value
         elif key == self.KEYS["P2_Z"]:
             p2[2] = value
         elif key == self.KEYS["COLOR"]:
             color = value
         else:
             pass
         key, value = self._read_key_value()
     end_line = self.line_number
     # The last lines were not used - they are just the marker for the next
     # item.
     if not key is None:
         self._push_on_stack(key, value)
     if (None in p1) or (None in p2):
         log.warn("DXFImporter: Incomplete LINE definition between line " \
                 + "%d and %d" % (start_line, end_line))
     else:
         if self._color_as_height and (not color is None):
             # use the color code as the z coordinate
             p1[2] = float(color) / 255
             p2[2] = float(color) / 255
         line = Line(Point(p1[0], p1[1], p1[2]), Point(p2[0], p2[1], p2[2]))
         if line.p1 != line.p2:
             self.lines.append(line)
         else:
             log.warn("DXFImporter: Ignoring zero-length LINE (between " \
                     + "input line %d and %d): %s" % (start_line, end_line,
                     line))
Beispiel #33
0
        def get_box_height_points(x, y):
            """ Get the positions and heights of the the four top corners of a
            height box.

            The result is a tuple of four Points (pycam.Geometry.Point) in the
            following order:
             - left below
             - right below
             - right above
             - left above
            ("above": greater x value; "right": greater y value)
            The height of each corner point is calculated as the average of the
            four neighbouring boxes. Thus a set of 3x3 adjacent boxes is used
            for calculating the heights of the four corners.
            """
            points = []
            # Go through a set of box index combinations (sharing a common
            # corner). The "offsets" tuple is used for indicating the relative
            # position of each corner.
            for offsets, index_list in (
                ((-1, -1), ((x - 1, y - 1), (x, y - 1), (x, y), (x - 1, y))),
                ((+1, -1), ((x, y - 1), (x, y), (x + 1, y), (x + 1, y - 1))),
                ((+1, +1), ((x, y), (x + 1, y), (x + 1, y + 1), (x, y + 1))),
                ((-1, +1), ((x - 1, y), (x, y), (x, y + 1), (x - 1, y + 1)))):
                divisor = 0
                height_sum = 0
                x_positions = []
                y_positions = []
                for ix, iy in index_list:
                    if (0 <= ix < len(height_field)) \
                            and (0 <= iy < len(height_field[ix])):
                        point = height_field[ix][iy]
                        height_sum += point.z
                        x_positions.append(point.x)
                        y_positions.append(point.y)
                        divisor += 1
                # Use the middle between the x positions of two adjacent boxes,
                # _if_ there is a neighbour attached to that corner.
                if (min(x_positions) < height_field[x][y].x) \
                        or (max(x_positions) > height_field[x][y].x):
                    x_value = (min(x_positions) + max(x_positions)) / 2.0
                else:
                    # There is no adjacent box in x direction. Use the step size
                    # to calculate the x value of this edge.
                    x_value = height_field[x][y].x \
                            + offsets[0] * self.x_step_width / 2.0
                # same as above for y instead of x
                if (min(y_positions) < height_field[x][y].y) \
                        or (max(y_positions) > height_field[x][y].y):
                    y_value = (min(y_positions) + max(y_positions)) / 2.0
                else:
                    y_value = height_field[x][y].y \
                            + offsets[1] * self.y_step_width / 2.0
                # Create a Point instance describing the position and the
                # average height.
                points.append(Point(x_value, y_value, height_sum / divisor))
            return points
Beispiel #34
0
def _get_position(minx, maxx, miny, maxy, z, position):
    if position & START_X > 0:
        x = minx
    else:
        x = maxx
    if position & START_Y > 0:
        y = miny
    else:
        y = maxy
    return Point(x, y, z)
Beispiel #35
0
 def add_point(p_array):
     # fill all "None" values with zero
     for index in range(len(p_array)):
         if p_array[index] is None:
             if (index == 0) or (index == 1):
                 log.debug("DXFImporter: weird LWPOLYLINE input " + \
                         "date in line %d: %s" % \
                         (self.line_number, p_array))
             p_array[index] = 0
     points.append(Point(p_array[0], p_array[1], p_array[2]))
Beispiel #36
0
def draw_direction_cone(p1, p2, position=0.5, precision=12, size=0.1):
    # convert p1 and p2 from list/tuple to Point
    if not hasattr(p1, "sub"):
        p1 = Point(*p1)
    if not hasattr(p2, "sub"):
        p2 = Point(*p2)
    distance = p2.sub(p1)
    length = distance.norm
    direction = distance.normalized()
    if direction is None:
        # zero-length line
        return
    cone_length = length * size
    cone_radius = cone_length / 3.0
    # move the cone to the middle of the line
    GL.glTranslatef((p1.x + p2.x) * position,
            (p1.y + p2.y) * position, (p1.z + p2.z) * position)
    # rotate the cone according to the line direction
    # The cross product is a good rotation axis.
    cross = direction.cross(Point(0, 0, -1))
    if cross.norm != 0:
        # The line direction is not in line with the z axis.
        try:
            angle = math.asin(sqrt(direction.x ** 2 + direction.y ** 2))
        except ValueError:
            # invalid angle - just ignore this cone
            return
        # convert from radians to degree
        angle = angle / math.pi * 180
        if direction.z < 0:
            angle = 180 - angle
        GL.glRotatef(angle, cross.x, cross.y, cross.z)
    elif direction.z == -1:
        # The line goes down the z axis - turn it around.
        GL.glRotatef(180, 1, 0, 0)
    else:
        # The line goes up the z axis - nothing to be done.
        pass
    # center the cone
    GL.glTranslatef(0, 0, -cone_length * position)
    # draw the cone
    GLUT.glutSolidCone(cone_radius, cone_length, precision, 1)
Beispiel #37
0
def intersect_torus_point(center, axis, majorradius, minorradius, majorradiussq,
        minorradiussq, direction, point):
    dist = 0
    if (direction.x == 0) and (direction.y == 0):
        # drop
        minlsq = (majorradius - minorradius) ** 2
        maxlsq = (majorradius + minorradius) ** 2
        l_sq = (point.x-center.x) ** 2 + (point.y - center.y) ** 2
        if (l_sq < minlsq + epsilon) or (l_sq > maxlsq - epsilon):
            return (None, None, INFINITE)
        l = sqrt(l_sq)
        z_sq = minorradiussq - (majorradius - l) ** 2
        if z_sq < 0:
            return (None, None, INFINITE)
        z = sqrt(z_sq)
        ccp = Point(point.x, point.y, center.z - z)
        dist = ccp.z - point.z
    elif direction.z == 0:
        # push
        z = point.z - center.z
        if abs(z) > minorradius - epsilon:
            return (None, None, INFINITE)
        l = majorradius + sqrt(minorradiussq - z * z)
        n = axis.cross(direction)
        d = n.dot(point) - n.dot(center)
        if abs(d) > l - epsilon:
            return (None, None, INFINITE)
        a = sqrt(l * l - d * d)
        ccp = center.add(n.mul(d).add(direction.mul(a)))
        ccp.z = point.z
        dist = point.sub(ccp).dot(direction)
    else:
        # general case
        x = point.sub(center)
        v = direction.mul(-1)
        x_x = x.dot(x)
        x_v = x.dot(v)
        x1 = Point(x.x, x.y, 0)
        v1 = Point(v.x, v.y, 0)
        x1_x1 = x1.dot(x1)
        x1_v1 = x1.dot(v1)
        v1_v1 = v1.dot(v1)
        R2 = majorradiussq
        r2 = minorradiussq
        a = 1.0
        b = 4 * x_v
        c = 2 * (x_x + 2 * x_v ** 2 + (R2 - r2) - 2 * R2 * v1_v1)
        d = 4 * (x_x * x_v + x_v * (R2 - r2) - 2 * R2 * x1_v1)
        e = (x_x) ** 2 + 2 * x_x * (R2 - r2) + (R2 - r2) ** 2 - 4 * R2 * x1_x1
        r = poly4_roots(a, b, c, d, e)
        if not r:
            return (None, None, INFINITE)
        else:
            l = min(r)
        ccp = point.add(direction.mul(-l))
        dist = l
    return (ccp, point, dist)
Beispiel #38
0
 def get_moves(self, safety_height, max_movement=None):
     class MoveContainer(object):
         def __init__(self, max_movement):
             self.max_movement = max_movement
             self.moved_distance = 0
             self.moves = []
             self.last_pos = None
             if max_movement is None:
                 self.append = self.append_without_movement_limit
             else:
                 self.append = self.append_with_movement_limit
         def append_with_movement_limit(self, new_position, rapid):
             if self.last_pos is None:
                 # first move with unknown start position - ignore it
                 self.moves.append((new_position, rapid))
                 self.last_pos = new_position
                 return True
             else:
                 distance = new_position.sub(self.last_pos).norm
                 if self.moved_distance + distance > self.max_movement:
                     partial = (self.max_movement - self.moved_distance) / \
                             distance
                     partial_dest = self.last_pos.add(new_position.sub(
                             self.last_pos).mul(partial))
                     self.moves.append((partial_dest, rapid))
                     self.last_pos = partial_dest
                     # we are finished
                     return False
                 else:
                     self.moves.append((new_position, rapid))
                     self.moved_distance += distance
                     self.last_pos = new_position
                     return True
         def append_without_movement_limit(self, new_position, rapid):
             self.moves.append((new_position, rapid))
             return True
     p_last = None
     max_safe_distance = 2 * self.toolpath_settings.get_tool().radius \
             + epsilon
     result = MoveContainer(max_movement)
     for path in self.get_paths():
         if not path:
             # ignore empty paths
             continue
         p_next = path.points[0]
         if p_last is None:
             p_last = Point(p_next.x, p_next.y, safety_height)
             if not result.append(p_last, True):
                 return result.moves
         if ((abs(p_last.x - p_next.x) > epsilon) \
                 or (abs(p_last.y - p_next.y) > epsilon)):
             # Draw the connection between the last and the next path.
             # Respect the safety height.
             if (abs(p_last.z - p_next.z) > epsilon) \
                     or (p_last.sub(p_next).norm > max_safe_distance):
                 # The distance between these two points is too far.
                 # This condition helps to prevent moves up/down for
                 # adjacent lines.
                 safety_last = Point(p_last.x, p_last.y, safety_height)
                 safety_next = Point(p_next.x, p_next.y, safety_height)
                 if not result.append(safety_last, True):
                     return result.moves
                 if not result.append(safety_next, True):
                     return result.moves
         for p in path.points:
             if not result.append(p, False):
                 return result.moves
         p_last = path.points[-1]
     if not p_last is None:
         p_last_safety = Point(p_last.x, p_last.y, safety_height)
         result.append(p_last_safety, True)
     return result.moves