def update_ground_line(self): self.circle = Circle( self.center_p, self.scooper_p_end.get_distance_with(self.center_p)) self.left_p_old = self.left_p_new self.right_p_old = self.right_p_new self.left_p_new = self.circle.get_intersection_with(self.left_segment0) self.right_p_new = self.circle.get_intersection_with( self.right_segment0) if self.left_p_new and self.right_p_new: self.left_segment0 = LineSegment(self.left_inf_p, self.left_p_new) self.right_segment0 = LineSegment(self.right_inf_p, self.right_p_new) self.left_segment1 = LineSegment(self.left_p_new, self.left_p_old) self.right_segment1 = LineSegment(self.right_p_new, self.right_p_old) self.middle_arc_old = self.middle_arc_new v1 = Vector(self.center_p, self.left_p_new) v2 = Vector(self.center_p, self.right_p_new) self.middle_arc_new = Arc(self.circle.p, self.circle.r, np.rad2deg(v1.angle), np.rad2deg(v2.angle)) else: self.plot_in_ground_flag = True self.left_segment1 = LineSegment( self.left_segment0.p2, Point([-self.circle.r, self.center_height])) self.right_segment1 = LineSegment( self.right_segment0.p2, Point([self.circle.r, self.center_height])) self.middle_arc_old = self.middle_arc_new self.middle_arc_new = Arc(self.circle.p, self.circle.r, 180, 360)
def update_scooper_position(self): scooper_s = LineSegment(Point([0, 0]), Point([self.scooper_length, 0])) scooper_a = scooper_s.transform( get_transform(self.scooper_angle, [self.arm_length, 0])) self.scooper_segment = scooper_a.transform( get_transform(self.arm_angle, [0, self.center_height])) self.scooper_p_end = self.scooper_segment.p2
def test_line_segment_generation(): LineSegment() with pytest.raises(ValueError): LineSegment(Point(0, 0, 0), Point(0, 0, 0)) LineSegment(Point(1, 1, 1), Point(2, 2, 2))
def partition_trajectory(trajectory_line_segs, partition_cost_func, no_partition_cost_func): if len(trajectory_line_segs) < 1: raise ValueError low = 0 length = 1 partition_points = [0] last_pt = trajectory_line_segs[len(trajectory_line_segs) - 1].end trajectory_line_segs.append(LineSegment(last_pt, last_pt)) for high in range(2, len(trajectory_line_segs)): partition_line = LineSegment(trajectory_line_segs[low].start, trajectory_line_segs[high].start) # , trajectory_line_segs[high].as_dict(), partition_cost_func(trajectory_line_segs, low, high), no_partition_cost_func(trajectory_line_segs, low, high)) print("mm", low, high, trajectory_line_segs[low].as_dict(), trajectory_line_segs[high].as_dict(), partition_cost_func(trajectory_line_segs, low, high), no_partition_cost_func(trajectory_line_segs, low, high)) if trajectory_line_segs[high - 2].unit_vector\ .almost_equals(trajectory_line_segs[high - 1].unit_vector): continue elif trajectory_line_segs[high].start.almost_equals(trajectory_line_segs[low].start) or \ partition_cost_func(trajectory_line_segs, low, high) > \ no_partition_cost_func(trajectory_line_segs, low, high): print("ll", low, high, trajectory_line_segs[low].as_dict(), trajectory_line_segs[high].as_dict()) partition_points.append(high - 1) low = high - 1 partition_points.append(len(trajectory_line_segs) - 1) print("PARTITION_POINTS", [p for p in partition_points]) return partition_points
def checkLoS(self, points): """Determine line of sight between all points in the list by constructing a line segment for the two points in question and comparing it for intersection with line segments in the BSP tree :param points: a list of Point objects :return: a list of lists, n by n, an entry at [i][j] tells wether point i and point j have line of sight with each other """ LoS = [] for point in points: LoS.append(['X'] * len(points)) for FromIndex, FromPoint in enumerate(points): for ToIndex, ToPoint in enumerate(points): # if LoS is not determined if (FromIndex != ToIndex) and (LoS[FromIndex][ToIndex] == 'X'): # Assume there is LoS LoS[FromIndex][ToIndex] = 'T' LoS[ToIndex][FromIndex] = 'T' SightSegment = LineSegment( points[FromIndex], points[ToIndex]) # Point to root node stack = [self.tree] IsIntersection = False # NumOfIntersections = 0 NumOfTraversals = 0 while len(stack) != 0 and IsIntersection == False: TreePointer = stack.pop() NumOfTraversals += 1 compareLoS = TreePointer.data[0].compare(SightSegment) if compareLoS == 'P': if SightSegment.split(TreePointer.data[0]) is not None: LoS[FromIndex][ToIndex] = 'F' LoS[ToIndex][FromIndex] = 'F' else: if TreePointer.left is not None: stack.append(TreePointer.left) if TreePointer.right is not None: stack.append(TreePointer.right) elif compareLoS == 'F': if TreePointer.left is not None: stack.append(TreePointer.left) elif compareLoS == 'B': if TreePointer.right is not None: stack.append(TreePointer.right) distance = points[FromIndex].getDistance(points[ToIndex]) if IsIntersection: print(('Distance: %0.1f' % distance) + ', # of traversals(F): ' + str(NumOfTraversals)) else: print(('Distance: %0.1f' % distance) + ', # of traversals(T): ' + str(NumOfTraversals)) return LoS
def absorbs_bullet(self, trajectory: LineSegment): if self.hp: for armor_line, armor_damage in zip(self._armor_lines, ROBOT.armor_damages): if trajectory.intersects(armor_line): self.hp -= armor_damage self.team.take_damage(armor_damage) return True return ROBOT.outline.intersects( trajectory.inv_transform(self.center, self.rotation))
def test_ray_intersection(): """Check for intersection of a ray with line segments.""" ray = Ray(Point(0, 0), Point(1, 1)) line_segment_1 = LineSegment(Point(5, 5), Point(4, 6)) assert ray.intersects(line_segment_1) assert not ray.properly_intersects(line_segment_1) line_segment_2 = LineSegment(Point(1, 2), Point(2, 1)) assert ray.intersects(line_segment_2) assert ray.properly_intersects(line_segment_2) line_segment_3 = LineSegment(Point(2, 1), Point(4, 2)) assert not ray.intersects(line_segment_3) assert not ray.properly_intersects(line_segment_3)
def call_partition_trajectory(trajectory_point_list): if len(trajectory_point_list) < 2: raise ValueError def encoding_cost_func(trajectory_line_segs, low, high, partition_line): return encoding_cost(trajectory_line_segs, low, high, partition_line=partition_line, angular_dist_func=angular_distance, perpendicular_dist_func=perpendicular_distance) def partition_cost_func(trajectory_line_segs, low, high): return partition_cost(trajectory_line_segs, low, high, model_cost_func=model_cost, encoding_cost_func=encoding_cost_func) #convert points in the trajectory_point_list into LineSegment(point[i],point[i+1]) trajectory_line_segs = map( lambda i: LineSegment(trajectory_point_list[i], trajectory_point_list[ i + 1]), range(0, len(trajectory_point_list) - 1)) return partition_trajectory(trajectory_line_segs=trajectory_line_segs, partition_cost_func=partition_cost_func, no_partition_cost_func=no_partition_cost)
def encoding_cost(self, start, end): self.check_indice_args(start, end) approximation_line = LineSegment(self.points[start], self.points[end]) total_perp = 0.0 total_angular = 0.0 for i in xrange(start, end): line_seg = LineSegment(self.points[i], self.points[i + 1]) total_perp += perpendicular_distance(approximation_line, line_seg) total_angular += angular_distance(approximation_line, line_seg) if total_perp < 1.0: total_perp = 1.0 if total_angular < 1.0: total_angular = 1.0 return math.log(total_perp, 2) + math.log(total_angular, 2)
def test_algorithms(self, path, polygon, s, t, pictures): """Run all algorithms on the given polygon and check that the results are equal.""" assert isinstance(polygon, Polygon) signal.signal(signal.SIGALRM, timeout) squared_timeout = ceil(0.001 * polygon.len**2) try: signal.alarm(squared_timeout) trapezoid_path = list(trapezoid_shortest_path(polygon, s, t)) signal.alarm(0) except TimeoutError: raise TimeoutError( 'Trapezoid calculation took longer than {0}s!'.format( squared_timeout)) try: signal.alarm(squared_timeout) lee_preparata_path = list( lee_preparata_shortest_path(polygon, s, t)) signal.alarm(0) except TimeoutError: raise TimeoutError( 'Lee-Preparata calculation took to longer than {0}s!'.format( squared_timeout)) if trapezoid_path != lee_preparata_path: if pictures: save_polygon(path, polygon, trapezoid_path, lee_preparata_path, s, t) assert trapezoid_path == lee_preparata_path for path_ix in range(len(trapezoid_path) - 1): for edge_ix in range(len(polygon)): if polygon.edge(edge_ix).properly_intersects( LineSegment(trapezoid_path[path_ix], trapezoid_path[path_ix + 1])): if pictures: save_polygon(path, polygon, trapezoid_path, lee_preparata_path, s, t) assert not polygon.edge(edge_ix).properly_intersects( LineSegment(trapezoid_path[path_ix], trapezoid_path[path_ix + 1]))
def get_rotated_line_segment(line_segment, angle_in_degrees): if angle_in_degrees > 90.0 or angle_in_degrees < -90.0: raise Exception("trying to rotate line segment by an illegal number of degrees: " + str(angle_in_degrees)) new_start = line_segment.start.rotated(angle_in_degrees) new_end = line_segment.end.rotated(angle_in_degrees) return LineSegment.from_tuples((new_start.x, new_start.y), (new_end.x, new_end.y))
def partition_cost(trajectory_line_segs, low, high, model_cost_func, encoding_cost_func): if low >= high: raise IndexError partition_line = LineSegment(trajectory_line_segs[low].start, trajectory_line_segs[high].start) model_cost = model_cost_func(partition_line) encoding_cost = encoding_cost_func(trajectory_line_segs, low, high, partition_line) return model_cost + encoding_cost
def line_intersects_barriers(self, point1, point2): """ Given two points, it checks if the line created by those points intersect any map barrier """ line = LineSegment(point1, point2) for barrier in self.barriers: if barrier.intersects(line): return True return False
def readLinesFromFile(self, filename): """Not in use currently""" with open(filename, 'r') as f: for line in f.readlines(): if line[0] != '#': data = [x for x in line.split('\t')] points = [int(x) for x in data[0].split(',')] self.tree.data.append(LineSegment(Point(points[0], points[1]), Point( points[2], points[3]), int(data[1]), data[2][0:len(data[2]) - 1]))
def get_rotated_segment(segment, angle_in_degrees): if (angle_in_degrees > 90.0) or (angle_in_degrees < -90.0): raise Exception("Illegal number of degrees: " + str(angle_in_degrees)) new_start = segment.start.rotated(angle_in_degrees) new_end = segment.end.rotated(angle_in_degrees) return LineSegment.from_tuples((new_start.x, new_start.y), (new_end.x, new_end.y))
def get_key_front_egde(p, angle): # front top of the key p0 = point_at_distance(p, KEYCAP_BOX['y'] / 2, angle + 90) # above front top of the key p1 = point_at_distance(p0, -KEY_INTERSECTION_CLEARANCE, angle) # below front bottom of the key p2 = point_at_distance(p1, (KEY_INTERSECTION_CLEARANCE * 3) + KEYCAP_BOX['z'], angle) return LineSegment(p1, p2)
def get_key_back_egde(p, angle): # back top of the key p0 = point_at_distance(p, KEYCAP_BOX['y'] / 2, angle - 90) # above back top of the key p1 = point_at_distance(p0, -KEY_INTERSECTION_CLEARANCE, angle) # back bottom of the key p2 = point_at_distance(p1, (KEY_INTERSECTION_CLEARANCE * 10) + KEYCAP_BOX['z'], angle) return LineSegment(p1, p2)
def test_quad_tree_load_test(self): quad_tree = QuadTree(BoundingBox(0, 100, 0, 100)) points_bounding_box = BoundingBox(-10, 110, -10, 110) for i in range(200): point1 = QuadTreeTests.random_point(points_bounding_box) point2 = QuadTreeTests.random_point(points_bounding_box) while point1 == point2: point2 = QuadTreeTests.random_point(points_bounding_box) quad_tree.add(LineSegment(point1, point2)) self.assertTrue( type(quad_tree.get(BoundingBox(10, 90, 10, 90))) is list)
def get_nearest_point_in_line_segs_to_point(point): min_dist_line_seg = None min_square_dist = None for other_traj in all_trajectories: if other_traj.id != traj.id: for other_pt in get_all_points_of_trajectory(other_traj): temp_square_dist = math.pow(other_pt.x - point.x, 2) + \ math.pow(other_pt.y - point.y, 2) if min_square_dist == None or temp_square_dist < min_square_dist: min_square_dist = temp_square_dist min_dist_line_seg = LineSegment(point, other_pt) return min_dist_line_seg
def test_contains_point(): ls1 = LineSegment(Point(1, 1, 1), Point(2, 2, 2)) assert ls1.contains_point(Point(1.2, 1.2, 1.2)) assert not ls1.contains_point(Point(0.5, 0.5, 0.5)) assert ls1.contains_point(Point(1, 1, 1)) assert ls1.contains_point(Point(2, 2, 2)) assert not ls1.contains_point(Point(0, 1, 0))
class ROBOT: outline = Box(Vector(0.6 * M, 0.5 * M)) _armor_length = 0.14 * M armor_lines = [ LineSegment(*y_mirrors(Vector(outline.dims.x / 2, _armor_length / 2))), LineSegment(*x_mirrors(Vector(_armor_length / 2, outline.dims.y / 2))), LineSegment(*x_mirrors(Vector(_armor_length / 2, -outline.dims.y / 2))), LineSegment(*y_mirrors(Vector(-outline.dims.x / 2, _armor_length / 2))) ] armor_damages = [20, 40, 40, 60] _drive_radius = 0.3 * M _factor = 1 / (math.sqrt(2) * _drive_radius) drive_config = MotionConfig(3 * MS, 6 * MS2, 0.1 * MS2) rotation_config = MotionConfig(_factor * drive_config.top_speed, _factor * drive_config.top_accel, _factor * drive_config.friction_decel) gimbal_yaw_config = MotionConfig(300 * DS, 1000 * DS2, 10 * DS2) zero_tolerance = 0.001 rebound_coeff = 0.4 shot_cooldown = 0.1 * S
def test_ray_intersection_implications(first_level, second_level, limit): """Check for intersection of a ray with line segments and its implications.""" for _ in range(first_level): ray = Ray( Point(random.uniform(-limit, limit), random.uniform(-limit, limit)), Point(random.uniform(-limit, limit), random.uniform(-limit, limit))) for _ in range(second_level): line_segment = LineSegment( Point(random.uniform(-limit, limit), random.uniform(-limit, limit)), Point(random.uniform(-limit, limit), random.uniform(-limit, limit))) assert (not ray.properly_intersects(line_segment)) or ( ray.intersects(line_segment))
def get_ls_list(pts): # emptyness check if not pts: raise ValueError("pts doesn't have any values") # size check if len(pts) < 2: raise ValueError("pts didn't have at least two points") ls_list = [] last_pt = None for pt in pts: if last_pt is not None: ls = LineSegment(last_pt, pt) ls_list.append(ls) last_pt = pt return ls_list
def partition_trajectory(trajectory_line_segs, partition_cost_func, no_partition_cost_func): if len(trajectory_line_segs) < 1: raise ValueError low = 0 partition_points = [0] last_pt = trajectory_line_segs[len(trajectory_line_segs) - 1].end trajectory_line_segs.append(LineSegment(last_pt, last_pt)) for high in range(2, len(trajectory_line_segs)): if trajectory_line_segs[high - 2].unit_vector\ .almost_equals(trajectory_line_segs[high - 1].unit_vector): continue elif trajectory_line_segs[high].start.almost_equals(trajectory_line_segs[low].start) or \ partition_cost_func(trajectory_line_segs, low, high) > \ no_partition_cost_func(trajectory_line_segs, low, high): partition_points.append(high - 1) low = high - 1 partition_points.append(len(trajectory_line_segs) - 1) return partition_points
def __init__(self): config_file_path = os.path.join(os.path.dirname(__file__), 'config/param_moon.json') with open(config_file_path) as config_file: param = json.load(config_file) param = dict([(str(k), v) for k, v in param.items()]) self.state = 0 self.arm_angle = 359 self.arm_length = param["arm_length"] self.arm_speed = 0 self.scooper_angle = 45 self.scooper_length = param["tool_length"] self.tool_depth = 0 self.tool_angle = 0 self.center_height = 2.5 self.center_height_step = 0.02 self.excavator_force = ExcavationForce(param) self.scooper_segment = None # 1 self.origin_p = Point([0, 0]) self.left_inf_p = Point([-10, 0]) self.right_inf_p = Point([10, 0]) self.left_segment0 = LineSegment(self.left_inf_p, self.origin_p) self.left_segment1 = LineSegment(self.left_inf_p, self.origin_p) self.middle_arc_old = None self.middle_arc_new = None self.right_segment1 = LineSegment(self.origin_p, self.right_inf_p) self.right_segment0 = LineSegment(self.origin_p, self.right_inf_p) self.circle = None self.left_segment1_temp = None self.middle_arc_temp_old = None self.middle_arc_temp_new = None self.right_segment1_temp = None self.left_p_new = self.origin_p self.left_p_old = self.origin_p self.right_p_new = self.origin_p self.right_p_old = self.origin_p self.center_p = Point([0, self.center_height]) self.scooper_p_end = None self.fig = plt.figure() self.ax = plt.gca() self.ax.set_aspect('equal') plt.show(block=False) plt.draw() self.plot_middle_arc_temp_old = None self.plot_middle_arc_temp_new = None self.plot_left_segment1_temp = None self.plot_right_segment1_temp = None self.plot_circle = None self.plot_left_segment0 = None self.plot_right_segment0 = None self.plot_scooper_segment = None self.plot_update_flag = False self.plot_in_ground_flag = False
def get_line_segment_from_points(point_a, point_b): return LineSegment(point_a, point_b)
class Excavator(object): def __init__(self): config_file_path = os.path.join(os.path.dirname(__file__), 'config/param_moon.json') with open(config_file_path) as config_file: param = json.load(config_file) param = dict([(str(k), v) for k, v in param.items()]) self.state = 0 self.arm_angle = 359 self.arm_length = param["arm_length"] self.arm_speed = 0 self.scooper_angle = 45 self.scooper_length = param["tool_length"] self.tool_depth = 0 self.tool_angle = 0 self.center_height = 2.5 self.center_height_step = 0.02 self.excavator_force = ExcavationForce(param) self.scooper_segment = None # 1 self.origin_p = Point([0, 0]) self.left_inf_p = Point([-10, 0]) self.right_inf_p = Point([10, 0]) self.left_segment0 = LineSegment(self.left_inf_p, self.origin_p) self.left_segment1 = LineSegment(self.left_inf_p, self.origin_p) self.middle_arc_old = None self.middle_arc_new = None self.right_segment1 = LineSegment(self.origin_p, self.right_inf_p) self.right_segment0 = LineSegment(self.origin_p, self.right_inf_p) self.circle = None self.left_segment1_temp = None self.middle_arc_temp_old = None self.middle_arc_temp_new = None self.right_segment1_temp = None self.left_p_new = self.origin_p self.left_p_old = self.origin_p self.right_p_new = self.origin_p self.right_p_old = self.origin_p self.center_p = Point([0, self.center_height]) self.scooper_p_end = None self.fig = plt.figure() self.ax = plt.gca() self.ax.set_aspect('equal') plt.show(block=False) plt.draw() self.plot_middle_arc_temp_old = None self.plot_middle_arc_temp_new = None self.plot_left_segment1_temp = None self.plot_right_segment1_temp = None self.plot_circle = None self.plot_left_segment0 = None self.plot_right_segment0 = None self.plot_scooper_segment = None self.plot_update_flag = False self.plot_in_ground_flag = False def update_scooper_position(self): scooper_s = LineSegment(Point([0, 0]), Point([self.scooper_length, 0])) scooper_a = scooper_s.transform( get_transform(self.scooper_angle, [self.arm_length, 0])) self.scooper_segment = scooper_a.transform( get_transform(self.arm_angle, [0, self.center_height])) self.scooper_p_end = self.scooper_segment.p2 def update_scooper_intersection(self): if self.state == 0: self.tool_depth = 0 self.tool_angle = 0 if not self.plot_update_flag: self.left_segment1_temp = self.left_segment1 self.middle_arc_temp_old = self.middle_arc_old self.middle_arc_temp_new = None self.right_segment1_temp = self.right_segment1 else: self.left_segment1_temp = None self.middle_arc_temp_old = None self.middle_arc_temp_new = self.middle_arc_new self.right_segment1_temp = None elif self.state == 1: # only intersect with left_segment1 p = self.left_segment1.get_intersection_with(self.scooper_segment) self.tool_depth = p.get_distance_with(self.scooper_p_end) self.tool_angle = abs(self.scooper_segment.angle) self.left_segment1_temp = LineSegment(p, self.left_p_old) self.middle_arc_temp_old = self.middle_arc_old v = Vector(self.middle_arc_new.p, p) self.middle_arc_temp_new = Arc(self.middle_arc_new.p, self.middle_arc_new.r, self.middle_arc_new.deg_angle1, np.rad2deg(v.angle)) self.right_segment1_temp = self.right_segment1 elif self.state == 2: # only intersect with middle_arc_old p = self.middle_arc_old.get_intersection_with(self.scooper_segment) self.tool_depth = p.get_distance_with(self.scooper_p_end) self.tool_angle = 90 - self.scooper_angle self.left_segment1_temp = None v1 = Vector(self.middle_arc_old.p, p) self.middle_arc_temp_old = Arc(self.middle_arc_old.p, self.middle_arc_old.r, np.rad2deg(v1.angle), self.middle_arc_old.deg_angle2) v2 = Vector(self.middle_arc_new.p, self.scooper_p_end) self.middle_arc_temp_new = Arc(self.middle_arc_new.p, self.middle_arc_new.r, self.middle_arc_new.deg_angle1, np.rad2deg(v2.angle)) self.right_segment1_temp = self.right_segment1 elif self.state == 3: # intersect with right_segment1 if self.middle_arc_old is not None and self.middle_arc_old.has_intersection_with( self.scooper_segment): p1 = self.middle_arc_old.get_intersection_with( self.scooper_segment) p2 = self.right_segment1.get_intersection_with( self.scooper_segment) self.tool_depth = p1.get_distance_with(p2) self.tool_angle = 90 - self.scooper_angle self.left_segment1_temp = None v1 = Vector(self.middle_arc_old.p, p1) self.middle_arc_temp_old = Arc(self.middle_arc_old.p, self.middle_arc_old.r, np.rad2deg(v1.angle), self.middle_arc_old.deg_angle2) self.middle_arc_temp_new = self.middle_arc_new self.right_segment1_temp = LineSegment(self.right_p_old, p2) else: p = self.right_segment1.get_intersection_with( self.scooper_segment) self.tool_depth = p.get_distance_with(self.scooper_p_end) self.tool_angle = abs(self.scooper_segment.angle) self.left_segment1_temp = None self.middle_arc_temp_old = None v2 = Vector(self.center_p, self.scooper_p_end) self.middle_arc_temp_new = Arc(self.middle_arc_new.p, self.middle_arc_new.r, self.middle_arc_new.deg_angle1, np.rad2deg(v2.angle)) self.right_segment1_temp = LineSegment(p, self.right_p_new) else: self.tool_depth = 0 def update_ground_line(self): self.circle = Circle( self.center_p, self.scooper_p_end.get_distance_with(self.center_p)) self.left_p_old = self.left_p_new self.right_p_old = self.right_p_new self.left_p_new = self.circle.get_intersection_with(self.left_segment0) self.right_p_new = self.circle.get_intersection_with( self.right_segment0) if self.left_p_new and self.right_p_new: self.left_segment0 = LineSegment(self.left_inf_p, self.left_p_new) self.right_segment0 = LineSegment(self.right_inf_p, self.right_p_new) self.left_segment1 = LineSegment(self.left_p_new, self.left_p_old) self.right_segment1 = LineSegment(self.right_p_new, self.right_p_old) self.middle_arc_old = self.middle_arc_new v1 = Vector(self.center_p, self.left_p_new) v2 = Vector(self.center_p, self.right_p_new) self.middle_arc_new = Arc(self.circle.p, self.circle.r, np.rad2deg(v1.angle), np.rad2deg(v2.angle)) else: self.plot_in_ground_flag = True self.left_segment1 = LineSegment( self.left_segment0.p2, Point([-self.circle.r, self.center_height])) self.right_segment1 = LineSegment( self.right_segment0.p2, Point([self.circle.r, self.center_height])) self.middle_arc_old = self.middle_arc_new self.middle_arc_new = Arc(self.circle.p, self.circle.r, 180, 360) def render(self): if self.middle_arc_temp_old is not None: # if self.plot_middle_arc_temp_old is not None: self.plot_middle_arc_temp_old = \ patches.Arc(self.middle_arc_temp_old.p.p, 2 * self.middle_arc_temp_old.r, 2 * self.middle_arc_temp_old.r, theta1=self.middle_arc_temp_old.deg_angle1, theta2=self.middle_arc_temp_old.deg_angle2, color='r') self.ax.add_patch(self.plot_middle_arc_temp_old) if self.middle_arc_temp_new is not None: # if self.plot_middle_arc_temp_new is not None: # self.ax.remove(self.plot_middle_arc_temp_new) self.plot_middle_arc_temp_new = \ patches.Arc(self.middle_arc_temp_new.p.p, 2 * self.middle_arc_temp_new.r, 2 * self.middle_arc_temp_new.r, theta1=self.middle_arc_temp_new.deg_angle1, theta2=self.middle_arc_temp_new.deg_angle2, color='k') self.ax.add_patch(self.plot_middle_arc_temp_new) if self.left_segment1_temp is not None: # if self.plot_middle_arc_temp_new is not None: # self.ax.remove(self.plot_left_segment1_temp) self.plot_left_segment1_temp = \ lines.Line2D([self.left_segment1_temp.p1.x, self.left_segment1_temp.p2.x], [self.left_segment1_temp.p1.y, self.left_segment1_temp.p2.y], color='r') self.ax.add_artist(self.plot_left_segment1_temp) if self.right_segment1_temp is not None: # if self.plot_middle_arc_temp_new is not None: # self.ax.remove(self.plot_right_segment1_temp) self.plot_right_segment1_temp = \ lines.Line2D([self.right_segment1_temp.p1.x, self.right_segment1_temp.p2.x], [self.right_segment1_temp.p1.y, self.right_segment1_temp.p2.y], color='r') self.ax.add_artist(self.plot_right_segment1_temp) # if self.plot_circle is not None: # self.ax.remove(self.plot_circle) self.plot_circle = \ patches.Circle(self.circle.p.p, self.circle.r, color='g', fill=False, linestyle='--') self.ax.add_artist(self.plot_circle) # if self.plot_left_segment0 is not None: # self.ax.remove(self.plot_left_segment0) self.plot_left_segment0 = \ lines.Line2D([self.left_segment0.p1.x, self.left_segment0.p2.x], [self.left_segment0.p1.y, self.left_segment0.p2.y]) self.ax.add_artist(self.plot_left_segment0) # if self.plot_right_segment0 is not None: # self.ax.remove(self.plot_right_segment0) self.plot_right_segment0 = \ lines.Line2D([self.right_segment0.p1.x, self.right_segment0.p2.x], [self.right_segment0.p1.y, self.right_segment0.p2.y]) self.ax.add_artist(self.plot_right_segment0) # if self.plot_scooper_segment is not None: # self.ax.remove(self.plot_scooper_segment) self.plot_scooper_segment = \ lines.Line2D([self.scooper_segment.p2.x, self.scooper_segment.p1.x, self.center_p.x], [self.scooper_segment.p2.y, self.scooper_segment.p1.y, self.center_p.y]) self.ax.add_artist(self.plot_scooper_segment) if self.plot_in_ground_flag: self.ax.add_artist( lines.Line2D( [self.left_segment1.p2.x, self.left_segment1.p1.x], [self.left_segment1.p2.y, self.left_segment1.p1.y])) self.ax.add_artist( lines.Line2D( [self.right_segment1.p2.x, self.right_segment1.p1.x], [self.right_segment1.p2.y, self.right_segment1.p1.y])) plt.text(0, 0, str(self.state)) plt.pause(0.001) plt.cla() plt.xlim([-4, 4]) plt.ylim([-4, 4]) def run(self, t): old_angle = self.arm_angle # old_state = self.state self.arm_angle = (t * 180) % 360 if old_angle > 330 and self.arm_angle < 30: self.center_height -= self.center_height_step self.center_p = Point([0, self.center_height]) self.update_scooper_position() if self.middle_arc_old is None: if self.left_segment1.has_intersection_with(self.scooper_segment): self.state = 1 elif self.right_segment1.has_intersection_with( self.scooper_segment): self.state = 3 else: self.state = 0 else: if self.left_segment1.has_intersection_with(self.scooper_segment): self.state = 1 elif self.middle_arc_old.has_intersection_with( self.scooper_segment): if self.right_segment1.has_intersection_with( self.scooper_segment): self.state = 3 else: self.state = 2 else: if self.right_segment1.has_intersection_with( self.scooper_segment): self.state = 3 else: self.state = 0 if self.state == 1: self.plot_update_flag = True # if old_state == 2 and self.state == 0: # rospy.logerr("Height step is too large!") if old_angle > 330 and self.arm_angle < 30: self.plot_update_flag = False self.update_ground_line() self.update_scooper_intersection() print(self.tool_depth, self.tool_angle) self.excavator_force.tool_depth = self.tool_depth self.excavator_force.tool_speed = self.arm_length * self.arm_speed self.excavator_force.rank_angle = np.deg2rad(self.scooper_angle) self.render() def set_states_parameter(self, scooper_angle, arm_angle, arm_speed): # Note that an offset value might be required to add to scooper angle self.scooper_angle = np.rad2deg(scooper_angle) # deg self.arm_angle = np.rad2deg(arm_angle) # deg self.arm_speed = arm_speed # rad/s def get_center_height(self): return self.center_height
def test_algorithms(self, path, polygon, s, t, pictures, ignore_delaunay): """Run all algorithms on the given polygon and check that the results are equal.""" assert isinstance(polygon, Polygon) signal.signal(signal.SIGALRM, timeout) s_point = polygon.point_inside_at(s) t_point = polygon.point_inside_at(t) cubed_timeout = ceil(0.001 * polygon.len**3) squared_timeout = ceil(0.001 * polygon.len**2) delaunay_path = None if not ignore_delaunay: signal.alarm(cubed_timeout) try: delaunay_path = list( delaunay_shortest_path(polygon, s_point, t_point)) signal.alarm(0) except TimeoutError: raise TimeoutError( 'Delaunay calculation took longer than {0}s!'.format( cubed_timeout)) signal.alarm(squared_timeout) try: trapezoid_path = list( trapezoid_shortest_path(polygon, s_point, t_point)) signal.alarm(0) except TimeoutError: raise TimeoutError( 'Trapezoid calculation took longer than {0}s!'.format( squared_timeout)) signal.alarm(squared_timeout) try: makestep_path = list( makestep_shortest_path(polygon, s_point, t_point)) signal.alarm(0) except TimeoutError: raise TimeoutError( 'Makestep calculation took to longer than {0}s!'.format( squared_timeout)) if (not delaunay_path == trapezoid_path == makestep_path and delaunay_path is not None) or (not trapezoid_path == makestep_path): if pictures: save_polygon(path, polygon, trapezoid_path, delaunay_path, makestep_path, s, t) assert delaunay_path == trapezoid_path assert delaunay_path == makestep_path assert makestep_path == trapezoid_path for path_ix in range(len(trapezoid_path) - 1): for edge_ix in range(len(polygon)): if polygon.edge(edge_ix).properly_intersects( LineSegment(trapezoid_path[path_ix], trapezoid_path[path_ix + 1])): if pictures: save_polygon(path, polygon, trapezoid_path, delaunay_path, makestep_path, s, t) assert not polygon.edge(edge_ix).properly_intersects( LineSegment(trapezoid_path[path_ix], trapezoid_path[path_ix + 1]))
def line_segment(x1, y1, x2, y2): """Create a line segment from four points.""" return LineSegment(Point(x1, y1), Point(x2, y2))
def update_scooper_intersection(self): if self.state == 0: self.tool_depth = 0 self.tool_angle = 0 if not self.plot_update_flag: self.left_segment1_temp = self.left_segment1 self.middle_arc_temp_old = self.middle_arc_old self.middle_arc_temp_new = None self.right_segment1_temp = self.right_segment1 else: self.left_segment1_temp = None self.middle_arc_temp_old = None self.middle_arc_temp_new = self.middle_arc_new self.right_segment1_temp = None elif self.state == 1: # only intersect with left_segment1 p = self.left_segment1.get_intersection_with(self.scooper_segment) self.tool_depth = p.get_distance_with(self.scooper_p_end) self.tool_angle = abs(self.scooper_segment.angle) self.left_segment1_temp = LineSegment(p, self.left_p_old) self.middle_arc_temp_old = self.middle_arc_old v = Vector(self.middle_arc_new.p, p) self.middle_arc_temp_new = Arc(self.middle_arc_new.p, self.middle_arc_new.r, self.middle_arc_new.deg_angle1, np.rad2deg(v.angle)) self.right_segment1_temp = self.right_segment1 elif self.state == 2: # only intersect with middle_arc_old p = self.middle_arc_old.get_intersection_with(self.scooper_segment) self.tool_depth = p.get_distance_with(self.scooper_p_end) self.tool_angle = 90 - self.scooper_angle self.left_segment1_temp = None v1 = Vector(self.middle_arc_old.p, p) self.middle_arc_temp_old = Arc(self.middle_arc_old.p, self.middle_arc_old.r, np.rad2deg(v1.angle), self.middle_arc_old.deg_angle2) v2 = Vector(self.middle_arc_new.p, self.scooper_p_end) self.middle_arc_temp_new = Arc(self.middle_arc_new.p, self.middle_arc_new.r, self.middle_arc_new.deg_angle1, np.rad2deg(v2.angle)) self.right_segment1_temp = self.right_segment1 elif self.state == 3: # intersect with right_segment1 if self.middle_arc_old is not None and self.middle_arc_old.has_intersection_with( self.scooper_segment): p1 = self.middle_arc_old.get_intersection_with( self.scooper_segment) p2 = self.right_segment1.get_intersection_with( self.scooper_segment) self.tool_depth = p1.get_distance_with(p2) self.tool_angle = 90 - self.scooper_angle self.left_segment1_temp = None v1 = Vector(self.middle_arc_old.p, p1) self.middle_arc_temp_old = Arc(self.middle_arc_old.p, self.middle_arc_old.r, np.rad2deg(v1.angle), self.middle_arc_old.deg_angle2) self.middle_arc_temp_new = self.middle_arc_new self.right_segment1_temp = LineSegment(self.right_p_old, p2) else: p = self.right_segment1.get_intersection_with( self.scooper_segment) self.tool_depth = p.get_distance_with(self.scooper_p_end) self.tool_angle = abs(self.scooper_segment.angle) self.left_segment1_temp = None self.middle_arc_temp_old = None v2 = Vector(self.center_p, self.scooper_p_end) self.middle_arc_temp_new = Arc(self.middle_arc_new.p, self.middle_arc_new.r, self.middle_arc_new.deg_angle1, np.rad2deg(v2.angle)) self.right_segment1_temp = LineSegment(p, self.right_p_new) else: self.tool_depth = 0