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