1 files changed, 66 insertions, 0 deletions
diff --git a/third_party/spiro/curves/clothoid.py b/third_party/spiro/curves/clothoid.py
new file mode 100644
index 0000000..b3ae6af
--- /dev/null
+++ b/third_party/spiro/curves/clothoid.py
@@ -0,0 +1,66 @@
+from math import *
+import cornu
+
+def mod_2pi(th):
+    u = th / (2 * pi)
+    return 2 * pi * (u - floor(u + 0.5))
+
+# Given clothoid k(s) = k0 + k1 s, compute th1 - th0 of chord from s = -.5
+# to .5.
+def compute_dth(k0, k1):
+    if k1 < 0:
+        return -compute_dth(k0, -k1)
+    elif k1 == 0:
+        return 0
+    sqrk1 = sqrt(2 * k1)
+    t0 = (k0 - .5 * k1) / sqrk1
+    t1 = (k0 + .5 * k1) / sqrk1
+    (y0, x0) = cornu.eval_cornu(t0)
+    (y1, x1) = cornu.eval_cornu(t1)
+    chord_th = atan2(y1 - y0, x1 - x0)
+    return mod_2pi(t1 * t1 - chord_th) - mod_2pi(chord_th - t0 * t0)
+
+def compute_chord(k0, k1):
+    if k1 == 0:
+        if k0 == 0:
+            return 1
+        else:
+            return sin(k0 * .5) / (k0 * .5)
+    sqrk1 = sqrt(2 * abs(k1))
+    t0 = (k0 - .5 * k1) / sqrk1
+    t1 = (k0 + .5 * k1) / sqrk1
+    (y0, x0) = cornu.eval_cornu(t0)
+    (y1, x1) = cornu.eval_cornu(t1)
+    return hypot(y1 - y0, x1 - x0) / abs(t1 - t0)
+
+# Given th0 and th1 at endpoints (measured from chord), return k0
+# and k1 such that the clothoid k(s) = k0 + k1 s, evaluated from
+# s = -.5 to .5, has the tangents given
+def solve_clothoid(th0, th1, verbose = False):
+    k0 = th0 + th1
+
+    # initial guess
+    k1 = 6 * (th1 - th0)
+    error = (th1 - th0) - compute_dth(k0, k1)
+    if verbose:
+        print k0, k1, error
+
+    k1_old, error_old = k1, error
+    # second guess based on d(dth)/dk1 ~ 1/6
+    k1 += 6 * error
+    error = (th1 - th0) - compute_dth(k0, k1)
+    if verbose:
+        print k0, k1, error
+
+    # secant method
+    for i in range(10):
+        if abs(error) < 1e-9: break
+        k1_old, error_old, k1 = k1, error, k1 + (k1_old - k1) * error / (error - error_old)
+        error = (th1 - th0) - compute_dth(k0, k1)
+        if verbose:
+            print k0, k1, error
+
+    return k0, k1
+
+if __name__ == '__main__':
+    print solve_clothoid(.06, .05, True)