def create_arc(length, curvature, lane_data, quad_number=10): """ Gives the chord line set of points for a curved road""" plt = [[], []] anti_clockwise = 1 tangent = Vector(1, 0, 0) radius = 1 / curvature end_angle = 0 if (radius > 0): tangent = tangent.rotate(90) anti_clockwise = 1 else: tangent = tangent.rotate(-90) radius = -1 * radius anti_clockwise = -1 center = radius * tangent tangent = tangent.rotate(180) start_angle = tangent.argument() end_angle = start_angle + anti_clockwise * 360 * (length / (2 * math.pi * radius)) angle = start_angle radius = Vector(radius, 0, 0) radius = radius.rotate(start_angle) tot_lane_vertices = [] for i in range(quad_number + 1): pt = center + radius lane_pts = generate_lane_verts(pt, radius.rotate(90 * anti_clockwise), lane_data) tot_lane_vertices.append(lane_pts) radius = radius.rotate((end_angle - start_angle) / quad_number) plt[0].append(pt.x) plt[1].append(pt.y) return tot_lane_vertices
def TransformSTtoXY(s, t, roadType, args): # args = [origin, heading, length, CurvStart, CurvEnd] if roadType == 'line': tangent = Vector(math.cos(args[1]), math.sin(args[1]), 0) tangent = tangent.normalize() left_normal = tangent.rotate(90) pt = s*tangent + t*left_normal + args[0] elif roadType == 'arc': R = 1/args[3] hdg = args[1] origin = args[0] anti_clockwise = 1 if R<0 : anti_clockwise = -1 theta = s/R tangent = Vector(math.cos(hdg), math.sin(hdg), 0) tangent = tangent.normalize() init_normal = tangent.rotate(anti_clockwise*90) centre = origin + init_normal*R #center = init_normal*R pt_vec = origin - centre pt_vec = pt_vec.normalize() rot_pt_vec = pt_vec.rotate(180*theta/math.pi) pt = (R-t)*pt_vec + centre elif roadType == 'spiral': Resolution = 0.1 num_sections = 100 distance = 0 ahead = 1 done = False scaling = 1 dx = length/num_sections prev_point = args[0] for i in range(num_sections): if ltoc: pt_hdg = anti_clockwise*(distance*a)**2 + args[1] else: pt_hdg = -(length*a)**2-anti_clockwise*((length-distance)*a)**2 + args[1] tangent = Vector(math.cos(pt_hdg), math.sin(pt_hdg), 0) t_norm = tangent.normalize() pt = prev_point + ahead*dx*t_norm prev_point = pt distance += ahead*dx*scaling if abs(distance-s) <= RESOLUTION: break elif distance > s: if ahead == 1: scaling = scaling/2 ahead = -1 elif distance < s: if ahead == -1: scaling = scaling/2 ahead = 1 left_normal = t_norm.rotate(90) pt = left_normal*t + pt x = pt.x y = pt.y return x,y
def test_vector_equality(): v1 = Vector() v2 = Vector() v3 = Vector(2, 10) assert v1 == v2 assert not v1 == v3 assert not v1 != v2 assert v1 != v3
def test_vector_increment_decrement(): v1 = Vector(1.2, 2.3) v2 = Vector(3.4, 4.5) v1 += v2 assert v1.x == 4.6 and v1.y == 6.8 v1 -= v2 assert round(v1.x, 1) == 1.2 and round(v1.y, 1) == 2.3
def test_vector_multiplication_eq_len(): v1 = Vector([3, 5, 6]) v2 = Vector([1, 3, 5]) try: res = (v1 * v2) except: res = 0 assert res == 48
def create_straight_line(length, lane_data, quad_number=10): """ Gives the chord line set of points for a straight road""" tangent = Vector(1, 0, 0) pt = Vector(0, 0, 0) tot_lane_vertices = [] for i in range(quad_number + 1): lane_verts = generate_lane_verts(pt, tangent, lane_data) tot_lane_vertices.append(lane_verts) pt = pt + tangent * (length / quad_number) return tot_lane_vertices
def get_vectors_from_ant_to_walls(ant): """ Get a list of vectors pointing to each wall in the game area. :param ant: The ant whose position is used to calculate wall vectors. :return: A list of vectors pointing from 'ant' to the walls of the game area. """ return [ Vector((-ant.x, 0)), Vector((game_rules.GAME_AREA_WIDTH - ant.x, 0)), Vector((0, -ant.y)), Vector((0, game_rules.GAME_AREA_HEIGHT - ant.y)) ]
def test_vector_multiplication_diff_len(): """ :return: Векторы разной длины не должны умножаться. """ v1 = Vector([3, 5, 6]) v2 = Vector([1, 3, 5, 7]) try: res = (v1 * v2) except: res = 'error' assert res != 'error'
def create_straight_line(length,current_road,start_s,hardCode = False,quad_number = 10): """ Gives the chord line set of points for a straight road""" tangent = Vector(1,0,0) pt= Vector(0,0,0) tot_lane_vertices = [] s = -0.1 for i in range(quad_number+1): lane_data = current_road.get_lane_data(s+start_s) # print(lane_data) current_pt = pt + Vector(0,0,current_road.get_elevation(s + start_s)) lane_verts = generate_lane_verts(current_pt, tangent, lane_data,hardCode) tot_lane_vertices.append(lane_verts) pt = pt + tangent * (length/quad_number) s = s + length/quad_number return tot_lane_vertices
def __init__(self, poly_string=""): """Builds a polynomial If a string is given in the right format it will split it to build the polynomial""" self.terms = Vector() if poly_string != "": poly_pieces = Vector() poly_pieces.build_from_list(poly_string.split(" ")) for i in range(0, len(poly_pieces), 2): coef = poly_pieces[i] exponent = poly_pieces[i + 1] self.terms.insert_rear(Term(int(coef), int(exponent))) #condense similar terms self.combine_like_terms() # Sort the terms self.sort_terms()
def sum_vectors(vectors): """ Calculates the sum of the given vectors. It is assumed that all vectors have the same dimensions :param vectors: The vectors to sum :return: Sum of the given vectors """ if len(vectors) == 0: return Vector((0, 0)) if len(vectors) == 1: return vectors[0] return Vector( tuple( sum(coordinate) for coordinate in zip(*(vector.values for vector in vectors))))
def create_spiral_road(length, curvStart, curvEnd, current_road, start_s, hardCode=False, quad_number=10): # if(curvStart==0): # return spiral_line_to_curve(length,curvEnd,lane_data) # else: # return spiral_curve_to_line(length,curvStart,lane_data) origin = Vector(0, 0, 0) hdg = 0 if curvStart == 0: ltoc = True R = 1 / curvEnd else: ltoc = False R = 1 / curvStart anti_clockwise = 1 if R < 0: anti_clockwise = -1 a = 1 / math.sqrt(2 * length * abs(R)) # Scale factor RESOLUTION = 0.1 num_sections = 100 distance = 0 ahead = 1 done = False scaling = 1 dx = length / num_sections prev_point = origin total_pts = [] for i in range(num_sections): if ltoc: pt_hdg = anti_clockwise * (distance * a)**2 + hdg else: pt_hdg = anti_clockwise * ((length * a)**2 - ((length - distance) * a)**2) + hdg tangent = Vector(math.cos(pt_hdg), math.sin(pt_hdg), 0) t_norm = tangent.normalize() pt = prev_point + ahead * dx * t_norm prev_point = pt distance += ahead * dx lane_data = current_road.get_lane_data(distance + start_s) current_pt = pt + Vector( 0, 0, current_road.get_elevation(distance + start_s)) total_pts.append( generate_lane_verts(current_pt, tangent, lane_data, hardCode)) return total_pts
def generate_lane_mark_verts(pt, tangent, lane_data, width, lane_marking = False): result_pts= [] tangent = tangent.normalize() left_normal = tangent.rotate(90) right_normal = tangent.rotate(-90) #print(lane_data) pt = pt + Vector(0,0,0.02) result_pts.append(pt + (-1*width)*left_normal) result_pts.append(pt + (width)*left_normal) new_pt = pt for x in lane_data['left']: new_pt = new_pt + x*left_normal new_pt1 = new_pt + (-1*width)*left_normal new_pt2 = new_pt + (width)*left_normal result_pts.append(new_pt1) result_pts.append(new_pt2) result_pts = list(reversed(result_pts)) new_pt=pt for x in lane_data['right']: new_pt = new_pt + x*right_normal new_pt1 = new_pt + (-width)*right_normal new_pt2 = new_pt + (+width)*right_normal result_pts.append(new_pt1) result_pts.append(new_pt2) return result_pts
def main(argv): input_file = "" output_file = "" # First we attempt to get the options from the command line try: options, args = getopt.getopt(argv, "i:o:h") except getopt.GetoptError: print( """Call via `python poly_calc.py -i <input_file> -o <output_file>`\n Use `python poly_calc.py -h` for help""") # exit status 2 for command line error sys.exit(2) # Now we check what options were entered and act appropriately for option, argument in options: if option == "-h": print("Call the script using:") print(" python poly_calc.py -i <input_file> -o <output_file>") print("The following flags are used:") print(" -i must be by the input file name") print(" -o can be followed by the output file name if desired") if option == "-i": input_file = argument if option == "-o": output_file = argument # Ensure that there is input data if input_file == "": print("An input file must be specified") else: data = Vector() # Holds all of the lines out of the input file stack = Stack() # Used to store the Polynomials and perform operations # Read all of the lines out of the file with open(input_file) as f: for line in f: data.append(line.strip()) for line in data: if line == "+": p1 = stack.top() stack.pop() p2 = stack.top() stack.pop() stack.push(p1 + p2) elif line == "-": p1 = stack.top() stack.pop() p2 = stack.top() stack.pop() stack.push(p1 - p2) elif line == "*": p1 = stack.top() stack.pop() p2 = stack.top() stack.pop() stack.push(p1 * p2) else: stack.push(Polynomial(line)) if output_file != "": with open(output_file, 'w+') as f: print(stack.top(), file=f) else: print(stack.top())
def _update_correspondence_reprojected_coords(self): if self.editor.layer_manager.trajectory_layer() is None: return if self.editor.layer_manager.correspondence_layer( create_new_layer=False) is None: return unreprojected_correspondences = \ self.editor.layer_manager.correspondence_layer().correspondences() if len(unreprojected_correspondences) == 0: return camera_intrinsics = \ self.editor.layer_manager.trajectory_layer().camera_config() vector_reprojector = VectorReprojector() vector_reprojector.set_intrinsics(camera_intrinsics) for correspondence in unreprojected_correspondences: camera_extrinsic = self.editor.layer_manager.trajectory_layer() \ .get_node_by_timestamp(correspondence.timestamp()).T_camera_to_world() vector_reprojector.set_extrinsic(camera_extrinsic) reprojected_shape = vector_reprojector.reproject( Vector(correspondence.reprojected_shape().origin_vertices())) if reprojected_shape is not None: correspondence.set_reprojected_shape(reprojected_shape)
def test_vector_setters_exception(value, exception_type): vector = Vector() with pytest.raises(exception_type): vector.x = value with pytest.raises(exception_type): vector.y = value
def test_vector_setters(): vector = Vector() vector.x = 42.0 vector.y = 42.0 assert vector.x == 42.0 assert vector.y == 42.0
def vector_between_points(p1, p2): """ Get a vector pointing from p1 to p2. :param p1: The first point, as tuple of coordinates. :param p2: The second point, as a tuple of coordinates. :return: A new vector pointing from p2 to p1. """ return Vector(tuple((x2 - x1 for x1, x2 in zip(p1, p2))))
def create_arc(length, curvature, current_road, start_s, hardCode=False, quad_number=10): """ Gives the chord line set of points for a curved road""" plt = [[], []] anti_clockwise = 1 tangent = Vector(1, 0, 0) radius = 1 / curvature end_angle = 0 if (radius > 0): tangent = tangent.rotate(90) anti_clockwise = 1 else: tangent = tangent.rotate(-90) radius = -1 * radius anti_clockwise = -1 center = radius * tangent tangent = tangent.rotate(180) start_angle = tangent.argument() end_angle = start_angle + anti_clockwise * 360 * (length / (2 * math.pi * radius)) angle = start_angle radius = Vector(radius, 0, 0) radius = radius.rotate(start_angle) tot_lane_vertices = [] s = -0.1 for i in range(quad_number + 1): pt = center + radius lane_data = current_road.get_lane_data(s + start_s) current_pt = pt + Vector(0, 0, current_road.get_elevation(s + start_s)) lane_pts = generate_lane_verts(current_pt, radius.rotate(90 * anti_clockwise), lane_data, hardCode) tot_lane_vertices.append(lane_pts) radius = radius.rotate((end_angle - start_angle) / quad_number) plt[0].append(pt.x) plt[1].append(pt.y) dTheta = (end_angle - start_angle) / quad_number dTheta = dTheta * 3.14 / 180 s = s + radius.norm() * dTheta return tot_lane_vertices
def get_sugar_vector(ant, entities): sugar_and_vectors = [(sugar, (util.vector_between_entities(ant, sugar))) for sugar in entities.sugar] sorted_sugar_vectors = [vector for sugar, vector in sorted(sugar_and_vectors, key=lambda sugar_and_vector: get_sugar_score(ant, sugar_and_vector[0], sugar_and_vector[1], entities))] if not sorted_sugar_vectors: return Vector((0, 0)) return sorted_sugar_vectors[0].normalize().multiply(1 / 50)
def __add__(self, other): """Overloaded addition operator""" temp = Vector() for term in self.terms: temp.append(term) for term in other.terms: temp.append(term) new_poly = Polynomial() new_poly.terms = temp new_poly.combine_like_terms() new_poly.sort_terms() return new_poly
def sort_terms(self): """Sorts the terms of the polynomial in O(n^2) time""" temp = Vector() for i in range(len(self.terms)): max_exp = self.terms[0].exponent max_exp_index = 0 for index, term in enumerate(self.terms): if term.exponent > max_exp: max_exp = term.exponent max_exp_index = index temp.append(self.terms[max_exp_index]) self.terms.erase(max_exp_index) self.terms = temp
def __mul__(self, other): """Overloaded multiplication operator""" temp = Vector() for term in self.terms: for item in other.terms: new_coef = term.coef * item.coef new_exp = term.exponent + item.exponent temp.append(Term(new_coef, new_exp)) new_poly = Polynomial() new_poly.terms = temp new_poly.combine_like_terms() new_poly.sort_terms() return new_poly
def combine_like_terms(self): max_exponent = 0 temp = Vector() for term in self.terms: max_exponent = max(max_exponent, term.exponent) for exponent in range(max_exponent + 1): new_coef = 0 for term in self.terms: if term.exponent == exponent: new_coef += term.coef if (new_coef != 0): temp.append(Term(new_coef, exponent)) self.terms = temp self.sort_terms
def __init__(self,geom): self.length = float(geom.attrib.get('length')) self.hdg = float(geom.attrib['hdg']) # tangent = Vector(math.sin(self.hdg), math.cos(self.hdg),0) # self.hdg = math.radians(tangent.argument()) self.s = float(geom.attrib['s']) self.origin = Vector(float(geom.attrib['x']), float(geom.attrib['y']), 0) for child in geom: self.type = child.tag if(self.type == 'arc'): self.curvature = float(child.attrib['curvature']) if(self.type == 'spiral'): self.init_curvature = float(child.attrib['curvStart']) self.final_curvature = float(child.attrib['curvEnd'])
def test_vector_len(): vector = Vector(2, 4) assert vector.len() == 4.47213595499958
def test_check_type_exception(): with pytest.raises(TypeError): v1 = Vector() v1 == dir
def test_vector_sum_diff(): v1 = Vector(1.2, 2.3) v2 = Vector(3.4, 4.5) assert (v1 + v2) == Vector(4.6, 6.8) assert (v2 - v1) == Vector(2.2, 2.2)
def test_vector_constructor(x, y): vector = Vector(x, y) assert vector.x == x assert vector.y == y
def test_vector_to_string(): vector = Vector() assert str(vector) == '(0.0, 0.0)'