1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
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
|
