def run(self, x, y):
cte = 0.
closest_dist = 1000000.
iClosest = 0
for iPath, pos in enumerate(self.path.path):
xp, yp = pos
d = dist(x, y, xp, yp)
if d < closest_dist:
closest_dist = d
iClosest = iPath
#check if next or prev is closer
iNext = (iClosest + 1) % len(self.path.path)
iPrev = (iClosest - 1) % len(self.path.path)
npx, npy = self.path.path[iNext]
dist_next = dist(x, y, npx, npy)
ppx, ppy = self.path.path[iPrev]
dist_prev = dist(x, y, ppx, ppy)
if dist_next < dist_prev:
iB = iNext
else:
iB = iPrev
ax, ay = self.path.path[iClosest]
bx, by = self.path.path[iB]
cx, cy = closest_pt_on_line(ax, ay, bx, by, x, y)
return cte
def run(self, x, y):
d = dist(x, y, self.x, self.y)
if d > self.min_dist:
self.path.append((x, y))
self.x = x
self.y = y
return self.path
def run(self, x, y):
d = dist(x, y, self.x, self.y)
if self.recording and d > self.min_dist:
self.path.append((x, y))
logging.info("path point (%f, %f)" % (x, y))
self.x = x
self.y = y
return self.path
def run(self, x, y):
self.x = x
self.y = y
d = dist(x, y, self.lastx, self.lasty)
if d > self.min_dist:
self.path.append((x, y))
self.lastx = x
self.lasty = y
return self.path, self.waypoints
def nearest_two_pts(self, path, x, y):
if len(path) < 2:
return None, None
distances = []
for iP, p in enumerate(path):
d = dist(p[0], p[1], x, y)
distances.append((d, iP, p))
distances.sort(key=lambda elem: elem[0])
iA = (distances[0][1] - 1) % len(path)
a = path[iA]
#iB is the next element in the path, wrapping around..
iB = (iA + 2) % len(path)
b = path[iB]
return a, b
def nearest_two_pts(self, path, x, y):
if len(path) < 2:
return None, None
min_dist = 1000000
min_idx = 0
for i, p in enumerate(path):
d = dist(p[0], p[1], x, y)
if d < min_dist:
min_dist = d
min_idx = i
iA = min_idx % len(path)
a = path[iA]
#iB is the next element in the path, wrapping around..
iB = (iA + 1) % len(path)
b = path[iB]
return a, b
