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