def line_walk_double_edges_e(he, ray, b):
nxt = he.nxt
prev = he.prev
v1 = he.node
v2 = nxt.node
v3 = prev.node
s = Segment(v1.p, v2.p)
inter = intersection(s, ray)
if inter != [] and is_segment(inter[0]):
end_node = v1
if ray.contains(v2.p):
end_node = v2
yield from line_walk_double_edges_v(end_node, Ray(end_node.p, b), b)
raise StopIteration
e1 = Segment(v1.p, v3.p)
e2 = Segment(v3.p, v2.p)
inter1 = intersection(e1, ray)
inter2 = intersection(e2, ray)
if inter1 != [] and inter2 != []:
yield from line_walk_double_edges_v(v3, Ray(inter1[0], b), b)
raise StopIteration
if inter1 != []:
yield e1
if (prev.twin != None):
yield from line_walk_double_edges_e(prev.twin, Ray(inter1[0], b), b)
raise StopIteration
if inter2 != []:
yield e2
if (nxt.twin != None):
yield from line_walk_double_edges_e(nxt.twin, Ray(inter2[0], b), b)
raise StopIteration
raise StopIteration
def initialize_status(self):
sweep_ray = Ray(self.origin, self.event_queue[0].p)
i = 0
for ep in self.event_queue:
if ep.type == DEFAULT_VERTEX:
segment = Segment(ep.p, ep.twin.p)
intersection_point = sweep_ray.intersection(segment)
if len(intersection_point) > 0:
# If the segments first point is the current event-point
if intersection_point[
0] == ep.p: # if the point is on the initial ray
if len(
Ray(self.origin, self.event_queue[
i + 1].p).intersection(segment)
) > 0: #if the point was a start point
self.initialize_segment(ep, intersection_point)
else:
self.initialize_segment(ep.twin,
intersection_point)
else:
self.initialize_segment(ep.twin, intersection_point)
else:
# Event-points not hit by the ray gets a type
ep.type = START_VERTEX
ep.twin.type = END_VERTEX
i += 1
def during_collision(cue_point, radius, stick_point, ball_coord):
future_point = cue_point
collision_ball_info = cue_point
min_distance = 1e9
temp_ray = Ray(cue_point, stick_point)
temp_Line = Line(cue_point, stick_point)
temp_circle = Circle(cue_point, radius)
temp_collision_points = intersection(temp_Line, temp_circle)
if temp_ray.contains(temp_collision_points[0]):
white_ray = Ray(cue_point, temp_collision_points[1])
else:
white_ray = Ray(cue_point, temp_collision_points[0])
for coord in ball_coord:
enlarged_ball = Circle(Point(coord[0], coord[1]), coord[2] + radius)
intersect_point = intersection(white_ray, enlarged_ball)
if len(intersect_point) == 2 and cue_point.distance(
intersect_point[0]) >= cue_point.distance(intersect_point[1]):
temp_point = intersect_point[1]
elif len(intersect_point) == 2 or len(intersect_point) == 1:
temp_point = intersect_point[0]
else:
continue
dist = cue_point.distance(temp_point)
if min_distance > dist:
min_distance = dist
future_point = temp_point
collision_ball_info = coord
return future_point, collision_ball_info
def __init__(self, origin: Point, theta: float, phi: float):
self.intensity = (abs(math.sin(abs(theta))) +
abs(math.sin(abs(phi)))) / 2
x = math.sin(phi) * math.cos(theta)
y = math.sin(phi) * math.sin(theta)
z = math.cos(phi)
self.ray = Ray(origin, Point(x, -y, z))
# Array used for storing segments of reflected ray
self.ray_array = [Ray(origin, Point(x, -y, z))]
self.road_intersection = []
def line_walk_nodes_and_edges_and_triangles_e(v1, v2, ray, b):
s = Segment(v1.p, v2.p)
edge = list(set(v1.edges) & set(v2.edges))[0]
inter = intersection(s, ray)
if (inter != [] and is_segment(inter[0])):
end_node1 = edge.nodes[0]
end_node2 = edge.nodes[1]
if ray.contains(end_node2.p):
end_node1 = end_node2
yield from line_walk_nodes_and_edges_and_triangles_v(end_node1, Ray(end_node1.p, b), b)
raise StopIteration
s_b = turn(cgPoint(int(b.x), int(b.y)), cgPoint(int(v1.p.x), int(v1.p.y)), cgPoint(int(v2.p.x), int(v2.p.y)))
se = None
for index, n_triangle in enumerate(edge.triangles):
mb_e = set(n_triangle.edges)
mb_e.remove(edge)
mb_e = list(mb_e)
o_v = list(set(mb_e[0].nodes) & set(mb_e[1].nodes))[0]
if s_b != turn(cgPoint(int(o_v.p.x), int(o_v.p.y)), cgPoint(int(v1.p.x), int(v1.p.y)),
cgPoint(int(v2.p.x), int(v2.p.y))):
continue
t_e1 = mb_e[0].nodes
t_e2 = mb_e[1].nodes
v1 = t_e1[0]
v1_n = t_e1[1]
e1 = Segment(v1.p, v1_n.p)
v2 = t_e2[0]
v2_n = t_e2[1]
e2 = Segment(v2.p, v2_n.p)
inter1 = intersection(e1, ray)
inter2 = intersection(e2, ray)
if inter1 != [] and inter2 != []:
yield from line_walk_nodes_and_edges_and_triangles_v(o_v, Ray(inter1[0], b), b)
raise StopIteration
if inter1 != []:
yield e1
yield from line_walk_nodes_and_edges_and_triangles_e(v1, v1_n, Ray(inter1[0], b), b)
raise StopIteration
if inter2 != []:
yield e2
yield from line_walk_nodes_and_edges_and_triangles_e(v2, v2_n, Ray(inter2[0], b), b)
raise StopIteration
raise StopIteration
def line_walk_nodes_and_triangles_e(v1, v2, ray, b):
s_b = turn(cgPoint(int(b.x), int(b.y)), cgPoint(int(v1.p.x), int(v1.p.y)), cgPoint(int(v2.p.x), int(v2.p.y)))
s = Segment(v1.p, v2.p)
inter = intersection(s, ray)
if inter != [] and is_segment(inter[0]):
end_node = v1
if ray.contains(v2.p):
end_node = v2
yield from line_walk_nodes_and_triangles_v(end_node, Ray(end_node.p, b), b)
tp = set(v1.triangles) & set(v2.triangles)
tp.discard(None)
vs = list(tp)
vp = []
for i in vs:
ts = set(i.nodes)
ts.discard(v1)
ts.discard(v2)
vp = vp + list(ts)
v3 = [i for i in vp
if s_b == turn(cgPoint(int(i.p.x), int(i.p.y)), cgPoint(int(v1.p.x), int(v1.p.y)), cgPoint(int(v2.p.x), int(v2.p.y)))
]
if v3 == []:
raise StopIteration
e1 = Segment(v1.p, v3[0].p)
e2 = Segment(v2.p, v3[0].p)
inter1 = intersection(e1, ray)
inter2 = intersection(e2, ray)
if inter1 != [] and inter2 != []:
yield from line_walk_nodes_and_triangles_v(v3[0], Ray(inter1[0], b), b)
raise StopIteration
if inter1 != []:
yield e1
yield from line_walk_nodes_and_triangles_e(v1, v3[0], Ray(inter1[0], b), b)
raise StopIteration
if inter2 != []:
yield e2
yield from line_walk_nodes_and_triangles_e(v2, v3[0], Ray(inter2[0], b), b)
raise StopIteration
raise StopIteration
def main():
image_address = str(input('Enter the image name: '))
image_address = "pool_Images/" + image_address
print(image_address)
ball_coord, cue_coord, stick_coord = detection.detect_coordinates2(image_address)
#print(cue_coord, ball_coord, stick_coord)
if len(cue_coord) == 0 or len(stick_coord) == 0:
print("No point detected")
return
cue_point = Point(cue_coord[0], cue_coord[1])
stick_point = Point(stick_coord[0], stick_coord[1])
path_of_white_ball(cue_point, stick_coord, cue_coord[2])
#path_of_white_ball(p1, p2, RADIUS)
future_point, collision_ball_info = during_collision(cue_point, cue_coord[2],stick_point,ball_coord)
if future_point == cue_point:
print("No collision")
return
else:
frame = cv.imread(image_address, 1)
cv.circle(frame, (int(collision_ball_info[0]), int(collision_ball_info[1])), 2*int(collision_ball_info[2]), (0, 255, 255), 2)
cv.circle(frame, (int(future_point.x), int(future_point.y)), int(cue_coord[2]), (255, 255, 255), -1)
temp_point = Point(collision_ball_info[0],collision_ball_info[1])
colored_future_point = intersection(Ray(future_point,temp_point),Circle(temp_point,collision_ball_info[2]*5))
cv.line(frame, (cue_point.x, cue_point.y), (future_point.x, future_point.y), (255, 255, 255), thickness=2)
cv.line(frame, ((int)(collision_ball_info[0]), (int)(collision_ball_info[1])), ((int)(colored_future_point[0].x), (int)(colored_future_point[0].y)), (0, 255, 0), thickness=2)
while 1:
cv.namedWindow('Image',cv.WND_PROP_FULLSCREEN)
cv.imshow('Image', frame)
if cv.waitKey(1) & 0xFF == 27:
break
cv.destroyAllWindows()
def __init__(self, origin: Point, ray_angle: float, base_angle: int):
self.ray_length = 1
# Intensity is calculated according to Lambertian distribution
self.intensity = abs(math.sin(math.radians(abs(ray_angle - base_angle))))
self.original_intensity = abs(math.sin(math.radians(abs(ray_angle - base_angle))))
self.end_intensity = self.intensity * 1 / (self.ray_length * self.ray_length)
# End coordinates are calculated from ray angle
x_coordinate = 10000 * math.cos(math.radians(ray_angle)) + origin.x
y_coordinate = 10000 * math.sin(math.radians(ray_angle)) + origin.y
# Ray goes from the origin in direction of end coordinates
self.ray = Ray(origin, Point(x_coordinate, y_coordinate))
# Array used for storing segments of reflected ray
self.ray_array = [Ray(origin, Point(x_coordinate, y_coordinate))]
self.road_intersection = []
# If the ray is reflected too many time or if it is reflected back into the device, it will be terminated
self.terminated = False
def line_walk_nodes_and_edges_and_triangles_v(node, ray, b):
for n_triangles in node.triangles:
for n_edge in n_triangles.edges:
end_node = None
if n_edge.nodes[0] == node:
end_node = n_edge.nodes[1]
elif n_edge.nodes[1] == node:
end_node = n_edge.nodes[0]
else:
continue
s = Segment(node.p, end_node.p)
inter = intersection(s, ray)
if inter != [] and is_segment(inter[0]):
yield s
yield from line_walk_nodes_and_edges_and_triangles_v(end_node, Ray(end_node.p, b), b)
raise StopIteration
for n_triangles in node.triangles:
for n_edge in n_triangles.edges:
if n_edge.nodes[0] != node and n_edge.nodes[1] != node:
node_i = n_edge.nodes[0]
node_j = n_edge.nodes[1]
s = Segment(node_i.p, node_j.p)
inter = intersection(s, ray)
if inter != []:
yield s
yield from line_walk_nodes_and_edges_and_triangles_e(node_i, node_j, Ray(inter[0], b), b)
raise StopIteration
raise StopIteration
def predict_path(circle_coordinates,cue_ball_coordinate,cue_stick_coordinate,frame):
global prev_stick_point, prev_cue_point
if len(cue_ball_coordinate) < 3 or len(cue_stick_coordinate) < 2:
print("No point detected")
return frame
cue_point = Point(cue_ball_coordinate[0], cue_ball_coordinate[1])
stick_point = Point(cue_stick_coordinate[0], cue_stick_coordinate[1])
future_point, collision_ball_info = during_collision(cue_point, cue_ball_coordinate[2],stick_point,circle_coordinates)
prev_stick_point = stick_point
prev_cue_point = cue_point
flag = 0
if future_point == cue_point:
print("No collision")
else:
print("Collision")
cv.circle(frame, (int(collision_ball_info[0]), int(collision_ball_info[1])), 2*int(collision_ball_info[2]), (0, 255, 255), 2)
cv.circle(frame, (int(future_point.x), int(future_point.y)), int(cue_ball_coordinate[2]), (255, 255, 255), -1)
temp_point = Point(collision_ball_info[0],collision_ball_info[1])
colored_future_point = intersection(Ray(future_point,temp_point),Circle(temp_point,collision_ball_info[2]*5))
cv.line(frame, (cue_point.x, cue_point.y), (future_point.x, future_point.y), (255, 255, 255), thickness=2)
cv.line(frame, ((int)(collision_ball_info[0]), (int)(collision_ball_info[1])), ((int)(colored_future_point[0].x),
(int)(colored_future_point[0].y)), (0, 255, 0), thickness=2)
cv.circle(frame, (int(cue_stick_coordinate[0]), int(cue_stick_coordinate[1])), 3, (123, 55, 55), 4)
cv.circle(frame, (int(cue_ball_coordinate[0]), int(cue_ball_coordinate[1])),int(cue_ball_coordinate[2])*2, (100, 100, 100), 2)
cv.circle(frame, (int(cue_ball_coordinate[0]), int(cue_ball_coordinate[1])), 1, (255, 100, 100), 2)
return frame
def angle_to_next(p, points):
start_point = p[0]
end_point = next_point(p, points)[0]
try:
return Ray(start_point, end_point).smallest_angle_between(p[1])
except:
breakpoint()
def line_walk_double_edges_v(node, ray, b):
list_of_n_triangles = node.get_neighbor_triangles()
for t in list_of_n_triangles:
c_he = t.he
while(c_he.node != node):
c_he = c_he.nxt
he_prev = c_he.prev
he_nxt = c_he.nxt
v1 = c_he.node
v2 = he_nxt.node
v3 = he_prev.node
s = Segment(v1.p, v2.p)
inter = intersection(s, ray)
if inter != [] and is_segment(inter[0]):
yield s
yield from line_walk_double_edges_v(v2, Ray(v2.p, b), b)
raise StopIteration
s = Segment(v1.p, v3.p)
inter = intersection(s, ray)
if inter != [] and is_segment(inter[0]):
yield s
yield from line_walk_double_edges_v(v3, Ray(v3.p, b), b)
raise StopIteration
s = Segment(v3.p, v2.p)
inter = intersection(s, ray)
if inter != []:
yield s
if he_nxt.twin != None:
yield from line_walk_double_edges_e(he_nxt.twin, Ray(inter[0], b), b)
raise StopIteration
else:
raise StopIteration
raise StopIteration
def line_walk_node_with_neighbours_v(node, ray, b):
for n_node in node.neigh_nodes:
p = n_node.p
s = Segment(node.p, p)
inter = intersection(s, ray)
if inter != [] and is_segment(inter[0]):
yield s
yield from line_walk_node_with_neighbours_v(n_node, Ray(p, b), b)
raise StopIteration
for i, j in itertools.combinations(node.neigh_nodes, r=2):
if i in set(j.neigh_nodes):
s = Segment(i.p, j.p)
inter = intersection(s, ray)
if inter != []:
yield s
yield from line_walk_node_with_neighbours_e(i, j, Ray(inter[0], b), b)
raise StopIteration
def project_on_line(
line: Line,
points: Iterable[ProjectedPoint]) -> List[ProjectedPoint]:
some_line_point = line.p1
line_points = []
for current_point in points:
projection = line.projection(current_point)
if projection == some_line_point:
line_coordinate = 0
else:
dst_ray = Ray(some_line_point, projection)
is_inversed = dst_ray.angle_between(line) != 0
line_coordinate = some_line_point.distance(projection)
if is_inversed:
line_coordinate = -line_coordinate
line_points.append(
SegmentsInLineFinder.ProjectedPoint(current_point, projection,
line_coordinate))
line_points.sort(key=lambda point: point.line_coordinate)
return line_points
def line_walk_node_with_neighbours_e(v1, v2, ray, b):
s_b = turn(cgPoint(int(b.x), int(b.y)), cgPoint(int(v1.p.x), int(v1.p.y)), cgPoint(int(v2.p.x), int(v2.p.y)))
s = Segment(v1.p, v2.p)
inter = intersection(s, ray)
if inter != [] and is_segment(inter[0]):
end_node = v1
if ray.contains(v2.p):
end_node = v2
yield from line_walk_node_with_neighbours_v(end_node, Ray(end_node.p, b), b)
raise StopIteration
vs = list(set(v1.neigh_nodes) & set(v2.neigh_nodes))
v3 = [i for i in vs
if s_b == turn(cgPoint(int(i.p.x), int(i.p.y)), cgPoint(int(v1.p.x), int(v1.p.y)), cgPoint(int(v2.p.x), int(v2.p.y)))
]
if v3 == []:
raise StopIteration
e1 = Segment(v1.p, v3[0].p)
e2 = Segment(v2.p, v3[0].p)
inter1 = intersection(e1, ray)
inter2 = intersection(e2, ray)
if inter1 != [] and inter2 != []:
yield from line_walk_node_with_neighbours_v(v3[0], Ray(inter1[0], b), b)
raise StopIteration
if inter1 != []:
yield e1
yield from line_walk_node_with_neighbours_e(v1, v3[0], Ray(inter1[0], b), b)
raise StopIteration
if inter2 != []:
yield e2
yield from line_walk_node_with_neighbours_e(v2, v3[0], Ray(inter2[0], b), b)
raise StopIteration
def line_walk_nodes_and_triangles_v(node, ray, b):
for n_triangles in node.triangles:
if n_triangles == None:
continue
for n_node in n_triangles.nodes:
if n_node == node:
continue
end_node = n_node.p
s = Segment(node.p, end_node)
inter = intersection(s, ray)
if inter != [] and is_segment(inter[0]):
yield s
yield from line_walk_nodes_and_triangles_v(n_node, Ray(end_node, b), b)
raise StopIteration
for n_triangles in node.triangles:
if n_triangles == None:
continue
ds = set(n_triangles.nodes)
ds.remove(node)
pnts = list(ds)
node_i = pnts[0]
node_j = pnts[1]
s = Segment(node_i.p, node_j.p)
inter = intersection(s, ray)
if inter != []:
yield s
yield from line_walk_nodes_and_triangles_e(node_i, node_j, Ray(inter[0], b), b)
raise StopIteration
raise StopIteration
def update_rays(p1, p2, p3, p4, angle):
p1 = [p1[0], Ray(p1[0], angle=p1[2] - angle), p1[2] - angle, p1[3]]
p2 = [p2[0], Ray(p2[0], angle=p2[2] - angle), p2[2] - angle, p2[3]]
p3 = [p3[0], Ray(p3[0], angle=p3[2] - angle), p3[2] - angle, p3[3]]
p4 = [p4[0], Ray(p4[0], angle=p4[2] - angle), p4[2] - angle, p4[3]]
return p1, p2, p3, p4
def smallest_rectangle(points) -> Polygon:
points = points[:-1]
p1 = [None, None, None, None] # (Point, Ray, angle, index)
p2 = [None, None, None, None]
p3 = [None, None, None, None]
p4 = [None, None, None, None]
# Get the leftmost, rightmost, top and bottom starting points
for i, p in enumerate(points):
# Leftmost point
if not p1[0] or p.x < p1[0].x:
p1 = [p, Ray(p, angle=pi / 2), pi / 2, i]
# Rightmost point
if not p2[0] or p.x > p2[0].x:
p2 = [p, Ray(p, angle=3 * pi / 2), 3 * pi / 2, i]
# Top point
if not p3[0] or p.y > p3[0].y:
p3 = [p, Ray(p, angle=pi), pi, i]
# Bottom point
if not p4[0] or p.y < p4[0].y:
p4 = [p, Ray(p, angle=0), 0, i]
rotated_angle = 0
polygons = []
# Do the "walk" around the convex hull
while rotated_angle < pi / 2:
p1_delta = angle_to_next(p1, points)
p2_delta = angle_to_next(p2, points)
p3_delta = angle_to_next(p3, points)
p4_delta = angle_to_next(p4, points)
if p1_delta < min(p2_delta, p3_delta, p4_delta):
print("p1")
delta = p1_delta
p1[0], p1[3] = next_point(p1, points)
elif p2_delta < min(p3_delta, p4_delta):
print("p2")
delta = p2_delta
p2[0], p2[3] = next_point(p2, points)
elif p3_delta < p4_delta:
print("p3")
delta = p3_delta
p3[0], p3[3] = next_point(p3, points)
else:
print("p4")
delta = p4_delta
p4[0], p4[3] = next_point(p4, points)
p1, p2, p3, p4 = update_rays(p1, p2, p3, p4, delta)
rotated_angle += delta
polygons.append(get_polygon(p1[1], p2[1], p3[1], p4[1]))
polygons.sort(key=lambda x: x.area)
return polygons[0]
# lp = points[(leftmost_point[3]+1) % len(points)]
# rp = points[(p2[3]+1) % len(points)]
# tp = points[(p3[3]+1) % len(points)]
# bp = points[(p4[3]+1) % len(points)]
# leftmost_point_delta = Ray(leftmost_point[0], lp).angle_between(leftmost_point[1]).evalf()
# p2_delta = Ray(p2[0], rp).angle_between(p2[1]).evalf()
# p3_delta = Ray(p3[0], tp).angle_between(p3[1]).evalf()
# p4_delta = Ray(p4[0], bp).angle_between(p4[1]).evalf()
# delta = min(leftmost_point_delta, p2_delta, p3_delta, p4_delta)
# lpa = leftmost_point[2]-delta
# leftmost_point = (lp, Ray(lp, angle=lpa), lpa, (leftmost_point[3]+1) % len(points))
# rpa = p2[2]-delta
# p2 = (p2[0], Ray(p2[0], angle=rpa), rpa, (p2[3]+1) % len(points))
# tpa = p3[2]-delta
# p3 = (p3[0], Ray(p3[0], angle=tpa), tpa, (p3[3]+1) % len(points))
# bpa = p4[2]-delta
# p4 = (p4[0], Ray(p4[0], angle=bpa), bpa, (p4[3]+1) % len(points))
# return get_polygon(leftmost_point[1], p2[1], p3[1], p4[1])
def visibility_polygon(origin, segments, canvas):
"""
Computes the visibility polygon given a list of segments and
a reference point (origin).
"""
(e1, e2, e3, e4, e5, e6, e7,
e8) = (Point(0, 0), Point(1, 0), Point(500, 0), Point(500, 1),
Point(500, 500), Point(499, 500), Point(0, 500), Point(0, 499))
(s1, s2, s3, s4) = (OrderedSegment(Segment(e2, e3), origin),
OrderedSegment(Segment(e4, e5), origin),
OrderedSegment(Segment(e6, e7), origin),
OrderedSegment(Segment(e8, e1), origin))
endpoints = ([
EndPoint(e1, origin, s4),
EndPoint(e2, origin, s1),
EndPoint(e3, origin, s1),
EndPoint(e4, origin, s2),
EndPoint(e5, origin, s2),
EndPoint(e6, origin, s3),
EndPoint(e7, origin, s3),
EndPoint(e8, origin, s4)
])
for s in segments:
s = OrderedSegment(s, origin)
endpoints.extend([
EndPoint(s.segment.p1, origin, s),
EndPoint(s.segment.p2, origin, s)
])
endpoints.sort()
# if not endpoints[0].is_starting_point():
# twin = endpoints[0].get_twin()
# while endpoints[0] != twin:
# endpoints = endpoints[-1:] + endpoints[:-1]
for i, e in enumerate(endpoints):
canvas.create_text(e.point.x.evalf() + 250,
e.point.y.evalf() + 250,
fill="blue",
text=f"{i}")
polygon_points = []
status = AVLTree()
# Make sure that we know about any segments that intersect
# the sweep line at t=0.
for p in endpoints[1:]:
if Ray(origin, endpoints[0].point).intersection(
p.segment.segment) != []:
if p.segment not in status:
status.insert(p.segment, p.segment)
#for e in endpoints:
i = 0
while len(polygon_points) < 2 or polygon_points[0] != polygon_points[-1]:
e = endpoints[i % len(endpoints)]
if e.segment in status:
if status.min_item()[0] == e.segment:
polygon_points.append(e.point)
draw_polygon(canvas, polygon_points)
if not e.is_starting_point() and len(status) > 1:
for above_segment in status:
if above_segment == e.segment:
continue
try:
polygon_points.append(
Ray(origin, e.point).intersection(
above_segment.segment)[0])
draw_polygon(canvas, polygon_points)
break
except:
pass
if not e.is_starting_point():
status.remove(e.segment)
else:
# if not e.is_starting_point():
# raise Exception("What the hell")
status.insert(e.segment, e.segment)
if status.min_item()[0] == e.segment:
if len(status) > 1:
for above_segment in status:
if above_segment == e.segment:
continue
try:
polygon_points.append(
Ray(origin, e.point).intersection(
above_segment.segment)[0])
draw_polygon(canvas, polygon_points)
break
except:
pass
polygon_points.append(e.point)
draw_polygon(canvas, polygon_points)
i += 1
if i > 100:
break
def perform_sweep(self):
print("\nStatus at start: " + str(len(self.status)))
for ep in self.event_queue:
print("\nStatus: " + str(len(self.status)))
if ep.type == START_VERTEX:
print("START_VERTEX")
status_segment = StatusSegment(ep, ep.twin, self.origin)
print("current segment: " + str(status_segment.segment) +
str(status_segment.current_distance))
ep.status_segment = status_segment
ep.twin.status_segment = status_segment
if self.status.__len__() == 0:
self.status.update(
{status_segment.current_distance: status_segment})
self.visibility_polygon.append(ep.p)
print("empty status. Append")
else:
first_in_status = self.status.peekitem(index=0)
print("first in status and distance: " +
str(first_in_status[1].segment) + ": " +
str(first_in_status[1].current_distance))
current_ray = Ray(self.origin, ep.p)
intersection_point = current_ray.intersection(
first_in_status[1].segment)
first_in_status[1].current_distance = distance(
intersection_point[0], self.origin)
print("First in status new distance: " +
str(first_in_status[1].current_distance))
self.status.update(
{status_segment.current_distance:
status_segment}) # insert the new segment to status
self.status.__delitem__(first_in_status[0])
self.status.update({
first_in_status[1].current_distance:
first_in_status[1]
}) #update the key distance to the origin
new_first_in_status = self.status.peekitem(index=0)
if new_first_in_status[1] != first_in_status[1]:
self.visibility_polygon.append(intersection_point[0])
self.visibility_polygon.append(ep.p)
print("normal status. Append")
elif ep.type == END_VERTEX:
print("END_VERTEX")
first_in_status = self.status.peekitem(index=0)
print("first in status and distance: " +
str(first_in_status[1].segment) + ": " +
str(first_in_status[1].current_distance))
self.status.__delitem__(ep.status_segment.current_distance)
print("ep status segment and distance: " +
str(ep.status_segment.segment) + ": " +
str(ep.status_segment.current_distance))
if self.status.__len__() == 0:
self.visibility_polygon.append(ep.p)
print("empty status. Append")
else:
new_first_in_status = self.status.peekitem(index=0)
if new_first_in_status[1] != first_in_status[1]:
current_ray = Ray(self.origin, ep.p)
intersection_point = current_ray.intersection(
new_first_in_status[1].segment)
self.visibility_polygon.append(ep.p)
self.visibility_polygon.append(intersection_point[0])
print("normal status. Append")
from typing import List, Tuple
import pytest
from sympy import Rational, Ray, Segment, Point, cos, pi
from custom_geometry import compute_reflection, prepare_intersections, compute_intersections, rotate_segment, \
change_size_segment
from custom_ray import MyRay
@pytest.mark.parametrize(
['ray', 'surface', 'intensity', 'r_factor', 'expected'],
[
[Ray(Point(0, 0), Point(1, 1)), Segment(Point(2, 0), Point(2, 4)), 1, 1, (Ray(Point(2, 2), Point(0, 4)), 1)],
[Ray(Point(0, 0), Point(1, 0)), Segment(Point(2, 0), Point(2, 4)), 0.5, 0.98,
(Ray(Point(2, 0), Point(0, 0)), 0.49)],
[Ray(Point(0, 0), Point(1, 0)), Segment(Point(1, 1), Point(3, 3)), 1, 0.9, None],
[Ray(Point(2, 4), Point(4, 0)), Segment(Point(2, 2), Point(3, 0)), 0.78, 0.5, None],
]
)
def test_compute_reflection(ray: Ray, surface: Segment, intensity: float, r_factor: float, expected: (Ray, float)):
actual = compute_reflection(ray=ray, surface=surface, intensity=intensity, reflective_factor=r_factor)
assert actual == expected
@pytest.mark.parametrize(
['rays', 'road', 'cosine_error', 'expected'],
[
[[MyRay(Point(0, 0), 270+45, 45)], Segment(Point(0, -4000), Point(8000, -4000)), "No", [(Rational(4000), 1.0)]],
[[MyRay(Point(0, 0), 270+45, 45)], Segment(Point(0, -4000), Point(8000, -4000)), "Yes",
[(Rational(4000), cos(pi/4))]],
def smallest_rectangle(p):
# mp.dps = 10
ch = convex_hull(p)
print(ch)
# Find extreme points
x_min_index = 0
y_max_index = uppermost_point_index(ch)
x_max_index = rightmost_point_index(ch)
y_min_index = lowermost_point_index(ch)
# Create vertical and horizontal Rays
ray_1 = Ray(ch[x_min_index], angle=pi / 2)
ray_2 = Ray(ch[y_min_index], angle=0)
ray_3 = Ray(ch[x_max_index], angle=pi / 2)
ray_4 = Ray(ch[y_max_index], angle=0)
min_rectangle = Polygon((0, 0), (99999999, 0), (99999999, 99999999),
(0, 99999999))
rotated_angle = 0
while rotated_angle <= pi / 2:
# Convert Rays to lines
line_1 = Line(ray_1)
line_2 = Line(ray_2)
line_3 = Line(ray_3)
line_4 = Line(ray_4)
# Find the intersections of the lines
p1 = line_1.intersection(line_2)[0].evalf()
p2 = line_2.intersection(line_3)[0].evalf()
p3 = line_3.intersection(line_4)[0].evalf()
p4 = line_4.intersection(line_1)[0].evalf()
current_rectangle = Polygon(p1, p2, p3, p4)
# Calculate the area of the rectangle
try:
if current_rectangle.area < min_rectangle.area:
min_rectangle = current_rectangle
except AttributeError:
min_rectangle = current_rectangle
# Find the next Ray on the convex hull for each Ray in counter clockwise direction
next_ray_1 = Ray(ch[x_min_index], ch[get_prev_index(ch, x_min_index)])
next_ray_2 = Ray(ch[y_min_index], ch[get_prev_index(ch, y_min_index)])
next_ray_3 = Ray(ch[x_max_index], ch[get_prev_index(ch, x_max_index)])
next_ray_4 = Ray(ch[y_max_index], ch[get_prev_index(ch, y_max_index)])
#print(ray_1)
#print(next_ray_1)
# Find the minimal angle to rotate each Ray until one aligns with the convex hull
angle_1 = float(positive_angle(next_ray_1.closing_angle(ray_1)))
angle_2 = float(positive_angle(next_ray_2.closing_angle(ray_2)))
angle_3 = float(positive_angle(next_ray_3.closing_angle(ray_3)))
angle_4 = float(positive_angle(next_ray_4.closing_angle(ray_4)))
#print(angle_1, angle_2, angle_3, angle_4)
min_angle = min(angle_1, angle_2, angle_3, angle_4)
# Rotate all Rays around its origin with the angle calculated
"""ray_1 = ray_1.rotate(min_angle)
ray_2 = ray_2.rotate(min_angle)
ray_3 = ray_3.rotate(min_angle)
ray_4 = ray_4.rotate(min_angle)"""
ray_1 = Ray(ray_1.p1, angle=atan(ray_1.slope) + min_angle)
ray_2 = Ray(ray_2.p1, angle=atan(ray_2.slope) + min_angle)
ray_3 = Ray(ray_3.p1, angle=atan(ray_3.slope) + min_angle)
ray_4 = Ray(ray_4.p1, angle=atan(ray_4.slope) + min_angle)
# Set the next point in the convex hull as the origin of the Ray that alligned
# and decrement the index
slope_1 = next_ray_1.slope
slope_2 = next_ray_2.slope
slope_3 = next_ray_3.slope
slope_4 = next_ray_4.slope
if min_angle == angle_1:
if slope_1 == oo:
slope_1 = pi / 2
ray_1 = Ray(next_ray_1.p2, angle=atan(slope_1))
x_min_index = get_prev_index(ch, x_min_index)
elif min_angle == angle_2:
if slope_2 == oo:
slope_2 = pi / 2
ray_2 = Ray(next_ray_2.p2, angle=atan(slope_2))
y_min_index = get_prev_index(ch, y_min_index)
elif min_angle == angle_3:
if slope_3 == oo:
slope_3 = pi / 2
ray_3 = Ray(next_ray_3.p2, angle=atan(slope_3))
x_max_index = get_prev_index(ch, x_max_index)
elif min_angle == angle_4:
if slope_4 == oo:
slope_4 = pi / 2
ray_4 = Ray(next_ray_4.p2, angle=atan(slope_4))
y_max_index = get_prev_index(ch, y_max_index)
else:
print("fail")
rotated_angle += min_angle
return min_rectangle
def repeat():
global waypoint_pointer
global in_pivot_mode
stopping_flag = False
msg = package_msg(0, 0, 0, 0)
#get latest available data
bot_x, bot_y, bot_front_x, bot_front_y = get_db_data()
#deal with goal changes
goal_x, goal_y, wp_count, loop_on = get_goal(waypoint_pointer)
Bot_Loc = Point2D(bot_x, bot_y)
Goal_Loc = Point2D(goal_x, goal_y)
#distance between goal and present loc
eucledian_dist_error = (Bot_Loc.distance(Goal_Loc)).evalf()
#angle to move so that vehicle heading is correct
r1 = Ray((bot_x, bot_y), (goal_x, goal_y))
r2 = Ray((bot_x, bot_y), (bot_front_x, bot_front_y))
angle_error = math.degrees(r1.closing_angle(r2).evalf())
#redundant check, do not remove.
if angle_error < -180:
angle_error = angle_error + 360
if angle_error > 180:
angle_error = angle_error - 360
#####################################UNCOMMENT THESE FOR DEBUGGING######################################
# print("angle error")
# print(angle_error)
# print("distance error")
# print(eucledian_dist_error)
# print("Steer PID")
# print(BOT_1.Steer)
# print("Throt PID")
# print(BOT_1.Thr)
# print("LEFT WHEEL")
# print(out_left_wheel)
# print("RIGHT WHEEL")
# print(out_right_wheel)
# print("Goal")
# print(get_goal(waypoint_pointer))
#######################################################################################################
# pivot if angle_error is too big
#TO-DO: change hard coded values into parameter's. Maybe a class that can be accessed from command line
if abs(angle_error) > 60 or in_pivot_mode:
in_pivot_mode = True
if abs(angle_error) >= 35:
pivot_speed = int(
np.interp(abs(angle_error), [35, 180], [400, 500]))
print("pivoting")
if angle_error > 0:
#pivot anti-cws
msg = package_msg(0, pivot_speed, 1, pivot_speed)
else:
#pivot cws
msg = package_msg(1, pivot_speed, 0, pivot_speed)
else:
in_pivot_mode = False
if in_pivot_mode == False:
#what to do when approaching goal, i.e run precision controller
if (eucledian_dist_error <= 100 and eucledian_dist_error > 40):
left_direction = 1
left_speed = 600 + 3 * angle_error
right_direction = 1
right_speed = 600 - 3 * angle_error
msg = package_msg(1, left_speed, 1, right_speed)
elif (eucledian_dist_error <= 40):
if stopping_flag == False:
waypoint_pointer = waypoint_pointer + 1
if waypoint_pointer == wp_count:
if loop_on:
waypoint_pointer = 0
else:
msg = package_msg(0, 0, 0, 0)
else:
#stop
msg = package_msg(0, 0, 0, 0)
#convert PID value into motor driver understandable value
#xyz_direction = 1 for forward
else:
#calculate PID values based on error obtained
BOT_1.Steer = BOT_1.pid_steering_controller(angle_error)
BOT_1.Thr = BOT_1.pid_throttle_controller(-eucledian_dist_error)
#get computed values from PID
out_left_wheel = BOT_1.Output_left_wheel()
out_right_wheel = BOT_1.Output_right_wheel()
if (out_left_wheel >= 0):
left_direction = 1
left_speed = out_left_wheel
else:
left_direction = 0
left_speed = -out_left_wheel
if (out_right_wheel >= 0):
right_direction = 1
right_speed = out_right_wheel
else:
right_direction = 0
right_speed = -out_right_wheel
msg = package_msg(left_direction, left_speed, right_direction,
right_speed)
#pack the message and send
sock.sendto(msg.encode(), (udp_host, udp_port))
#repeat every x seconds
threading.Timer(0.3, repeat).start()
