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)'
