1 files changed, 160 insertions, 0 deletions
diff --git a/third_party/spiro/curves/pcorn.py b/third_party/spiro/curves/pcorn.py
new file mode 100644
index 0000000..723a82f
--- /dev/null
+++ b/third_party/spiro/curves/pcorn.py
@@ -0,0 +1,160 @@
+# Utilities for piecewise cornu representation of curves
+
+from math import *
+
+import clothoid
+import cornu
+
+class Segment:
+    def __init__(self, z0, z1, th0, th1):
+        self.z0 = z0
+        self.z1 = z1
+        self.th0 = th0
+        self.th1 = th1
+        self.compute()
+    def __repr__(self):
+        return '[' + `self.z0` + `self.z1` + ' ' + `self.th0` + ' ' + `self.th1` + ']'
+    def compute(self):
+        dx = self.z1[0] - self.z0[0]
+        dy = self.z1[1] - self.z0[1]
+        chord = hypot(dy, dx)
+        chth = atan2(dy, dx)
+        k0, k1 = clothoid.solve_clothoid(self.th0, self.th1)
+        charc = clothoid.compute_chord(k0, k1)
+
+        self.chord = chord
+        self.chth = chth
+        self.k0, self.k1 = k0, k1
+        self.charc = charc
+        self.arclen = chord / charc
+        self.thmid = self.chth - self.th0 + 0.5 * self.k0 - 0.125 * self.k1
+
+        self.setup_xy_fresnel()
+
+    def setup_xy_fresnel(self):
+        k0, k1 = self.k0, self.k1
+        if k1 == 0: k1 = 1e-6 # hack
+        if k1 != 0:
+            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)
+            chord_th = atan2(y1 - y0, x1 - x0)
+            chord = hypot(y1 - y0, x1 - x0)
+            scale = self.chord / chord
+            if k1 >= 0:
+                th = self.chth - chord_th
+                self.mxx = scale * cos(th)
+                self.myx = scale * sin(th)
+                self.mxy = -self.myx
+                self.myy = self.mxx
+            else:
+                th = self.chth + chord_th
+                self.mxx = scale * cos(th)
+                self.myx = scale * sin(th)
+                self.mxy = self.myx
+                self.myy = -self.mxx
+                # rotate -chord_th, flip top/bottom, rotate self.chth
+            self.x0 = self.z0[0] - (self.mxx * x0 + self.mxy * y0)
+            self.y0 = self.z0[1] - (self.myx * x0 + self.myy * y0)
+
+    def th(self, s):
+        u = s / self.arclen - 0.5
+        return self.thmid + (0.5 * self.k1 * u + self.k0) * u
+
+    def xy(self, s):
+        # using fresnel integrals; polynomial approx might be better
+        u = s / self.arclen - 0.5
+        k0, k1 = self.k0, self.k1
+        if k1 == 0: k1 = 1e-6 # hack
+        if k1 != 0:
+            sqrk1 = sqrt(2 * abs(k1))
+            t = (k0 + u * k1) / sqrk1
+            (y, x) = cornu.eval_cornu(t)
+            return [self.x0 + self.mxx * x + self.mxy * y,
+                    self.y0 + self.myx * x + self.myy * y]
+
+    def find_extrema(self):
+        # find solutions of th(s) = 0 mod pi/2
+        # todo: find extra solutions when there's an inflection
+        th0 = self.thmid + 0.125 * self.k1 - 0.5 * self.k0
+        th1 = self.thmid + 0.125 * self.k1 + 0.5 * self.k0
+        twooverpi = 2 / pi
+        n0 = int(floor(th0 * twooverpi))
+        n1 = int(floor(th1 * twooverpi))
+        if th1 > th0: signum = 1
+        else: signum = -1
+        result = []
+        for i in range(n0, n1, signum):
+            th = pi/2 * (i + 0.5 * (signum + 1))
+            a = .5 * self.k1
+            b = self.k0
+            c = self.thmid - th
+            if a == 0:
+                u1 = -c/b
+                u2 = 1000
+            else:
+                sqrtdiscrim = sqrt(b * b - 4 * a * c)
+                u1 = (-b - sqrtdiscrim) / (2 * a)
+                u2 = (-b + sqrtdiscrim) / (2 * a)
+            if u1 >= -0.5 and u1 < 0.5:
+                result.append(self.arclen * (u1 + 0.5))
+            if u2 >= -0.5 and u2 < 0.5:
+                result.append(self.arclen * (u2 + 0.5))
+        return result
+
+class Curve:
+    def __init__(self, segs):
+        self.segs = segs
+        self.compute()
+    def compute(self):
+        arclen = 0
+        sstarts = []
+        for seg in self.segs:
+            sstarts.append(arclen)
+            arclen += seg.arclen
+
+        self.arclen = arclen
+        self.sstarts = sstarts
+    def th(self, s, deltas = False):
+        u = s / self.arclen
+        s = self.arclen * (u - floor(u))
+        if s == 0 and not deltas: s = self.arclen
+        i = 0
+        while i < len(self.segs) - 1:
+            # binary search would make a lot of sense here
+            snext = self.sstarts[i + 1] 
+            if s < snext or (not deltas and s == snext):
+                break
+            i += 1
+        return self.segs[i].th(s - self.sstarts[i])
+    def xy(self, s):
+        u = s / self.arclen
+        s = self.arclen * (u - floor(u))
+        i = 0
+        while i < len(self.segs) - 1:
+            # binary search would make a lot of sense here
+            if s <= self.sstarts[i + 1]:
+                break
+            i += 1
+        return self.segs[i].xy(s - self.sstarts[i])
+    def find_extrema(self):
+        result = []
+        for i in range(len(self.segs)):
+            seg = self.segs[i]
+            for s in seg.find_extrema():
+                result.append(s + self.sstarts[i])
+        return result
+    def find_breaks(self):
+        result = []
+        for i in range(len(self.segs)):
+            pseg = self.segs[(i + len(self.segs) - 1) % len(self.segs)]
+            seg = self.segs[i]
+            th = clothoid.mod_2pi(pseg.chth + pseg.th1 - (seg.chth - seg.th0))
+            print '% pseg', pseg.chth + pseg.th1, 'seg', seg.chth - seg.th0
+            pisline = pseg.k0 == 0 and pseg.k1 == 0
+            sisline = seg.k0 == 0 and seg.k1 == 0
+            if fabs(th) > 1e-3 or (pisline and not sisline) or (sisline and not pisline):
+                result.append(self.sstarts[i])
+        return result