Merge pull request #2027 from inkstitch/george-steel/fix-running-stitch

Replace running stitch algorithm to give consistent stitch lengths

4 files changed, 214 insertions, 69 deletions

diff --git a/lib/elements/satin_column.py b/lib/elements/satin_column.py
index 1841bba1..be614a04 100644
--- a/lib/elements/satin_column.py
+++ b/lib/elements/satin_column.py
@@ -877,9 +877,11 @@ class SatinColumn(EmbroideryElement):
         pairs = self.plot_points_on_rails(
             self.contour_underlay_stitch_length,
             -self.contour_underlay_inset_px, -self.contour_underlay_inset_percent/100)
-        stitches = [p[0] for p in pairs] + [p[1] for p in reversed(pairs)]
+
         if self._center_walk_is_odd():
-            stitches = list(reversed(stitches))
+            stitches = [p[0] for p in reversed(pairs)] + [p[1] for p in pairs]
+        else:
+            stitches = [p[1] for p in pairs] + [p[0] for p in reversed(pairs)]
 
         return StitchGroup(
             color=self.color,
diff --git a/lib/elements/stroke.py b/lib/elements/stroke.py
index 73973bf7..6aca3847 100644
--- a/lib/elements/stroke.py
+++ b/lib/elements/stroke.py
@@ -432,21 +432,20 @@ class Stroke(EmbroideryElement):
         return patch
 
     def running_stitch(self, path, stitch_length, tolerance):
-        repeated_path = []
+        stitches = running_stitch(path, stitch_length, tolerance)
 
+        repeated_stitches = []
         # go back and forth along the path as specified by self.repeats
         for i in range(self.repeats):
             if i % 2 == 1:
                 # reverse every other pass
-                this_path = path[::-1]
+                this_path = stitches[::-1]
             else:
-                this_path = path[:]
+                this_path = stitches[:]
 
-            repeated_path.extend(this_path)
+            repeated_stitches.extend(this_path)
 
-        stitches = running_stitch(repeated_path, stitch_length, tolerance)
-
-        return StitchGroup(self.color, stitches)
+        return StitchGroup(self.color, repeated_stitches)
 
     def ripple_stitch(self):
         return StitchGroup(
diff --git a/lib/stitches/running_stitch.py b/lib/stitches/running_stitch.py
index e6a7d010..c1c2d99c 100644
--- a/lib/stitches/running_stitch.py
+++ b/lib/stitches/running_stitch.py
@@ -4,12 +4,14 @@
 # Licensed under the GNU GPL version 3.0 or later.  See the file LICENSE for details.
 
 import math
+from math import tau
 import typing
 from copy import copy
 
 import numpy as np
 from shapely import geometry as shgeo
 from ..utils import prng
+from ..utils.geometry import Point
 
 """ Utility functions to produce running stitches. """
 
@@ -49,69 +51,205 @@ def split_segment_random_phase(a, b, length: float, length_sigma: float, random_
     return [line.interpolate(x, normalized=False) for x in splits]
 
 
-def running_stitch(points, stitch_length, tolerance):
-    """Generate running stitch along a path.
-
-    Given a path and a stitch length, walk along the path in increments of the
-    stitch length.  If sharp corners are encountered, an extra stitch will be
-    added at the corner to avoid rounding the corner.  The starting and ending
-    point are always stitched.
-
-    The path is described by a set of line segments, each connected to the next.
-    The line segments are described by a sequence of points.
-    """
-
+class AngleInterval():
+    # Modular interval containing either the entire circle or less than half of it
+    # partially based on https://fgiesen.wordpress.com/2015/09/24/intervals-in-modular-arithmetic/
+
+    def __init__(self, a: float, b: float, all: bool = False):
+        self.all = all
+        self.a = a
+        self.b = b
+
+    @staticmethod
+    def all():
+        return AngleInterval(0, math.tau, True)
+
+    @staticmethod
+    def fromBall(p: Point, epsilon: float):
+        d = p.length()
+        if d <= epsilon:
+            return AngleInterval.all()
+        center = p.angle()
+        delta = math.asin(epsilon / d)
+        return AngleInterval(center - delta, center + delta)
+
+    @staticmethod
+    def fromSegment(a: Point, b: Point):
+        angleA = a.angle()
+        angleB = b.angle()
+        diff = (angleB - angleA) % tau
+        if diff == 0 or diff == math.pi:
+            return None
+        elif diff < math.pi:
+            return AngleInterval(angleA - 1e-6, angleB + 1e-6)
+            # slightly larger than normal to avoid rounding error when this method is used in cutSegment
+        else:
+            return AngleInterval(angleB - 1e-6, angleA + 1e-6)
+
+    def containsAngle(self, angle: float):
+        if self.all:
+            return True
+        return (angle - self.a) % tau <= (self.b - self.a) % tau
+
+    def containsPoint(self, p: Point):
+        return self.containsAngle(math.atan2(p.y, p.x))
+
+    def intersect(self, other):
+        # assume that each interval contains less than half the circle (or all of it)
+        if other is None:
+            return None
+        elif self.all:
+            return other
+        elif other.all:
+            return self
+        elif self.containsAngle(other.a):
+            if other.containsAngle(self.b):
+                return AngleInterval(other.a, self.b)
+            else:
+                return AngleInterval(other.a, other.b)
+        elif other.containsAngle(self.a):
+            if self.containsAngle(other.b):
+                return AngleInterval(self.a, other.b)
+            else:
+                return AngleInterval(self.a, self.b)
+        else:
+            return None
+
+    def cutSegment(self, origin: Point, a: Point, b: Point):
+        if self.all:
+            return None
+        segArc = AngleInterval.fromSegment(a - origin, b - origin)
+        if segArc is None:
+            return a  # b is exactly behind origin from a
+        if segArc.containsAngle(self.a):
+            return cut_segment_with_angle(origin, self.a, a, b)
+        elif segArc.containsAngle(self.b):
+            return cut_segment_with_angle(origin, self.b, a, b)
+        else:
+            return None
+
+
+def cut_segment_with_angle(origin: Point, angle: float, a: Point, b: Point) -> Point:
+    # Assumes the crossing is inside the segment
+    p = a - origin
+    d = b - a
+    c = Point(math.cos(angle), math.sin(angle))
+    t = (p.y*c.x - p.x*c.y) / (d.x*c.y - d.y*c.x)
+    if t < -0.000001 or t > 1.000001:
+        raise Exception("cut_segment_with_angle returned a parameter of {0} with points {1} {2} and cut line {3} ".format(t, p, b-origin, c))
+    return a + d*t
+
+
+def cut_segment_with_circle(origin: Point, r: float, a: Point, b: Point) -> Point:
+    # assumes that a is inside the circle and b is outside
+    p = a - origin
+    d = b - a
+    # inner products
+    p2 = p * p
+    d2 = d * d
+    r2 = r * r
+    pd = p * d
+    # r2 = p2 + 2*pd*t + d2*t*t, quadratic formula
+    t = (math.sqrt(pd*pd + r2*d2 - p2*d2) - pd) / d2
+    if t < -0.000001 or t > 1.000001:
+        raise Exception("cut_segment_with_circle returned a parameter of {0}".format(t))
+    return a + d*t
+
+
+def take_stitch(start: Point, points: typing.Sequence[Point], idx: int, stitch_length: float, tolerance: float):
+    # Based on a single step of the Zhao-Saalfeld curve simplification algorithm.
+    # https://cartogis.org/docs/proceedings/archive/auto-carto-13/pdf/linear-time-sleeve-fitting-polyline-simplification-algorithms.pdf
+    # Adds early termination condition based on stitch length.
+    if idx >= len(points):
+        return None, None
+
+    sleeve = AngleInterval.all()
+    last = start
+    for i in range(idx, len(points)):
+        p = points[i]
+        if sleeve.containsPoint(p - start):
+            if start.distance(p) < stitch_length:
+                sleeve = sleeve.intersect(AngleInterval.fromBall(p - start, tolerance))
+                last = p
+                continue
+            else:
+                cut = cut_segment_with_circle(start, stitch_length, last, p)
+                return cut, i
+        else:
+            cut = sleeve.cutSegment(start, last, p)
+            if start.distance(cut) > stitch_length:
+                cut = cut_segment_with_circle(start, stitch_length, last, p)
+            return cut, i
+    return points[-1], None
+
+
+def stitch_curve_evenly(points: typing.Sequence[Point], stitch_length: float, tolerance: float):
+    # Will split a straight line into even-length stitches while still handling curves correctly.
+    # Includes end point but not start point.
     if len(points) < 2:
         return []
+    distLeft = [0] * len(points)
+    for i in reversed(range(0, len(points) - 1)):
+        distLeft[i] = distLeft[i + 1] + points[i].distance(points[i+1])
+
+    i = 1
+    last = points[0]
+    stitches = []
+    while i is not None and i < len(points):
+        d = last.distance(points[i]) + distLeft[i]
+        if d == 0:
+            return stitches
+        stitch_len = d / math.ceil(d / stitch_length) + 0.000001  # correction for rounding error
+
+        stitch, newidx = take_stitch(last, points, i, stitch_len, tolerance)
+        i = newidx
+        if stitch is not None:
+            stitches.append(stitch)
+            last = stitch
+    return stitches
+
+
+def path_to_curves(points: typing.List[Point], min_len: float):
+    # split a path at obvious corner points so that they get stitched exactly
+    # min_len controls the minimum length after splitting for which it won't split again,
+    # which is used to avoid creating large numbers of corner points when encouintering micro-messes.
+    if len(points) < 3:
+        return [points]
+    curves = []
+
+    last = 0
+    last_seg = points[1] - points[0]
+    seg_len = last_seg.length()
+    for i in range(1, len(points) - 1):
+        # vectors of the last and next segments
+        a = last_seg
+        b = points[i + 1] - points[i]
+        aabb = (a * a) * (b * b)
+        abab = (a * b) * abs(a * b)
+
+        # Test if the turn angle from vectors a to b is more than 45 degrees.
+        # Optimized version of checking if cos(angle(a,b)) <= sqrt(0.5) and is defined
+        if aabb > 0 and abab <= 0.5 * aabb:
+            if seg_len >= min_len:
+                curves.append(points[last: i + 1])
+                last = i
+            seg_len = 0
+
+        if b * b > 0:
+            last_seg = b
+        seg_len += b.length()
+
+    curves.append(points[last:])
+    return curves
+
 
-    # simplify will remove as many points as possible while ensuring that the
-    # resulting path stays within the specified tolerance of the original path.
-    path = shgeo.LineString(points)
-    simplified = path.simplify(tolerance, preserve_topology=False)
-
-    # save the points that simplify picked and make sure we stitch them
-    important_points = set(simplified.coords)
-    important_point_indices = [i for i, point in enumerate(points) if point.as_tuple() in important_points]
-
-    output = []
-    for start, end in zip(important_point_indices[:-1], important_point_indices[1:]):
-        # consider sections of the original path, each one starting and ending
-        # with an important point
-        section = points[start:end + 1]
-        if not output or output[-1] != section[0]:
-            output.append(section[0])
-
-        # Now split each section up evenly into stitches, each with a length no
-        # greater than the specified stitch_length.
-        section_ls = shgeo.LineString(section)
-        section_length = section_ls.length
-        if section_length > stitch_length:
-            # a fractional stitch needs to be rounded up, which will make all
-            # the stitches shorter
-            num_stitches = math.ceil(section_length / stitch_length)
-            actual_stitch_length = section_length / num_stitches
-
-            distance = actual_stitch_length
-
-            segment_start = section[0]
-            for segment_end in section[1:]:
-                segment = segment_end - segment_start
-                segment_length = segment.length()
-
-                if distance < segment_length:
-                    segment_direction = segment.unit()
-
-                    while distance < segment_length:
-                        output.append(segment_start + distance * segment_direction)
-                        distance += actual_stitch_length
-
-                distance -= segment_length
-                segment_start = segment_end
-
-    if points[-1] != output[-1]:
-        output.append(points[-1])
-
-    return output
+def running_stitch(points, stitch_length, tolerance):
+    # Turn a continuous path into a running stitch.
+    stitches = [points[0]]
+    for curve in path_to_curves(points, 2 * tolerance):
+        # segments longer than twice the tollerance will usually be forced by it, so set that as the minimum for corner detection
+        stitches.extend(stitch_curve_evenly(curve, stitch_length, tolerance))
+    return stitches
 
 
 def bean_stitch(stitches, repeats):
diff --git a/lib/utils/geometry.py b/lib/utils/geometry.py
index 0ca13d8f..789f8720 100644
--- a/lib/utils/geometry.py
+++ b/lib/utils/geometry.py
@@ -211,6 +211,9 @@ class Point:
     def unit(self):
         return self.mul(1.0 / self.length())
 
+    def angle(self):
+        return math.atan2(self.y, self.x)
+
     def rotate_left(self):
         return self.__class__(-self.y, self.x)
 
@@ -229,6 +232,9 @@ class Point:
     def __len__(self):
         return 2
 
+    def __str__(self):
+        return "({0:.3f}, {1:.3f})".format(self.x, self.y)
+
 
 def line_string_to_point_list(line_string):
     return [Point(*point) for point in line_string.coords]