def closest_node_on_segment_to_node(node, segment) -> Node.Node:
A_start, B_start, C_start = perpendicular(segment.start, segment)
A_end, B_end, C_end = perpendicular(segment.end, segment)
if line_value(A_start, B_start, C_start, node) * line_value(
A_end, B_end, C_end, node) < 0:
node_A, node_B, node_C = perpendicular(node, segment)
x = 10000
y = (-node_A * x - node_C) / node_B
end = Node.Node(x, y)
node_segment = Edge.Edge(node, end)
if geometry.intersect(segment, node_segment):
closest_node = geometry.intersection(segment, node_segment)
else:
x = -10000
y = (-node_A * x - node_C) / node_B
end = Node.Node(x, y)
node_segment = Edge.Edge(node, end)
closest_node = geometry.intersection(segment, node_segment)
else:
closest_node = segment.start
distance_to_start = dist(node, segment.start)
distance_to_end = dist(node, segment.end)
if distance_to_start > distance_to_end:
closest_node = segment.end
return closest_node
def visible(node1, node2, obstacle_list):
# check if node1 and node2 are visible regarding to obstacle
line = Edge(node1, node2)
for obstacle in obstacle_list:
for edge in obstacle.edge:
if intersect(line, edge):
return False
return True
def test_square_diagonal_intersect(self):
spoke = Spoke(math.pi / 4, 2, 2, 1)
spoke.x = 4
spoke.y = 4
seg1 = (2, 3)
seg2 = (4, 3)
self.assertTrue(geometry.intersect(spoke, seg1, seg2))
def test_parallel_intersect_spoke_envelopes(self):
spoke = Spoke(0.463647609, 10, 10, 1)
spoke.x = 6
spoke.y = 8
seg1 = (8, 9)
seg2 = (10, 10)
self.assertTrue(geometry.intersect(spoke, seg1, seg2))
def test_does_not_intersect_diag2(self):
spoke = Spoke(0, 12, 2, 1)
spoke.x = 12
spoke.y = 5
seg1 = (13, 7)
seg2 = (15, 7)
self.assertFalse(geometry.intersect(spoke, seg1, seg2))
def test_does_not_intersect_diag3(self):
spoke = Spoke((math.pi / 4), 4, 14, 1)
spoke.x = 6
spoke.y = 12
seg1 = (5, 11)
seg2 = (7, 9)
self.assertFalse(geometry.intersect(spoke, seg1, seg2))
def test_not_intersect(self):
spoke = Spoke(0.463647609, 10, 10, 1)
spoke.x = 8
spoke.y = 9
seg1 = (1, 4)
seg2 = (3, 2)
self.assertFalse(geometry.intersect(spoke, seg1, seg2))
def test_does_not_intersect_diag4(self):
spoke = Spoke(math.pi /4, 14, 10.001, 1)
spoke.x = 16
spoke.y = 9
seg1 = (15, 11.0001)
seg2 = (17, 13)
self.assertFalse(geometry.intersect(spoke, seg1, seg2))
def test_parallel_intersect_start_same_place(self):
spoke = Spoke(0.463647609, 10, 10, 1)
spoke.x = 8
spoke.y = 9
seg1 = (8, 9)
seg2 = (10, 10)
self.assertTrue(geometry.intersect(spoke, seg1, seg2))
def test_not_intersect_blue_green(self):
spoke = Spoke(2.5132741228718345, 66, 111, 1)
spoke.x = 67.76335575687743
spoke.y = 108.57294901687517
seg1 = (233.0, 109.0)
seg2 = (234.7633557568774, 108.42705098312483)
self.assertFalse(geometry.intersect(spoke, seg1, seg2))
def test_square_intersect(self):
spoke = Spoke(math.pi / 2, 2.1, 2.1111, 1)
spoke.x = 4.0010101
spoke.y = 2.12
seg1 = (3.011, 1.1553534) # It helps if you write a test that actually should pass.
seg2 = (3.01434, 4.213132)
self.assertTrue(geometry.intersect(spoke, seg1, seg2))
def test_diagonal_intersect(self):
#spokeangle =
spoke = Spoke(math.pi / 4, 2.1, 2.1, 1)
spoke.x = 4.2
spoke.y = 4.2
seg1 = (4.001, 2.001)
seg2 = (2.002323, 4.23123)
self.assertTrue(geometry.intersect(spoke, seg1, seg2))
def test_does_not_intersect_diag1(self):
spoke = Spoke(math.pi, 5, 2.01, 1)
spoke.x = 5
spoke.y = 5.011
seg1 = (3, 7.01)
seg2 = (8, 7.09)
self.assertFalse(geometry.intersect(spoke, seg1, seg2))
spoke = Spoke(0, 5, 5.02, 1)
spoke.x = 5.012
spoke.y = 2.0
seg1 = (3.02, 7.23)
seg2 = (8.12, 7.12)
self.assertFalse(geometry.intersect(spoke, seg1, seg2))
def edge_collision_check(self, edge: Edge.Edge) -> bool:
"""
:param edge:
:return: False: edge collide with obstacle
True: collision free
"""
for obstacle in self.map.obstacle_list:
for edge1 in obstacle.edge_list:
if geometry.intersect(edge, edge1):
return False
return True
def ray_hit(self, ir_pos, angle, rel_angle):
# simple hit test, does the ray hit any walls?
# position is position of sensor
# angle is robot orientation
# rel_angle is relative angle of ray
ray_end = self.ray_end(ir_pos[0], ir_pos[1], angle, rel_angle)
for wall in world.walls:
wall_start = wall[0]
wall_end = wall[1]
# does segment [ir_pos, ray_end] intersect with a wall?
if geometry.intersect(ir_pos, ray_end, wall_start, wall_end):
return True
return False
def get_active_list(node, angle, edges):
active_list = {}
# set large length
length = 100000
x = node.x + length * cos(angle)
y = node.y + length * sin(angle)
end = Node(x, y)
for edge in edges:
if intersect(edge, Edge(node, end)):
active_list[edge] = distance_node_to_segment(node, edge)
active_list = sorted(active_list.items(),
key=lambda x: x[1],
reverse=False)
# active_list is list, not dictionary
return active_list
def distance_node_to_segment(node: Node.Node, segment: Edge.Edge):
# distance between node and segment, return min distance and closest point on segment to the node
A_start, B_start, C_start = perpendicular(segment.start, segment)
A_end, B_end, C_end = perpendicular(segment.end, segment)
if line_value(A_start, B_start, C_start, node) * line_value(
A_end, B_end, C_end, node) < 0:
distance = abs(
line_value(segment.A, segment.B, segment.C, node) /
sqrt(segment.A**2 + segment.B**2))
node_A, node_B, node_C = perpendicular(node, segment)
x = 10000
y = (-node_A * x - node_C) / node_B
end = Node.Node(x, y)
node_segment = Edge.Edge(node, end)
if geometry.intersect(segment, node_segment):
closest_node = geometry.intersection(segment, node_segment)
else:
distance = min(dist(node, segment.start), dist(node, segment.end))
return distance
def write_slices(data, num_slices, output_path):
"""Découpe le modèle 3D en tranches et écrit le résultat dans des fichiers
SVG.
"""
height = abs(data['zmax'] - data['zmin'])
slices = [
data['zmin'] + i * height / (num_slices + 1)
for i in range(1, num_slices + 1)
]
for i, height in enumerate(slices):
print('Création de la tranche numéro', i)
path = os.path.join(output_path, 'slice_{}.svg'.format(i))
file_ = open(path, 'w')
svg_width = int(abs(data['xmax'] - data['xmin'])) + 10
svg_height = int(abs(data['ymax'] - data['ymin'])) + 10
file_.write(header(svg_width, svg_height))
for triangle in data['triangles']:
segment = []
for side in itertools.combinations(triangle.vertices, 2):
if intersect(height, side):
xa, ya, za = side[0].position
xb, yb, zb = side[1].position
if zb - za != 0:
c = (height - za) / (zb - za)
vertex = Vertex(
(xa + c * (xb - xa), ya + c * (yb - ya), height))
segment.append(vertex)
else:
print('divide by 0')
if len(segment) > 0:
file_.write(
line(segment[0].x - data['xmin'],
segment[0].y - data['ymin'],
segment[1].x - data['xmin'],
segment[1].y - data['ymin']))
file_.write(footer())
file_.close()
def add_street(self, street):
# 1. add its own nodes to the path
# new_graph_street = GraphStreet(street)
# 2. calculate intersect and add to the path
for seg_new in street.segments:
for key in self.graph_streets:
for seg_old in self.graph_streets[key].segments:
# do not consider times when segments are overlapped
# if not geometry.isOverlap(seg_old, seg_new):
# zero or one intersect, if zero intersect, return none
# new_graph_street.intersects.append(intersect)
pts = geometry.intersect(seg_new, seg_old)
for pt in pts:
isEndpoint = (pt == seg_new.src or pt == seg_new.dst)
intersect = streetlib.Intersect(pt.x, pt.y, True)
seg_new.add_intersect(intersect, key)
isEndpoint = (pt == seg_old.src or pt == seg_old.dst)
intersect = streetlib.Intersect(pt.x, pt.y, True)
seg_old.add_intersect(intersect, street.name)
# self.graph_streets.append(street)
self.graph_streets[street.name] = street
def ray_hit_nearest(self, ray_start, ray_end):
# check if ray hit any walls
# ray is line segment [p1,p2]
# w1 is index of world for wall hit by ray with shortest dist d
hit = False
min_dist = world.xmax
wall_index = None
n_wall = 0
for wall in world.walls:
# [p1,p2] is the ray, [p3,p4] is the wall
wall_start = wall[0]
wall_end = wall[1]
if geometry.intersect(ray_start, ray_end, wall_start, wall_end):
# find intersection point
intersection = geometry.intersection_point(
ray_start, ray_end, wall_start, wall_end)
# distace to intersection from sensor
dist = np.linalg.norm(ray_start - intersection)
if dist < min_dist:
hit = True
wall_index = n_wall
min_dist = dist
n_wall += 1
return hit, wall_index, min_dist
y: float = y_star - yC
x_g: float = x_gas - xC
y_g: float = y_gas - yC
degrees: float = 10
radians: float = math.radians(degrees)
# Bridge rectangle ----------------------------------------------
Ba1, Ba2, Ba3, Ba4 = rectangle_slopes(radians)
# BL, BR, TL, TR: Bottom Left, Bottom Right, Top Left, Top Right
BxBL, ByBL, BxBR, ByBR = -10, -25, 19.544, -19.791
BxTL, ByTL, BxTR, ByTR = -12, -13, 17.5, -8
# intersections
Bb1 = intersect(Ba1, BxBL, ByBL)
Bb2 = intersect(Ba2, BxTR, ByTR)
Bb3 = intersect(Ba3, BxTL, ByTL)
Bb4 = intersect(Ba4, BxTR, ByTR)
Gas_RectangleIDs = np.where((y_g > Ba1 * x_g + Bb1) * (y_g < Ba2 * x_g + Bb2) *
(y_g > Ba3 * x_g + Bb3) * (y_g < Ba4 * x_g + Bb4))
RectangleIDs = np.where((y > Ba1 * x + Bb1) * (y < Ba2 * x + Bb2) *
(y > Ba3 * x + Bb3) * (y < Ba4 * x + Bb4))
hB, wB = 12, 30 # height and width
AB: float = hB * wB # area = 360
ratioB = sum(SFR_gas[Gas_RectangleIDs]) / AB # 0.000293586789828
# print(f'{ratioB =}')
# areas of galaxy1 and galaxy2 rectangles
hG, wG = 24, 30 # heights and widths
def test_intersect():
intersection: int = 10.5
assert geometry.intersect(.5, 3,
12) == intersection, f"Should be {intersection}"
def intersect(self, p):
return intersect(p, self.rect)
def calc_score(places):
score = 0
spent_money = 0
visited = []
spent_time = 0
spent_time_on_foot = 0
# если маршрут начинается пешком, то это неплохо
if places[0].position == start:
score += 50
# если маршрут коначается пешком, то это неплохо
if places[-1].position == finish:
score += 50
for place in places:
place_type = place.info.get('type', None)
visited.append(place_type)
# если мы хотим сходить в кафе и сходили
if place_type == cafe_type and cafe_type is not None:
score -= rel_error(spent_time, time_cafe) ** 2 * 1000
# посетил кафе? ты нереально крут
score += 300
# в среднем 20 минут сидишь, можно сделать разные времена для разных мест # todo
spent_time += 20
spent_money += place.info.get('money', 250)
# рейтинг тоже может увеличить очки
score += place.info.get('rating', 0) * 300
if place_type in temp_place:
# гуляешь в каком-то месте 10 минут
spent_time += 10
# походили 10 минут
spent_time_on_foot += 10
# рейтинг влияет чуть хуже
score += place.info.get('rating', 0) * 100
if cafe_type is not None and cafe_type not in visited:
score -= 10 ** 4
# считаем время между точками пешком
for i in range(len(places) - 1):
transit_mode = 'walking'
if (i == 0 and start != places[i].position) or \
(i == len(places) - 2 and finish != places[i + 1].position):
transit_mode = 'transit'
time_to_next = gmap.get_duration(places[i].position, places[i + 1].position, transit_mode=transit_mode)
if time_to_next is not None:
if transit_mode == 'walking':
spent_time_on_foot += time_to_next
spent_time += time_to_next
else:
score -= 10 ** 10
# Самопересечения пути и острые углы - это плохо
for i in range(1, len(places) - 1):
for j in range(i - 1):
if geometry.intersect(places[i].position, places[i + 1].position,
places[j].position, places[j + 1].position):
score -= 1000
if geometry.sca(places[i - 1].position, places[i].position, places[i + 1].position):
score -= 300
# если мы потратили больше, чем планировали, то очень плохо
if money < spent_money:
score -= 10 ** 5
# считаем очки за посещенные цели
score += len(set(temp_place) & set(visited)) * 40
for visited_place in set(visited):
if visited_place in temp_place:
index = temp_place.index(visited_place)
score += 1 / (5 + index) * 1000
# считаем штраф за время на ногах
dif_time_on_foot = rel_error(duration_on_foot, spent_time_on_foot)
if dif_time_on_foot < 0:
score -= dif_time_on_foot ** 2 * 300
else:
score -= abs(dif_time_on_foot) ** 4 * 10000
# считаем штраф за время
dif_time = rel_error(duration, spent_time)
if dif_time < 0:
score -= dif_time ** 2 * 300
else:
score -= abs(dif_time) ** 3 * 6000
if abs(duration - spent_time) > 60:
score -= 10 ** 4
print(spent_time, spent_time_on_foot)
return score
