implement rotation

fix crop issues

gcode-tools/gcode-shifter/gcode_shift.py

@@ -4,6 +4,7 @@ import argparse
 import os
 import re
 import sys
+import math
 from decimal import Decimal as d
 
 AXIS = "XYZ"
@@ -55,6 +56,10 @@ class GCodeFilter(object):
                 yield handler.get_line(None, None)
             else:
                 for pos in positions:
+                    if callable(pos):
+                        pos, line = pos()
+                        yield line
+                    else:
                         changed_axes = self.target_pos.get_changed_axes(pos)
                         yield handler.get_line(pos, changed_axes)
                     self.target_pos.update(pos)
@@ -152,6 +157,25 @@ class ShiftFilter(GCodeFilter):
             yield self.pos
 
 
+class MatrixFilter(GCodeFilter):
+
+    def __init__(self, *args, **kwargs):
+        self.matrix = kwargs.pop("matrix", tuple([0] * len(AXIS)))
+        super(MatrixFilter, self).__init__(*args, **kwargs)
+
+    def _multiply_with_matrix(self, pos):
+        rows = []
+        for row in self.matrix:
+            rows.append(sum([(p * r).quantize(p) for p, r in zip(pos, row)]))
+        return tuple(rows)
+
+    def transform_position(self):
+        if self.is_inside(self.pos):
+            yield self._multiply_with_matrix(self.pos)
+        else:
+            yield self.pos
+
+
 class DensifyFilter(GCodeFilter):
 
     def __init__(self, *args, **kwargs):
@@ -224,6 +248,7 @@ class CropFilter(GCodeFilter):
 
     def transform_position(self):
         stack = self.source_pos.stack
+        target_stack = self.target_pos.stack
         is_inside = self.is_inside(stack[0])
         was_inside = (len(stack) < 2) or self.is_inside(stack[1])
         result = None
@@ -232,7 +257,21 @@ class CropFilter(GCodeFilter):
             yield stack[0]
         elif not is_inside and not was_inside:
             # outside -> outside
-            pass
+            # only apply relative upward moves (safety height)
+            z_index = AXIS.lower().index("z")
+            diff = [(now - prev) for now, prev in zip(stack[0], stack[1])]
+            for index, value in enumerate(diff):
+                if index == z_index:
+                    if value <= 0:
+                        break
+                elif value != 0:
+                    break
+            else:
+                # this looks like an upward move
+                if target_stack[0][z_index] <= stack[0][z_index]:
+                    new_target = list(target_stack[0])
+                    new_target[z_index] = stack[0][z_index]
+                    yield tuple(new_target)
         else:
             # outside -> inside OR inside -> outside
             if len(stack) >= 2:
@@ -241,6 +280,13 @@ class CropFilter(GCodeFilter):
                 border_pos = tuple([value.quantize(template)
                         for value, template in zip(border_pos, stack[0])])
                 # the border position is always the first step
+                if is_inside:
+                    # outside -> inside
+                    line = "G0 %s" % " ".join([axis.upper() + str(value)
+                                for axis, value in zip(AXIS, border_pos)])
+                    yield lambda: (border_pos, line + os.linesep)
+                else:
+                    # inside -> outside
                     yield border_pos
             # omit the current position if we are outside
             if is_inside:
@@ -262,7 +308,26 @@ class CropFilter(GCodeFilter):
                     depth_limit=depth_limit-1)
 
 
+def get_rotate_matrix(axis, angle):
+    rot = [d(0)] * 3
+    rot[AXIS.lower().index(axis.lower())] = d(1)
+    # clockwise angle
+    angle_pi = -angle * d(math.pi / 180.0)
+    sin = d(math.sin(angle_pi))
+    cos = d(math.cos(angle_pi))
+    return ((cos + rot[0] * rot[0] * (1 - cos),
+                rot[0] * rot[1] * (1-cos) - rot[2] * sin,
+                rot[0] * rot[2] * (1-cos) + rot[1] * sin),
+            (rot[1] * rot[0] *(1 - cos) + rot[2] * sin,
+                cos + rot[1] * rot[1] * (1 - cos),
+                rot[1] * rot[2] * (1 - cos) - rot[0] * sin),
+            (rot[2] * rot[0] * (1 - cos) - rot[1] * sin,
+                rot[2] * rot[1] * (1 - cos) + rot[0] * sin,
+                cos + rot[2] * rot[2] * (1 - cos)))
+
+
 if __name__ == "__main__":
+    axes_choices = tuple(AXIS.lower() + AXIS.upper())
     parser = argparse.ArgumentParser(description="Shift parts of gcode")
     parser.add_argument('--minx', dest="minx", type=d)
     parser.add_argument('--maxx', dest="maxx", type=d)
@@ -273,9 +338,10 @@ if __name__ == "__main__":
     parser.add_argument('--shiftx', dest="shiftx", type=d, default=d(0))
     parser.add_argument('--shifty', dest="shifty", type=d, default=d(0))
     parser.add_argument('--shiftz', dest="shiftz", type=d, default=d(0))
-    parser.add_argument('--rotatex', dest="rotatex", type=d, default=d(0))
-    parser.add_argument('--rotatey', dest="rotatey", type=d, default=d(0))
-    parser.add_argument('--rotatez', dest="rotatez", type=d, default=d(0))
+    parser.add_argument('--rotate-axis', dest="rotate_axis",
+            choices=axes_choices, default=AXIS[0])
+    parser.add_argument('--rotate-angle', dest="rotate_angle", type=d,
+            default=d(90))
     parser.add_argument('--densify-dir', dest="densify_dir",
             choices=("x", "y", "z"), default="x")
     parser.add_argument('--densify-loops', dest="densify_loops", type=int,
@@ -295,8 +361,8 @@ if __name__ == "__main__":
     elif options.action == "crop":
         gcode_filter = CropFilter(low, high)
     elif options.action == "rotate":
-        rotate = (options.rotatex, options.rotatey, options.rotatez)
-        gcode_filter = RotateFilter(low, high, rotate=rotate)
+        matrix = get_rotate_matrix(options.rotate_axis, options.rotate_angle)
+        gcode_filter = MatrixFilter(low, high, matrix=matrix)
     elif options.action == "densify":
         gcode_filter = DensifyFilter(low, high, densify_dir=options.densify_dir,
                 densify_loops=options.densify_loops,