def get_intersection_time(A: Projectile, seg: LineSegment):
o = VecE2.VecE2(A.pos.x, A.pos.y)
v_1 = o - seg.A
v_2 = seg.B - seg.A
v_3 = VecE2.polar(1.0, A.pos.theta + math.pi / 2)
denom = geom.dot_product(v_2, v_3)
if not math.isclose(denom, 0.0, abs_tol=1e-10):
d_1 = geom.cross_product(v_2,
v_1) / denom # time for projectile (0 ≤ t₁)
t_2 = geom.dot_product(v_1,
v_3) / denom # time for segment (0 ≤ t₂ ≤ 1)
if 0.0 <= d_1 and 0.0 <= t_2 <= 1.0:
return d_1 / A.v
else:
# denom is zero if the segment and the projectile are parallel
# only collide if they are perfectly aligned
if geom.are_collinear(A.pos, seg.A, seg.B):
dist_a = VecE2.normsquared(seg.A - o)
dist_b = VecE2.normsquared(seg.B - o)
return math.sqrt(min(dist_a, dist_b)) / A.v
return math.inf
def proj_2(posG: VecSE2.VecSE2, roadway: Roadway):
best_dist2 = math.inf
best_proj = RoadProjection(
CurvePt.CurveProjection(CurvePt.CurveIndex(-1, -1), None, None),
NULL_LANETAG)
for seg in roadway.segments:
for lane in seg.lanes:
roadproj = proj_1(posG, lane, roadway,
move_along_curves=False) # return RoadProjection
targetlane = roadway.get_by_tag(roadproj.tag) # return Lane
footpoint = targetlane.get_by_ind_roadway(roadproj.curveproj.ind,
roadway)
vec = posG - footpoint.pos
dist2 = VecE2.normsquared(VecE2.VecE2(vec.x, vec.y))
# print(best_dist2, dist2, roadproj.curveproj.ind.i)
if dist2 < best_dist2:
best_dist2 = dist2
best_proj = roadproj
return best_proj
def index_closest_to_point(curve: list,
target: VecSE2.VecSE2): # curve: list(CurvePt)
"""
index_closest_to_point(curve::Curve, target::posG(VecSE2))
returns the curve index closest to the point described by `target`.
`target` must be [x, y].
"""
a = 1
b = len(curve)
c = div(a + b, 2)
assert (len(curve) >= b)
sqdist_a = curve[a - 1].pos - target
sqdist_b = curve[b - 1].pos - target
sqdist_c = curve[c - 1].pos - target
# sqdist_a.show()
# sqdist_b.show()
# sqdist_c.show()
sqdist_a = VecE2.normsquared(VecE2.VecE2(sqdist_a.x, sqdist_a.y))
sqdist_b = VecE2.normsquared(VecE2.VecE2(sqdist_b.x, sqdist_b.y))
sqdist_c = VecE2.normsquared(VecE2.VecE2(sqdist_c.x, sqdist_c.y))
# print(target.x, target.y, sqdist_a, sqdist_b, sqdist_c)
# curve[a - 1].pos.show()
# curve[b - 1].pos.show()
# curve[c - 1].pos.show()
while True:
if b == a:
return a - 1
elif b == a + 1:
return (b - 1) if sqdist_b < sqdist_a else (a - 1)
elif c == a + 1 and c == b - 1:
if sqdist_a < sqdist_b and sqdist_a < sqdist_c:
return a - 1
elif sqdist_b < sqdist_a and sqdist_b < sqdist_c:
return b - 1
else:
return c - 1
left = div(a + c, 2)
sqdist_l = curve[left - 1].pos - target
sqdist_l = VecE2.normsquared(VecE2.VecE2(sqdist_l.x, sqdist_l.y))
right = div(c + b, 2)
sqdist_r = curve[right - 1].pos - target
sqdist_r = VecE2.normsquared(VecE2.VecE2(sqdist_r.x, sqdist_r.y))
if sqdist_l < sqdist_r:
b = c
sqdist_b = sqdist_c
c = left
sqdist_c = sqdist_l
else:
a = c
sqdist_a = sqdist_c
c = right
sqdist_c = sqdist_r
raise OverflowError("index_closest_to_point reached unreachable statement")
def is_potentially_colliding(A: Vehicle, B: Vehicle):
vec = A.state.posG - B.state.posG
delta_square = VecE2.normsquared(VecE2.VecE2(vec.x, vec.y))
r_a = _bounding_radius(A)
r_b = _bounding_radius(B)
return delta_square <= r_a * r_a + 2 * r_a * r_b + r_b * r_b
def integrate(centerline_fn: str,
boundary_fn: str,
dist_threshold_lane_connect: float = 2.0,
desired_distance_between_curve_samples: float = 1.0):
'''
:param centerline_path: center line file path
:param boundary_path: boundary file path
:param dist_threshold_lane_connect: [m]
:param desired_distance_between_curve_samples: [m]
:return:
'''
centerline_path = os.path.join(DIR, "../data/", centerline_fn)
boundary_path = os.path.join(DIR, "../data/", boundary_fn)
input_params = RoadwayInputParams(filepath_boundaries=boundary_path,
filepath_centerlines=centerline_path)
roadway_data = read_roadway(input_params)
print("Finish loading centerlines and boundaries.")
lane_pts_dict = dict()
for (handle_int, lane) in enumerate(roadway_data.centerlines):
segid = get_segid(lane)
N = len(lane)
pts = [None for _ in range(N)] # VecE2 list
for i in range(N):
x = lane[i].pos.x
y = lane[i].pos.y
pts[i] = VecE2.VecE2(x, y)
laneid = 1
for tag in lane_pts_dict.keys():
if tag.segment == segid:
laneid += 1
lane_pts_dict[roadway.LaneTag(segid, laneid)] = pts
###################################################
# Shift pts to connect to previous / next pts
lane_next_dict = dict()
lane_prev_dict = dict()
for (tag, pts) in lane_pts_dict.items():
# see if can connect to next
best_tag = roadway.NULL_LANETAG
best_ind = -1
best_sq_dist = dist_threshold_lane_connect
for (tag2, pts2) in lane_pts_dict.items():
if tag2.segment != tag.segment:
for (ind, pt) in enumerate(pts2):
sq_dist = VecE2.normsquared(VecE2.VecE2(pt - pts[-1]))
if sq_dist < best_sq_dist:
best_sq_dist = sq_dist
best_ind = ind
best_tag = tag2
if best_tag != roadway.NULL_LANETAG:
# remove our last pt and set next to pt to their pt
pts.pop()
lane_next_dict[tag] = (lane_pts_dict[best_tag][best_ind], best_tag)
if best_ind == 0: # set connect prev as well
lane_prev_dict[best_tag] = (pts[-1], tag)
for (tag, pts) in lane_pts_dict.items():
# see if can connect to prev
if tag not in lane_prev_dict.keys():
best_tag = roadway.NULL_LANETAG
best_ind = -1
best_sq_dist = dist_threshold_lane_connect
for (tag2, pts2) in lane_pts_dict.items():
if tag2.segment != tag.segment:
for (ind, pt) in enumerate(pts2):
sq_dist = VecE2.normsquared(VecE2.VecE2(pt - pts[0]))
if sq_dist < best_sq_dist:
best_sq_dist = sq_dist
best_ind = ind
best_tag = tag2
if best_tag != roadway.NULL_LANETAG:
lane_prev_dict[tag] = (lane_pts_dict[best_tag][best_ind],
best_tag)
###################################################
# Build the roadway
retval = roadway.Roadway()
for (tag, pts) in lane_pts_dict.items():
if not retval.has_segment(tag.segment):
retval.segments.append(roadway.RoadSegment(tag.segment))
lane_new_dict = dict() # old -> new tag
for seg in retval.segments:
# pull lanetags for this seg
lanetags = [] # LaneTag
for tag in lane_pts_dict.keys():
if tag.segment == seg.id:
lanetags.append(tag)
# sort the lanes such that the rightmost lane is lane 1
# do this by taking the first lane,
# then project each lane's midpoint to the perpendicular at the midpoint
assert len(lanetags) != 0
proj_positions = [None for _ in range(len(lanetags))] # list of float
first_lane_pts = lane_pts_dict[lanetags[0]]
n = len(first_lane_pts)
lo = first_lane_pts[n // 2 - 1]
hi = first_lane_pts[n // 2]
midpt_orig = (lo + hi) / 2
dir = VecE2.polar(
1.0, (hi - lo).atan() +
math.pi / 2) # direction perpendicular (left) of lane
for (i, tag) in enumerate(lanetags):
pts = lane_pts_dict[tag]
n = len(pts)
midpt = (pts[n // 2 - 1] + pts[n // 2]) / 2
proj_positions[i] = VecE2.proj_(midpt - midpt_orig, dir)
for (i, j) in enumerate(
sorted(range(len(proj_positions)),
key=proj_positions.__getitem__)):
tag = lanetags[j]
boundary_left = roadway.LaneBoundary("solid", "white") if i == len(proj_positions) - 1 \
else roadway.LaneBoundary("broken", "white")
boundary_right = roadway.LaneBoundary("solid", "white") if i == 0 \
else roadway.LaneBoundary("broken", "white")
pts = lane_pts_dict[tag]
pt_matrix = np.zeros((2, len(pts)))
for (k, P) in enumerate(pts):
pt_matrix[0, k] = P.x
pt_matrix[1, k] = P.y
print("fitting curve ", len(pts), " ")
curve = _fit_curve(pt_matrix,
desired_distance_between_curve_samples)
tag_new = roadway.LaneTag(seg.id, len(seg.lanes) + 1)
lane = roadway.Lane(tag_new,
curve,
boundary_left=boundary_left,
boundary_right=boundary_right)
seg.lanes.append(lane)
lane_new_dict[tag] = tag_new
###################################################
# Connect the lanes
for (tag_old, tup) in lane_next_dict.items():
next_pt, next_tag_old = tup
lane = retval.get_by_tag(lane_new_dict[tag_old])
next_tag_new = lane_new_dict[next_tag_old]
dest = retval.get_by_tag(next_tag_new)
roadproj = roadway.proj_1(VecSE2.VecSE2(next_pt, 0.0), dest, retval)
print("connecting {} to {}".format(lane.tag, dest.tag))
cindS = CurvePt.curveindex_end(lane.curve)
cindD = roadproj.curveproj.ind
if cindD == CurvePt.CURVEINDEX_START: # a standard connection
lane, dest = roadway.connect(lane, dest)
# remove any similar connection from lane_prev_dict
if next_tag_old in lane_prev_dict.keys(
) and lane_prev_dict[next_tag_old][1] == tag_old:
lane_prev_dict.pop(next_tag_old)
else:
lane.exits.insert(
0,
roadway.LaneConnection(True, cindS,
roadway.RoadIndex(cindD, dest.tag)))
dest.entrances.append(
roadway.LaneConnection(False, cindD,
roadway.RoadIndex(cindS, lane.tag)))
for (tag_old, tup) in lane_prev_dict.items():
prev_pt, prev_tag_old = tup
lane = retval.get_by_tag(lane_new_dict[tag_old])
prev_tag_new = lane_new_dict[prev_tag_old]
prev = retval.get_by_tag(prev_tag_new)
roadproj = roadway.proj_1(VecSE2.VecSE2(prev_pt, 0.0), prev, retval)
print("connecting {} from {}".format(lane.tag, prev.tag))
cindS = roadproj.curveproj.ind
cindD = CurvePt.CURVEINDEX_START
if cindS == CurvePt.curveindex_end(
prev.curve): # a standard connection
assert roadway.has_prev(prev)
prev, lane = roadway.connect(prev, lane)
else:
# a standard connection
prev.exits.append(
roadway.LaneConnection(True, cindS,
roadway.RoadIndex(cindD, lane.tag)))
lane.entrances.insert(
0,
roadway.LaneConnection(False, cindD,
roadway.RoadIndex(cindS, prev.tag)))
retval = convert_curves_feet_to_meters(retval)