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)