def center_divide(road0, road1):
bp0, ep0, bp1, ep1 = road0.point_list[0], road0.point_list[
-1], road1.point_list[0], road1.point_list[-1]
if bp0 == bp1 or bp0 == ep1 or ep0 == bp1 or ep0 == ep1:
return
for i, seg0 in enumerate(road0.seg_list):
for j, seg1 in enumerate(road1.seg_list):
if calc_include_angle2(
seg0, seg1) < math.sqrt(3) / 2 and is_segment_cross(
seg0, seg1):
_, px, py = get_cross_point(seg0, seg1)
if px is None:
print 'None'
cr0 = Point(px, py)
cr0.cross, cr0.cross_name, cr0.cross_other_seg = 1, road1.name, j
cr0.cross_seg = i
bp, ep = seg0.begin_point, seg0.end_point
w0, w1 = get_dist(cr0, bp), get_dist(cr0, ep)
if w0 > 1e-5 and w1 > 1e-5:
road0.cross_list.append(cr0)
cr1 = Point(px, py)
cr1.cross, cr1.cross_name, cr1.cross_other_seg = 1, road0.name, i
cr1.cross_seg = j
bp, ep = seg1.begin_point, seg1.end_point
w0, w1 = get_dist(cr1, bp), get_dist(cr1, ep)
if w0 > 1e-5 and w1 > 1e-5:
road1.cross_list.append(cr1)
return
def center_cross(road0, road1):
"""
检查道路与其他道路的交叉，并且合并交叉点
只有在道路尽头才会发生，因此和其他道路如有merge，应该不进行操作
:param road0: Road
:param road1: Road
:return:
"""
bp, ep = road0.point_list[0], road0.point_list[-1]
min_dist = 1e10
sel_seg = None
for seg in road1.seg_list:
dist = point2segment2(bp, seg)
if dist < min_dist:
min_dist, sel_seg = dist, seg
if min_dist < 50:
if road0.bs == 1:
return
if is_segment_cross(road0.seg_list[0], sel_seg) or calc_include_angle2(
road0.seg_list[0], sel_seg) > 0.8:
return
_, px, py = get_cross_point(road0.seg_list[0], sel_seg)
new_point_list = [Point(px, py)]
for pt in road0.point_list:
new_point_list.append(pt)
road0.set_point_list(new_point_list)
road0.gene_segment()
return
min_dist = 1e10
sel_seg = None
for seg in road1.seg_list:
dist = point2segment2(ep, seg)
if dist < min_dist:
min_dist, sel_seg = dist, seg
if min_dist < 50:
if road0.es == 1:
return
if is_segment_cross(road0.seg_list[-1],
sel_seg) or calc_include_angle2(
road0.seg_list[-1], sel_seg) > 0.8:
return
_, px, py = get_cross_point(road0.seg_list[-1], sel_seg)
road0.add_point(Point(px, py))
road0.gene_segment()
def par_cross(road0, road1):
"""
类似par divide，但是要标记每个pt点的进出路口情况
:param road0:
:param road1:
:return:
"""
rid0, rid1 = road0.rid, road1.rid
rbp0, rep0, rbp1, rep1 = road0.point_list[0], road0.point_list[
-1], road1.point_list[0], road1.point_list[-1]
if rbp0 == rbp1 or rbp0 == rep1 or rep0 == rbp1 or rep0 == rep1:
return
for i, seg0 in enumerate(road0.seg_list):
for j, seg1 in enumerate(road1.seg_list):
if calc_include_angle2(seg0, seg1) > 0.8: # 平行
continue
if is_segment_cross(seg0, seg1):
d, px, py = get_cross_point(seg0, seg1)
bp0, ep0 = seg0.begin_point, seg0.end_point
bp1, ep1 = seg1.begin_point, seg1.end_point
cr = Point(px, py)
if bp0 == cr:
if cr == rep1 or cr == rbp1:
pass
else:
if d < 0:
road0.point_list[i].cross = 1
else:
road0.point_list[i].cross = 2
elif ep0 == cr:
if cr == rep1 or cr == rbp1:
pass
else:
if d < 0 and not ():
road0.point_list[i + 1].cross = 1
else:
road0.point_list[i + 1].cross = 2
if bp1 == cr:
if cr == rep0 or cr == rbp0:
pass
else:
if d < 0:
road1.point_list[j].cross = 2
else:
road1.point_list[j].cross = 1
elif ep1 == cr:
if cr == rep0 or cr == rbp0:
pass
else:
if d < 0:
road1.point_list[j + 1].cross = 2
else:
road1.point_list[j + 1].cross = 1
return
def draw_seg(seg_list):
last_seg = None
cross_point = []
for seg in seg_list:
x_list, y_list = zip(*seg)
plt.plot(x_list, y_list, marker='None', color='k', linestyle='-')
if last_seg is not None:
d, px, py = get_cross_point(seg, last_seg)
cross_point.append([px, py])
# plt.text(x_list[0] + 5, y_list[0] + 5, "{0:2f}".format(d))
last_seg = seg
x_list, y_list = zip(*cross_point)
plt.plot(x_list, y_list, marker='o', color='k', linestyle='', markersize=3)
def par_divide(road0, road1):
"""
as center divide, divide cross point for each other
:param road0:
:param road1:
:return:
"""
bp0, ep0, bp1, ep1 = road0.point_list[0], road0.point_list[
-1], road1.point_list[0], road1.point_list[-1]
if bp0 == bp1:
road0.bs, road1.bs = 1, 1
return
elif bp0 == ep1:
road0.bs, road1.es = 1, 1
return
elif ep0 == bp1:
road0.es, road1.bs = 1, 1
return
elif ep0 == ep1:
road0.es, road1.es = 1, 1
return
for i, seg0 in enumerate(road0.seg_list):
for j, seg1 in enumerate(road1.seg_list):
if calc_include_angle2(seg0, seg1) > 0.8: # 平行
continue
if is_segment_cross(seg0, seg1):
_, px, py = get_cross_point(seg0, seg1)
cr0 = Point(px, py)
cr0.cross, cr0.cross_name, cr0.cross_other_seg = 1, road1.rid, j
cr0.cross_seg = i
bp, ep = seg0.begin_point, seg0.end_point
w0, w1 = get_dist(cr0, bp), get_dist(cr0, ep)
if w0 > 1e-5 and w1 > 1e-5:
road0.cross_list.append(cr0)
cr1 = Point(px, py)
cr1.cross, cr1.cross_name, cr1.cross_other_seg = 1, road0.rid, i
cr1.cross_seg = j
bp, ep = seg1.begin_point, seg1.end_point
w0, w1 = get_dist(cr1, bp), get_dist(cr1, ep)
if w0 > 1e-5 and w1 > 1e-5:
road1.cross_list.append(cr1)
return
def par():
PAR = 40
road_data = load_model2road('./road_new/center1.txt')
par_road = []
# road_index = 0
for i, road in enumerate(road_data):
name, point_list = road.name, road.point_list
rid = road.rid
last_pt = None
road0, road1 = Road(name, 0, rid * 2), Road(name, 0, rid * 2 + 1)
road0.set_grid_set(road.grid_set)
road1.set_grid_set(road.grid_set)
# road_index += 2
seg_list0, seg_list1 = [], []
for pt in point_list:
if last_pt is not None:
# 获取两条平移线段
seg0, seg1 = get_parallel(last_pt, pt, PAR)
seg_list0.append(seg0)
seg1.set_invert()
seg_list1.append(seg1)
last_pt = pt
# 计算线段之间的交点
last_seg = None
for j, seg in enumerate(seg_list0):
if last_seg is None:
road0.add_point(seg.begin_point)
else:
_, px, py = get_cross_point(last_seg, seg)
if px is None: # 平行
road0.add_point(seg.begin_point)
else:
cp = Point(px, py)
road0.add_point(cp)
last_seg = seg
road0.add_point(last_seg.end_point)
last_seg = None
for seg in reversed(seg_list1):
if last_seg is None:
road1.add_point(seg.begin_point)
else:
_, px, py = get_cross_point(last_seg, seg)
if px is None: # 平行
road1.add_point(seg.begin_point)
else:
cp = Point(px, py)
road1.add_point(cp)
last_seg = seg
road1.add_point(last_seg.end_point)
# 并生成线段
road0.gene_segment()
road1.gene_segment()
par_road.append(road0)
par_road.append(road1)
# for test
# save_par('./road/par_0.txt', par_road)
# 端点处有merge的可能
for i, road0 in enumerate(par_road):
for j, road1 in enumerate(par_road):
if i < j and road_near(road0, road1):
par_merge(road0, road1)
# for road in par_road:
# par_check(road)
save_par('./road_new/par.txt', par_road)
def par_offset(road0, road1):
"""
边缘点的起点或终点从路中央偏移到路上
一定偏移到偶数序号的道路（中心线右手边）
:param road0: Road 待修正的道路道路
:param road1: Road 偏移到的（与road0垂直）道路
:return:
"""
OFFSET = 80
if road1.rid & 1:
return
# 当端点接近某道路且该道路没有路口交点时，需要偏移该端点，延长至下一个道路
# 端点必须没有和其他线段相交
bp, ep = road0.point_list[0], road0.point_list[-1]
# 首先是起点
min_dist = 1e10
sel_seg = None
for seg in road1.seg_list:
dist = point2segment2(bp, seg)
if dist < min_dist:
min_dist, sel_seg = dist, seg
# 寻找延长至的道路线段
if min_dist < OFFSET:
if road0.bs == 1:
return
min_dist = 1e10
for pt in road1.cross_list:
dist = get_dist(pt, bp)
min_dist = min(dist, min_dist)
if min_dist > OFFSET:
_, px, py = get_cross_point(sel_seg, road0.seg_list[0])
cr = Point(px, py)
# 该延长线应落在预计道路sel_seg上
# 不然与其余道路有可能偏离在50米内
extended_segment = Segment(cr, bp)
if not is_segment_cross(extended_segment, sel_seg):
return
pt_list = [cr]
pt_list.extend(road0.point_list[:])
road0.set_point_list(pt_list)
road0.gene_segment()
return
min_dist = 1e10
sel_seg = None
for seg in road1.seg_list:
dist = point2segment2(ep, seg)
if dist < min_dist:
min_dist, sel_seg = dist, seg
if min_dist < OFFSET:
if road0.es == 1:
return
min_dist = 1e10
for pt in road1.cross_list:
dist = get_dist(pt, ep)
min_dist = min(dist, min_dist)
if min_dist > OFFSET:
_, px, py = get_cross_point(sel_seg, road0.seg_list[-1])
cr = Point(px, py)
extended_segment = Segment(cr, bp)
if not is_segment_cross(extended_segment, sel_seg):
return
road0.point_list.append(cr)
road0.gene_segment()
return
def par_merge(road0, road1):
"""
一条线段的起点与另一条(平行线段）的终点相接
:param road0:
:param road1:
:return: road0, road1重新生成道路
"""
THREAD = 10
bp0, ep0, bp1, ep1 = road0.point_list[0], road0.point_list[
-1], road1.point_list[0], road1.point_list[-1]
try:
if bp0 == ep1 or bp1 == ep0:
return
except TypeError:
print 'par merge', road0.rid, road1.rid, 'TypeError'
return
# 在生成平行线后两条道路之间必然（除非是一直线上）有空隙或者交叉
# 这个THREAD值不能太大，否则和对向车道也能查找到一起
if get_dist(bp0, ep1) < THREAD:
begin_seg, end_seg = road0.seg_list[0], road1.seg_list[-1]
# 首先是平行
if calc_include_angle2(begin_seg, end_seg) <= 0.8:
return
if is_segment_cross(begin_seg, end_seg):
# 相交时cut
_, px, py = get_cross_point(begin_seg, end_seg)
cr = Point(px, py)
road0.point_list[0] = cr
road1.point_list[-1] = cr
road0.gene_segment()
road1.gene_segment()
else:
# 不相交时延长
_, px, py = get_cross_point(begin_seg, end_seg)
cr = Point(px, py)
pt_list = [cr]
pt_list.extend(road0.point_list)
road0.point_list = pt_list
road1.point_list.append(cr)
road0.gene_segment()
road1.gene_segment()
elif get_dist(bp1, ep0) < THREAD:
begin_seg, end_seg = road1.seg_list[0], road0.seg_list[-1]
# 首先是平行
if calc_include_angle2(begin_seg, end_seg) <= 0.8:
return
if is_segment_cross(begin_seg, end_seg):
# 相交时cut
_, px, py = get_cross_point(begin_seg, end_seg)
cr = Point(px, py)
road1.point_list[0] = cr
road0.point_list[-1] = cr
road0.gene_segment()
road1.gene_segment()
else:
# 不相交时延长
_, px, py = get_cross_point(begin_seg, end_seg)
cr = Point(px, py)
pt_list = [cr]
pt_list.extend(road1.point_list)
road1.point_list = pt_list
road0.point_list.append(cr)
road0.gene_segment()
road1.gene_segment()