def intersect_point(p1, p2, edge):
"""Return intersect Point where the edge from p1, p2 intersects edge"""
if p1 in edge: return p1
if p2 in edge: return p2
if edge.p1.x == edge.p2.x:
if p1.x == p2.x:
return None
pslope = (p1.y - p2.y) / (p1.x - p2.x)
intersect_x = edge.p1.x
intersect_y = pslope * (intersect_x - p1.x) + p1.y
return Point(intersect_x, intersect_y)
if p1.x == p2.x:
eslope = (edge.p1.y - edge.p2.y) / (edge.p1.x - edge.p2.x)
intersect_x = p1.x
intersect_y = eslope * (intersect_x - edge.p1.x) + edge.p1.y
return Point(intersect_x, intersect_y)
pslope = (p1.y - p2.y) / (p1.x - p2.x)
eslope = (edge.p1.y - edge.p2.y) / (edge.p1.x - edge.p2.x)
if eslope == pslope:
return None
intersect_x = (eslope * edge.p1.x - pslope * p1.x + p1.y -
edge.p1.y) / (eslope - pslope)
intersect_y = eslope * (intersect_x - edge.p1.x) + edge.p1.y
return Point(intersect_x, intersect_y)
def __init__(self, config):
self.start = Point(config["rrt"]["start"][0], config["rrt"]["start"][1])
self.end = Point(config["rrt"]["end"][0], config["rrt"]["end"][1])
self.delta = config["rrt"]["delta"]
self.tree_start = Tree(self.start)
self.tree_end = Tree(self.end)
self.graph: Graph = Graph(config)
self.graph.start_point = self.start
self.graph.end_point = self.end
def __init__(self, config, paraboloid=True):
self.graph = Graph(config)
self.rois = config["apf"]["rois"]
assert len(self.rois) == len(self.graph.obstacles)
self.start = Point(config["rrt"]["start"][0],
config["rrt"]["start"][1])
self.end = Point(config["rrt"]["end"][0], config["rrt"]["end"][1])
self.ka = config["apf"]["ka"]
self.kr = config["apf"]["kr"]
self.gamma = config["apf"]["gamma"]
self.convergence_delta = config["apf"]["convergence_delta"]
self.graph.start_point = self.start
self.graph.end_point = self.end
self.paraboloid = paraboloid
def __attractive_potential_derivative(self, point: Point) -> Point:
"""
Returns the attractive potential derivative at a point.
:param point:
:return: Point
"""
if self.paraboloid:
derivative = Point(self.ka * (self.end.x - point.x),
self.ka * (self.end.y - point.y))
else:
derivative = Point(self.ka * (self.end.x - point.x),
self.ka * (self.end.y - point.y))
derivative = derivative / derivative.norm()
return derivative
def __get_random_point(self) -> Point:
"""
Returns a random point as per the bounds of the current graph limits.
:return:
"""
rand_x = self.start.x + random() * (self.end.x - self.start.x)
rand_y = self.start.y + random() * (self.end.y - self.start.y)
return Point(rand_x, rand_y)
def edge_in_polygon(p1, p2, graph):
"""Return true if the edge from p1 to p2 is interior to any polygon
in graph."""
if p1.polygon_id != p2.polygon_id:
return False
if p1.polygon_id == -1 or p2.polygon_id == -1:
return False
mid_point = Point((p1.x + p2.x) / 2, (p1.y + p2.y) / 2)
return polygon_crossing(mid_point, graph.polygons[p1.polygon_id])
def __repulsive_potential_derivative(self, point: Point, obstacle: Circle,
roi) -> Point:
"""
Returns the repulsive potential derivative at a point by a given obstacle.
:param point:
:param obstacle:
:param roi:
:return: Point
"""
distance_from_boundary = point.dist(obstacle.center) - obstacle.radius
if distance_from_boundary > roi:
return Point(0, 0)
# TODO removed negative sign
e = point - obstacle.center
e = e / e.norm()
derivative = self.kr * pow(
(1 / distance_from_boundary - 1 / roi), self.gamma - 1) / pow(
distance_from_boundary, 2) * e
return derivative
def polygon_crossing(p1, poly_edges):
"""Returns True if Point p1 is internal to the polygon"""
p2 = Point(INF, p1.y)
intersect_count = 0
for edge in poly_edges:
if p1.y < edge.p1.y and p1.y < edge.p2.y: continue
if p1.y > edge.p1.y and p1.y > edge.p2.y: continue
if p1.x > edge.p1.x and p1.x > edge.p2.x: continue
edge_p1_collinear = (ccw(p1, edge.p1, p2) == COLLINEAR)
edge_p2_collinear = (ccw(p1, edge.p2, p2) == COLLINEAR)
if edge_p1_collinear and edge_p2_collinear: continue
if edge_p1_collinear or edge_p2_collinear:
collinear_point = edge.p1 if edge_p1_collinear else edge.p2
if edge.get_adjacent(collinear_point).y > p1.y:
intersect_count += 1
elif edge_intersect(p1, p2, edge):
intersect_count += 1
if intersect_count % 2 == 0:
return False
return True
def polygon_crossing(p1, poly_edges):
"""Returns True if Point p1 is internal to the polygon. The polygon is
defined by the Edges in poly_edges. Uses crossings algorithm and takes into
account edges that are collinear to p1."""
p2 = Point(INF, p1.y)
intersect_count = 0
for edge in poly_edges:
if p1.y < edge.p1.y and p1.y < edge.p2.y: continue
if p1.y > edge.p1.y and p1.y > edge.p2.y: continue
if p1.x > edge.p1.x and p1.x > edge.p2.x: continue
# Deal with points collinear to p1
edge_p1_collinear = (ccw(p1, edge.p1, p2) == COLLINEAR)
edge_p2_collinear = (ccw(p1, edge.p2, p2) == COLLINEAR)
if edge_p1_collinear and edge_p2_collinear: continue
if edge_p1_collinear or edge_p2_collinear:
collinear_point = edge.p1 if edge_p1_collinear else edge.p2
if edge.get_adjacent(collinear_point).y > p1.y:
intersect_count += 1
elif edge_intersect(p1, p2, edge):
intersect_count += 1
if intersect_count % 2 == 0:
return False
return True
def closest_point(p, graph, polygon_id, length=0.001):
"""Assumes p is interior to the polygon with polygon_id. Returns the
closest point c outside the polygon to p, where the distance from c to
the intersect point from p to the edge of the polygon is length."""
polygon_edges = graph.polygons[polygon_id]
close_point = None
close_edge = None
close_dist = None
# Finds point closest to p, but on a edge of the polygon.
# Solution from http://stackoverflow.com/a/6177788/4896361
for i, e in enumerate(polygon_edges):
num = ((p.x - e.p1.x) * (e.p2.x - e.p1.x) + (p.y - e.p1.y) *
(e.p2.y - e.p1.y))
denom = ((e.p2.x - e.p1.x)**2 + (e.p2.y - e.p1.y)**2)
u = num / denom
pu = Point(e.p1.x + u * (e.p2.x - e.p1.x),
e.p1.y + u * (e.p2.y - e.p1.y))
pc = pu
if u < 0:
pc = e.p1
elif u > 1:
pc = e.p2
d = edge_distance(p, pc)
if i == 0 or d < close_dist:
close_dist = d
close_point = pc
close_edge = e
# Extend the newly found point so it is outside the polygon by `length`.
if close_point in close_edge:
c = close_edge.p1 if close_point == close_edge.p1 else close_edge.p2
edges = list(graph[c])
v1 = unit_vector(c, edges[0].get_adjacent(c))
v2 = unit_vector(c, edges[1].get_adjacent(c))
vsum = unit_vector(Point(0, 0), Point(v1.x + v2.x, v1.y + v2.y))
close1 = Point(c.x + (vsum.x * length), c.y + (vsum.y * length))
close2 = Point(c.x - (vsum.x * length), c.y - (vsum.y * length))
if point_in_polygon(close1, graph) == -1:
return close1
return close2
else:
v = unit_vector(p, close_point)
return Point(close_point.x + v.x * length,
close_point.y + v.y * length)
def __expand_tree(self, tree: Tree): # TODO fix when line gets out of bounds
"""
Inserts one new node to a given tree as per RRT.
:param tree:
:return:
"""
while True:
random_point = self.__get_random_point()
nearest_point = min(tree.nodes(), key=lambda x: random_point.dist(x))
if random_point.dist(nearest_point) <= self.delta:
delta_point = random_point
else:
diff_point = random_point - nearest_point
theta = atan2(diff_point.y, diff_point.x)
delta_point = Point(nearest_point.x + self.delta * cos(theta),
nearest_point.y + self.delta * sin(theta))
joining_line = LineSegment(nearest_point, delta_point)
if self.__line_collides_with_obstacles(joining_line):
continue
self.graph.add_line(joining_line)
self.graph.add_point(delta_point)
tree.insert(delta_point, nearest_point)
return delta_point
from MPT import Road
from cros import Intersection, create_uid
from graph import Point, Edge
import xml.dom.minidom as xmldom
import matplotlib.pyplot as plt
#id->point
pointMap = dict()
domObj = xmldom.parse("simple.osm")
elementObj = domObj.documentElement
nodes = elementObj.getElementsByTagName("node")
for node in nodes:
nodeId = node.getAttribute("id")
x = node.getAttribute("x")
y = node.getAttribute("y")
point = Point(0, nodeId, x, y)
pointMap[nodeId] = point
ways = elementObj.getElementsByTagName("way")
for way in ways:
points = way.getElementsByTagName("nd")
edge = Edge()
xx = []
yy = []
for index in range(0, len(points)):
pointId = points[index].getAttribute("ref")
edge.addPoint(pointId)
xx.append(float(pointMap[pointId].x))
yy.append(float(pointMap[pointId].y))
#plt.plot(xx, yy, color = 'r')
nd0 = Node(conf0)
nd1 = Node(conf1)
ed = Edge(nd0, nd1, turning_radius)
s = 1
phi_max = 1
rou_min = 0.5
ob_center = [(0, -1), (0, 1)]
dt = 0.2
ob_r = [1 - dt, 1 - dt]
cspace = [(-3, 3), (-1, 1)]
obs = []
obs_ct = Point.point_from_list(ob_center)
path = []
rrt = RRt(phi_max=phi_max,
s=s,
rou_min=rou_min,
cspace=cspace,
ob_center=obs_ct,
ob_radio=ob_r)
flag, inter = rrt.check_collision(ed, 0)
print(flag)
print(inter)
fig, ax = plt.subplots()
def move_step(self, pt1: 'Point', pt2: 'Point', ratio) -> 'DubinsConfig':
x = pt1.x + ratio * (pt2.x - pt1.x)
y = pt1.y + ratio * (pt2.y - pt1.y)
return Point(x, y)
size = 1000
array = np.ones((size, size))
array[500:700, 0:300] += 1
array[298:303, 0:300] += 1
array[590:610, 330:410] += 1
array[600:700, 430:440] += 1
with open('wyniki.csv', 'a', encoding='utf-8') as csvfile:
csvwriter = csv.writer(csvfile)
csvwriter.writerow(['liczba nodow', 'nody ogolem', 'czas tworzenia grafu', 'czas szukania sciezki'])
amounts = (2, 4, 5, 8, 10, 20, 25, 40, 50, 100, 125, 200)
for i in amounts:
graph = Graph()
start1 = datetime.datetime.now()
graph.generate_from_map(array, i)
duration1 = datetime.datetime.now() - start1
print('-------------------------------------')
print(i)
print(duration1)
start2 = datetime.datetime.now()
x = graph.get_path(Point(10, 10), Point(650, 350))
duration2 = datetime.datetime.now() - start2
print(duration2)
print('-------------------------------------')
with open('wyniki.csv', 'a', encoding='utf-8') as csvfile:
csvwriter = csv.writer(csvfile)
csvwriter.writerow([i, i*i, duration1, duration2])
array = np.ones((size, size))
array[500:700, 0:300] += 1
array[298:303, 0:300] += 1
array[590:610, 330:410] += 1
array[600:700, 430:440] += 1
start = datetime.datetime.now()
graph = Graph()
graph.generate_from_map(array, 100)
duration = datetime.datetime.now() - start
print('-------------------------------------')
print(duration)
#print('grafik ok')
start = datetime.datetime.now()
x = graph.get_path(Point(50, 50), Point(650, 350))
duration = datetime.datetime.now() - start
print(duration)
# x1 = list()
# y1 = list()
# xs = list()
# ys = list()
#
# for node in graph.nodes:
# xs.append(node.position.x)
# ys.append(node.position.y)
# for n in node.neighbours:
# plt.plot((node.position.x, n[0].position.x), (node.position.y, n[0].position.y), 'k-', lw=2)
#
# for node in x:
def plot(self, d, r):
#r+d -d =r
L = self.findL(r + d)
#plt.xlim(0,35)
#plt.ylim(5,35)
#plt.gca().set_aspect('equal',adjustable = 'box')
#绘制第i条弧线,位于第i点与i-1之间
for i in range(self._n):
if abs(math.pi - self.theta[i]) > 0.01: #不为直线
#旋转方向:
xx = []
yy = []
n1 = np.array([self.road[i - 1][2], self.road[i - 1][3], 0])
n2 = np.array([self.road[i][2], self.road[i][3], 0])
o = np.cross(n1, n2)
o = o / np.linalg.norm(o)
lam = np.sign(o[2])
p = np.cross(n1, o) * (-lam)
p2 = np.cross(n2, o) * (-lam)
#圆心
n3 = n1 + n2
n3 = n3 / np.linalg.norm(n3)
if (lam >= 0):
ox = self.ox + n3[0] * L / (math.cos(0.5 * self.theta[i]))
oy = self.oy + n3[1] * L / (math.cos(0.5 * self.theta[i]))
else:
ox = self.ox + n3[0] * L / (math.cos(math.pi -
0.5 * self.theta[i]))
oy = self.oy + n3[1] * L / (math.cos(math.pi -
0.5 * self.theta[i]))
#初始点
'''
xx.append(self.road[i-1][0]+d*p[0])
yy.append(self.road[i-1][1]+d*p[1])
'''
'''
print(">>>>>>>")
print(L)
print(self.ox+L*self.road[i-1][2])
print(self.oy+L*self.road[i-1][3])
print(">>>>>>>")
'''
self.pointMap[self.road[i - 1][5]].adjVertex.remove(self.Id)
point = Point(1, create_uid(),
self.ox + L * self.road[i - 1][2],
self.oy + L * self.road[i - 1][3])
self.pointMap[self.road[i - 1][5]].adjVertex.append(
point.pointId)
self.pointMap[point.pointId] = point
point.adjVertex.append(self.road[i - 1][5])
self.oneDegreePoints.append(point.pointId)
curvex = [self.ox + L * self.road[i - 1][2] + d * p[0]]
curvey = [self.oy + L * self.road[i - 1][3] + d * p[1]]
for _i in range(1,
int(lam * (math.pi - self.theta[i]) / 0.01)):
x0 = (curvex[0] - ox) * math.cos(-lam * _i * 0.01) - (
curvey[0] - oy) * math.sin(-lam * _i * 0.01) + ox
y0 = (curvex[0] - ox) * math.sin(-lam * _i * 0.01) + (
curvey[0] - oy) * math.cos(-lam * _i * 0.01) + oy
curvex.append(x0)
curvey.append(y0)
#结束点
curvex.append(self.ox + L * self.road[i][2] - d * p2[0])
curvey.append(self.oy + L * self.road[i][3] - d * p2[1])
xx = xx + curvex
yy = yy + curvey
#终点
'''
xx.append(self.road[i][0]-d*p2[0])
yy.append(self.road[i][1]-d*p2[1])
'''
plt.plot(xx, yy, color='coral')
else: #绘制直线
#计算上一个弧线以便获取方向，负值是循环的
n1 = np.array([self.road[i - 2][2], self.road[i - 2][3], 0])
n2 = np.array([self.road[i - 1][2], self.road[i - 1][3], 0])
o = np.cross(n2, n1)
o = o / np.linalg.norm(o)
p = np.cross(n1, o)
p2 = np.cross(n2, o)
xx = []
yy = []
#初始点
'''
xx.append(self.road[i-1][0]+d*p2[0])
yy.append(self.road[i-1][1]+d*p2[1])
'''
self.pointMap[self.road[i - 1][5]].adjVertex.remove(self.Id)
point = Point(1, create_uid(),
self.ox + L * self.road[i - 1][2],
self.oy + L * self.road[i - 1][3])
self.pointMap[self.road[i - 1][5]].adjVertex.append(
point.pointId)
self.pointMap[point.pointId] = point
point.adjVertex.append(self.road[i - 1][5])
self.oneDegreePoints.append(point.pointId)
#旋转起始点
xx.append(self.ox + L * self.road[i - 1][2] + d * p2[0])
yy.append(self.oy + L * self.road[i - 1][3] + d * p2[1])
#旋转结束点
xx.append(self.ox + L * self.road[i][2] + d * p2[0])
yy.append(self.oy + L * self.road[i][3] + d * p2[1])
#结束点
'''
xx.append(self.road[i][0]+d*p2[0])
yy.append(self.road[i][1]+d*p2[1])
'''
plt.plot(xx, yy, color='coral')
def game_loop():
sim = Simulator()
gameExit = False
sx = 0
sy = 0
ex = 0
ey = 0
i = 0
flag = False
a = False
d = False
while not gameExit:
# Event loop
for event in pygame.event.get():
pos = pygame.mouse.get_pos()
if event.type == pygame.KEYUP:
if event.key == pygame.K_q:
pygame.quit()
quit()
elif event.key == pygame.K_h:
help_screen()
elif event.key == pygame.K_g:
sim.show_static_visgraph = not sim.show_static_visgraph
elif event.key == pygame.K_m:
sim.show_mouse_visgraph = not sim.show_mouse_visgraph
elif event.key == pygame.K_d:
sim.toggle_draw_mode()
elif event.key == pygame.K_s:
sim.toggle_shortest_path_mode()
d = True
a = False
elif event.key == pygame.K_a:
sim.toggle_shortest_path_mode1()
a = True
d = False
elif event.key == pygame.K_e:
flag = True
sx = ex
sy = ey
ex = 0
ey = 0
if d:
sim.toggle_shortest_path_mode()
if a:
sim.toggle_shortest_path_mode1()
if sim.mode_draw:
if event.type == pygame.KEYUP:
if event.key == pygame.K_u:
sim.draw_point_undo()
elif event.key == pygame.K_c:
sim.clear_all()
elif event.type == pygame.MOUSEBUTTONUP:
if event.button == LEFT:
sim.work_polygon.append(Point(pos[0], pos[1]))
elif event.button == RIGHT:
sim.close_polygon()
if sim.mode_path and sim.built:
if event.type == pygame.MOUSEBUTTONUP or any(
pygame.mouse.get_pressed()):
if pygame.mouse.get_pressed()[LEFT -
1] or event.button == LEFT:
sim.start_point = Point(pos[0], pos[1])
sx = pos[0]
sy = pos[1]
elif pygame.mouse.get_pressed()[
RIGHT - 1] or event.button == RIGHT:
if flag:
sim.start_point = Point(sx, sy)
sim.end_point = Point(pos[0], pos[1])
ex = pos[0]
ey = pos[1]
if sim.start_point and sim.end_point:
sim.path.append(
sim.g.shortest_path(sim.start_point,
sim.end_point))
if sim.mode_path1 and sim.built:
if event.type == pygame.MOUSEBUTTONUP or any(
pygame.mouse.get_pressed()):
if pygame.mouse.get_pressed()[LEFT -
1] or event.button == LEFT:
sim.start_point = Point(pos[0], pos[1])
sx = pos[0]
sy = pos[1]
elif pygame.mouse.get_pressed()[
RIGHT - 1] or event.button == RIGHT:
if flag:
sim.start_point = Point(sx, sy)
sim.end_point = Point(pos[0], pos[1])
ex = pos[0]
ey = pos[1]
if sim.start_point and sim.end_point:
sim.path.append(
sim.g.shortest_path1(sim.start_point,
sim.end_point))
if sim.show_mouse_visgraph and sim.built:
if event.type == pygame.MOUSEMOTION:
sim.mouse_point = Point(pos[0], pos[1])
sim.mouse_vertices = sim.g.find_visible(sim.mouse_point)
# Display loop
gameDisplay.fill(white)
gameDisplay.blit(background1, (0, 0))
gameDisplay.blit(startpt, (sx, sy))
gameDisplay.blit(endpt, (ex, ey))
if len(sim.work_polygon) > 1:
draw_polygon(sim.work_polygon, black, 3, complete=False)
if len(sim.polygons) > 0:
for polygon in sim.polygons:
draw_polygon(polygon, black, 3)
if sim.built and sim.show_static_visgraph:
draw_visible_vertices(sim.g.visgraph.get_edges(), gray, 1)
if sim.built and sim.show_mouse_visgraph and len(
sim.mouse_vertices) > 0:
draw_visible_mouse_vertices(sim.mouse_point, sim.mouse_vertices,
gray, 1)
if len(sim.path) > 1:
for p in sim.path:
draw_polygon(p, red, 3, complete=False)
if sim.mode_draw:
draw_text("-- DRAW MODE --", black, 25, 5, 5)
elif sim.mode_path:
draw_text("-- Dijkstra Algorithm --", black, 25, 5, 5)
elif sim.mode_path1:
draw_text("-- A* Algorithm --", black, 25, 5, 5)
else:
draw_text("-- VIEW MODE --", black, 25, 5, 5)
pygame.display.update()
clock.tick(20)
def visible_vertices(point, graph, origin=None, destination=None, scan='full'):
"""Returns list of Points in graph visible by point.
If origin and/or destination Points are given, these will also be checked
for visibility.
"""
edges = graph.get_edges()
points = graph.get_points()
if origin: points.append(origin)
if destination: points.append(destination)
points.sort(key=lambda p: (angle(point, p), edge_distance(point, p)))
# Initialize open_edges with any intersecting edges on the half line from
open_edges = OpenEdges()
point_inf = Point(INF, point.y)
for edge in edges:
if point in edge: continue
if edge_intersect(point, point_inf, edge):
if on_segment(point, edge.p1, point_inf): continue
if on_segment(point, edge.p2, point_inf): continue
open_edges.insert(point, point_inf, edge)
visible = []
prev = None
prev_visible = None
for p in points:
if p == point: continue
if scan == 'half' and angle(point, p) > pi: break
# Update open_edges - remove clock wise edges incident on p
if open_edges:
for edge in graph[p]:
if ccw(point, p, edge.get_adjacent(p)) == CW:
open_edges.delete(point, p, edge)
is_visible = False
# Non-collinear points
if prev is None or ccw(point, prev, p) != COLLINEAR or not on_segment(
point, prev, p):
if len(open_edges) == 0:
is_visible = True
elif not edge_intersect(point, p, open_edges.smallest()):
is_visible = True
# For collinear points
elif not prev_visible:
is_visible = False
# For collinear points, if previous point was visible, need to check
# that the edge from prev to p does not intersect any open edge.
else:
is_visible = True
for edge in open_edges:
if prev not in edge and edge_intersect(prev, p, edge):
is_visible = False
break
if is_visible and edge_in_polygon(prev, p, graph):
is_visible = False
# Check if the visible edge is interior to its polygon
if is_visible and p not in graph.get_adjacent_points(point):
is_visible = not edge_in_polygon(point, p, graph)
if is_visible: visible.append(p)
# Update open_edges - Add counter clock wise edges incident on p
for edge in graph[p]:
if (point not in edge) and ccw(point, p,
edge.get_adjacent(p)) == CCW:
open_edges.insert(point, p, edge)
prev = p
prev_visible = is_visible
return visible
def unit_vector(c, p):
magnitude = edge_distance(c, p)
return Point((p.x - c.x) / magnitude, (p.y - c.y) / magnitude)
def main():
point1 = Point(-1, -1, 1)
point2 = Point(-5, -1, 1)
point3 = Point(-5, 2, 1)
point4 = Point(-3, 2, 1)
point5 = Point(-2, 2, 2)
point6 = Point(2, 2, 2)
point7 = Point(2, -4, 2)
point8 = Point(-1, -1.25, 3)
point9 = Point(-5, -2.5, 3)
point10 = Point(-5, -5, 3)
point11 = Point(1.2, -5, 3)
point12 = Point(1.2, -4, 3)
s1 = []
s2 = []
s3 = []
s1.append(point1)
s1.append(point2)
s1.append(point3)
s1.append(point4)
s2.append(point5)
s2.append(point6)
s2.append(point7)
s3.append(point8)
s3.append(point9)
s3.append(point10)
s3.append(point11)
s3.append(point12)
Polylist = []
poly1 = Polygon(s1)
poly2 = Polygon(s2)
poly3 = Polygon(s3)
Polylist.append(poly1)
Polylist.append(poly2)
Polylist.append(poly3)
start = Point(-1, -1.75, 0)
end = Point(-5, -1.75, 0)
gtuple = generate_visible_edges(Polylist, start, end)
visibility_graph = Graph(len(gtuple[1]))
for edge in gtuple[0]:
dist = edge.gx()
# print(edge, dist)
visibility_graph.addEdge(edge.p1.point_id, edge.p2.point_id, dist,
edge.p1.point_distance(end),
edge.p2.point_distance(end))
# for x, y in visibility_graph.graph.items():
# print(x, y)
path = visibility_graph.a_star(start.point_id, end.point_id)
solution = []
for p in path:
for i in gtuple[1]:
if p == i.point_id:
solution.append(i)
print(solution)
def create_diagram(self,
points: list,
vis_steps=False,
vis_result=False,
vis_tree=False,
vis_before_clipping=False,
verbose=False,
justEdge=False):
"""
Create the Voronoi diagram.
:param points: (list) The list of cell points to make the diagram for
:param vis_steps: (bool) Visualize intermediate steps
:param vis_result: (bool) Visualize the final result
:param vis_tree: (bool) Visualize the status of the binary tree (text-based)
:param vis_before_clipping: Visualize the intermediate final result before clipping
:param verbose: (bool) Print debug information
"""
points = [Point(x, y) for x, y in points]
# Initialize all points
self.initialize(points)
index = 0
# The first point (needed for bounding box)
genesis_point = None
while not self.event_queue.empty():
Tell.print(verbose, "Queue", self.event_queue.queue)
# Pop the event queue with the highest priority
event = self.event_queue.get()
# Set genesis point
genesis_point = genesis_point or event.point
# Handle circle events
if isinstance(event, CircleEvent) and event.is_valid:
# Update sweep line position
self.sweep_line = event.y
# Debugging
Tell.print(
verbose,
f"-> Handle circle event at {event.y} with center {event.center} and arcs {event.point_triple}"
)
# Handle the event
self.handle_circle_event(event, verbose=verbose)
# Visualization
if vis_steps and vis_tree:
self.beach_line.visualize()
if vis_steps:
visualize(self.sweep_line,
current_event=event,
bounding_poly=self.bounding_poly,
points=self.points,
vertices=self.vertices,
edges=self.edges,
arc_list=self.arcs,
event_queue=self.event_queue)
# Handle site events
elif isinstance(event, SiteEvent):
# Give the points a simple name
event.point.name = "ABCDEFGHIJKLMNOPQRSTUVWXYZ"[index % 26]
index += 1
# Update sweep line position
self.sweep_line = event.y
# Debugging
Tell.print(
verbose,
f"-> Handle site event at {event.y} with point {event.point}"
)
# Handle the event
self.handle_site_event(event, verbose=verbose)
# Visualization
if vis_steps and vis_tree:
self.beach_line.visualize()
if vis_steps:
visualize(y=self.sweep_line,
current_event=event,
bounding_poly=self.bounding_poly,
points=self.points,
vertices=self.vertices,
edges=self.edges,
arc_list=self.arcs,
event_queue=self.event_queue)
if vis_before_clipping:
visualize(y=-1000,
current_event="nothing",
bounding_poly=self.bounding_poly,
points=self.points,
vertices=self.vertices,
edges=self.edges,
arc_list=self.arcs,
event_queue=self.event_queue)
# Finish with the bounding box
self.edges, polygon_vertices = self.bounding_poly.finish_edges(
self.edges, verbose)
self.edges, self.vertices = self.bounding_poly.finish_polygon(
self.edges, self.vertices, self.points)
# Final visualization
if vis_result:
visualize(-1000,
current_event="Final result",
bounding_poly=self.bounding_poly,
points=self.points,
vertices=self.vertices,
edges=self.edges,
arc_list=self.arcs,
event_queue=self.event_queue,
calc_cell_sizes=True)
# This is where you add reroute for basicVisual
if justEdge:
basicVisualize(-1000,
current_event="Just Edges",
bounding_poly=self.bounding_poly,
points=self.points,
vertices=self.vertices,
edges=self.edges)
