From 5846ffbfdc637df10de7366f216907a675ca9027 Mon Sep 17 00:00:00 2001
From: looooo <sppedflyer@gmail.com>
Date: Sat, 4 Dec 2021 13:25:20 +0100
Subject: [PATCH] connector: add support for shifted gears

---
 freecad/gears/connector.py | 13 ++++++++++-
 pygears/computation.py     | 46 +++++++++++++++++++-------------------
 2 files changed, 35 insertions(+), 24 deletions(-)

diff --git a/freecad/gears/connector.py b/freecad/gears/connector.py
index 0842f84..fc429a3 100644
--- a/freecad/gears/connector.py
+++ b/freecad/gears/connector.py
@@ -22,6 +22,7 @@ import numpy as np
 import FreeCAD
 from pygears import __version__
 from .features import InvoluteGear
+from pygears.computation import compute_shifted_gears
 
 class ViewProviderGearConnector(object):
     def __init__(self, vobj, icon_fn=None):
@@ -66,8 +67,18 @@ class GearConnector(object):
             angle_master = fp.master_gear.Placement.Rotation.Angle
             dw_master = fp.master_gear.dw
             dw_slave = fp.slave_gear.dw
+            dist = (dw_master + dw_slave) / 2
+            if fp.master_gear.shift != 0 or fp.slave_gear.shift != 0:
+                dist, alpha_w = compute_shifted_gears(
+                    fp.master_gear.module,
+                    np.deg2rad(fp.master_gear.pressure_angle.Value),
+                    fp.master_gear.teeth,
+                    fp.slave_gear.teeth,
+                    fp.master_gear.shift,
+                    fp.slave_gear.shift)
+
             mat0 = FreeCAD.Matrix()  # unity matrix
-            trans = FreeCAD.Vector(dw_master + dw_slave) / 2
+            trans = FreeCAD.Vector(dist)
             mat0.move(trans)
             rot = FreeCAD.Rotation(FreeCAD.Vector(0,0,1), fp.angle1).toMatrix()
             angle2 = dw_master / dw_slave * fp.angle1.Value
diff --git a/pygears/computation.py b/pygears/computation.py
index ad82fb0..084154d 100644
--- a/pygears/computation.py
+++ b/pygears/computation.py
@@ -17,7 +17,6 @@
 # ***************************************************************************
 
 import numpy as np
-from scipy import optimize as opt
 
 
 def compute_shifted_gears(m, alpha, t1, t2, x1, x2):
@@ -34,31 +33,32 @@ def compute_shifted_gears(m, alpha, t1, t2, x1, x2):
     Returns:
         (float, float): distance between gears [length], pressure angle of the assembly [rad]
     """
-    def inv(x): return np.tan(x) - x
+    def inv(x): 
+        return np.tan(x) - x
+
     inv_alpha_w = inv(alpha) + 2 * np.tan(alpha) * (x1 + x2) / (t1 + t2)
 
-    def root_inv(x): return inv(x) - inv_alpha_w
-    alpha_w = opt.fsolve(root_inv, 0.)
+    def root_inv(x):
+        return inv(x) - inv_alpha_w
+
+    def d_root_inv(x):
+        return 1. / np.cos(x) - 1
+
+    alpha_w = find_root(alpha, root_inv, d_root_inv)
     dist = m * (t1 + t2) / 2 * np.cos(alpha) / np.cos(alpha_w)
     return dist, alpha_w
 
 
-def shifted_pitch_diameter(m, alpha, t1, x1):
-    """Summary
-
-    Args:
-        m (float): common module of both gears [length]
-        alpha (float): pressure-angle [rad]
-        t1 (int): number of teeth of gear1
-        x1 (float): relative profile-shift of gear1
-
-    Returns:
-        (float, float): distance between gears [length], pressure angle of the assembly [rad]
-    """
-    def inv(x): return np.tan(x) - x
-    inv_alpha_w = inv(alpha) + 2 * np.tan(alpha) * x1 / t1
-
-    def root_inv(x): return inv(x) - inv_alpha_w
-    alpha_w = opt.fsolve(root_inv, 0.)
-    pitch_diameter = m * t1  * np.cos(alpha) / np.cos(alpha_w)
-    return pitch_diameter, alpha_w
+def find_root(x0, f, df, epsilon=2e-10, max_iter=100):
+    x_n = x0
+    for i in range(max_iter):
+        f_xn = f(x_n)
+        if abs(f_xn) < epsilon:
+            return x_n
+        else:
+            df_xn = df(x_n)
+            if df_xn == 0:
+                return None
+            else:
+                x_n = x_n - f_xn / df_xn / 2  # adding (/ 2) to avoid oscillation
+    return None
\ No newline at end of file