From 4a87209cb572c5679d67cec91d8b5e4e131f0105 Mon Sep 17 00:00:00 2001
From: Gabriel Wicke <gswicke@gmail.com>
Date: Sat, 23 May 2020 20:18:37 -0700
Subject: [PATCH] Path: Safer step over optimization

- Only apply aggressive optimizations to short horizontal moves within
  cutter diameter. The safe model STL does not accurately reflect stock
  state, so using it for determining long distance move safe heights is
  not safe. There would be a high chance of hitting the stock at rapid
  speeds.

  Thankfully, the vast majority of step-overs tend to be short, so are
  still optimized.
- For short moves, only allow completely lift-free transitions when
  there is (almost) no Z change, and the min safe travel height does not
  rise above the same level.
  Otherwise, lift to the max of end points and min safe travel height
  first, then move horizontally. A future optimization would be to
  directly use the drop scan for transition path generation.
---
 src/Mod/Path/PathScripts/PathSurface.py | 92 ++++++++++++++-----------
 1 file changed, 50 insertions(+), 42 deletions(-)

diff --git a/src/Mod/Path/PathScripts/PathSurface.py b/src/Mod/Path/PathScripts/PathSurface.py
index 9c6f2d3c0c..00b5de01c0 100644
--- a/src/Mod/Path/PathScripts/PathSurface.py
+++ b/src/Mod/Path/PathScripts/PathSurface.py
@@ -1451,48 +1451,56 @@ class ObjectSurface(PathOp.ObjectOp):
         PNTS.pop()  # Remove temp end point
 
         return output
-
-    def _stepTransitionCmds(self, obj, lstPnt, first, minSTH, tolrnc):
-        cmds = list()
-        rtpd = False
-        horizGC = 'G0'
-        hSpeed = self.horizRapid
-        height = obj.SafeHeight.Value
-
-        if obj.CutPattern in ['Line', 'Circular']:
-            if obj.OptimizeStepOverTransitions is True:
-                height = minSTH + 2.0
-            # if obj.LayerMode == 'Multi-pass':
-            #    rtpd = minSTH
-        elif obj.CutPattern in ['ZigZag', 'CircularZigZag']:
-            if obj.OptimizeStepOverTransitions is True:
-                zChng = first.z - lstPnt.z
-                # PathLog.debug('first.z: {}'.format(first.z))
-                # PathLog.debug('lstPnt.z: {}'.format(lstPnt.z))
-                # PathLog.debug('zChng: {}'.format(zChng))
-                # PathLog.debug('minSTH: {}'.format(minSTH))
-                if abs(zChng) < tolrnc:  # transitions to same Z height
-                    PathLog.debug('abs(zChng) < tolrnc')
-                    if (minSTH - first.z) > tolrnc:
-                        PathLog.debug('(minSTH - first.z) > tolrnc')
-                        height = minSTH + 2.0
-                    else:
-                        PathLog.debug('ELSE (minSTH - first.z) > tolrnc')
-                        horizGC = 'G1'
-                        height = first.z
-                elif (minSTH + (2.0 * tolrnc)) >= max(first.z, lstPnt.z):
-                        height = False  # allow end of Zig to cut to beginning of Zag
-                    
-
-        # Create raise, shift, and optional lower commands
-        if height is not False:
-            cmds.append(Path.Command('G0', {'Z': height, 'F': self.vertRapid}))
-            cmds.append(Path.Command(horizGC, {'X': first.x, 'Y': first.y, 'F': hSpeed}))
-        if rtpd is not False:  # ReturnToPreviousDepth
-            cmds.append(Path.Command('G0', {'Z': rtpd, 'F': self.vertRapid}))
-
-        return cmds
-
+
+    def _stepTransitionCmds(self, obj, lstPnt, first, minSTH, tolrnc):
+        cmds = list()
+        rtpd = False
+        horizGC = 'G0'
+        hSpeed = self.horizRapid
+        height = obj.SafeHeight.Value
+        maxXYDistanceSqrd = (self.cutter.getDiameter() + tolrnc)**2
+
+        if obj.OptimizeStepOverTransitions is True:
+            # Short distance within step over
+            xyDistanceSqrd = (abs(first.x - lstPnt.x)**2 +
+                              abs(first.y - lstPnt.y)**2)
+            zChng = abs(first.z - lstPnt.z)
+            # Only optimize short distances <= cutter diameter. Staying at
+            # minSTH over long distances is not safe for multi layer paths,
+            # since minSTH is calculated from the model, and not based on
+            # stock cut so far.
+            if xyDistanceSqrd <= maxXYDistanceSqrd:
+                horizGC = "G1"
+                hSpeed = self.horizFeed
+                if (minSTH <= max(first.z, lstPnt.z) + tolrnc
+                        and zChng < tolrnc):
+                    # Allow direct transition without any lift over short
+                    # distances, and only when there is very little z change.
+                    height = False
+                else:
+                    # Avoid a full lift, but stay at least at minSTH along the
+                    # entire transition.
+                    # TODO: Consider using an actual scan path for the
+                    # transition.
+                    height = max(minSTH, first.z, lstPnt.z)
+            else:
+                # We conservatively lift to SafeHeight for lack of an accurate
+                # stock model, but then speed up the drop back down
+                # When using multi pass, only drop quickly to previous layer
+                # depth
+                stepDown = obj.StepDown.Value if hasattr(obj, "StepDown") else 0
+                rtpd = min(height,
+                           max(minSTH, first.z, lstPnt.z) + stepDown + 2)
+
+        # Create raise, shift, and optional lower commands
+        if height is not False:
+            cmds.append(Path.Command('G0', {'Z': height, 'F': self.vertRapid}))
+            cmds.append(Path.Command(horizGC, {'X': first.x, 'Y': first.y, 'F': hSpeed}))
+        if rtpd is not False:  # ReturnToPreviousDepth
+            cmds.append(Path.Command('G0', {'Z': rtpd, 'F': self.vertRapid}))
+
+        return cmds
+
     def _breakCmds(self, obj, lstPnt, first, minSTH, tolrnc):
         cmds = list()
         rtpd = False