# 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