def find_true_widths(pts, nvecs, ws, check_scan_location): tx = pts[:, 0] ty = pts[:, 1] onws = ws[:, 0] opws = ws[:, 1] stp_sze = 0.1 sf = 0.5 # safety factor N = len(pts) nws, pws = [], [] for i in range(N): pt = [tx[i], ty[i]] nvec = nvecs[i] if not check_scan_location(pt): j = stp_sze s_pt = lib.add_locations(pt, nvec, j) while not check_scan_location(s_pt) and j < opws[i]: j += stp_sze s_pt = lib.add_locations(pt, nvec, j) pws.append(j*sf) j = stp_sze s_pt = lib.sub_locations(pt, nvec, j) while not check_scan_location(s_pt) and j < onws[i]: j += stp_sze s_pt = lib.sub_locations(pt, nvec, j) nws.append(j*sf) # print(f"Pt added without being adjusted") else: print(f"Obs in way of pt: {i}") for j in np.linspace(0, onws[i], 10): p_pt = lib.add_locations(pt, nvec, j) n_pt = lib.sub_locations(pt, nvec, j) if not check_scan_location(p_pt): nws.append(-j*(1+sf)) pws.append(opws[i]) print(f"PosPt NewW: [{-j*(1+sf)}, {opws[i]}]") break elif not check_scan_location(n_pt): pws.append(-j*(1+sf)) nws.append(onws[i]) print(f"PosPt NewW: [{-j*(1+sf)}, {onws[i]}]") break if j == onws[i]: print(f"Problem - no space found") nws, pws = np.array(nws), np.array(pws) ws = np.concatenate([nws[:, None], pws[:, None]], axis=-1) return ws
def expand_wpts(self): n = 5 # number of pts per orig pt dz = 1 / n o_line = self.wpts o_ss = self.ss o_vs = self.vs new_line = [] new_ss = [] new_vs = [] for i in range(self.N - 1): dd = lib.sub_locations(o_line[i + 1], o_line[i]) for j in range(n): pt = lib.add_locations(o_line[i], dd, dz * j) new_line.append(pt) ds = o_ss[i + 1] - o_ss[i] new_ss.append(o_ss[i] + dz * j * ds) dv = o_vs[i + 1] - o_vs[i] new_vs.append(o_vs[i] + dv * j * dz) self.wpts = np.array(new_line) self.ss = np.array(new_ss) self.vs = np.array(new_vs) self.N = len(new_line)
def expand_wpts(self): n = 5 # number of pts per orig pt dz = 1 / n o_line = self.waypoints[:, 0:2] # o_ss = self.ss o_vs = self.waypoints[:, 2] new_line = [] # new_ss = [] new_vs = [] for i in range(len(self.waypoints) - 1): dd = lib.sub_locations(o_line[i + 1], o_line[i]) for j in range(n): pt = lib.add_locations(o_line[i], dd, dz * j) new_line.append(pt) # ds = o_ss[i+1] - o_ss[i] # new_ss.append(o_ss[i] + dz*j*ds) dv = o_vs[i + 1] - o_vs[i] new_vs.append(o_vs[i] + dv * j * dz) wpts = np.array(new_line) # self.ss = np.array(new_ss) vs = np.array(new_vs) self.waypoints = np.concatenate([wpts, vs[:, None]], axis=-1)